From 93c9780637aa7da3dc4a78c0ae118c5f8fb0ee63 Mon Sep 17 00:00:00 2001 From: Andreas Eversberg Date: Mon, 15 Apr 2024 09:36:19 +0200 Subject: [PATCH] WIP VoLTE support for Asterisk --- res/res_pjsip_volte.c | 291 ++++++++++++++++++++++++----- res/res_pjsip_volte/netlink_xfrm.h | 2 +- 2 files changed, 245 insertions(+), 48 deletions(-) diff --git a/res/res_pjsip_volte.c b/res/res_pjsip_volte.c index 3ffa9544d3..c908e09e9f 100644 --- a/res/res_pjsip_volte.c +++ b/res/res_pjsip_volte.c @@ -26,15 +26,25 @@ #define PJ_CONSUME PJ_TRUE +#define fmt_str(str) (int)(str).slen, (str).ptr + const pj_str_t STR_SUPPORTED = { "Supported", 9 }; const pj_str_t STR_REQUIRE = { "Require", 7 }; const pj_str_t STR_PROXY_REQUIRE = { "Proxy-Require", 13 }; const pj_str_t STR_PATH = { "path", 4 }; const pj_str_t STR_SEC_AGREE = { "sec-agree", 9 }; const pj_str_t STR_AUTHORIZATION = { "Authorization", 13 }; - const pj_str_t STR_SECURITY_CLIENT = { "Security-Client", 15 }; const pj_str_t STR_SECURITY_SERVER = { "Security-Server", 15 }; +const pj_str_t STR_Q = { "q", 1 }; +const pj_str_t STR_PROT = { "prot", 4 }; +const pj_str_t STR_MOD = { "mod", 3 }; +const pj_str_t STR_SPI_C = { "spi-c", 5 }; +const pj_str_t STR_SPI_S = { "spi-s", 5 }; +const pj_str_t STR_PORT_C = { "port-c", 6 }; +const pj_str_t STR_PORT_S = { "port-s", 6 }; +const pj_str_t STR_ALG = { "alg", 3 }; +const pj_str_t STR_EALG = { "ealg", 4 }; struct ipsec_alg { const char *sip_name; @@ -42,22 +52,27 @@ struct ipsec_alg { }; const struct ipsec_alg ipsec_alg[] = { - { "hmac-md5-96", "hmac(md5)" }, - { "hmac-sha-1-96", "hmac(sha1)" }, + { "hmac-md5-96", "md5" }, + { "hmac-sha-1-96", "sha1" }, { NULL, NULL } }; const struct ipsec_alg ipsec_ealg[] = { - { "null", "ecb(cipher_null)" }, + { "null", "cipher_null" }, { NULL, NULL } }; static unsigned char aka_res[8]; 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; +struct sockaddr_storage g_local_addr_c, g_local_addr_s; +struct sockaddr_storage g_remote_addr_c, g_remote_addr_s; +static uint32_t g_local_spi_c, g_local_spi_s; +static uint32_t g_remote_spi_c, g_remote_spi_s; +static pj_bool_t g_local_sa_c_set = PJ_FALSE, g_local_sa_s_set = PJ_FALSE; +static pj_bool_t g_remote_sa_c_set = PJ_FALSE, g_remote_sa_s_set = PJ_FALSE; +static pj_bool_t g_local_sp_c_set = PJ_FALSE, g_local_sp_s_set = PJ_FALSE; +static pj_bool_t g_remote_sp_c_set = PJ_FALSE, g_remote_sp_s_set = PJ_FALSE; static void copy_pj_sockaddr_to_sockaddr_storage(const pj_sockaddr *src, struct sockaddr_storage *dst) { @@ -93,17 +108,122 @@ static char *sockaddr_storage_to_string(const struct sockaddr_storage *addr, cha return ip_string; } +static void sockaddr_storage_port(struct sockaddr_storage *addr, uint16_t port) +{ + switch (((struct sockaddr *)addr)->sa_family) { + case AF_INET: + ((struct sockaddr_in *)addr)->sin_port = htons(port); + break; + case AF_INET6: + ((struct sockaddr_in6 *)addr)->sin6_port = htons(port); + break; + } +} + /* Delete old SA and SP entries upon new registration or module exit. */ 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_local_sa_c_set) { + xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&g_local_addr_c, (const struct sockaddr *)&g_remote_addr_c, g_local_spi_c); + g_local_sa_c_set = 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; + if (g_local_sa_s_set) { + xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&g_local_addr_s, (const struct sockaddr *)&g_remote_addr_s, g_local_spi_s); + g_local_sa_s_set = PJ_FALSE; } + if (g_remote_sa_c_set) { + xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&g_remote_addr_c, (const struct sockaddr *)&g_local_addr_c, g_remote_spi_c); + g_remote_sa_c_set = PJ_FALSE; + } + if (g_remote_sa_s_set) { + xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&g_remote_addr_s, (const struct sockaddr *)&g_local_addr_s, g_remote_spi_s); + g_remote_sa_s_set = PJ_FALSE; + } + if (g_local_sp_c_set) { + xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&g_local_addr_c, (const struct sockaddr *)&g_remote_addr_c, false); + g_local_sp_c_set = PJ_FALSE; + } + if (g_local_sp_s_set) { + xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&g_local_addr_s, (const struct sockaddr *)&g_remote_addr_s, false); + g_local_sp_s_set = PJ_FALSE; + } + if (g_remote_sp_c_set) { + xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&g_remote_addr_c, (const struct sockaddr *)&g_local_addr_c, true); + g_remote_sp_c_set = PJ_FALSE; + } + if (g_remote_sp_s_set) { + xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&g_remote_addr_s, (const struct sockaddr *)&g_local_addr_s, true); + g_remote_sp_s_set = PJ_FALSE; + } + +} + +struct security_server { + pj_str_t q; + pj_str_t prot; + pj_str_t mod; + pj_str_t spi_c; + pj_str_t spi_s; + pj_str_t port_c; + pj_str_t port_s; + pj_str_t alg; + pj_str_t ealg; +}; + +static void on_syntax_error(pj_scanner *scanner) +{ + PJ_UNUSED_ARG(scanner); +} + +static void parse_security_server(pj_pool_t *pool, char *buf, pj_size_t size, struct security_server *sec) +{ + pj_scanner scanner; + + memset(sec, 0, sizeof(*sec)); + + pj_scan_init(&scanner, buf, size, 0, &on_syntax_error); + + for (;;) { + pj_str_t name, value; + + pjsip_parse_param_imp(&scanner, pool, &name, &value, 0); + + if (!pj_stricmp(&name, &STR_Q)) { + sec->q = value; + } else + if (!pj_stricmp(&name, &STR_PROT)) { + sec->prot = value; + } else + if (!pj_stricmp(&name, &STR_MOD)) { + sec->mod = value; + } else + if (!pj_stricmp(&name, &STR_SPI_C)) { + sec->spi_c = value; + } else + if (!pj_stricmp(&name, &STR_SPI_S)) { + sec->spi_s = value; + } else + if (!pj_stricmp(&name, &STR_PORT_C)) { + sec->port_c = value; + } else + if (!pj_stricmp(&name, &STR_PORT_S)) { + sec->port_s = value; + } else + if (!pj_stricmp(&name, &STR_ALG)) { + sec->alg = value; + } else + if (!pj_stricmp(&name, &STR_EALG)) { + sec->ealg = value; + } + + if (pj_scan_is_eof(&scanner)) + break; + + /* Eat semicolon */ + if (*scanner.curptr == ';') + pj_scan_get_char(&scanner); + } + pj_scan_fini(&scanner); } static pj_status_t on_load(pjsip_endpoint *endpt) @@ -221,7 +341,6 @@ static pj_status_t on_tx_register_request(pjsip_tx_data *tdata) pjsip_generic_array_hdr *hdr; int rc; - ast_log(LOG_ERROR, "Unsetting RES paassword.\n"); volte_auth = NULL; if (!is_ims(tdata->msg)) @@ -236,23 +355,27 @@ static pj_status_t on_tx_register_request(pjsip_tx_data *tdata) cleanup_xfrm(); /* 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))); + copy_pj_sockaddr_to_sockaddr_storage(&tdata->tp_info.transport->local_addr, &g_local_addr_c); + copy_pj_sockaddr_to_sockaddr_storage(&tdata->tp_info.transport->local_addr, &g_local_addr_s); + copy_pj_sockaddr_to_sockaddr_storage(&tdata->tp_info.dst_addr, &g_remote_addr_c); + copy_pj_sockaddr_to_sockaddr_storage(&tdata->tp_info.dst_addr, &g_remote_addr_s); + sockaddr_storage_port(&g_remote_addr_c, 0x0001); + sockaddr_storage_port(&g_remote_addr_s, 0x0001); + ast_log(LOG_DEBUG, "peers %s->%s\n", sockaddr_storage_to_string(&g_local_addr_c, src_str, sizeof(src_str)), sockaddr_storage_to_string(&g_remote_addr_c, 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); + rc = xfrm_spi_alloc(g_mnl_socket, 2342, &g_local_spi_c, (const struct sockaddr *)&g_local_addr_c, (const struct sockaddr *)&g_remote_addr_c); if (rc < 0) { - spi_alloc_failed: +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); + g_local_sa_s_set = PJ_TRUE; + rc = xfrm_spi_alloc(g_mnl_socket, 2342, &g_local_spi_s, (const struct sockaddr *)&g_local_addr_s, (const struct sockaddr *)&g_remote_addr_s); 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); + g_local_sa_c_set = PJ_TRUE; + ast_log(LOG_DEBUG, "local SPI-C=0x%08x SPI-S=0x%08x\n", g_local_spi_s, g_local_spi_c); /* "Require: sec-agree" */ @@ -263,7 +386,8 @@ static pj_status_t on_tx_register_request(pjsip_tx_data *tdata) add_value_array_hdr(tdata, &STR_SUPPORTED, &STR_PATH); /* "Security-Client: ..." */ add_value_array_hdr(tdata, &STR_SUPPORTED, &STR_SEC_AGREE); - add_securety_client_hdr(tdata, ipsec_alg, ipsec_ealg, g_spi_c, g_spi_s, 43419, 42318); +#warning fix port numbers + add_securety_client_hdr(tdata, ipsec_alg, ipsec_ealg, g_local_spi_c, g_local_spi_s, 43419, 42318); #warning HACKING: Asterisk must do this on first register! Or we must do it! char xxx[] = "Digest uri=\"sip:ims.mnc001.mcc238.3gppnetwork.org\",username=\"238010000090828@ims.mnc001.mcc238.3gppnetwork.org\",response=\"\",realm=\"ims.mnc001.mcc238.3gppnetwork.org\",nonce=\"\""; @@ -306,46 +430,40 @@ 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) +static pj_bool_t on_rx_401_407_response(pjsip_rx_data *rdata, pjsip_hdr_e auth_type) { - pjsip_generic_string_hdr *hdr; if (!is_ims(rdata->msg_info.msg)) return PJ_FALSE; /* 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."); + struct security_server sec; + pjsip_generic_string_hdr *sec_hdr = pjsip_msg_find_hdr_by_name(rdata->msg_info.msg, &STR_SECURITY_SERVER, NULL); + if (!sec_hdr || !sec_hdr->hvalue.ptr) { + ast_log(LOG_ERROR, "Missing 'Security-Server' in REGISTER response."); return PJ_FALSE; } -#if 0 - if (!hdr->count) { - ast_log(LOG_ERROR, "Missing value in 'Security-Server' in REGISTER reply."); - return PJ_CONSUME; + parse_security_server(rdata->tp_info.pool, sec_hdr->hvalue.ptr, sec_hdr->hvalue.slen, &sec); + if (!sec.prot.ptr || !sec.spi_c.ptr || !sec.spi_s.ptr || !sec.port_c.ptr || !sec.port_s.ptr || !sec.alg.ptr || !sec.ealg.ptr) { + ast_log(LOG_ERROR, "Missing 'Security-Server' elements in REGISTER response. header=\"%.*s\", prot=%.*s, spi-c=%.*s, spi-s=%.*s, port-c=%.*s, port-s=%.*s, alg=%.*s, ealg=%.*s", fmt_str(sec_hdr->hvalue), fmt_str(sec.prot), fmt_str(sec.spi_c), fmt_str(sec.spi_s), fmt_str(sec.port_c), fmt_str(sec.port_s), fmt_str(sec.alg), fmt_str(sec.ealg)); + return PJ_FALSE; } -#endif - puts("1"); - printf("hurra: %s\n", hdr->hvalue.ptr); - pjsip_www_authenticate_hdr *auth_hdr = pjsip_msg_find_hdr(rdata->msg_info.msg, PJSIP_H_WWW_AUTHENTICATE, NULL); + pjsip_www_authenticate_hdr *auth_hdr = pjsip_msg_find_hdr(rdata->msg_info.msg, auth_type, NULL); if (!auth_hdr || !auth_hdr->challenge.digest.nonce.ptr || !auth_hdr->challenge.digest.algorithm.ptr) { - ast_log(LOG_ERROR, "Authentication header missing or incomplete.\n"); + ast_log(LOG_ERROR, "Authentication header missing or incomplete in REGISTER response.\n"); return PJ_FALSE; } - printf("NONCE: %s\n", auth_hdr->challenge.digest.nonce.ptr); - printf("algo: %s\n", auth_hdr->challenge.digest.algorithm.ptr); - if (!strncmp(auth_hdr->challenge.digest.algorithm.ptr, "AKAv2-MD5", 9)) { + if (!pj_strncmp2(&auth_hdr->challenge.digest.algorithm, "AKAv2-MD5", 9)) { ast_log(LOG_ERROR, "Authentication algorithm not supported. See third-party/pjproject/source/pjsip/src/pjsip/sip_auth_aka.c for implementation.\n"); return PJ_FALSE; } - if (!!strncmp(auth_hdr->challenge.digest.algorithm.ptr, "AKAv1-MD5", 9)) { + if (!!pj_strncmp2(&auth_hdr->challenge.digest.algorithm, "AKAv1-MD5", 9)) { ast_log(LOG_ERROR, "Authentication algorithm not supported.\n"); return PJ_FALSE; } -#if 1 #warning hacking const uint8_t opc[16] = { 0x77, 0x5A, 0x1F, 0x88, 0x7D, 0x2A, 0xD6, 0x6F, 0x97, 0x19, 0xC2, 0xC7, 0x9F, 0x84, 0x7B, 0x50 }; const uint8_t ki[16] = { 0xD5, 0x34, 0xE0, 0x78, 0x54, 0xB7, 0x5E, 0x47, 0x5C, 0x66, 0x7A, 0x85, 0x6A, 0xA3, 0x1F, 0x9C }; @@ -360,14 +478,92 @@ static pj_bool_t on_rx_401_response(pjsip_rx_data *rdata) milenage_generate(opc, amf, ki, sqn, rand, out_autn, out_ik, out_ck, aka_res, &out_res_len); if (out_res_len != 8) { ast_log(LOG_ERROR, "Milenage computation failed.\n"); + return PJ_FALSE; } hexdump(LOG_DEBUG, "IK", out_ik, 16); hexdump(LOG_DEBUG, "CK", out_ck, 16); - ast_log(LOG_ERROR, "Setting RES as paassword.\n"); volte_auth = aka_res; -#endif + struct xfrm_algobuf auth_algo, ciph_algo; + int i, j; + int rc; + for (i = 0; ipsec_alg[i].sip_name; i++) { + if (!pj_strncmp2(&sec.alg, ipsec_alg[i].sip_name, strlen(ipsec_alg[i].sip_name))) + break; + } + if (!ipsec_alg[i].kernel_name) { + ast_log(LOG_ERROR, "Given 'alg' in Security-Server header not found.\n"); + return PJ_FALSE; + } + for (j = 0; ipsec_ealg[j].sip_name; j++) { + if (!pj_strncmp2(&sec.ealg, ipsec_ealg[j].sip_name, strlen(ipsec_ealg[j].sip_name))) + break; + } + g_remote_spi_c = atoi(sec.spi_c.ptr); + g_remote_spi_s = atoi(sec.spi_s.ptr); + ast_log(LOG_DEBUG, "remote SPI-C=0x%08x SPI-S=0x%08x\n", g_remote_spi_s, g_remote_spi_c); + sockaddr_storage_port(&g_remote_addr_c, atoi(sec.port_c.ptr)); + sockaddr_storage_port(&g_remote_addr_s, atoi(sec.port_s.ptr)); + strcpy(auth_algo.algo.alg_name, ipsec_alg[i].kernel_name); + switch (i) { + case 0: + memcpy(auth_algo.algo.alg_key, out_ik, 16); + auth_algo.algo.alg_key_len = 16; + break; + case 1: + memcpy(auth_algo.algo.alg_key, out_ik, 16); + memset(auth_algo.algo.alg_key + 16, 0x00, 4); + auth_algo.algo.alg_key_len = 20; + break; + } + strcpy(ciph_algo.algo.alg_name, ipsec_ealg[j].kernel_name); + switch (j) { + case 0: + ciph_algo.algo.alg_key_len = 0; + break; + } + cleanup_xfrm(); + rc = xfrm_sa_add(g_mnl_socket, g_local_spi_c, (const struct sockaddr *)&g_local_addr_c, (const struct sockaddr *)&g_remote_addr_c, g_local_spi_c, &auth_algo.algo, &ciph_algo.algo); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SA.\n"); + else + g_local_sa_c_set = PJ_TRUE; + rc = xfrm_sa_add(g_mnl_socket, g_local_spi_s, (const struct sockaddr *)&g_local_addr_s, (const struct sockaddr *)&g_remote_addr_s, g_local_spi_s, &auth_algo.algo, &ciph_algo.algo); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SA.\n"); + else + g_local_sa_s_set = PJ_TRUE; + rc = xfrm_sa_add(g_mnl_socket, g_remote_spi_c, (const struct sockaddr *)&g_remote_addr_c, (const struct sockaddr *)&g_local_addr_c, g_remote_spi_c, &auth_algo.algo, &ciph_algo.algo); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SA.\n"); + else + g_remote_sa_c_set = PJ_TRUE; + rc = xfrm_sa_add(g_mnl_socket, g_remote_spi_s, (const struct sockaddr *)&g_remote_addr_s, (const struct sockaddr *)&g_local_addr_s, g_remote_spi_s, &auth_algo.algo, &ciph_algo.algo); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SA.\n"); + else + g_remote_sa_s_set = PJ_TRUE; + rc = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&g_local_addr_c, (const struct sockaddr *)&g_remote_addr_c, g_local_spi_c, false); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SP.\n"); + else + g_local_sp_c_set = PJ_TRUE; + rc = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&g_local_addr_s, (const struct sockaddr *)&g_remote_addr_s, g_local_spi_s, false); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SP.\n"); + else + g_local_sp_s_set = PJ_TRUE; + rc = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&g_remote_addr_c, (const struct sockaddr *)&g_local_addr_c, g_remote_spi_c, true); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SP.\n"); + else + g_remote_sp_c_set = PJ_TRUE; + rc = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&g_remote_addr_s, (const struct sockaddr *)&g_local_addr_s, g_remote_spi_s, true); + if (rc < 0) + ast_log(LOG_ERROR, "Failed to add IPSec SP.\n"); + else + g_remote_sp_s_set = PJ_TRUE; return PJ_FALSE; @@ -386,8 +582,9 @@ static pj_bool_t on_rx_response(pjsip_rx_data *rdata) switch ((int)msg->line.req.method.id) { case 401: - puts("401"); - return on_rx_401_response(rdata); + return on_rx_401_407_response(rdata, PJSIP_H_WWW_AUTHENTICATE); + case 407: + return on_rx_401_407_response(rdata, PJSIP_H_PROXY_AUTHENTICATE); default: break; } diff --git a/res/res_pjsip_volte/netlink_xfrm.h b/res/res_pjsip_volte/netlink_xfrm.h index 485c78d8f0..c6618dd21d 100644 --- a/res/res_pjsip_volte/netlink_xfrm.h +++ b/res/res_pjsip_volte/netlink_xfrm.h @@ -9,7 +9,7 @@ struct mnl_socket; struct xfrm_algobuf { struct xfrm_algo algo; - uint8_t buf[sizeof(struct xfrm_algo) + 128]; + uint8_t buf[128]; }; struct mnl_socket *xfrm_init_mnl_socket(void);