/*
 * IMS IPSEC PCSCF module
 *
 * Copyright (C) 2018 Alexander Yosifov
 * Copyright (C) 2018 Tsvetomir Dimitrov
 *
 * This file is part of Kamailio, a free SIP server.
 *
 * Kamailio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version
 *
 * Kamailio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#include "ipsec.h"
#include "spi_gen.h"
#include "port_gen.h"

#include "../../core/dprint.h"
#include "../../core/mem/pkg.h"
#include "../../core/ip_addr.h"
#include "../../core/resolve.h"
#include "../ims_usrloc_pcscf/usrloc.h"

#include <errno.h>
#include <arpa/inet.h>
#include <libmnl/libmnl.h>
#include <linux/xfrm.h>
#include <time.h>


#define XFRM_TMPLS_BUF_SIZE 1024
#define NLMSG_BUF_SIZE 4096
#define NLMSG_DELETEALL_BUF_SIZE 8192

extern usrloc_api_t ul;

extern str ipsec_listen_addr;
extern str ipsec_listen_addr6;
extern ip_addr_t ipsec_listen_ip_addr;
extern ip_addr_t ipsec_listen_ip_addr6;
extern int xfrm_user_selector;

struct xfrm_buffer {
    char buf[NLMSG_DELETEALL_BUF_SIZE];
    int offset;
};

struct del_tunnels {
    void *contacts;
    struct xfrm_buffer delmsg_buf;
};

//
// This file contains all Linux specific IPSec code.
//

struct mnl_socket* init_mnl_socket()
{
    struct mnl_socket*  mnl_socket = mnl_socket_open(NETLINK_XFRM);
    if(NULL == mnl_socket) {
        LM_ERR("Error opening a MNL socket\n");
        return NULL;
    }

    if(mnl_socket_bind(mnl_socket, 0, MNL_SOCKET_AUTOPID) < 0) {
        LM_ERR("Error binding a MNL socket\n");
        close_mnl_socket(mnl_socket);
        return NULL;
    }

    return mnl_socket;
}

void close_mnl_socket(struct mnl_socket* sock)
{
    if(mnl_socket_close(sock) != 0) {
        LM_WARN("Error closing netlink socket\n");
    }
}

static void string_to_key(char* dst, const str key_string)
{
    int i = 0;
    char *pos = key_string.s;

    for (i = 0; i < key_string.len/2; i++) {
        sscanf(pos, "%2hhx", &dst[i]);
        pos += 2;
    }
}

static uint choose_nlmsg_seq (void)
{
    static double Tini=0;
    struct timespec ts;
    ksr_clock_gettime(&ts);
    return(1000*(ts.tv_sec - Tini + (ts.tv_nsec * 1E-9))); // us
}

int add_sa(struct mnl_socket* nl_sock, const struct ip_addr *src_addr_param, const struct ip_addr *dest_addr_param, int s_port, int d_port, int long id, str ck, str ik, str r_alg, str r_ealg)
{
    char l_msg_buf[MNL_SOCKET_BUFFER_SIZE];
    char l_auth_algo_buf[XFRM_TMPLS_BUF_SIZE];
    char l_enc_algo_buf[XFRM_TMPLS_BUF_SIZE];
    struct nlmsghdr* l_nlh = NULL;
    struct xfrm_usersa_info* l_xsainfo = NULL;

    struct xfrm_algo* l_auth_algo = NULL;
    struct xfrm_algo* l_enc_algo  = NULL;


    memset(l_msg_buf, 0, sizeof(l_msg_buf));
    memset(l_auth_algo_buf, 0, sizeof(l_auth_algo_buf));
    memset(l_enc_algo_buf, 0, sizeof(l_enc_algo_buf));

    // nlmsghdr initialization
    l_nlh = mnl_nlmsg_put_header(l_msg_buf);
    l_nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL;
    l_nlh->nlmsg_type = XFRM_MSG_NEWSA;
    l_nlh->nlmsg_seq = choose_nlmsg_seq();
    l_nlh->nlmsg_pid = id;

    // add Security association
    l_xsainfo = (struct xfrm_usersa_info*)mnl_nlmsg_put_extra_header(l_nlh, sizeof(struct xfrm_usersa_info));
    l_xsainfo->sel.family       = dest_addr_param->af;
    if(dest_addr_param->af == AF_INET6) {
        memcpy(l_xsainfo->sel.daddr.a6, dest_addr_param->u.addr32, sizeof(l_xsainfo->sel.daddr.a6));
        memcpy(l_xsainfo->sel.saddr.a6, src_addr_param->u.addr32, sizeof(l_xsainfo->sel.saddr.a6));
        l_xsainfo->sel.prefixlen_d  = 128;
        l_xsainfo->sel.prefixlen_s  = 128;
    }
    else {
        l_xsainfo->sel.daddr.a4     = dest_addr_param->u.addr32[0];
        l_xsainfo->sel.saddr.a4     = src_addr_param->u.addr32[0];
        l_xsainfo->sel.prefixlen_d  = 32;
        l_xsainfo->sel.prefixlen_s  = 32;
    }
    l_xsainfo->sel.dport        = htons(d_port);
    l_xsainfo->sel.dport_mask   = 0xFFFF;
    l_xsainfo->sel.sport        = htons(s_port);
    l_xsainfo->sel.sport_mask   = 0xFFFF;
    l_xsainfo->sel.user         = htonl(xfrm_user_selector);

    if(dest_addr_param->af == AF_INET6) {
        memcpy(l_xsainfo->id.daddr.a6, dest_addr_param->u.addr32, sizeof(l_xsainfo->id.daddr.a6));
        memcpy(l_xsainfo->saddr.a6, src_addr_param->u.addr32, sizeof(l_xsainfo->saddr.a6));
    }
    else {
        l_xsainfo->id.daddr.a4      = dest_addr_param->u.addr32[0];
        l_xsainfo->saddr.a4         = src_addr_param->u.addr32[0];
    }
    l_xsainfo->id.spi           = htonl(id);
    l_xsainfo->id.proto         = IPPROTO_ESP;

    l_xsainfo->lft.soft_byte_limit      = XFRM_INF;
    l_xsainfo->lft.hard_byte_limit      = XFRM_INF;
    l_xsainfo->lft.soft_packet_limit    = XFRM_INF;
    l_xsainfo->lft.hard_packet_limit    = XFRM_INF;
    l_xsainfo->reqid                    = id;
    l_xsainfo->family                   = dest_addr_param->af;
    l_xsainfo->mode                     = XFRM_MODE_TRANSPORT;
    l_xsainfo->replay_window            = 32;

    // Add authentication algorithm for this SA
    // 3GPP TS 33.203 Annex I
    // NOTE: hmac-md5-96 and des-ede3-cbc has been deprecated in Rel12+

    // The cast below is performed because alg_key from struct xfrm_algo is char[0]
    // The point is to provide a continuous chunk of memory with the key in it
    l_auth_algo = (struct xfrm_algo *)l_auth_algo_buf;

    // Set the proper algorithm by r_alg str
    if(strncasecmp(r_alg.s, "hmac-md5-96", r_alg.len) == 0) {
        strcpy(l_auth_algo->alg_name,"md5");
    }
    else if(strncasecmp(r_alg.s, "hmac-sha-1-96", r_alg.len) == 0) {
        strcpy(l_auth_algo->alg_name,"sha1");
    } else {
        // set default algorithm to sha1
        strcpy(l_auth_algo->alg_name,"sha1");
    }

    l_auth_algo->alg_key_len = ik.len * 4;
    string_to_key(l_auth_algo->alg_key, ik);

    mnl_attr_put(l_nlh, XFRMA_ALG_AUTH, sizeof(struct xfrm_algo) + l_auth_algo->alg_key_len, l_auth_algo);

    // add encription algorithm for this SA
    l_enc_algo = (struct xfrm_algo *)l_enc_algo_buf;
    // cipher_null, des,  des3_ede, aes
    strcpy(l_enc_algo->alg_name,"cipher_null");
    if (strncasecmp(r_ealg.s,"aes-cbc",r_ealg.len) == 0) {
        LM_DBG("Creating security associations: AES\n");
        strcpy(l_enc_algo->alg_name,"aes");
        l_enc_algo->alg_key_len = ck.len * 4;
        string_to_key(l_enc_algo->alg_key, ck);
    }
    else if (strncasecmp(r_ealg.s,"des-ede3-cbc",r_ealg.len) == 0) {
        LM_DBG("Creating security associations: DES, ck.len=%d\n",ck.len);
        strcpy(l_enc_algo->alg_name,"des3_ede");
        str ck1;
        ck1.s = pkg_malloc (128);
        strncpy(ck1.s,ck.s,32);
        strncat(ck1.s,ck.s,16);
        ck1.len=32+16;

        l_enc_algo->alg_key_len = ck1.len * 4;
        string_to_key(l_enc_algo->alg_key, ck1);

        pkg_free(ck1.s);
    }

    mnl_attr_put(l_nlh, XFRMA_ALG_CRYPT, sizeof(struct xfrm_algo) + l_enc_algo->alg_key_len, l_enc_algo);

    // send it to Netlink socket
    if(mnl_socket_sendto(nl_sock, l_nlh, l_nlh->nlmsg_len) < 0)
    {
        LM_ERR("Failed to send Netlink message for SA creation, error: %s\n", strerror(errno));
        return -3;
    }

    return 0;
}


int remove_sa(struct mnl_socket* nl_sock, str src_addr_param, str dest_addr_param, int s_port, int d_port, int long id, unsigned int af)
{
    char* src_addr = NULL;
    char* dest_addr = NULL;

    // convert input IP addresses to char*
    if((src_addr = pkg_malloc(src_addr_param.len+1)) == NULL) {
        LM_ERR("Error allocating memory for src addr during SA removal\n");
        return -1;
    }

    if((dest_addr = pkg_malloc(dest_addr_param.len+1)) == NULL) {
        pkg_free(src_addr);
        LM_ERR("Error allocating memory for dest addr during SA removal\n");
        return -2;
    }

    memset(src_addr, 0, src_addr_param.len+1);
    memset(dest_addr, 0, dest_addr_param.len+1);

    memcpy(src_addr, src_addr_param.s, src_addr_param.len);
    memcpy(dest_addr, dest_addr_param.s, dest_addr_param.len);

    struct {
        struct nlmsghdr n;
        struct xfrm_usersa_id   xsid;
        char buf[XFRM_TMPLS_BUF_SIZE];

    } req = {
        .n.nlmsg_len    = NLMSG_LENGTH(sizeof(req.xsid)),
        .n.nlmsg_flags  = NLM_F_REQUEST,
        .n.nlmsg_type   = XFRM_MSG_DELSA,
        .n.nlmsg_pid    = id,
        .xsid.spi       = htonl(id),
        .xsid.family    = af,
        .xsid.proto     = IPPROTO_ESP
    };

    xfrm_address_t saddr;
    memset(&saddr, 0, sizeof(saddr));

    if(af == AF_INET6){
        ip_addr_t ip_addr;

        if(str2ipxbuf(&dest_addr_param, &ip_addr) < 0){
            LM_ERR("Unable to convert dest address [%.*s]\n", dest_addr_param.len, dest_addr_param.s);
            pkg_free(src_addr);
            pkg_free(dest_addr);
            return -1;
        }
        memcpy(req.xsid.daddr.a6, ip_addr.u.addr32, sizeof(req.xsid.daddr.a6));

        memset(&ip_addr, 0, sizeof(ip_addr_t));
        if(str2ipxbuf(&src_addr_param, &ip_addr) < 0){
            LM_ERR("Unable to convert src address [%.*s]\n", src_addr_param.len, src_addr_param.s);
            pkg_free(src_addr);
            pkg_free(dest_addr);
            return -1;
        }
        memcpy(saddr.a6, ip_addr.u.addr32, sizeof(saddr.a6));
    }else{
        req.xsid.daddr.a4   = inet_addr(dest_addr);
        saddr.a4            = inet_addr(src_addr);
    }

    mnl_attr_put(&req.n, XFRMA_SRCADDR, sizeof(saddr), (void *)&saddr);

    if(mnl_socket_sendto(nl_sock, &req.n, req.n.nlmsg_len) < 0)
    {
        LM_ERR("Failed to send Netlink message, error: %s\n", strerror(errno));
        pkg_free(src_addr);
        pkg_free(dest_addr);
        return -1;
    }

    pkg_free(src_addr);
    pkg_free(dest_addr);

    return 0;
}


int add_policy(struct mnl_socket* mnl_socket, const struct ip_addr *src_addr_param, const struct ip_addr *dest_addr_param, int src_port, int dst_port, int long p_id, enum ipsec_policy_direction dir)
{
    char                            l_msg_buf[MNL_SOCKET_BUFFER_SIZE];
    char                            l_tmpls_buf[XFRM_TMPLS_BUF_SIZE];
    struct nlmsghdr*                l_nlh;
    struct xfrm_userpolicy_info*    l_xpinfo;

    memset(l_msg_buf, 0, sizeof(l_msg_buf));
    memset(l_tmpls_buf, 0, sizeof(l_tmpls_buf));

    // nlmsghdr initialization
    l_nlh = mnl_nlmsg_put_header(l_msg_buf);
    l_nlh->nlmsg_flags  = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL;
    l_nlh->nlmsg_type   = XFRM_MSG_NEWPOLICY;
    l_nlh->nlmsg_seq    = time(NULL);
    l_nlh->nlmsg_pid    = p_id;

    // add OUT policy
    l_xpinfo = (struct xfrm_userpolicy_info*)mnl_nlmsg_put_extra_header(l_nlh, sizeof(struct xfrm_userpolicy_info));
    l_xpinfo->sel.family        = dest_addr_param->af;
    if(dest_addr_param->af == AF_INET6) {
        memcpy(l_xpinfo->sel.daddr.a6, dest_addr_param->u.addr32, sizeof(l_xpinfo->sel.daddr.a6));
        memcpy(l_xpinfo->sel.saddr.a6, src_addr_param->u.addr32, sizeof(l_xpinfo->sel.saddr.a6));
        l_xpinfo->sel.prefixlen_d  = 128;
        l_xpinfo->sel.prefixlen_s  = 128;
    }
    else {
        l_xpinfo->sel.daddr.a4     = dest_addr_param->u.addr32[0];
        l_xpinfo->sel.saddr.a4     = src_addr_param->u.addr32[0];
        l_xpinfo->sel.prefixlen_d  = 32;
        l_xpinfo->sel.prefixlen_s  = 32;
    }
    l_xpinfo->sel.dport         = htons(dst_port);
    l_xpinfo->sel.dport_mask    = 0xFFFF;
    l_xpinfo->sel.sport         = htons(src_port);
    l_xpinfo->sel.sport_mask    = 0xFFFF;
    //l_xpinfo->sel.proto         = sel_proto;
    l_xpinfo->sel.user          = htonl(xfrm_user_selector);

    l_xpinfo->lft.soft_byte_limit   = XFRM_INF;
    l_xpinfo->lft.hard_byte_limit   = XFRM_INF;
    l_xpinfo->lft.soft_packet_limit = XFRM_INF;
    l_xpinfo->lft.hard_packet_limit = XFRM_INF;
    l_xpinfo->priority              = 2080;
    l_xpinfo->action                = XFRM_POLICY_ALLOW;
    l_xpinfo->share                 = XFRM_SHARE_ANY;

    if (dir == IPSEC_POLICY_DIRECTION_IN) {
        l_xpinfo->dir               = XFRM_POLICY_IN;
    }
    else if(dir == IPSEC_POLICY_DIRECTION_OUT) {
        l_xpinfo->dir               = XFRM_POLICY_OUT;
    }
    else {
        LM_ERR("Invalid direction parameter passed to add_policy: %d\n", dir);

        return -3;
    }

    // xfrm_user_tmpl initialization
    struct xfrm_user_tmpl* l_tmpl = (struct xfrm_user_tmpl*)l_tmpls_buf;
    l_tmpl->id.proto    = IPPROTO_ESP;
    l_tmpl->family      = dest_addr_param->af;
    if(dest_addr_param->af == AF_INET6) {
        memcpy(l_tmpl->id.daddr.a6, dest_addr_param->u.addr32, sizeof(l_tmpl->id.daddr.a6));
        memcpy(l_tmpl->saddr.a6, src_addr_param->u.addr32, sizeof(l_tmpl->saddr.a6));
    }
    else {
        l_tmpl->id.daddr.a4        = dest_addr_param->u.addr32[0];
        l_tmpl->saddr.a4           = src_addr_param->u.addr32[0];
    }
    l_tmpl->reqid       = p_id;
    l_tmpl->mode        = XFRM_MODE_TRANSPORT;
    l_tmpl->aalgos      = (~(__u32)0);
    l_tmpl->ealgos      = (~(__u32)0);
    l_tmpl->calgos      = (~(__u32)0);

    mnl_attr_put(l_nlh, XFRMA_TMPL, sizeof(struct xfrm_user_tmpl), l_tmpl);

    if(mnl_socket_sendto(mnl_socket, l_nlh, l_nlh->nlmsg_len) < 0)
    {
        LM_ERR("Failed to send Netlink message, error: %s\n", strerror(errno));
        return -4;
    }

    return 0;
}

int remove_policy(struct mnl_socket* mnl_socket, str src_addr_param, str dest_addr_param, int src_port, int dst_port, int long p_id, unsigned int af, enum ipsec_policy_direction dir)
{
    unsigned char policy_dir = 0;

    if(dir == IPSEC_POLICY_DIRECTION_IN) {
         policy_dir = XFRM_POLICY_IN;
    }
    else if(dir == IPSEC_POLICY_DIRECTION_OUT) {
         policy_dir = XFRM_POLICY_OUT;
    }
    else {
        LM_ERR("Invalid direction parameter passed to remove_policy: %d\n", dir);
        return -1;
    }

    char* src_addr = NULL;
    char* dest_addr = NULL;

    // convert input IP addresses to char*
    if((src_addr = pkg_malloc(src_addr_param.len+1)) == NULL) {
        LM_ERR("Error allocating memory for src addr during SA removal\n");
        return -1;
    }

    if((dest_addr = pkg_malloc(dest_addr_param.len+1)) == NULL) {
        pkg_free(src_addr);
        LM_ERR("Error allocating memory for dest addr during SA removal\n");
        return -2;
    }

    memset(src_addr, 0, src_addr_param.len+1);
    memset(dest_addr, 0, dest_addr_param.len+1);

    memcpy(src_addr, src_addr_param.s, src_addr_param.len);
    memcpy(dest_addr, dest_addr_param.s, dest_addr_param.len);

    struct {
        struct nlmsghdr n;
        struct xfrm_userpolicy_id xpid;
        char buf[XFRM_TMPLS_BUF_SIZE];
    } req = {
        .n.nlmsg_len            = NLMSG_LENGTH(sizeof(req.xpid)),
        .n.nlmsg_flags          = NLM_F_REQUEST,
        .n.nlmsg_type           = XFRM_MSG_DELPOLICY,
        .n.nlmsg_pid            = p_id,
        .xpid.dir               = policy_dir,
        .xpid.sel.family        = af,
        .xpid.sel.dport         = htons(dst_port),
        .xpid.sel.dport_mask    = 0xFFFF,
        .xpid.sel.sport         = htons(src_port),
        .xpid.sel.sport_mask    = 0xFFFF,
        .xpid.sel.user          = htonl(xfrm_user_selector)
        //.xpid.sel.proto         = sel_proto
    };

    if(af == AF_INET6){
        ip_addr_t ip_addr;

        if(str2ipxbuf(&dest_addr_param, &ip_addr) < 0){
            LM_ERR("Unable to convert dest address [%.*s]\n", dest_addr_param.len, dest_addr_param.s);
            pkg_free(src_addr);
            pkg_free(dest_addr);
            return -1;
        }
        memcpy(req.xpid.sel.daddr.a6, ip_addr.u.addr32, sizeof(req.xpid.sel.daddr.a6));

        if(str2ipxbuf(&src_addr_param, &ip_addr) < 0){
            LM_ERR("Unable to convert src address [%.*s]\n", src_addr_param.len, src_addr_param.s);
            pkg_free(src_addr);
            pkg_free(dest_addr);
            return -1;
        }
        memcpy(req.xpid.sel.saddr.a6, ip_addr.u.addr32, sizeof(req.xpid.sel.saddr.a6));

        req.xpid.sel.prefixlen_d = 128;
        req.xpid.sel.prefixlen_s = 128;
    }else{
        req.xpid.sel.daddr.a4       = inet_addr(dest_addr);
        req.xpid.sel.saddr.a4       = inet_addr(src_addr);

        req.xpid.sel.prefixlen_d    = 32;
        req.xpid.sel.prefixlen_s    = 32;
    }

    if(mnl_socket_sendto(mnl_socket, &req.n, req.n.nlmsg_len) < 0)
    {
        LM_ERR("Failed to send Netlink message, error: %s\n", strerror(errno));
        pkg_free(src_addr);
        pkg_free(dest_addr);
        return -1;
    }

    pkg_free(src_addr);
    pkg_free(dest_addr);

    return 0;
}

static int delsa_data_cb(const struct nlmsghdr *nlh, void *data)
{
    struct xfrm_usersa_info *xsinfo = NLMSG_DATA(nlh);
    int xfrm_userid = ntohl(xsinfo->sel.user);

    //Check if user id is different from Kamailio's
    if(xfrm_userid != xfrm_user_selector)
        return MNL_CB_OK;

    struct xfrm_buffer* delmsg_buf = (struct xfrm_buffer*)data;
    uint32_t new_delmsg_len = NLMSG_LENGTH(sizeof(struct xfrm_usersa_id));

    if(delmsg_buf->offset + new_delmsg_len > sizeof(delmsg_buf->buf)/sizeof(delmsg_buf->buf[0])) {
        LM_ERR("Not enough memory allocated for delete SAs netlink command\n");
        return MNL_CB_ERROR;
    }

    struct nlmsghdr *new_delmsg = (struct nlmsghdr *)&delmsg_buf->buf[delmsg_buf->offset];
    new_delmsg->nlmsg_len = new_delmsg_len;
    new_delmsg->nlmsg_flags = NLM_F_REQUEST;
    new_delmsg->nlmsg_type = XFRM_MSG_DELSA;
    new_delmsg->nlmsg_seq = time(NULL);

    struct xfrm_usersa_id *xsid = NLMSG_DATA(new_delmsg);
    xsid->family = xsinfo->family;
    memcpy(&xsid->daddr, &xsinfo->id.daddr, sizeof(xsid->daddr));
    xsid->spi = xsinfo->id.spi;
    xsid->proto = xsinfo->id.proto;

    mnl_attr_put(new_delmsg, XFRMA_SRCADDR, sizeof(xsid->daddr), &xsinfo->saddr);

    delmsg_buf->offset += new_delmsg->nlmsg_len;

    return MNL_CB_OK;
}

static int delpolicy_data_cb(const struct nlmsghdr *nlh, void *data)
{
    struct xfrm_userpolicy_info *xpinfo = NLMSG_DATA(nlh);
    int xfrm_userid = ntohl(xpinfo->sel.user);

    //Check if user id is different from Kamailio's
    if(xfrm_userid != xfrm_user_selector)
        return MNL_CB_OK;

    struct xfrm_buffer* delmsg_buf = (struct xfrm_buffer*)data;
    uint32_t new_delmsg_len = NLMSG_LENGTH(sizeof(struct xfrm_userpolicy_id));

    if(delmsg_buf->offset + new_delmsg_len > sizeof(delmsg_buf->buf)/sizeof(delmsg_buf->buf[0])) {
        LM_ERR("Not enough memory allocated for delete policies netlink command\n");
        return MNL_CB_ERROR;
    }

    struct nlmsghdr *new_delmsg = (struct nlmsghdr *)&delmsg_buf->buf[delmsg_buf->offset];
    new_delmsg->nlmsg_len = new_delmsg_len;
    new_delmsg->nlmsg_flags = NLM_F_REQUEST;
    new_delmsg->nlmsg_type = XFRM_MSG_DELPOLICY;
    new_delmsg->nlmsg_seq = time(NULL);

    struct xfrm_userpolicy_id *xpid = NLMSG_DATA(new_delmsg);
    memcpy(&xpid->sel, &xpinfo->sel, sizeof(xpid->sel));
    xpid->dir = xpinfo->dir;
    xpid->index = xpinfo->index;

    delmsg_buf->offset += new_delmsg->nlmsg_len;

    return MNL_CB_OK;
}

int clean_sa(struct mnl_socket*  mnl_socket)
{
    struct {
        struct nlmsghdr n;
        //char buf[NLMSG_DELETEALL_BUF_SIZE];
    } req = {
        .n.nlmsg_len = NLMSG_HDRLEN,
        .n.nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST,
        .n.nlmsg_type = XFRM_MSG_GETSA,
        .n.nlmsg_seq = time(NULL),
    };

    if(mnl_socket_sendto(mnl_socket, &req, req.n.nlmsg_len) == -1) {
        LM_ERR("Error sending get all SAs command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    char buf[NLMSG_DELETEALL_BUF_SIZE];
    memset(&buf, 0, sizeof(buf));

    struct xfrm_buffer delmsg_buf;
    memset(&delmsg_buf, 0, sizeof(struct xfrm_buffer));

    int ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    while (ret > 0) {
        ret = mnl_cb_run(buf, ret, req.n.nlmsg_seq, mnl_socket_get_portid(mnl_socket), delsa_data_cb, &delmsg_buf);
        if (ret <= MNL_CB_STOP) {

            break;
        }
        ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    }

    // DELETE SAs
    if(mnl_socket_sendto(mnl_socket, &delmsg_buf.buf, delmsg_buf.offset) == -1) {
        LM_ERR("Error sending delete SAs command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    return 0;
}

int clean_policy(struct mnl_socket*  mnl_socket)
{
    struct {
        struct nlmsghdr n;
        //char buf[NLMSG_DELETEALL_BUF_SIZE];
    } req = {
        .n.nlmsg_len = NLMSG_HDRLEN,
        .n.nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST,
        .n.nlmsg_type = XFRM_MSG_GETPOLICY,
        .n.nlmsg_seq = time(NULL),
    };

    if(mnl_socket_sendto(mnl_socket, &req, req.n.nlmsg_len) == -1) {
        LM_ERR("Error sending get all policies command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    char buf[NLMSG_DELETEALL_BUF_SIZE];
    memset(&buf, 0, sizeof(buf));

    struct xfrm_buffer delmsg_buf;
    memset(&delmsg_buf, 0, sizeof(struct xfrm_buffer));

    int ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    while (ret > 0) {
        ret = mnl_cb_run(buf, ret, req.n.nlmsg_seq, mnl_socket_get_portid(mnl_socket), delpolicy_data_cb, &delmsg_buf);
        if (ret <= MNL_CB_STOP) {

            break;
        }
        ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    }

    // DELETE POLICIES
    if(mnl_socket_sendto(mnl_socket, &delmsg_buf.buf, delmsg_buf.offset) == -1) {
        LM_ERR("Error sending delete policies command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    return 0;
}

static int delete_unused_sa_cb(const struct nlmsghdr *nlh, void *data)
{
    struct xfrm_usersa_info *xsinfo = NLMSG_DATA(nlh);

    // Check if user id is different from Kamailio's
    if (ntohl(xsinfo->sel.user) != xfrm_user_selector) {
        return MNL_CB_OK;
    }

    ip_addr_t sa_src_addr, sa_dst_addr;
    unsigned int sa_spi;
    unsigned short sa_sport, sa_dport;

    ip_addr_t *proxy_ip_addr = NULL;

    sa_src_addr.af = sa_dst_addr.af = xsinfo->sel.family;
    if(xsinfo->sel.family == AF_INET6) {
        memcpy(sa_src_addr.u.addr32, xsinfo->sel.saddr.a6, sizeof(xsinfo->sel.saddr.a6));
        memcpy(sa_dst_addr.u.addr32, xsinfo->sel.daddr.a6, sizeof(xsinfo->sel.daddr.a6));
        sa_src_addr.len = sa_dst_addr.len = 16;

        proxy_ip_addr = &ipsec_listen_ip_addr6;
    } else if(xsinfo->sel.family == AF_INET) {
        sa_src_addr.u.addr32[0] = xsinfo->sel.saddr.a4;
        sa_dst_addr.u.addr32[0] = xsinfo->sel.daddr.a4;
        sa_src_addr.len = sa_dst_addr.len = 4;

        proxy_ip_addr = &ipsec_listen_ip_addr;
    } else {
        LM_CRIT("Unknown AF %d\n", xsinfo->sel.family);
        return MNL_CB_OK;
    }

    sa_spi = htonl(xsinfo->id.spi);
    sa_sport = htons(xsinfo->sel.sport);
    sa_dport = htons(xsinfo->sel.dport);

    char saddr_buf[IP_ADDR_MAX_STRZ_SIZE];
    char daddr_buf[IP_ADDR_MAX_STRZ_SIZE];
    memset(saddr_buf, 0, IP_ADDR_MAX_STRZ_SIZE);
    memset(daddr_buf, 0, IP_ADDR_MAX_STRZ_SIZE);
    ip_addr2sbufz(&sa_src_addr, saddr_buf, IP_ADDR_MAX_STRZ_SIZE);
    ip_addr2sbufz(&sa_dst_addr, daddr_buf, IP_ADDR_MAX_STRZ_SIZE);

//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |received host.len|received host.s|spi_uc|spi_us|spi_pc|spi_ps|port_uc|port_us|port_pc|port_ps|
//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |received host.len|received host.s|spi_uc|spi_us|spi_pc|spi_ps|port_uc|port_us|port_pc|port_ps|
//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |.............................................................................................|
//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |0000|
//  * +----+
    void  *cp;
    str received_host;
    ip_addr_t received_host_addr;
    ipsec_t ipsec = {0};

    cp = ((struct del_tunnels*)data)->contacts;
    while (1) {
        memcpy(&(received_host.len), cp, sizeof(received_host.len));
        if (received_host.len == 0) {
            break; // no more records
        }

        received_host.s = (char*)cp + sizeof(received_host.len);
        cp = (char*)cp + sizeof(received_host.len) + received_host.len;

        memcpy(&ipsec.spi_uc, cp, sizeof(ipsec.spi_uc));
        cp = (char*)cp + sizeof(ipsec.spi_uc);

        memcpy(&ipsec.spi_us, cp, sizeof(ipsec.spi_us));
        cp = (char*)cp + sizeof(ipsec.spi_us);

        memcpy(&ipsec.spi_pc, cp, sizeof(ipsec.spi_pc));
        cp = (char*)cp + sizeof(ipsec.spi_pc);

        memcpy(&ipsec.spi_ps, cp, sizeof(ipsec.spi_ps));
        cp = (char*)cp + sizeof(ipsec.spi_ps);

        memcpy(&ipsec.port_uc, cp, sizeof(ipsec.port_uc));
        cp = (char*)cp + sizeof(ipsec.port_uc);

        memcpy(&ipsec.port_us, cp, sizeof(ipsec.port_us));
        cp = (char*)cp + sizeof(ipsec.port_us);

        memcpy(&ipsec.port_pc, cp, sizeof(ipsec.port_pc));
        cp = (char*)cp + sizeof(ipsec.port_pc);

        memcpy(&ipsec.port_ps, cp, sizeof(ipsec.port_ps));
        cp = (char*)cp + sizeof(ipsec.port_ps);

        // convert 'received host' ip string to ip_addr_t
        if (str2ipxbuf(&received_host, &received_host_addr) < 0){
            LM_ERR("Unable to convert received host [%.*s]\n", received_host.len, received_host.s);
            return 1;
        }

        // SA  Src address  Dst address Src port    Dst port        SPI
        // SA1 UE-IP        Proxy-IP    UE-Server   Proxy-Client    Proxy-Client
        // SA2 UE-IP        Proxy-IP    UE-Client   Proxy-Server    Proxy-Server
        // SA3 Proxy-IP     UE-IP       P-Server    UE-Client       UE-Client
        // SA4 Proxy-IP     UE-IP       P-Client    UE-Server       UE-Server

        // Check for SA1 or SA2
        if(ip_addr_cmp(&sa_src_addr, &received_host_addr) && ip_addr_cmp(&sa_dst_addr, proxy_ip_addr)) {
            // check for SA1
            if(sa_sport == ipsec.port_us && sa_dport == ipsec.port_pc && sa_spi == ipsec.spi_pc){
                return MNL_CB_OK;
            }

            // check for SA2
            if(sa_sport == ipsec.port_uc && sa_dport == ipsec.port_ps && sa_spi == ipsec.spi_ps){
                return MNL_CB_OK;
            }
        }

        // Check for SA3 or SA4
        if(ip_addr_cmp(&sa_src_addr, proxy_ip_addr) && ip_addr_cmp(&sa_dst_addr, &received_host_addr)) {
            // check for SA3
            if(sa_sport == ipsec.port_ps && sa_dport == ipsec.port_uc && sa_spi == ipsec.spi_uc){
                return MNL_CB_OK;
            }

            // check for SA4
            if(sa_sport == ipsec.port_pc && sa_dport == ipsec.port_us && sa_spi == ipsec.spi_us){
                return MNL_CB_OK;
            }
        }
    }

    LM_DBG("The SA is not used and will be deleted: spi:%u | saddr:%s:%u | daddr:%s:%u\n", sa_spi, saddr_buf, sa_sport, daddr_buf, sa_dport);

    struct xfrm_buffer* delmsg_buf = &((struct del_tunnels*)data)->delmsg_buf;
    uint32_t new_delmsg_len = NLMSG_LENGTH(sizeof(struct xfrm_usersa_id));

    if(delmsg_buf->offset + new_delmsg_len > sizeof(delmsg_buf->buf)/sizeof(delmsg_buf->buf[0])) {
        LM_ERR("Not enough memory allocated for delete SAs netlink command\n");
        return MNL_CB_ERROR;
    }

    struct nlmsghdr *new_delmsg = (struct nlmsghdr *)&delmsg_buf->buf[delmsg_buf->offset];
    new_delmsg->nlmsg_len = new_delmsg_len;
    new_delmsg->nlmsg_flags = NLM_F_REQUEST;
    new_delmsg->nlmsg_type = XFRM_MSG_DELSA;
    new_delmsg->nlmsg_seq = time(NULL);

    struct xfrm_usersa_id *xsid = NLMSG_DATA(new_delmsg);
    xsid->family = xsinfo->family;
    memcpy(&xsid->daddr, &xsinfo->id.daddr, sizeof(xsid->daddr));
    xsid->spi = xsinfo->id.spi;
    xsid->proto = xsinfo->id.proto;

    mnl_attr_put(new_delmsg, XFRMA_SRCADDR, sizeof(xsid->daddr), &xsinfo->saddr);

    delmsg_buf->offset += new_delmsg->nlmsg_len;

    // NOTE: Release the Proxy SPIs and Ports only here. Do not release the same SPIs and ports in delete unsused policy callback.
    // Release SPIs
    release_spi(ipsec.spi_pc);
    release_spi(ipsec.spi_ps);

    // Release the client and the server ports
    release_cport(ipsec.port_pc);
    release_sport(ipsec.port_ps);

    return MNL_CB_OK;
}

static int delete_unused_sa(struct mnl_socket *mnl_socket, void *contacts)
{
    struct {
        struct nlmsghdr n;
        //char buf[NLMSG_DELETEALL_BUF_SIZE];
    } req = {
        .n.nlmsg_len = NLMSG_HDRLEN,
        .n.nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST,
        .n.nlmsg_type = XFRM_MSG_GETSA,
        .n.nlmsg_seq = time(NULL),
    };

    if(mnl_socket_sendto(mnl_socket, &req, req.n.nlmsg_len) == -1) {
        LM_ERR("Error sending get all SAs command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    char buf[NLMSG_DELETEALL_BUF_SIZE];
    memset(&buf, 0, sizeof(buf));

    struct del_tunnels del_data;
    del_data.contacts = contacts;
    memset(&del_data.delmsg_buf, 0, sizeof(del_data.delmsg_buf));

    int ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    while (ret > 0) {
        ret = mnl_cb_run(buf, ret, req.n.nlmsg_seq, mnl_socket_get_portid(mnl_socket), delete_unused_sa_cb, &del_data);
        if (ret <= MNL_CB_STOP) {
            break;
        }

        ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    }

    // DELETE unused SAs
    if(mnl_socket_sendto(mnl_socket, &del_data.delmsg_buf.buf, del_data.delmsg_buf.offset) == -1) {
        LM_ERR("Error sending delete unused SAs command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    return 0;
}

static int delete_unused_policy_cb(const struct nlmsghdr *nlh, void *data)
{
    struct xfrm_userpolicy_info *xpinfo = NLMSG_DATA(nlh);

    //Check if user id is different from Kamailio's
    if (ntohl(xpinfo->sel.user) != xfrm_user_selector) {
        return MNL_CB_OK;
    }

    ip_addr_t sa_src_addr, sa_dst_addr;
    unsigned short sa_sport, sa_dport;

    ip_addr_t *proxy_ip_addr = NULL;

    sa_src_addr.af = sa_dst_addr.af = xpinfo->sel.family;
    if(xpinfo->sel.family == AF_INET6) {
        memcpy(sa_src_addr.u.addr32, xpinfo->sel.saddr.a6, sizeof(xpinfo->sel.saddr.a6));
        memcpy(sa_dst_addr.u.addr32, xpinfo->sel.daddr.a6, sizeof(xpinfo->sel.daddr.a6));
        sa_src_addr.len = sa_dst_addr.len = 16;

        proxy_ip_addr = &ipsec_listen_ip_addr6;
    } else if(xpinfo->sel.family == AF_INET) {
        sa_src_addr.u.addr32[0] = xpinfo->sel.saddr.a4;
        sa_dst_addr.u.addr32[0] = xpinfo->sel.daddr.a4;
        sa_src_addr.len = sa_dst_addr.len = 4;

        proxy_ip_addr = &ipsec_listen_ip_addr;
    } else {
        LM_CRIT("Unknown AF %d\n", xpinfo->sel.family);
        return MNL_CB_OK;
    }

    sa_sport = htons(xpinfo->sel.sport);
    sa_dport = htons(xpinfo->sel.dport);

    char saddr_buf[IP_ADDR_MAX_STRZ_SIZE];
    char daddr_buf[IP_ADDR_MAX_STRZ_SIZE];
    memset(saddr_buf, 0, IP_ADDR_MAX_STRZ_SIZE);
    memset(daddr_buf, 0, IP_ADDR_MAX_STRZ_SIZE);
    ip_addr2sbufz(&sa_src_addr, saddr_buf, IP_ADDR_MAX_STRZ_SIZE);
    ip_addr2sbufz(&sa_dst_addr, daddr_buf, IP_ADDR_MAX_STRZ_SIZE);

//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |received host.len|received host.s|spi_uc|spi_us|spi_pc|spi_ps|port_uc|port_us|port_pc|port_ps|
//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |received host.len|received host.s|spi_uc|spi_us|spi_pc|spi_ps|port_uc|port_us|port_pc|port_ps|
//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |.............................................................................................|
//  * +-----------------+---------------+------+------+------+------+-------+-------+-------+-------+
//  * |0000|
//  * +----+
    void  *cp;
    str received_host;
    ip_addr_t received_host_addr;
    ipsec_t ipsec;

    cp = ((struct del_tunnels*)data)->contacts;
    while (1) {
        memcpy(&(received_host.len), cp, sizeof(received_host.len));
        if (received_host.len == 0) {
            break; // no more records
        }

        received_host.s = (char*)cp + sizeof(received_host.len);
        cp = (char*)cp + sizeof(received_host.len) + received_host.len;

        memcpy(&ipsec.spi_uc, cp, sizeof(ipsec.spi_uc));
        cp = (char*)cp + sizeof(ipsec.spi_uc);

        memcpy(&ipsec.spi_us, cp, sizeof(ipsec.spi_us));
        cp = (char*)cp + sizeof(ipsec.spi_us);

        memcpy(&ipsec.spi_pc, cp, sizeof(ipsec.spi_pc));
        cp = (char*)cp + sizeof(ipsec.spi_pc);

        memcpy(&ipsec.spi_ps, cp, sizeof(ipsec.spi_ps));
        cp = (char*)cp + sizeof(ipsec.spi_ps);

        memcpy(&ipsec.port_uc, cp, sizeof(ipsec.port_uc));
        cp = (char*)cp + sizeof(ipsec.port_uc);

        memcpy(&ipsec.port_us, cp, sizeof(ipsec.port_us));
        cp = (char*)cp + sizeof(ipsec.port_us);

        memcpy(&ipsec.port_pc, cp, sizeof(ipsec.port_pc));
        cp = (char*)cp + sizeof(ipsec.port_pc);

        memcpy(&ipsec.port_ps, cp, sizeof(ipsec.port_ps));
        cp = (char*)cp + sizeof(ipsec.port_ps);

        // convert 'received host' ip string to ip_addr_t
        if (str2ipxbuf(&received_host, &received_host_addr) < 0){
            LM_ERR("Unable to convert received host [%.*s]\n", received_host.len, received_host.s);
            return 1;
        }

        // Policy  Src address  Dst address Src port    Dst port
        // Policy1 UE-IP        Proxy-IP    UE-Server   Proxy-Client
        // Policy2 UE-IP        Proxy-IP    UE-Client   Proxy-Server
        // Policy3 Proxy-IP     UE-IP       P-Server    UE-Client
        // Policy4 Proxy-IP     UE-IP       P-Client    UE-Server

        // Check for Policy1 or Policy2
        if(ip_addr_cmp(&sa_src_addr, &received_host_addr) && ip_addr_cmp(&sa_dst_addr, proxy_ip_addr)) {
            // check for SA1
            if(sa_sport == ipsec.port_us && sa_dport == ipsec.port_pc){
                return MNL_CB_OK;
            }

            // check for SA2
            if(sa_sport == ipsec.port_uc && sa_dport == ipsec.port_ps){
                return MNL_CB_OK;
            }
        }

        // Check for Policy3 or Policy4
        if(ip_addr_cmp(&sa_src_addr, proxy_ip_addr) && ip_addr_cmp(&sa_dst_addr, &received_host_addr)) {
            // check for SA3
            if(sa_sport == ipsec.port_ps && sa_dport == ipsec.port_uc){
                return MNL_CB_OK;
            }

            // check for SA4
            if(sa_sport == ipsec.port_pc && sa_dport == ipsec.port_us){
                return MNL_CB_OK;
            }
        }
    }

    LM_DBG("The POLICY is not used and will be deleted: saddr:%s:%u | daddr:%s:%u\n", saddr_buf, sa_sport, daddr_buf, sa_dport);

    struct xfrm_buffer* delmsg_buf = &((struct del_tunnels*)data)->delmsg_buf;
    uint32_t new_delmsg_len = NLMSG_LENGTH(sizeof(struct xfrm_userpolicy_id));

    if(delmsg_buf->offset + new_delmsg_len > sizeof(delmsg_buf->buf)/sizeof(delmsg_buf->buf[0])) {
        LM_ERR("Not enough memory allocated for delete policies netlink command\n");
        return MNL_CB_ERROR;
    }

    struct nlmsghdr *new_delmsg = (struct nlmsghdr *)&delmsg_buf->buf[delmsg_buf->offset];
    new_delmsg->nlmsg_len = new_delmsg_len;
    new_delmsg->nlmsg_flags = NLM_F_REQUEST;
    new_delmsg->nlmsg_type = XFRM_MSG_DELPOLICY;
    new_delmsg->nlmsg_seq = time(NULL);

    struct xfrm_userpolicy_id *xpid = NLMSG_DATA(new_delmsg);
    memcpy(&xpid->sel, &xpinfo->sel, sizeof(xpid->sel));
    xpid->dir = xpinfo->dir;
    xpid->index = xpinfo->index;

    delmsg_buf->offset += new_delmsg->nlmsg_len;

    return MNL_CB_OK;
}

static int delete_unused_policy(struct mnl_socket *mnl_socket, void *contacts)
{
    struct {
        struct nlmsghdr n;
        //char buf[NLMSG_DELETEALL_BUF_SIZE];
    } req = {
        .n.nlmsg_len = NLMSG_HDRLEN,
        .n.nlmsg_flags = NLM_F_DUMP | NLM_F_REQUEST,
        .n.nlmsg_type = XFRM_MSG_GETPOLICY,
        .n.nlmsg_seq = time(NULL),
    };

    if(mnl_socket_sendto(mnl_socket, &req, req.n.nlmsg_len) == -1) {
        LM_ERR("Error sending get all POLICY command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    char buf[NLMSG_DELETEALL_BUF_SIZE];
    memset(&buf, 0, sizeof(buf));

    struct del_tunnels del_data;
    del_data.contacts = contacts;
    memset(&del_data.delmsg_buf, 0, sizeof(del_data.delmsg_buf));

    int ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    while (ret > 0) {
        ret = mnl_cb_run(buf, ret, req.n.nlmsg_seq, mnl_socket_get_portid(mnl_socket), delete_unused_policy_cb, &del_data);
        if (ret <= MNL_CB_STOP) {
            break;
        }

        ret = mnl_socket_recvfrom(mnl_socket, buf, sizeof(buf));
    }

    // DELETE unused POLICIES
    if(mnl_socket_sendto(mnl_socket, &del_data.delmsg_buf.buf, del_data.delmsg_buf.offset) == -1) {
        LM_ERR("Error sending delete unused policies command via netlink socket: %s\n", strerror(errno));
        return 1;
    }

    return 0;
}

int delete_unused_tunnels()
{
    int rval, len = 0;
    void *buf = NULL;

    // first try to fetch required size for all contacts
    rval = ul.get_all_ucontacts(buf, len, 0, 0, 1);
    LM_DBG("Minimum required size %d\n", rval);
    if (rval < 0) {
        LM_ERR("Failed to fetch contacts\n");
        return 1;
    }

    if (rval > 0) {
        len = rval * 2;
        buf = malloc(len);
        if (buf == NULL) {
            LM_ERR("Out of memory\n");
            return 1;
        }

        rval = ul.get_all_ucontacts(buf, len, 0, 0, 1);
        if (rval != 0) {
            free(buf);
            return 1;
        }
    }
    if (buf == NULL) {
        return 1;
    }

    struct mnl_socket* sock = init_mnl_socket();
    if (sock == NULL) {
        LM_ERR("Can't init mnl socket\n");
        free(buf);
        return 1;
    }

    delete_unused_sa(sock, buf);
    delete_unused_policy(sock, buf);

    close_mnl_socket(sock);

    free(buf);
    return 0;
}
