From 79df5c780d3af4cf4413653142115767f5751740 Mon Sep 17 00:00:00 2001 From: Andreas Eversberg Date: Mon, 8 Apr 2024 20:34:22 +0200 Subject: [PATCH] WIP VoLTE support for Asterisk --- build_tools/menuselect-deps.in | 1 + res/Makefile | 1 + res/res_pjsip_volte.c | 200 +++++++++++-- res/res_pjsip_volte/netlink_xfrm.c | 439 +++++++++++++++++++++++++++++ res/res_pjsip_volte/netlink_xfrm.h | 33 +++ 5 files changed, 646 insertions(+), 28 deletions(-) create mode 100644 res/res_pjsip_volte/netlink_xfrm.c create mode 100644 res/res_pjsip_volte/netlink_xfrm.h diff --git a/build_tools/menuselect-deps.in b/build_tools/menuselect-deps.in index 1d9703419a..3922b631a9 100644 --- a/build_tools/menuselect-deps.in +++ b/build_tools/menuselect-deps.in @@ -58,6 +58,7 @@ SPEEX=@PBX_SPEEX@ SPEEXDSP=@PBX_SPEEXDSP@ SPEEX_PREPROCESS=@PBX_SPEEX_PREPROCESS@ VEVS=@PBX_VEVS@ +LIBMNL=@PBX_LIBMNL@ SQLITE3=@PBX_SQLITE3@ SRTP=@PBX_SRTP@ SS7=@PBX_SS7@ diff --git a/res/Makefile b/res/Makefile index 96c95a69cc..7aaf1ddda9 100644 --- a/res/Makefile +++ b/res/Makefile @@ -68,6 +68,7 @@ $(call MOD_ADD_C,res_stasis_recording,stasis_recording/stored.c) $(call MOD_ADD_C,res_stir_shaken,$(wildcard res_stir_shaken/*.c)) $(call MOD_ADD_C,res_aeap,$(wildcard res_aeap/*.c)) $(call MOD_ADD_C,res_geolocation,$(wildcard res_geolocation/*.c)) +$(call MOD_ADD_C,res_pjsip_volte,$(wildcard res_pjsip_volte/*.c)) # These are the xml and xslt files to be embedded res_geolocation.so: res_geolocation/pidf_lo_test.o res_geolocation/pidf_to_eprofile.o res_geolocation/eprofile_to_pidf.o diff --git a/res/res_pjsip_volte.c b/res/res_pjsip_volte.c index 11b199e224..3ffaf6278d 100644 --- a/res/res_pjsip_volte.c +++ b/res/res_pjsip_volte.c @@ -1,4 +1,5 @@ /*** MODULEINFO + libmnl pjproject res_pjsip res_pjsip_session @@ -19,6 +20,8 @@ #include "asterisk/causes.h" #include "asterisk/threadpool.h" +#include "res_pjsip_volte/netlink_xfrm.h" + #define PJ_CONSUME PJ_TRUE const pj_str_t STR_SUPPORTED = { "Supported", 9 }; @@ -28,19 +31,74 @@ const pj_str_t STR_PATH = { "path", 4 }; const pj_str_t STR_SEC_AGREE = { "sec-agree", 9 }; const pj_str_t STR_SECURITY_CLIENT = { "Security-Client", 15 }; +const pj_str_t STR_SECURITY_SERVER = { "Security-Server", 15 }; -const char *ipsec_alg[] = { - "hmac-md5-96", - "hmac-sha-1-96", - NULL +struct ipsec_alg { + const char *sip_name; + const char *kernel_name; }; -const char *ipsec_ealg[] = { - "null", - NULL +const struct ipsec_alg ipsec_alg[] = { + { "hmac-md5-96", "hmac(md5)" }, + { "hmac-sha-1-96", "hmac(sha1)" }, + { NULL, NULL } }; +const struct ipsec_alg ipsec_ealg[] = { + { "null", "cipher_null" }, + { NULL, NULL } +}; +static struct mnl_socket *g_mnl_socket = NULL; +struct sockaddr_storage g_src_addr, g_dst_addr; +static uint32_t g_spi_c, g_spi_s; +static pj_bool_t g_spi_c_valid = PJ_FALSE, g_spi_s_valid = PJ_TRUE; + +static void copy_pj_sockaddr_to_sockaddr_storage(const pj_sockaddr *src, struct sockaddr_storage *dst) +{ + memset(dst, 0, sizeof(struct sockaddr_storage)); + + dst->ss_family = (src->addr.sa_family == PJ_AF_INET) ? AF_INET : AF_INET6; + + if (dst->ss_family == AF_INET) { + struct sockaddr_in *dst_in = (struct sockaddr_in *)dst; + const pj_sockaddr_in *src_in = (const pj_sockaddr_in *)src; + dst_in->sin_port = htons(src_in->sin_port); + memcpy(&dst_in->sin_addr, &src_in->sin_addr, sizeof(struct in_addr)); + } else if (dst->ss_family == AF_INET6) { + struct sockaddr_in6 *dst_in6 = (struct sockaddr_in6 *)dst; + const pj_sockaddr_in6 *src_in6 = (const pj_sockaddr_in6 *)src; + dst_in6->sin6_port = htons(src_in6->sin6_port); + memcpy(&dst_in6->sin6_addr, &src_in6->sin6_addr, sizeof(struct in6_addr)); + } +} + +static char *sockaddr_storage_to_string(const struct sockaddr_storage *addr, char *ip_string, size_t ip_string_len) +{ + if (addr->ss_family == AF_INET) { + const struct sockaddr_in *addr_in = (const struct sockaddr_in *)addr; + inet_ntop(AF_INET, &addr_in->sin_addr, ip_string, ip_string_len); + } else if (addr->ss_family == AF_INET6) { + const struct sockaddr_in6 *addr_in6 = (const struct sockaddr_in6 *)addr; + inet_ntop(AF_INET6, &addr_in6->sin6_addr, ip_string, ip_string_len); + } else { + snprintf(ip_string, ip_string_len, ""); + } + + return ip_string; +} + +static void cleanup_xfrm(void) +{ + if (g_spi_c_valid) { + xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&g_src_addr, (const struct sockaddr *)&g_dst_addr, g_spi_c); + g_spi_c_valid = PJ_FALSE; + } + if (g_spi_s_valid) { + xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&g_src_addr, (const struct sockaddr *)&g_dst_addr, g_spi_s); + g_spi_s_valid = PJ_FALSE; + } +} static pj_status_t on_load(pjsip_endpoint *endpt) { printf("endpoint!\n"); @@ -74,16 +132,12 @@ static pj_bool_t is_ims(const pjsip_msg *msg) return PJ_TRUE; } - -#warning ip xfrm state add src dst proto esp spi auth hmac-sha1-96 0x - -static void add_securety_client_hdr(pjsip_tx_data *tdata, const char *alg[], const char *ealg[], uint32_t spi_c, uint32_t spi_s, uint16_t port_c, uint16_t port_s) +static void add_securety_client_hdr(pjsip_tx_data *tdata, const struct ipsec_alg alg[], const struct ipsec_alg ealg[], uint32_t spi_c, uint32_t spi_s, uint16_t port_c, uint16_t port_s) { pjsip_generic_array_hdr *hdr; char str[256]; int i, j; - puts("add sec"); /* Add Security-Client header. */ hdr = pjsip_generic_array_hdr_create(tdata->pool, &STR_SECURITY_CLIENT); if (!hdr) { @@ -92,9 +146,9 @@ static void add_securety_client_hdr(pjsip_tx_data *tdata, const char *alg[], con } /* Create tupple for given algorithms. */ - for (i = 0; alg[i]; i++) { - for (j = 0; ealg[j]; j++) { - snprintf(str, sizeof(str), "ipsec-3gpp; alg=%s; ealg=%s; spi-c=%u; spi-s=%u; port-c=%u; port-s=%u", alg[i], ealg[j], spi_c, spi_s, port_c, port_s); + for (i = 0; alg[i].sip_name; i++) { + for (j = 0; ealg[j].sip_name; j++) { + snprintf(str, sizeof(str), "ipsec-3gpp; alg=%s; ealg=%s; spi-c=%u; spi-s=%u; port-c=%u; port-s=%u", alg[i].sip_name, ealg[j].sip_name, spi_c, spi_s, port_c, port_s); if (hdr->count == PJSIP_GENERIC_ARRAY_MAX_COUNT) { ast_log(LOG_ERROR, "Too many evalue in array, skipping '%s'.", str); continue; @@ -150,18 +204,41 @@ static void add_value_array_hdr(pjsip_tx_data *tdata, const pj_str_t *name, cons static pj_status_t on_tx_register_request(pjsip_tx_data *tdata) { + char src_str[64], dst_str[64]; + int rc; + if (!is_ims(tdata->msg)) return PJ_SUCCESS; + /* Get local and remote peer address. */ + copy_pj_sockaddr_to_sockaddr_storage(&tdata->tp_info.transport->local_addr, &g_src_addr); + copy_pj_sockaddr_to_sockaddr_storage(&tdata->tp_info.dst_addr, &g_dst_addr); + ast_log(LOG_DEBUG, "peers %s->%s\n", sockaddr_storage_to_string(&g_src_addr, src_str, sizeof(src_str)), sockaddr_storage_to_string(&g_dst_addr, dst_str, sizeof(dst_str))); + + /* Allocate SPI-C and SPI-S towards remote peer. */ + rc = xfrm_spi_alloc(g_mnl_socket, 2342, &g_spi_c, (const struct sockaddr *)&g_src_addr, (const struct sockaddr *)&g_dst_addr); + if (rc < 0) { +spi_alloc_failed: + ast_log(LOG_ERROR, "Failed to request SPI.\n"); + return PJ_CONSUME; + } + g_spi_s_valid = PJ_TRUE; + rc = xfrm_spi_alloc(g_mnl_socket, 2342, &g_spi_s, (const struct sockaddr *)&g_src_addr, (const struct sockaddr *)&g_dst_addr); + if (rc < 0) + goto spi_alloc_failed; + g_spi_c_valid = PJ_TRUE; + ast_log(LOG_DEBUG, "SPI-C=0x%08x SPI-S=0x%08x\n", g_spi_s, g_spi_c); + + /* "Require: sec-agree" */ add_value_array_hdr(tdata, &STR_REQUIRE, &STR_SEC_AGREE); /* "Proxy-Require: sec-agree" */ add_value_string_hdr(tdata, &STR_PROXY_REQUIRE, &STR_SEC_AGREE); /* "Supported: path,sec-agree" */ add_value_array_hdr(tdata, &STR_SUPPORTED, &STR_PATH); - /* "Seciruty-Client: ..." */ + /* "Security-Client: ..." */ add_value_array_hdr(tdata, &STR_SUPPORTED, &STR_SEC_AGREE); - add_securety_client_hdr(tdata, ipsec_alg, ipsec_ealg, 0xbea96586, 0x14ac7e17, 43419, 42318); + add_securety_client_hdr(tdata, ipsec_alg, ipsec_ealg, g_spi_c, g_spi_s, 43419, 42318); @@ -198,7 +275,6 @@ static pj_status_t on_tx_request(pjsip_tx_data *tdata) case PJSIP_REGISTER_METHOD: puts("register"); return on_tx_register_request(tdata); - break; case PJSIP_INVITE_METHOD: break; default: @@ -208,6 +284,28 @@ static pj_status_t on_tx_request(pjsip_tx_data *tdata) return PJ_SUCCESS; } +static pj_bool_t on_rx_401_response(pjsip_rx_data *rdata) +{ + pjsip_generic_string_hdr *hdr; + + /* Get Security-Server from header. */ + hdr = pjsip_msg_find_hdr_by_name(rdata->msg_info.msg, &STR_SECURITY_SERVER, NULL); + if (!hdr) { + ast_log(LOG_ERROR, "Missing 'Security-Server' in REGISTER reply."); + return PJ_CONSUME; + } +#if 0 + if (!hdr->count) { + ast_log(LOG_ERROR, "Missing value in 'Security-Server' in REGISTER reply."); + return PJ_CONSUME; + } +#endif + puts("1"); + printf("hurra: %s\n", hdr->hvalue.ptr); + + return PJ_FALSE; +} + /* What response PJSIP receives */ static pj_bool_t on_rx_response(pjsip_rx_data *rdata) { @@ -219,17 +317,10 @@ static pj_bool_t on_rx_response(pjsip_rx_data *rdata) return PJ_CONSUME; } - const pj_str_t REFER_TO = { "Server", 6 }; - pjsip_generic_string_hdr *refer_to_hdr; - - refer_to_hdr = pjsip_msg_find_hdr_by_name(msg, &REFER_TO, NULL); - printf("server=%s\n", refer_to_hdr->hvalue.ptr); - - printf("%d\n", msg->line.req.method.id); switch ((int)msg->line.req.method.id) { - case 403: - puts("forbidden"); - break; + case 401: + puts("401"); + return on_rx_401_response(rdata); default: break; } @@ -262,6 +353,43 @@ static pjsip_module volte_module = { }; + + + + + + + +static int gai_helper(struct sockaddr_storage *out, const char *node, const char *port) +{ + struct addrinfo hints = { + .ai_flags = AI_NUMERICSERV | AI_NUMERICHOST, + }; + struct addrinfo *res; + int rc; + + rc = getaddrinfo(node, port, &hints, &res); + if (rc != 0) { + fprintf(stderr, "getaddrinfo(%s): %s\n", node, gai_strerror(rc)); + return -1; + } + + memcpy(out, res->ai_addr, res->ai_addrlen); + + freeaddrinfo(res); + + return 0; +} + + + + + + + + + + static int load_module(void) { if (ast_sip_register_service(&volte_module) != PJ_SUCCESS) { @@ -269,13 +397,29 @@ static int load_module(void) return AST_MODULE_LOAD_DECLINE; } + g_mnl_socket = xfrm_init_mnl_socket(); + if (!g_mnl_socket) { + ast_log(LOG_ERROR, "Failed to init mnl socket to admin xfrm rules.\n"); + ast_sip_unregister_service(&volte_module); + return AST_MODULE_LOAD_DECLINE; + } + + + + + + + + return AST_MODULE_LOAD_SUCCESS; } static int unload_module(void) { ast_sip_unregister_service(&volte_module); + xfrm_exit_mnl_socket(g_mnl_socket); + cleanup_xfrm(); return 0; } diff --git a/res/res_pjsip_volte/netlink_xfrm.c b/res/res_pjsip_volte/netlink_xfrm.c new file mode 100644 index 0000000000..c56e578c86 --- /dev/null +++ b/res/res_pjsip_volte/netlink_xfrm.c @@ -0,0 +1,439 @@ +/* Linux kernel IPsec interfacing via netlink XFRM + * + * Copyright (C) 2021 Harald Welte + * + * DOUBANGO 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. + * + * DOUBANGO 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 DOUBANGO. + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include "netlink_xfrm.h" + +#define XFRM_USER_ID 0x240299 /* some random number; let's use TS 24.299 */ + +struct mnl_socket *xfrm_init_mnl_socket(void) +{ + struct mnl_socket *mnl_socket = mnl_socket_open(NETLINK_XFRM); + if (!mnl_socket) { + fprintf(stderr, "ERR: Could not open XFRM netlink socket: %s", strerror(errno)); + return NULL; + } + + if (mnl_socket_bind(mnl_socket, 0, MNL_SOCKET_AUTOPID) < 0) { + fprintf(stderr, "ERR: Could not open XFRM netlink socket: %s", strerror(errno)); + mnl_socket_close(mnl_socket); + return NULL; + } + + return mnl_socket; +} + +void xfrm_exit_mnl_socket(struct mnl_socket *mnl_socket) +{ + mnl_socket_close(mnl_socket); +} + +static unsigned int get_next_nlmsg_seq(void) +{ + static unsigned int next_seq; + return next_seq++; +} + + +/* this is just a simple call-back which returns the nlmsghdr via 'data' */ +static int data_cb(const struct nlmsghdr *nlh, void *data) +{ + const struct nlmsghdr **rx = data; + + *rx = nlh; + + /* FIXME: is there a situation in which we'd want to return OK and not STOP? */ + return MNL_CB_STOP; +} + +/* send 'tx' via 'mnl_sock' and receive messages from kernel, using caller-provided + * rx_buf/rx_buf_size as temporary storage buffer; return response nlmsghdr in 'rx' */ +static int transceive_mnl(struct mnl_socket *mnl_sock, const struct nlmsghdr *tx, + uint8_t *rx_buf, size_t rx_buf_size, struct nlmsghdr **rx) +{ + int rc; + + rc = mnl_socket_sendto(mnl_sock, tx, tx->nlmsg_len); + if (rc < 0) { + fprintf(stderr, "ERR: cannot create IPsec SA: %s\n", strerror(errno)); + return -1; + } + puts("sent"); + + /* iterate until it is our answer, handing to mnl_cb_run, ... */ + while (1) { + rc = mnl_socket_recvfrom(mnl_sock, rx_buf, rx_buf_size); + printf("rc=%d\n", rc); + if (rc == -1) { + perror("mnl_socket_recvfrom"); + return -EIO; + } + + rc = mnl_cb_run(rx_buf, rc, tx->nlmsg_seq, mnl_socket_get_portid(mnl_sock), data_cb, rx); + if (rc == -1) { + perror("mnl_cb_run"); + return -EIO; + } else if (rc <= MNL_CB_STOP) + break; + } + + return 0; +} + +static int sockaddrs2xfrm_sel(struct xfrm_selector *sel, const struct sockaddr *src, + const struct sockaddr *dst) +{ + const struct sockaddr_in *sin; + const struct sockaddr_in6 *sin6; + + switch (src->sa_family) { + case AF_INET: + sin = (const struct sockaddr_in *) src; + sel->saddr.a4 = sin->sin_addr.s_addr; + sel->prefixlen_s = 32; + sel->sport = sin->sin_port; + sin = (const struct sockaddr_in *) dst; + sel->daddr.a4 = sin->sin_addr.s_addr; + sel->prefixlen_d = 32; + sel->dport = sin->sin_port; + break; + case AF_INET6: + sin6 = (const struct sockaddr_in6 *) src; + memcpy(sel->saddr.a6, &sin6->sin6_addr, sizeof(sel->saddr.a6)); + sel->prefixlen_s = 128; + sel->sport = sin6->sin6_port; + sin6 = (const struct sockaddr_in6 *) dst; + memcpy(sel->daddr.a6, &sin6->sin6_addr, sizeof(sel->daddr.a6)); + sel->prefixlen_d = 128; + sel->dport = sin6->sin6_port; + break; + default: + return -EINVAL; + } + sel->dport_mask = 0xffff; + sel->sport_mask = 0xffff; + sel->family = src->sa_family; + + return 0; +} + + +/*********************************************************************** + * SPI Allocation + ***********************************************************************/ + +/* allocate a local SPI for ESP between given src+dst address */ +int xfrm_spi_alloc(struct mnl_socket *mnl_sock, uint32_t reqid, uint32_t *spi_out, + const struct sockaddr *src, const struct sockaddr *dst) +{ + uint8_t msg_buf[MNL_SOCKET_BUFFER_SIZE]; + uint8_t rx_buf[MNL_SOCKET_BUFFER_SIZE]; + struct xfrm_userspi_info *xui, *rx_xui; + struct nlmsghdr *nlh, *rx_nlh = NULL; + const struct sockaddr_in *sin; + const struct sockaddr_in6 *sin6; + int rc; + + memset(msg_buf, 0, sizeof(msg_buf)); + + if (src->sa_family != dst->sa_family) + return -EINVAL; + + nlh = mnl_nlmsg_put_header(msg_buf); + nlh->nlmsg_flags = NLM_F_REQUEST, + nlh->nlmsg_type = XFRM_MSG_ALLOCSPI, + nlh->nlmsg_seq = get_next_nlmsg_seq(); + //nlh->nlmsg_pid = reqid; //FIXME + + xui = (struct xfrm_userspi_info *) mnl_nlmsg_put_extra_header(nlh, sizeof(*xui)); + + xui->info.family = src->sa_family; + + /* RFC4303 reserves 0..255 */ + xui->min = 0x100; + xui->max = 0xffffffff; + + /* ID src, dst, proto */ + switch (src->sa_family) { + case AF_INET: + sin = (const struct sockaddr_in *) src; + printf("src=%s ", inet_ntoa(sin->sin_addr)); + xui->info.saddr.a4 = sin->sin_addr.s_addr; + sin = (const struct sockaddr_in *) dst; + printf("dst=%s ", inet_ntoa(sin->sin_addr)); + xui->info.id.daddr.a4 = sin->sin_addr.s_addr; + //xui->info.sel.prefixlen_d = 32; + break; + case AF_INET6: + sin6 = (const struct sockaddr_in6 *) src; + memcpy(xui->info.saddr.a6, &sin6->sin6_addr, sizeof(xui->info.saddr.a6)); + //xui->info.sel.prefixlen_s = 128; + sin6 = (const struct sockaddr_in6 *) dst; + memcpy(xui->info.id.daddr.a6, &sin6->sin6_addr, sizeof(xui->info.id.daddr.a6)); + //xui->info.sel.prefixlen_d = 128; + break; + default: + fprintf(stderr, "ERR: unsupported address family %u\n", src->sa_family); + return -1; + } + + xui->info.id.proto = IPPROTO_ESP; + xui->info.reqid = reqid; + xui->info.mode = XFRM_MODE_TRANSPORT; + //xui->info.replay_window = 32; // TODO: check spec + + rc = transceive_mnl(mnl_sock, nlh, rx_buf, MNL_SOCKET_BUFFER_SIZE, &rx_nlh); + if (rc < 0) { + fprintf(stderr, "ERR: cannot create IPsec SA: %s\n", strerror(errno)); + return -1; + } + + /* parse response */ + rx_xui = (void *)rx_nlh + sizeof(*rx_nlh); + //printf("Allocated SPI=0x%08x\n", ntohl(xui->info.id.spi)); + *spi_out = ntohl(rx_xui->info.id.spi); + + return 0; +} + +/*********************************************************************** + * SA (Security Association) + ***********************************************************************/ + +int xfrm_sa_del(struct mnl_socket *mnl_sock, + const struct sockaddr *src, const struct sockaddr *dst, uint32_t spi) +{ + uint8_t msg_buf[MNL_SOCKET_BUFFER_SIZE]; + uint8_t rx_buf[MNL_SOCKET_BUFFER_SIZE]; + struct xfrm_usersa_id *said; + struct nlmsghdr *nlh, *rx_nlh; + const struct sockaddr_in *sin; + const struct sockaddr_in6 *sin6; + xfrm_address_t saddr; + int rc; + + memset(&saddr, 0, sizeof(saddr)); + + nlh = mnl_nlmsg_put_header(msg_buf); + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + nlh->nlmsg_type = XFRM_MSG_DELSA; + nlh->nlmsg_seq = get_next_nlmsg_seq(); + //nlh->nlmsg_pid = reqid; //FIXME + + said = (struct xfrm_usersa_id *) mnl_nlmsg_put_extra_header(nlh, sizeof(*said)); + said->spi = htonl(spi); + said->proto = IPPROTO_ESP; + + said->family = src->sa_family; + switch (src->sa_family) { + case AF_INET: + sin = (const struct sockaddr_in *) src; + saddr.a4 = sin->sin_addr.s_addr; + sin = (const struct sockaddr_in *) dst; + said->daddr.a4 = sin->sin_addr.s_addr; + break; + case AF_INET6: + sin6 = (const struct sockaddr_in6 *) src; + memcpy(saddr.a6, &sin6->sin6_addr, sizeof(saddr.a6)); + sin6 = (const struct sockaddr_in6 *) dst; + memcpy(said->daddr.a6, &sin6->sin6_addr, sizeof(said->daddr.a6)); + break; + default: + fprintf(stderr, "ERR: unsupported address family %u\n", src->sa_family); + return -1; + } + + mnl_attr_put(nlh, XFRMA_SRCADDR, sizeof(saddr), (void *)&saddr); + + rc = transceive_mnl(mnl_sock, nlh, rx_buf, MNL_SOCKET_BUFFER_SIZE, &rx_nlh); + if (rc < 0) { + fprintf(stderr, "ERR: cannot delete IPsec SA: %s\n", strerror(errno)); + return -1; + } + + /* FIXME: parse response */ + + return 0; +} + +int xfrm_sa_add(struct mnl_socket *mnl_sock, uint32_t reqid, + const struct sockaddr *src, const struct sockaddr *dst, uint32_t spi, + const struct xfrm_algo *auth_algo, const struct xfrm_algo *ciph_algo) +{ + uint8_t msg_buf[MNL_SOCKET_BUFFER_SIZE]; + uint8_t rx_buf[MNL_SOCKET_BUFFER_SIZE]; + struct xfrm_usersa_info *sainfo; + struct nlmsghdr *nlh, *rx_nlh; + int rc; + + nlh = mnl_nlmsg_put_header(msg_buf); + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL | NLM_F_ACK; + nlh->nlmsg_type = XFRM_MSG_NEWSA; + nlh->nlmsg_seq = get_next_nlmsg_seq(); + //nlh->nlmsg_pid = reqid; //FIXME + + sainfo = (struct xfrm_usersa_info *) mnl_nlmsg_put_extra_header(nlh, sizeof(*sainfo)); + sainfo->sel.family = src->sa_family; + rc = sockaddrs2xfrm_sel(&sainfo->sel, src, dst); + if (rc < 0) + return -EINVAL; + + sainfo->sel.user = htonl(XFRM_USER_ID); + + sainfo->saddr = sainfo->sel.saddr; + sainfo->id.daddr = sainfo->sel.daddr; + + sainfo->id.spi = htonl(spi); + sainfo->id.proto = IPPROTO_ESP; + + sainfo->lft.soft_byte_limit = XFRM_INF; + sainfo->lft.hard_byte_limit = XFRM_INF; + sainfo->lft.soft_packet_limit = XFRM_INF; + sainfo->lft.hard_packet_limit = XFRM_INF; + sainfo->reqid = reqid; + sainfo->family = src->sa_family; + sainfo->mode = XFRM_MODE_TRANSPORT; + sainfo->replay_window = 32; + + mnl_attr_put(nlh, XFRMA_ALG_AUTH, sizeof(struct xfrm_algo) + auth_algo->alg_key_len, auth_algo); + + mnl_attr_put(nlh, XFRMA_ALG_CRYPT, sizeof(struct xfrm_algo) + ciph_algo->alg_key_len, ciph_algo); + + rc = transceive_mnl(mnl_sock, nlh, rx_buf, MNL_SOCKET_BUFFER_SIZE, &rx_nlh); + if (rc < 0) { + fprintf(stderr, "ERR: cannot create IPsec SA: %s\n", strerror(errno)); + return -1; + } + + /* FIXME: parse response */ + + return 0; +} + +/*********************************************************************** + * Security Policy + ***********************************************************************/ + +int xfrm_policy_add(struct mnl_socket *mnl_sock, + const struct sockaddr *src, const struct sockaddr *dst, uint32_t spi, bool dir_in) +{ + uint8_t msg_buf[MNL_SOCKET_BUFFER_SIZE]; + uint8_t rx_buf[MNL_SOCKET_BUFFER_SIZE]; + struct xfrm_userpolicy_info *pinfo; + struct xfrm_user_tmpl tmpl; + struct nlmsghdr *nlh, *rx_nlh; + int rc; + + memset(&tmpl, 0, sizeof(tmpl)); + + nlh = mnl_nlmsg_put_header(msg_buf); + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_CREATE | NLM_F_EXCL | NLM_F_ACK; + nlh->nlmsg_type = XFRM_MSG_NEWPOLICY; + nlh->nlmsg_seq = get_next_nlmsg_seq(); + //nlh->nlmsg_pid = reqid; //FIXME + + pinfo = (struct xfrm_userpolicy_info *) mnl_nlmsg_put_extra_header(nlh, sizeof(*pinfo)); + + rc = sockaddrs2xfrm_sel(&pinfo->sel, src, dst); + if (rc < 0) + return -EINVAL; + + pinfo->sel.user = htonl(XFRM_USER_ID); + + pinfo->lft.soft_byte_limit = XFRM_INF; + pinfo->lft.hard_byte_limit = XFRM_INF; + pinfo->lft.soft_packet_limit = XFRM_INF; + pinfo->lft.hard_packet_limit = XFRM_INF; + pinfo->priority = 2342; // FIXME + pinfo->action = XFRM_POLICY_ALLOW; + pinfo->share = XFRM_SHARE_ANY; + + if (dir_in) + pinfo->dir = XFRM_POLICY_IN; + else + pinfo->dir = XFRM_POLICY_OUT; + + tmpl.id.proto = IPPROTO_ESP; + tmpl.id.daddr = pinfo->sel.daddr; + tmpl.saddr = pinfo->sel.saddr; + tmpl.family = pinfo->sel.family; + tmpl.reqid = spi; + tmpl.mode = XFRM_MODE_TRANSPORT; + tmpl.aalgos = 0xffffffff; + tmpl.ealgos = 0xffffffff; + tmpl.calgos = 0xffffffff; + mnl_attr_put(nlh, XFRMA_TMPL, sizeof(tmpl), &tmpl); + + rc = transceive_mnl(mnl_sock, nlh, rx_buf, MNL_SOCKET_BUFFER_SIZE, &rx_nlh); + if (rc < 0) { + fprintf(stderr, "ERR: cannot create IPsec policy: %s\n", strerror(errno)); + return -1; + } + + /* FIXME: parse response */ + + return 0; +} + + +int xfrm_policy_del(struct mnl_socket *mnl_sock, + const struct sockaddr *src, const struct sockaddr *dst, bool dir_in) +{ + uint8_t msg_buf[MNL_SOCKET_BUFFER_SIZE]; + uint8_t rx_buf[MNL_SOCKET_BUFFER_SIZE]; + struct xfrm_userpolicy_id *pid; + struct nlmsghdr *nlh, *rx_nlh; + int rc; + + nlh = mnl_nlmsg_put_header(msg_buf); + nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK; + nlh->nlmsg_type = XFRM_MSG_DELPOLICY; + nlh->nlmsg_seq = get_next_nlmsg_seq(); + //nlh->nlmsg_pid = reqid; //FIXME + + pid = (struct xfrm_userpolicy_id *) mnl_nlmsg_put_extra_header(nlh, sizeof(*pid)); + + rc = sockaddrs2xfrm_sel(&pid->sel, src, dst); + if (rc < 0) + return -EINVAL; + + pid->sel.user = htonl(XFRM_USER_ID); + + if (dir_in) + pid->dir = XFRM_POLICY_IN; + else + pid->dir = XFRM_POLICY_OUT; + + rc = transceive_mnl(mnl_sock, nlh, rx_buf, MNL_SOCKET_BUFFER_SIZE, &rx_nlh); + if (rc < 0) { + fprintf(stderr, "ERR: cannot delete IPsec policy: %s\n", strerror(errno)); + return -1; + } + + /* FIXME: parse response */ + + return 0; +} diff --git a/res/res_pjsip_volte/netlink_xfrm.h b/res/res_pjsip_volte/netlink_xfrm.h new file mode 100644 index 0000000000..485c78d8f0 --- /dev/null +++ b/res/res_pjsip_volte/netlink_xfrm.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include +#include +#include + +struct mnl_socket; + +struct xfrm_algobuf { + struct xfrm_algo algo; + uint8_t buf[sizeof(struct xfrm_algo) + 128]; +}; + +struct mnl_socket *xfrm_init_mnl_socket(void); + +void xfrm_exit_mnl_socket(struct mnl_socket *mnl_socket); + +int xfrm_spi_alloc(struct mnl_socket *mnl_sock, uint32_t reqid, uint32_t *spi_out, + const struct sockaddr *src, const struct sockaddr *dst); + +int xfrm_sa_add(struct mnl_socket *mnl_sock, uint32_t reqid, + const struct sockaddr *src, const struct sockaddr *dst, uint32_t spi, + const struct xfrm_algo *auth_algo, const struct xfrm_algo *ciph_algo); + +int xfrm_sa_del(struct mnl_socket *mnl_sock, + const struct sockaddr *src, const struct sockaddr *dst, uint32_t spi); + +int xfrm_policy_add(struct mnl_socket *mnl_sock, + const struct sockaddr *src, const struct sockaddr *dst, uint32_t spi, bool dir_in); + +int xfrm_policy_del(struct mnl_socket *mnl_sock, + const struct sockaddr *src, const struct sockaddr *dst, bool dir_in);