asterisk/res/res_pjsip_volte.c

/*** MODULEINFO
	<depend>libmnl</depend>
	<depend>pjproject</depend>
	<depend>res_pjsip</depend>
	<depend>res_pjsip_session</depend>
	<support_level>core</support_level>
 ***/

#include "asterisk.h"

#include <pjsip.h>
#include <pjsip_ua.h>
#include <pjlib.h>
#include <pjmedia.h>

#include "asterisk/res_pjsip.h"
#include "asterisk/res_pjsip_session.h"
#include "asterisk/utils.h"
#include "asterisk/module.h"
#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 };
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_SECURITY_CLIENT = { "Security-Client", 15 };
const pj_str_t STR_SECURITY_SERVER = { "Security-Server", 15 };

struct ipsec_alg {
	const char *sip_name;
	const char *kernel_name;
};

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, "<Unknown AF>");
	}

	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");
	return PJ_SUCCESS;
}

/* Check if message is exchanged with IMS.
 * This approach uses the host name of the URI. */
static pj_bool_t is_ims(const pjsip_msg *msg)
{
        pjsip_fromto_hdr *hdr;
        pjsip_sip_uri *uri;
	size_t uri_len;

	/* Get URI. */
        hdr = pjsip_msg_find_hdr(msg, PJSIP_H_TO, NULL);
        if (!hdr)
		return PJ_FALSE;
        uri = pjsip_uri_get_uri(hdr->uri);
	uri_len = strlen(uri->host.ptr);

	if (uri_len < 20)
		return PJ_FALSE;

	if (!!strncmp(uri->host.ptr, "ims.", 4))
		return PJ_FALSE;

	if (!!strncmp(uri->host.ptr + uri_len - 16, ".3gppnetwork.org", 16))
		return PJ_FALSE;

        return PJ_TRUE;
}

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;

	/* Add Security-Client header. */
	hdr = pjsip_generic_array_hdr_create(tdata->pool, &STR_SECURITY_CLIENT);
	if (!hdr) {
		ast_log(LOG_ERROR, "Failed to create header.");
		return;
	}

	/* Create tupple for given algorithms. */
	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;
			}
			pj_strdup2(tdata->pool, &hdr->values[hdr->count++], str);
		}
	}

	/* Append header */
	pjsip_msg_add_hdr(tdata->msg, (pjsip_hdr *)hdr);
}

static void add_value_string_hdr(pjsip_tx_data *tdata, const pj_str_t *name, const pj_str_t *value)
{
	pjsip_generic_string_hdr *hdr;

	/* Add Proxy-Require header. */
	hdr = pjsip_generic_string_hdr_create(tdata->pool, name, value);
	if (!hdr) {
		ast_log(LOG_ERROR, "Failed to create header.");
		return;
	}

	/* Append header */
	pjsip_msg_add_hdr(tdata->msg, (pjsip_hdr *)hdr);
}

static void add_value_array_hdr(pjsip_tx_data *tdata, const pj_str_t *name, const pj_str_t *value)
{
	pjsip_generic_array_hdr *hdr;
	pj_bool_t created = PJ_FALSE;

	/* Create header, if not yet created. */
	hdr = pjsip_msg_find_hdr_by_name(tdata->msg, name, NULL);
	if (!hdr) {
		hdr = pjsip_generic_array_hdr_create(tdata->pool, name);
		created = PJ_TRUE;
	}
	if (!hdr) {
		ast_log(LOG_ERROR, "Failed to create header.");
		return;
	}
	if (hdr->count == PJSIP_GENERIC_ARRAY_MAX_COUNT) {
		ast_log(LOG_ERROR, "Too many evalue in array, skipping %s.", value->ptr);
		return;
	}
	pj_strdup(tdata->pool, &hdr->values[hdr->count++], value);

	/* Append header, if created. */
	if (created)
		pjsip_msg_add_hdr(tdata->msg, (pjsip_hdr *)hdr);
}

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);
	/* "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 HACKING: Asterisk must do this on first register! Or we must do it!
	const pj_str_t STR_AUTHORIZATION = { "Authorization", 13 };
	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=\"\"";
	const pj_str_t STR_XXX = { xxx, strlen(xxx)};
	add_value_string_hdr(tdata, &STR_AUTHORIZATION, &STR_XXX);





	return PJ_SUCCESS;
}

/* What PJSIP will requests */
static pj_status_t on_tx_request(pjsip_tx_data *tdata)
{
	const pjsip_msg *msg = tdata->msg;

	puts("tx request");
	if (!msg) {
		ast_log(LOG_ERROR, "No message in request.");
		return PJ_CONSUME;

	}
	const pj_str_t REFER_TO = { "User-Agent", 10 };
        pjsip_generic_string_hdr *refer_to_hdr;

	switch (msg->line.req.method.id) {
	case PJSIP_REGISTER_METHOD:
		puts("register");
		return on_tx_register_request(tdata);
	case PJSIP_INVITE_METHOD:
		break;
	default:
		break;
	}

	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)
{
	const pjsip_msg *msg = rdata->msg_info.msg;

	puts("rx response");
	if (!msg) {
		ast_log(LOG_ERROR, "No message in request.");
		return PJ_CONSUME;
	}

	switch ((int)msg->line.req.method.id) {
	case 401:
		puts("401");
		return on_rx_401_response(rdata);
	default:
		break;
	}

	return PJ_FALSE;
}

/* What request PJSIP receives */
static pj_bool_t on_rx_request(pjsip_rx_data *rdata)
{
	puts("rx request");
	return PJ_TRUE;
}

/* What PJSIP will response */
static pj_status_t on_tx_response(pjsip_tx_data *tdata)
{
	puts("tx response");
	return PJ_SUCCESS;
}

static pjsip_module volte_module = {
	.name = {"VoLTE Support Module", 18},
	.priority = PJSIP_MOD_PRIORITY_TRANSPORT_LAYER,
	.load = on_load,
	.on_tx_request = on_tx_request,
	.on_rx_response = on_rx_response,
	.on_tx_response = on_tx_response,
	.on_rx_request = on_rx_request,
};









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) {
		ast_log(LOG_ERROR, "Failed to load VoLTE support module.\n");
		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;
}

AST_MODULE_INFO(ASTERISK_GPL_KEY, AST_MODFLAG_GLOBAL_SYMBOLS | AST_MODFLAG_LOAD_ORDER, "VoLTE support",
				.support_level = AST_MODULE_SUPPORT_CORE,
				.load = load_module,
				.unload = unload_module,
				.load_pri = AST_MODPRI_APP_DEPEND,
				.requires = "res_pjsip,res_pjsip_session", );