WIP: Volte support for outgoing SIP registration

include/asterisk/res_pjsip.h

@@ -297,6 +297,8 @@ struct ast_sip_transport {
 	int allow_reload;
 	/*! Automatically send requests out the same transport requests have come in on */
 	int symmetric_transport;
+	/*! Local ports for client and server to use with IMS */
+	int ims_port_c, ims_port_s;
 	/*! This is a flow to another target */
 	int flow;
 };
@@ -580,6 +582,9 @@ struct ast_sip_auth {
 		AST_STRING_FIELD(auth_user);
 		/*! Authentication password */
 		AST_STRING_FIELD(auth_pass);
+		/*! IMS Authentication password */
+		char ims_res[8];
+		int ims_res_len;
 		/*! Authentication credentials in MD5 format (hash of user:realm:pass) */
 		AST_STRING_FIELD(md5_creds);
 		/*! Refresh token to use for OAuth authentication */
@@ -591,7 +596,6 @@ struct ast_sip_auth {
 		/*! Use USIM emulation with these parameters */
 		AST_STRING_FIELD(usim_opc);
 		AST_STRING_FIELD(usim_k);
-		AST_STRING_FIELD(usim_amf);
 		AST_STRING_FIELD(usim_sqn);
 	);
 	/*! Use AMI interface for communication with USIM (instead of emulation) */

res/res_pjsip/config_auth.c

@@ -405,8 +405,6 @@ int ast_sip_initialize_sorcery_auth(void)
 			"", OPT_STRINGFIELD_T, 0, STRFLDSET(struct ast_sip_auth, usim_opc));
 	ast_sorcery_object_field_register(sorcery, SIP_SORCERY_AUTH_TYPE, "usim_k",
 			"", OPT_STRINGFIELD_T, 0, STRFLDSET(struct ast_sip_auth, usim_k));
-	ast_sorcery_object_field_register(sorcery, SIP_SORCERY_AUTH_TYPE, "usim_amf",
-			"", OPT_STRINGFIELD_T, 0, STRFLDSET(struct ast_sip_auth, usim_amf));
 	ast_sorcery_object_field_register(sorcery, SIP_SORCERY_AUTH_TYPE, "usim_sqn",
 			"", OPT_STRINGFIELD_T, 0, STRFLDSET(struct ast_sip_auth, usim_sqn));
 	ast_sorcery_object_field_register_custom(sorcery, SIP_SORCERY_AUTH_TYPE, "auth_type",

res/res_pjsip/config_transport.c

@@ -1770,6 +1770,8 @@ int ast_sip_initialize_sorcery_transport(void)
 	ast_sorcery_object_field_register(sorcery, "transport", "websocket_write_timeout", AST_DEFAULT_WEBSOCKET_WRITE_TIMEOUT_STR, OPT_INT_T, PARSE_IN_RANGE, FLDSET(struct ast_sip_transport, write_timeout), 1, INT_MAX);
 	ast_sorcery_object_field_register(sorcery, "transport", "allow_reload", "no", OPT_BOOL_T, 1, FLDSET(struct ast_sip_transport, allow_reload));
 	ast_sorcery_object_field_register(sorcery, "transport", "symmetric_transport", "no", OPT_BOOL_T, 1, FLDSET(struct ast_sip_transport, symmetric_transport));
+	ast_sorcery_object_field_register(sorcery, "transport", "ims_port_c", "0", OPT_UINT_T, 0, FLDSET(struct ast_sip_transport, ims_port_c));
+	ast_sorcery_object_field_register(sorcery, "transport", "ims_port_s", "0", OPT_UINT_T, 0, FLDSET(struct ast_sip_transport, ims_port_s));
 
 	ast_sip_register_endpoint_formatter(&endpoint_transport_formatter);
 

res/res_pjsip_outbound_authenticator_digest.c

@@ -313,10 +313,14 @@ static pj_status_t set_outbound_authentication_credentials(pjsip_auth_clt_sess *
 		pj_cstr(&auth_cred.scheme, "digest");
 		switch (auth->type) {
 		case AST_SIP_AUTH_TYPE_USER_PASS:
-		case AST_SIP_AUTH_TYPE_IMS_AKA:
 			pj_cstr(&auth_cred.data, auth->auth_pass);
 			auth_cred.data_type = PJSIP_CRED_DATA_PLAIN_PASSWD;
 			break;
+		case AST_SIP_AUTH_TYPE_IMS_AKA:
+			auth_cred.data.ptr = auth->ims_res;
+			auth_cred.data.slen = auth->ims_res_len;
+			auth_cred.data_type = PJSIP_CRED_DATA_PLAIN_PASSWD;
+			break;
 		case AST_SIP_AUTH_TYPE_MD5:
 			pj_cstr(&auth_cred.data, auth->md5_creds);
 			auth_cred.data_type = PJSIP_CRED_DATA_DIGEST;

res/res_pjsip_outbound_registration.c

@@ -273,8 +273,8 @@
 /* forward declarations */
 static int set_outbound_initial_authentication_credentials(pjsip_regc *regc,
 		const struct ast_sip_auth_vector *auth_vector);
-static int add_outbound_initial_authorization(pjsip_tx_data *tdata, const char *fromdomain,
-					      const struct ast_sip_auth_vector *auth_vector);
+static int ims_add_outbound_initial_authorization(pjsip_tx_data *tdata, const char *fromdomain,
+						  const struct ast_sip_auth_vector *auth_vector);
 
 /*! \brief Some thread local storage used to determine if the running thread invoked the callback */
 AST_THREADSTORAGE(register_callback_invoked);
@@ -384,6 +384,12 @@ struct sip_outbound_registration {
 	unsigned int ims_aka;
 };
 
+enum ims_state {
+	IMS_STATE_REGISTER,
+	IMS_STATE_AUTHENTICATE,
+	IMS_STATE_RESYNC
+};
+
 /*! \brief Outbound registration client state information (persists for lifetime of regc) */
 struct sip_outbound_registration_client_state {
 	/*! \brief Current state of this registration */
@@ -451,6 +457,10 @@ struct sip_outbound_registration_client_state {
 	char *user_agent;
 	/*! \brief VoLTE support */
 	unsigned int ims_aka;
+	/*! \brief Current state of registration process */
+	enum ims_state ims_state;
+	/*! \brief Current states related to VoLTE process */
+	struct volte_states volte;
 };
 
 /*! \brief Outbound registration state information (persists for lifetime that registration should exist) */
@@ -734,6 +744,80 @@ static void add_security_headers(struct sip_outbound_registration_client_state *
 	ao2_cleanup(reg);
 }
 
+static pj_status_t ims_registration_client(struct sip_outbound_registration_client_state *client_state,
+					   pjsip_tx_data *tdata)
+{
+	struct sip_outbound_registration *reg = NULL;
+	struct ast_sip_endpoint *endpt = NULL;
+	struct ast_sip_transport *transp = NULL;
+	int rc = -1;
+
+	if (g_volte_init()) {
+		goto out;
+	}
+
+	reg = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "registration",
+					 client_state->registration_name);
+	if (!reg) {
+		ast_log(LOG_ERROR, "Internal error\n");
+		goto out;
+	}
+	if (!reg->endpoint ||
+	    (!(endpt = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "endpoint", reg->endpoint)))) {
+		ast_log(LOG_ERROR, "No endpoint configured in registration config for '%s'\n",
+			client_state->registration_name);
+		goto out;
+	}
+	if (!reg->transport ||
+	    (!(transp = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "transport", reg->transport)))) {
+		ast_log(LOG_ERROR, "No transport configured in registration config for '%s'\n",
+			client_state->registration_name);
+		goto out;
+	}
+	if (!endpt->fromdomain) {
+		ast_log(LOG_ERROR, "No from_domain defined in endpoint config for '%s'\n", reg->endpoint);
+		goto out;
+	}
+	if (!transp->ims_port_c || !transp->ims_port_s) {
+		ast_log(LOG_ERROR, "No ims_port_c or ims_port_s defined in transport config for '%s'\n",
+			reg->transport);
+		goto out;
+	}
+
+	if (volte_del_authorization(tdata)) {
+		ast_log(LOG_ERROR, "Failed to remove authorization header.\n");
+		goto out;
+	}
+
+	if (ims_add_outbound_initial_authorization(tdata, endpt->fromdomain, &client_state->outbound_auths)) {
+		ast_log(LOG_ERROR, "Failed to add initial authorization header.\n");
+		goto out;
+	}
+
+	if (volte_add_sec_agree(tdata)) {
+		ast_log(LOG_ERROR, "Failed to add sec agree header.\n");
+		goto out;
+	}
+
+	if (volte_reset_transport(&client_state->volte, tdata)) {
+		ast_log(LOG_ERROR, "Failed to reset transport.\n");
+		goto out;
+	}
+
+	if (volte_add_security_client(&client_state->volte, tdata, transp->ims_port_c, transp->ims_port_s)) {
+		ast_log(LOG_ERROR, "Failed to add security client header.\n");
+		goto out;
+	}
+
+	rc = 0;
+
+out:
+	ao2_cleanup(reg);
+	ao2_cleanup(endpt);
+	ao2_cleanup(transp);
+	return rc;
+}
+
 /*! \brief Helper function which sends a message and cleans up, if needed, on failure */
 static pj_status_t registration_client_send(struct sip_outbound_registration_client_state *client_state,
 	pjsip_tx_data *tdata)
@@ -791,6 +875,12 @@ static pj_status_t registration_client_send(struct sip_outbound_registration_cli
 	pjsip_regc_set_transport(client_state->client, &selector);
 	ast_sip_tpselector_unref(&selector);
 
+	/* Create initial IMS headers and reset transport. */
+	if (client_state->ims_aka && client_state->ims_state == IMS_STATE_REGISTER) {
+		if (ims_registration_client(client_state, tdata))
+			return -1;
+	}
+
 	status = pjsip_regc_send(client_state->client, tdata);
 
 	/*
@@ -899,21 +989,7 @@ static int handle_client_registration(void *data)
 		return -1;
 	}
 
-	/* Create initial authorization header. */
-	if (client_state->ims_aka) {
-		struct sip_outbound_registration *reg = NULL;
-		struct ast_sip_endpoint *endpt = NULL;
-		reg = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "registration",client_state->registration_name);
-		endpt = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "endpoint", reg->endpoint);
-		if ((reg = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "registration",
-						     client_state->registration_name)) &&
-		    reg->endpoint &&
-		    (endpt = ast_sorcery_retrieve_by_id(ast_sip_get_sorcery(), "endpoint", reg->endpoint)) &&
-		    add_outbound_initial_authorization(tdata, endpt->fromdomain, &client_state->outbound_auths)) {
-			ast_log(LOG_WARNING, "Failed to add initial authorization header.\n");
-			return -1;
-		}
-	}
+	client_state->ims_state = IMS_STATE_REGISTER;
 
 	registration_client_send(client_state, tdata);
 
@@ -1272,6 +1348,86 @@ static void save_response_fields_to_transport(struct registration_response *resp
 	}
 }
 
+static struct ast_sip_auth *ims_get_sip_auth(const struct ast_sip_auth_vector *auth_vector)
+{
+	size_t auth_size = AST_VECTOR_SIZE(auth_vector);
+	struct ast_sip_auth *auths[auth_size], *auth = NULL;
+	int idx;
+
+	memset(auths, 0, sizeof(auths));
+	if (ast_sip_retrieve_auths(auth_vector, auths)) {
+		ast_log(LOG_ERROR, "No authentication vector found. Please configure authentication for IMS\n");
+		goto cleanup;
+	}
+
+	for (idx = 0; idx < auth_size; ++idx) {
+		if (auths[idx]->type == AST_SIP_AUTH_TYPE_IMS_AKA)
+			break;
+	}
+
+	if (idx == auth_size) {
+		ast_log(LOG_ERROR, "No authentication vector found with type=ims_aka. Please fix config.\n");
+		goto cleanup;
+	}
+
+	auth = auths[idx];
+
+cleanup:
+	ast_sip_cleanup_auths(auths, auth_size);
+	return auth;
+}
+
+static int handle_ims_unauthorized(struct registration_response *response)
+{
+	struct security_server sec;
+	struct ast_sip_auth *auth;
+	uint8_t out_ik[16], out_ck[16], out_auts[14];
+	int rc;
+
+	if (response->client_state->ims_state == IMS_STATE_REGISTER) {
+		/* Remove initial autorization header. */
+		if (volte_del_authorization(response->old_request)) {
+			ast_log(LOG_ERROR, "Failed to remove authorization header.\n");
+			return -1;
+		}
+		/* Get security server */
+		if (volte_get_security_server(&response->client_state->volte, response->rdata, &sec)) {
+			ast_log(LOG_ERROR, "Failed to parse the security server header.\n");
+			return -1;
+		}
+
+		if (!(auth = ims_get_sip_auth(&response->client_state->outbound_auths)))
+			return -1;
+
+		rc = volte_authenticate(&response->client_state->volte, response->rdata,
+					(response->code == 401) ? PJSIP_H_WWW_AUTHENTICATE : PJSIP_H_PROXY_AUTHENTICATE,
+					auth->usim_opc, auth->usim_k, auth->usim_sqn, (uint8_t *)auth->ims_res,
+					out_ik, out_ck, out_auts);
+		if (rc == -EAGAIN) {
+			ast_log(LOG_WARNING, "SQN out of sequence, syncing.\n");
+			auth->ims_res_len = 0;
+			return -1;
+		}
+		if (rc) {
+			ast_log(LOG_ERROR, "Authentication failed.\n");
+			return -1;
+		}
+		auth->ims_res_len = 8;
+
+		if (volte_set_transport(&response->client_state->volte, response->old_request, &sec.alg, &sec.ealg,
+					out_ik, pj_strtoul(&sec.spi_c), pj_strtoul(&sec.spi_s),
+					pj_strtoul(&sec.port_c), pj_strtoul(&sec.port_s))) {
+			ast_log(LOG_ERROR, "Failed to set transport.\n");
+			return -1;
+		}
+		if (volte_add_security_verify(&response->client_state->volte, response->old_request)) {
+			ast_log(LOG_ERROR, "Failed to add security verify.\n");
+			return -1;
+		}
+	}
+
+	return 0;
+}
 
 /*! \brief Callback function for handling a response to a registration attempt */
 static int handle_registration_response(void *data)
@@ -1280,6 +1436,7 @@ static int handle_registration_response(void *data)
 	pjsip_regc_info info;
 	char server_uri[PJSIP_MAX_URL_SIZE];
 	char client_uri[PJSIP_MAX_URL_SIZE];
+	bool ims_failed = false;
 
 	if (response->client_state->status == SIP_REGISTRATION_STOPPED) {
 		ao2_ref(response, -1);
@@ -1311,9 +1468,9 @@ static int handle_registration_response(void *data)
 		pjsip_cseq_hdr *cseq_hdr;
 		pjsip_tx_data *tdata;
 
-		/* Remove initial autorization header. */
 		if (response->client_state->ims_aka) {
-			volte_del_authorization(response->old_request);
+			if (handle_ims_unauthorized(response))
+				ims_failed = true;
 		}
 
 		if (response->client_state->security_negotiation == AST_SIP_SECURITY_NEG_MEDIASEC) {
@@ -1356,9 +1513,10 @@ static int handle_registration_response(void *data)
 			schedule_registration(response->client_state, 0);
 			ao2_ref(response, -1);
 			return 0;
-		} else if (!ast_sip_create_request_with_auth(&response->client_state->outbound_auths,
+		} else if (!ims_failed && !ast_sip_create_request_with_auth(&response->client_state->outbound_auths,
 				response->rdata, response->old_request, &tdata)) {
 			response->client_state->auth_attempted = 1;
+			response->client_state->ims_state = IMS_STATE_AUTHENTICATE;
 			ast_debug(1, "Sending authenticated REGISTER to server '%s' from client '%s'\n",
 					server_uri, client_uri);
 			pjsip_tx_data_add_ref(tdata);
@@ -1381,6 +1539,7 @@ static int handle_registration_response(void *data)
 		/* Otherwise, fall through so the failure is processed appropriately */
 	}
 
+	response->client_state->ims_state = IMS_STATE_REGISTER;
 	response->client_state->auth_attempted = 0;
 
 	if (PJSIP_IS_STATUS_IN_CLASS(response->code, 200)) {
@@ -1879,41 +2038,18 @@ cleanup:
 }
 
 /* Add intial authorization header for IMS AKA */
-static int add_outbound_initial_authorization(pjsip_tx_data *tdata, const char *fromdomain,
-					      const struct ast_sip_auth_vector *auth_vector)
+static int ims_add_outbound_initial_authorization(pjsip_tx_data *tdata, const char *fromdomain,
+						  const struct ast_sip_auth_vector *auth_vector)
 {
-	size_t auth_size = AST_VECTOR_SIZE(auth_vector);
-	struct ast_sip_auth *auths[auth_size];
-	int res = 0;
-	int idx;
-	const char *username;
+	struct ast_sip_auth *auth;
 
-puts("1");
-	memset(auths, 0, sizeof(auths));
-	if (ast_sip_retrieve_auths(auth_vector, auths)) {
-		res = -1;
-		goto cleanup;
-	}
+	if (!(auth = ims_get_sip_auth(auth_vector)))
+		return -1;
+printf("JOLLY und hier hat auths den pointer %p\n", auth);
 
-puts("2");
-	for (idx = 0; idx < auth_size; ++idx) {
-		if (auths[idx]->type == AST_SIP_AUTH_TYPE_IMS_AKA)
-			break;
-	}
+	volte_init_authorization(tdata, fromdomain, auth->auth_user);
 
-	if (idx == auth_size) {
-		res = -1;
-		goto cleanup;
-	}
-
-puts("3");
-	username = auths[idx]->auth_user;
-
-	volte_init_authorization(tdata, fromdomain, username);
-
-cleanup:
-	ast_sip_cleanup_auths(auths, auth_size);
-	return res;
+	return 0;
 }
 
 /*! \brief Helper function that allocates a pjsip registration client and configures it */
@@ -2855,6 +2991,8 @@ static int unload_module(void)
 	ao2_cleanup(shutdown_group);
 	shutdown_group = NULL;
 
+	g_volte_exit();
+
 	return 0;
 }
 

res/res_pjsip_outbound_registration/milenage.c

@@ -299,6 +299,8 @@ int milenage_check(const u8 *opc, const u8 *k, const u8 *sqn, const u8 *_rand,
 	u8 mac_a[8], ak[6], rx_sqn[6];
 	const u8 *amf;
 
+	hexdump(LOG_DEBUG, "Milenage: OPC", opc, 16);
+	hexdump(LOG_DEBUG, "Milenage: K", k, 16);
 	hexdump(LOG_DEBUG, "Milenage: AUTN", autn, 16);
 	hexdump(LOG_DEBUG, "Milenage: RAND", _rand, 16);
 
@@ -314,7 +316,8 @@ int milenage_check(const u8 *opc, const u8 *k, const u8 *sqn, const u8 *_rand,
 	/* AUTN = (SQN ^ AK) || AMF || MAC */
 	for (i = 0; i < 6; i++)
 		rx_sqn[i] = autn[i] ^ ak[i];
-	hexdump(LOG_DEBUG, "Milenage: SQN", rx_sqn, 6);
+	hexdump(LOG_DEBUG, "Milenage: RX SQN", rx_sqn, 6);
+	hexdump(LOG_DEBUG, "Milenage: SQN", sqn, 6);
 
 	if (memcmp(rx_sqn, sqn, 6) <= 0) {
 		u8 auts_amf[2] = { 0x00, 0x00 }; /* TS 33.102 v7.0.0, 6.3.3 */

res/res_pjsip_outbound_registration/volte.c

@@ -22,37 +22,52 @@
 #include <pjsip.h>
 
 #include "volte.h"
+#include "milenage.h"
+
+#define fmt_str(str) (int)(str).slen, (str).ptr
 
 /* Socket for transform configuration */ 
 static struct mnl_socket *g_mnl_socket = NULL;
 
-/* Security address and SPI settings */
-static pj_sockaddr g_local_addr_c, g_remote_addr_s;
-static pj_sockaddr g_remote_addr_c, g_local_addr_s;
-static uint32_t g_local_spi_c, g_remote_spi_s;
-static uint32_t g_remote_spi_c, g_local_spi_s;
-static pj_bool_t g_local_sa_c_set = PJ_FALSE, g_remote_sa_s_set = PJ_FALSE;
-static pj_bool_t g_remote_sa_c_set = PJ_FALSE, g_local_sa_s_set = PJ_FALSE;
-static pj_bool_t g_local_sp_c_set = PJ_FALSE, g_remote_sp_s_set = PJ_FALSE;
-static pj_bool_t g_remote_sp_c_set = PJ_FALSE, g_local_sp_s_set = PJ_FALSE;
-
 /* Supported authentication and encryption algorithms. */
 struct ipsec_alg {
 	const char *sip_name;
 	const char *kernel_name;
 };
 
-const struct ipsec_alg ipsec_alg[] = {
+const struct ipsec_alg g_ipsec_alg[] = {
 	{ "hmac-md5-96", "md5" },
 	{ "hmac-sha-1-96", "sha1" },
 	{ NULL, NULL }
 };
 
-const struct ipsec_alg ipsec_ealg[] = {
+const struct ipsec_alg g_ipsec_ealg[] = {
 	{ "null", "cipher_null" },
 	{ NULL, NULL }
 };
 
+/* Global init function. Must be called before the first registration is made. May be called again. */
+pj_status_t g_volte_init(void)
+{
+	if (!g_mnl_socket)
+		g_mnl_socket = xfrm_init_mnl_socket();
+	if (!g_mnl_socket) {
+		ast_log(LOG_ERROR, "Failed to init mnl socket to admin xfrm rules. "
+			"Please make sure that the user running asterisk has the rights to do so. "
+			"(E.g use \"setcap 'cap_net_admin,cap_sys_resource=ep' /usr/sbin/asterisk\")\n");
+		return -EPERM;
+	}
+
+	return PJ_SUCCESS;
+}
+
+/* Global exit function. Must be called when module is unloaded. */
+void g_volte_exit(void)
+{
+	if (g_mnl_socket)
+		xfrm_exit_mnl_socket(g_mnl_socket);
+}
+
 /* Convert from pj_sockaddr to sockaddr_storage. */
 static void copy_pj_sockaddr_to_sockaddr_storage(const pj_sockaddr *src, struct sockaddr_storage *dst)
 {
@@ -90,110 +105,112 @@ static char *sockaddr_storage_to_string(const struct sockaddr_storage *addr, cha
 }
 
 /* Delete old SA and SP entries upon new registration or module exit. */
-void volte_cleanup_xfrm(void)
+void volte_cleanup_xfrm(struct volte_states *volte)
 {
 	struct sockaddr_storage local_addr_c, local_addr_s;
 	struct sockaddr_storage remote_addr_c, remote_addr_s;
 
 	/* Convert from pj_sockaddr to sockaddr_storage. */
-	copy_pj_sockaddr_to_sockaddr_storage(&g_local_addr_c, &local_addr_c);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_remote_addr_c, &remote_addr_c);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_local_addr_s, &local_addr_s);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_remote_addr_s, &remote_addr_s);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->local_addr_c, &local_addr_c);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->remote_addr_c, &remote_addr_c);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->local_addr_s, &local_addr_s);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->remote_addr_s, &remote_addr_s);
 
-	if (g_local_sa_c_set || g_local_sa_s_set || g_remote_sa_c_set || g_remote_sa_s_set ||
-	    g_local_sp_c_set || g_local_sp_s_set || g_remote_sp_c_set || g_remote_sp_s_set)
+	if (volte->local_sa_c_set || volte->local_sa_s_set || volte->remote_sa_c_set || volte->remote_sa_s_set ||
+	    volte->local_sp_c_set || volte->local_sp_s_set || volte->remote_sp_c_set || volte->remote_sp_s_set)
 		ast_log(LOG_DEBUG, "Remove old security associations/policies\n");
 
 	/* Remove current security associations and policies. */
-	if (g_local_sa_c_set) {
+	if (volte->local_sa_c_set) {
 		xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&local_addr_c,
-			    (const struct sockaddr *)&remote_addr_s, g_remote_spi_s);
-		g_local_sa_c_set = PJ_FALSE;
+			    (const struct sockaddr *)&remote_addr_s, volte->remote_spi_s);
+		volte->local_sa_c_set = PJ_FALSE;
 	}
-	if (g_local_sa_s_set) {
+	if (volte->local_sa_s_set) {
 		xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&local_addr_s,
-			    (const struct sockaddr *)&remote_addr_c, g_remote_spi_c);
-		g_local_sa_s_set = PJ_FALSE;
+			    (const struct sockaddr *)&remote_addr_c, volte->remote_spi_c);
+		volte->local_sa_s_set = PJ_FALSE;
 	}
-	if (g_remote_sa_c_set) {
+	if (volte->remote_sa_c_set) {
 		xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&remote_addr_c,
-			    (const struct sockaddr *)&local_addr_s, g_local_spi_s);
-		g_remote_sa_c_set = PJ_FALSE;
+			    (const struct sockaddr *)&local_addr_s, volte->local_spi_s);
+		volte->remote_sa_c_set = PJ_FALSE;
 	}
-	if (g_remote_sa_s_set) {
+	if (volte->remote_sa_s_set) {
 		xfrm_sa_del(g_mnl_socket, (const struct sockaddr *)&remote_addr_s,
-			    (const struct sockaddr *)&local_addr_c, g_local_spi_c);
-		g_remote_sa_s_set = PJ_FALSE;
+			    (const struct sockaddr *)&local_addr_c, volte->local_spi_c);
+		volte->remote_sa_s_set = PJ_FALSE;
 	}
-	if (g_local_sp_c_set) {
+	if (volte->local_sp_c_set) {
 		xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&local_addr_c,
 			    (const struct sockaddr *)&remote_addr_s, false);
-		g_local_sp_c_set = PJ_FALSE;
+		volte->local_sp_c_set = PJ_FALSE;
 	}
-	if (g_local_sp_s_set) {
+	if (volte->local_sp_s_set) {
 		xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&local_addr_s,
 			    (const struct sockaddr *)&remote_addr_c, false);
-		g_local_sp_s_set = PJ_FALSE;
+		volte->local_sp_s_set = PJ_FALSE;
 	}
-	if (g_remote_sp_c_set) {
+	if (volte->remote_sp_c_set) {
 		xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&remote_addr_c,
 			    (const struct sockaddr *)&local_addr_s, true);
-		g_remote_sp_c_set = PJ_FALSE;
+		volte->remote_sp_c_set = PJ_FALSE;
 	}
-	if (g_remote_sp_s_set) {
+	if (volte->remote_sp_s_set) {
 		xfrm_policy_del(g_mnl_socket, (const struct sockaddr *)&remote_addr_s,
 			    (const struct sockaddr *)&local_addr_c, true);
-		g_remote_sp_s_set = PJ_FALSE;
+		volte->remote_sp_s_set = PJ_FALSE;
 	}
 
 }
 
-pj_status_t volte_alloc_spi(void)
+pj_status_t volte_alloc_spi(struct volte_states *volte)
 {
 	struct sockaddr_storage local_addr_c, local_addr_s;
 	struct sockaddr_storage remote_addr_c, remote_addr_s;
-	char src_str[64], dst_str[64];
+//	char src_str[64], dst_str[64];
 	pj_status_t status;
 
 	/* Convert from pj_sockaddr to sockaddr_storage. */
-	copy_pj_sockaddr_to_sockaddr_storage(&g_local_addr_c, &local_addr_c);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_remote_addr_c, &remote_addr_c);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_local_addr_s, &local_addr_s);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_remote_addr_s, &remote_addr_s);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->local_addr_c, &local_addr_c);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->remote_addr_c, &remote_addr_c);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->local_addr_s, &local_addr_s);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->remote_addr_s, &remote_addr_s);
 
+#if 0
 	ast_log(LOG_DEBUG, "SPI allocation: local client: %s:%d remote server: %s:%d\n",
 		sockaddr_storage_to_string(&local_addr_c, src_str, sizeof(src_str)),
-		pj_sockaddr_get_port(&g_local_addr_c),
+		pj_sockaddr_get_port(&volte->local_addr_c),
 		sockaddr_storage_to_string(&remote_addr_s, dst_str, sizeof(dst_str)),
-		pj_sockaddr_get_port(&g_remote_addr_s));
+		pj_sockaddr_get_port(&volte->remote_addr_s));
 	ast_log(LOG_DEBUG, "SPI allocation: remote client: %s:%d local server: %s:%d\n",
 		sockaddr_storage_to_string(&remote_addr_c, src_str, sizeof(src_str)),
-		pj_sockaddr_get_port(&g_remote_addr_s),
+		pj_sockaddr_get_port(&volte->remote_addr_s),
 		sockaddr_storage_to_string(&local_addr_c, dst_str, sizeof(dst_str)),
-		pj_sockaddr_get_port(&g_local_addr_s));
+		pj_sockaddr_get_port(&volte->local_addr_s));
+#endif
 
 	/* Allocate SPI-C and SPI-S towards remote peer. */
-	status = xfrm_spi_alloc(g_mnl_socket, 2342, &g_local_spi_c, (const struct sockaddr *)&local_addr_c,
+	status = xfrm_spi_alloc(g_mnl_socket, 2342, &volte->local_spi_c, (const struct sockaddr *)&local_addr_c,
 			    (const struct sockaddr *)&remote_addr_c);
 	if (status) {
 spi_alloc_failed:
 		ast_log(LOG_ERROR, "Failed to allocate SPI.\n");
 		return status;
 	}
-	g_local_sa_s_set = PJ_TRUE;
-	status = xfrm_spi_alloc(g_mnl_socket, 2342, &g_local_spi_s, (const struct sockaddr *)&local_addr_s,
+//	volte->local_sa_s_set = PJ_TRUE;
+	status = xfrm_spi_alloc(g_mnl_socket, 2342, &volte->local_spi_s, (const struct sockaddr *)&local_addr_s,
 			    (const struct sockaddr *)&remote_addr_s);
 	if (status)
 		goto spi_alloc_failed;
-	g_local_sa_c_set = PJ_TRUE;
-	ast_log(LOG_DEBUG, "SPI allocation: SPI-C=0x%08x SPI-S=0x%08x\n", g_local_spi_s, g_local_spi_c);
+//	volte->local_sa_c_set = PJ_TRUE;
+	ast_log(LOG_DEBUG, "SPI allocation: SPI-C=0x%08x SPI-S=0x%08x\n", volte->local_spi_s, volte->local_spi_c);
 
-	return PJ_TRUE;
+	return PJ_SUCCESS;
 }
 
 /* Set new SA and SP entries upon secuirty handshake. */
-pj_status_t volte_set_xfrm(const pj_str_t *alg, const pj_str_t *ealg, uint8_t *ik)
+static pj_status_t volte_set_xfrm(struct volte_states *volte, const pj_str_t *alg, const pj_str_t *ealg, uint8_t *ik)
 {
 	struct xfrm_algobuf auth_algo, ciph_algo;
 	int i, j;
@@ -204,23 +221,23 @@ pj_status_t volte_set_xfrm(const pj_str_t *alg, const pj_str_t *ealg, uint8_t *i
 	pj_status_t status;
 
 	/* Set authentication and encryption algorithms and key. */ 
-	for (i = 0; ipsec_alg[i].sip_name; i++) {
-		if (!pj_strncmp2(alg, ipsec_alg[i].sip_name, strlen(ipsec_alg[i].sip_name)))
+	for (i = 0; g_ipsec_alg[i].sip_name; i++) {
+		if (!pj_strncmp2(alg, g_ipsec_alg[i].sip_name, strlen(g_ipsec_alg[i].sip_name)))
 			break;
 	}
-	if (!ipsec_alg[i].kernel_name) {
+	if (!g_ipsec_alg[i].kernel_name) {
 		ast_log(LOG_ERROR, "Given 'alg' not supported.\n");
 		return -EINVAL;
 	}
-	for (j = 0; ipsec_ealg[j].sip_name; j++) {
-		if (!pj_strncmp2(ealg, ipsec_ealg[j].sip_name, strlen(ipsec_ealg[j].sip_name)))
+	for (j = 0; g_ipsec_ealg[j].sip_name; j++) {
+		if (!pj_strncmp2(ealg, g_ipsec_ealg[j].sip_name, strlen(g_ipsec_ealg[j].sip_name)))
 			break;
 	}
-	if (!ipsec_ealg[i].kernel_name) {
+	if (!g_ipsec_ealg[j].kernel_name) {
 		ast_log(LOG_ERROR, "Given 'ealg' not supported.\n");
 		return -EINVAL;
 	}
-	strcpy(auth_algo.algo.alg_name, ipsec_alg[i].kernel_name);
+	strcpy(auth_algo.algo.alg_name, g_ipsec_alg[i].kernel_name);
 	switch (i) {
 	case 0:
 		memcpy(auth_algo.algo.alg_key, ik, 16);
@@ -232,79 +249,83 @@ pj_status_t volte_set_xfrm(const pj_str_t *alg, const pj_str_t *ealg, uint8_t *i
 		auth_algo.algo.alg_key_len = 160;
 		break;
 	}
-	strcpy(ciph_algo.algo.alg_name, ipsec_ealg[j].kernel_name);
+	strcpy(ciph_algo.algo.alg_name, g_ipsec_ealg[j].kernel_name);
 	switch (j) {
 	case 0:
 		ciph_algo.algo.alg_key_len = 0;
 		break;
 	}
 
-	copy_pj_sockaddr_to_sockaddr_storage(&g_local_addr_c, &local_addr_c);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_remote_addr_c, &remote_addr_c);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_local_addr_s, &local_addr_s);
-	copy_pj_sockaddr_to_sockaddr_storage(&g_remote_addr_s, &remote_addr_s);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->local_addr_c, &local_addr_c);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->remote_addr_c, &remote_addr_c);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->local_addr_s, &local_addr_s);
+	copy_pj_sockaddr_to_sockaddr_storage(&volte->remote_addr_s, &remote_addr_s);
 
 	ast_log(LOG_DEBUG, "xfrm: local client: %s:%d (SPI=0x%08x) remote server: %s:%d (SPI=0x%08x)\n",
 		sockaddr_storage_to_string(&local_addr_c, src_str, sizeof(src_str)),
-		pj_sockaddr_get_port(&g_local_addr_c), g_local_spi_c,
+		pj_sockaddr_get_port(&volte->local_addr_c), volte->local_spi_c,
 		sockaddr_storage_to_string(&remote_addr_s, dst_str, sizeof(dst_str)),
-		pj_sockaddr_get_port(&g_remote_addr_s), g_remote_spi_s);
+		pj_sockaddr_get_port(&volte->remote_addr_s), volte->remote_spi_s);
 	ast_log(LOG_DEBUG, "xfrm: remote client: %s:%d (SPI=0x%08x) local server: %s:%d (SPI=0x%08x)\n",
 		sockaddr_storage_to_string(&remote_addr_c, src_str, sizeof(src_str)),
-		pj_sockaddr_get_port(&g_remote_addr_s), g_remote_spi_c,
+		pj_sockaddr_get_port(&volte->remote_addr_s), volte->remote_spi_c,
 		sockaddr_storage_to_string(&local_addr_c, dst_str, sizeof(dst_str)),
-		pj_sockaddr_get_port(&g_local_addr_s), g_local_spi_s);
+		pj_sockaddr_get_port(&volte->local_addr_s), volte->local_spi_s);
 	ast_log(LOG_DEBUG, "xfrm: alg: %s ealg: %s\n", auth_algo.algo.alg_name, ciph_algo.algo.alg_name);
 
-	status = xfrm_sa_add(g_mnl_socket, g_remote_spi_s, (const struct sockaddr *)&local_addr_c,
-			 (const struct sockaddr *)&remote_addr_s, g_remote_spi_s, &auth_algo.algo, &ciph_algo.algo);
+	status = xfrm_sa_add(g_mnl_socket, volte->remote_spi_s, (const struct sockaddr *)&local_addr_c,
+			     (const struct sockaddr *)&remote_addr_s, volte->remote_spi_s,
+			     &auth_algo.algo, &ciph_algo.algo);
 	if (status)
 		sa_add_failed = status;
 	else
-		g_local_sa_c_set = PJ_TRUE;
-	status = xfrm_sa_add(g_mnl_socket, g_remote_spi_c, (const struct sockaddr *)&local_addr_s,
-			 (const struct sockaddr *)&remote_addr_c, g_remote_spi_c, &auth_algo.algo, &ciph_algo.algo);
+		volte->local_sa_c_set = PJ_TRUE;
+	status = xfrm_sa_add(g_mnl_socket, volte->remote_spi_c, (const struct sockaddr *)&local_addr_s,
+			     (const struct sockaddr *)&remote_addr_c, volte->remote_spi_c,
+			     &auth_algo.algo, &ciph_algo.algo);
 	if (status)
 		sa_add_failed = status;
 	else
-		g_local_sa_s_set = PJ_TRUE;
-	status = xfrm_sa_add(g_mnl_socket, g_local_spi_s, (const struct sockaddr *)&remote_addr_c,
-			 (const struct sockaddr *)&local_addr_s, g_local_spi_s, &auth_algo.algo, &ciph_algo.algo);
+		volte->local_sa_s_set = PJ_TRUE;
+	status = xfrm_sa_add(g_mnl_socket, volte->local_spi_s, (const struct sockaddr *)&remote_addr_c,
+			     (const struct sockaddr *)&local_addr_s, volte->local_spi_s,
+			     &auth_algo.algo, &ciph_algo.algo);
 	if (status)
 		sa_add_failed = status;
 	else
-		g_remote_sa_c_set = PJ_TRUE;
+		volte->remote_sa_c_set = PJ_TRUE;
 
-	status = xfrm_sa_add(g_mnl_socket, g_local_spi_c, (const struct sockaddr *)&remote_addr_s,
-			 (const struct sockaddr *)&local_addr_c, g_local_spi_c, &auth_algo.algo, &ciph_algo.algo);
+	status = xfrm_sa_add(g_mnl_socket, volte->local_spi_c, (const struct sockaddr *)&remote_addr_s,
+			     (const struct sockaddr *)&local_addr_c, volte->local_spi_c,
+			     &auth_algo.algo, &ciph_algo.algo);
 	if (status)
 		sa_add_failed = status;
 	else
-		g_remote_sa_s_set = PJ_TRUE;
+		volte->remote_sa_s_set = PJ_TRUE;
 	status = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&local_addr_c,
-			     (const struct sockaddr *)&remote_addr_s, g_remote_spi_s, false);
+				(const struct sockaddr *)&remote_addr_s, volte->remote_spi_s, false);
 	if (status)
 		sp_add_failed = status;
 	else
-		g_local_sp_c_set = PJ_TRUE;
+		volte->local_sp_c_set = PJ_TRUE;
 	status = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&local_addr_s,
-			     (const struct sockaddr *)&remote_addr_c, g_remote_spi_c, false);
+				(const struct sockaddr *)&remote_addr_c, volte->remote_spi_c, false);
 	if (status)
 		sp_add_failed = status;
 	else
-		g_local_sp_s_set = PJ_TRUE;
+		volte->local_sp_s_set = PJ_TRUE;
 	status = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&remote_addr_c,
-			     (const struct sockaddr *)&local_addr_s, g_local_spi_s, true);
+				(const struct sockaddr *)&local_addr_s, volte->local_spi_s, true);
 	if (status)
 		sp_add_failed = status;
 	else
-		g_remote_sp_c_set = PJ_TRUE;
+		volte->remote_sp_c_set = PJ_TRUE;
 	status = xfrm_policy_add(g_mnl_socket, (const struct sockaddr *)&remote_addr_s,
-			     (const struct sockaddr *)&local_addr_c, g_local_spi_c, true);
+				(const struct sockaddr *)&local_addr_c, volte->local_spi_c, true);
 	if (status)
 		sp_add_failed = status;
 	else
-		g_remote_sp_s_set = PJ_TRUE;
+		volte->remote_sp_s_set = PJ_TRUE;
 
 	if (sa_add_failed)
 		ast_log(LOG_ERROR, "Failed to add IPSec SA.\n");
@@ -314,7 +335,7 @@ pj_status_t volte_set_xfrm(const pj_str_t *alg, const pj_str_t *ealg, uint8_t *i
 	return (sa_add_failed) ? sa_add_failed : sp_add_failed;
 }
 
-/* Header field names. */
+/* Header field names */
 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 };
@@ -322,13 +343,25 @@ 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_AUTS = { "auts", 4 };
+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_SECURITY_VERIFY = { "Security-Verify", 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 };
 
 /* Create string header and add given value. */
 static pj_status_t 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. */
+	/* Add header. */
 	hdr = pjsip_generic_string_hdr_create(tdata->pool, name, value);
 	if (!hdr) {
 		ast_log(LOG_ERROR, "Failed to create string header.");
@@ -370,6 +403,43 @@ static pj_status_t add_value_array_hdr(pjsip_tx_data *tdata, const pj_str_t *nam
 	return PJ_SUCCESS;
 }
 
+/* Add security client header to SIP message. */
+static pj_status_t 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 -ENOMEM;
+	}
+
+	/* 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);
+
+	return PJ_SUCCESS;
+}
+
+
 /* Add Sec-Agree to header. */
 pj_status_t volte_add_sec_agree(pjsip_tx_data *tdata)
 {
@@ -400,7 +470,9 @@ pj_status_t volte_init_authorization(pjsip_tx_data *tdata, const char *fromdomai
 	char authorization[1024];
 	pj_status_t status;
 
-	snprintf(authorization, sizeof(authorization), "Digest uri=\"sip:%s\",usernmame=\"%s@%s\",response=\"\",realm=\"%s\",nonce=\"\"", fromdomain, username, fromdomain, fromdomain);
+	snprintf(authorization, sizeof(authorization),
+		 "Digest uri=\"sip:%s\",usernmame=\"%s@%s\",response=\"\",realm=\"%s\",nonce=\"\"",
+		 fromdomain, username, fromdomain, fromdomain);
 
 	const pj_str_t authorization_str = {authorization, strlen(authorization)};
 	status = add_value_string_hdr(tdata, &STR_AUTHORIZATION, &authorization_str);
@@ -423,3 +495,333 @@ pj_status_t volte_del_authorization(pjsip_tx_data *tdata)
 
 	return PJ_SUCCESS;
 }
+
+/* Reset old transport and clear IPSec transformations */
+pj_status_t volte_reset_transport(struct volte_states *volte, pjsip_tx_data *tdata)
+{
+	pj_status_t status;
+	int old_port_c;
+
+	/* Cleanup IPSec transform. */
+	volte_cleanup_xfrm(volte);
+
+	/* Cleanup old transport. */
+	old_port_c = pj_sockaddr_get_port(&volte->local_addr_c);
+	if (old_port_c > 0 && old_port_c < 65535) {
+		if (!tdata->tp_info.transport) {
+			ast_log(LOG_ERROR, "The Message has not transport. Please fix!\n");
+			return -ENOTSUP;
+		}
+		/* Create socket with default transport port. */
+		if (!tdata->tp_info.transport->create_new_sock || !tdata->tp_info.transport->connect_new_sock) {
+			ast_log(LOG_ERROR, "The transport protocol does not support socket change. Please fix!\n");
+			return -ENOTSUP;
+		}
+		status = tdata->tp_info.transport->create_new_sock(tdata->tp_info.transport, NULL);
+		if (status != PJ_SUCCESS) {
+			ast_log(LOG_ERROR, "Failed to get connection addresses (errno=%d).\n", errno);
+			return status;
+		}
+		status = tdata->tp_info.transport->connect_new_sock(tdata->tp_info.transport,
+								    &volte->local_addr_c, &volte->remote_addr_s);
+		if (status != PJ_SUCCESS) {
+			ast_log(LOG_ERROR, "Failed to change connection addresses (errno=%d).\n", errno);
+			return status;
+		}
+	}
+
+	return PJ_SUCCESS;
+}
+
+/* Add security client header. */
+pj_status_t volte_add_security_client(struct volte_states *volte, pjsip_tx_data *tdata, int port_c, int port_s)
+{
+	pj_status_t status;
+
+	/* Reset addresses. */
+	memset(&volte->local_addr_c, 0, sizeof(pj_sockaddr));
+	volte->local_addr_c.addr.sa_family = PJ_AF_INET;
+	memset(&volte->local_addr_s, 0, sizeof(pj_sockaddr));
+	volte->local_addr_s.addr.sa_family = PJ_AF_INET;
+	memset(&volte->remote_addr_c, 0, sizeof(pj_sockaddr));
+	volte->remote_addr_c.addr.sa_family = PJ_AF_INET;
+	memset(&volte->remote_addr_s, 0, sizeof(pj_sockaddr));
+	volte->remote_addr_s.addr.sa_family = PJ_AF_INET;
+
+	/* Set new local ports */
+	pj_sockaddr_set_port(&volte->local_addr_c, port_c);
+	pj_sockaddr_set_port(&volte->local_addr_s, port_s);
+
+	/* Allocate SPI */
+	status = volte_alloc_spi(volte);
+	if (status)
+		return status;
+
+	status = add_securety_client_hdr(tdata, g_ipsec_alg, g_ipsec_ealg, volte->local_spi_c, volte->local_spi_s,
+					 port_c, port_s);
+	if (status)
+		return status;
+
+	return PJ_SUCCESS;
+}
+
+/* Set new transport and set IPSec transformations */
+pj_status_t volte_set_transport(struct volte_states *volte, pjsip_tx_data *tdata, const pj_str_t *alg,
+				const pj_str_t *ealg, uint8_t *ik, uint32_t remote_spi_c, uint32_t remote_spi_s,
+				uint16_t remote_port_c, uint16_t remote_port_s)
+{
+	int local_port_c, local_port_s;
+	pj_status_t status;
+
+	/* Cleanup IPSec transform. */
+	volte_cleanup_xfrm(volte);
+
+	/* Get addresses from current transport and replace the local ports. */
+	local_port_c = pj_sockaddr_get_port(&volte->local_addr_c);
+	local_port_s = pj_sockaddr_get_port(&volte->local_addr_s);
+	memcpy(&volte->local_addr_c, &tdata->tp_info.transport->local_addr, sizeof(pj_sockaddr));
+	memcpy(&volte->local_addr_s, &tdata->tp_info.transport->factory->local_addr, sizeof(pj_sockaddr));
+	memcpy(&volte->remote_addr_c, &tdata->tp_info.dst_addr, sizeof(pj_sockaddr));
+	memcpy(&volte->remote_addr_s, &tdata->tp_info.dst_addr, sizeof(pj_sockaddr));
+	pj_sockaddr_set_port(&volte->local_addr_c, local_port_c);
+	pj_sockaddr_set_port(&volte->local_addr_s, local_port_s);
+	pj_sockaddr_set_port(&volte->remote_addr_c, remote_port_c);
+	pj_sockaddr_set_port(&volte->remote_addr_s, remote_port_s);
+
+	/* Set IPSec transform. */
+	volte->remote_spi_c = remote_spi_c;
+	volte->remote_spi_s = remote_spi_s;
+	volte_set_xfrm(volte, alg, ealg, ik);
+
+	/* Create socket with new transport port. */
+	if (!tdata->tp_info.transport) {
+		ast_log(LOG_ERROR, "The Message has not transport. Please fix!\n");
+		return -ENOTSUP;
+	}
+	if (!tdata->tp_info.transport->create_new_sock || !tdata->tp_info.transport->connect_new_sock) {
+		ast_log(LOG_ERROR, "The transport protocol does not support socket change. Please fix!\n");
+		return -ENOTSUP;
+	}
+	status = tdata->tp_info.transport->create_new_sock(tdata->tp_info.transport, &volte->local_addr_c);
+	if (status != PJ_SUCCESS) {
+		ast_log(LOG_ERROR, "Failed to get connection addresses (errno=%d).\n", errno);
+		return status;
+	}
+	status = tdata->tp_info.transport->connect_new_sock(tdata->tp_info.transport,
+							    &volte->local_addr_c, &volte->remote_addr_s);
+	if (status != PJ_SUCCESS) {
+		ast_log(LOG_ERROR, "Failed to change connection addresses (errno=%d).\n", errno);
+		return status;
+	}
+
+	return PJ_SUCCESS;
+}
+
+static void on_syntax_error(pj_scanner *scanner)
+{
+	PJ_UNUSED_ARG(scanner);
+}
+
+/* Store and parse security server from SIP header and fill a structure. */
+pj_status_t volte_get_security_server(struct volte_states *volte, pjsip_rx_data *rdata, struct security_server *sec)
+{
+	pjsip_generic_string_hdr *sec_hdr;
+	pj_scanner scanner;
+	int i, j;
+
+	/* Get Security-Server from header. */
+	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 -EINVAL;
+	}
+
+	/* Store for security verify. */
+	if (sec_hdr->hvalue.slen < sizeof(volte->security_server)) {
+		memcpy(volte->security_server, sec_hdr->hvalue.ptr, sec_hdr->hvalue.slen);
+		volte->security_server[sec_hdr->hvalue.slen] = '\0';
+	} else {
+		ast_log(LOG_ERROR, "Security-Server' too large.");
+		return -EINVAL;
+	}
+
+	memset(sec, 0, sizeof(*sec));
+
+	pj_scan_init(&scanner, sec_hdr->hvalue.ptr, sec_hdr->hvalue.slen, 0, &on_syntax_error);
+
+	for (;;) {
+		pj_str_t name, value;
+
+		pjsip_parse_param_imp(&scanner, rdata->tp_info.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);
+
+	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 -EINVAL;
+	}
+
+	for (i = 0; g_ipsec_alg[i].sip_name; i++) {
+		if (!pj_strncmp2(&sec->alg, g_ipsec_alg[i].sip_name, strlen(g_ipsec_alg[i].sip_name)))
+			break;
+	}
+	if (!g_ipsec_alg[i].kernel_name) {
+		ast_log(LOG_ERROR, "Given 'alg=%.*s' in Security-Server header not found.\n", fmt_str(sec->alg));
+		return -EINVAL;
+	}
+	for (j = 0; g_ipsec_ealg[j].sip_name; j++) {
+		if (!pj_strncmp2(&sec->ealg, g_ipsec_ealg[j].sip_name, strlen(g_ipsec_ealg[j].sip_name)))
+			break;
+	}
+	if (!g_ipsec_ealg[j].kernel_name) {
+		ast_log(LOG_ERROR, "Given 'ealg=%.*s' in Security-Server header not found.\n", fmt_str(sec->ealg));
+		return -EINVAL;
+	}
+
+	return PJ_SUCCESS;
+}
+
+pj_status_t volte_add_security_verify(struct volte_states *volte, pjsip_tx_data *tdata)
+{
+	const pj_str_t security = { volte->security_server, strlen(volte->security_server)};
+	pj_status_t status;
+
+	status = add_value_string_hdr(tdata, &STR_SECURITY_VERIFY, &security);
+	if (status)
+		return status;
+
+	return PJ_SUCCESS;
+}
+
+static pj_status_t my_hex_to_octet_string(const char *name, const char *input, uint8_t *output, size_t output_size)
+{
+	int i, n;
+
+	if (!input || !input[0]) {
+		ast_log(LOG_ERROR, "Missing value for hex string '%s'.\n", name);
+		return -EINVAL;
+	}
+	i = n = 0;
+	while (*input) {
+		if (i == output_size) {
+			ast_log(LOG_ERROR, "Value for hex string '%s' too long, expecting %zu bytes.\n", name,
+				output_size);
+			return -EINVAL;
+		}
+		if (*input >= '0' && *input <= '9')
+			output[i] = (output[i] << 4) | (*input - '0');
+		else if (*input >= 'a' && *input <= 'f')
+			output[i] = (output[i] << 4) | (*input - 'a' + 10);
+		else if (*input >= 'A' && *input <= 'F')
+			output[i] = (output[i] << 4) | (*input - 'A' + 10);
+		else {
+			ast_log(LOG_ERROR, "Value for hex string '%s' has invalid character '%c'.\n", name, *input);
+			return -EINVAL;
+		}
+		if (++n == 2) {
+			n = 0;
+			i++;
+		}
+		input++;
+	}
+	if (i < output_size) {
+		ast_log(LOG_ERROR, "Value for hex string '%s' too short, expecting %zu bytes.\n", name, output_size);
+		return -EINVAL;
+	}
+
+	return PJ_SUCCESS;
+}
+
+pj_status_t volte_authenticate(struct volte_states *volte, pjsip_rx_data *rdata, pjsip_hdr_e auth_type,
+			       const char *opc_str, const char *k_str, const char *sqn_str, uint8_t *out_res,
+			       uint8_t *out_ik, uint8_t *out_ck, uint8_t *out_auts)
+{
+	pjsip_www_authenticate_hdr *auth_hdr;
+	uint8_t opc[16], k[16], sqn[6], rand_auth[32], *rand = rand_auth, *autn = rand_auth + 16;
+	size_t out_res_len = 8;
+	int rc;
+	pj_status_t status;
+
+	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 in REGISTER response.\n");
+		return -EINVAL;
+	}
+
+	if (!pj_strncmp2(&auth_hdr->challenge.digest.algorithm, "AKAv2-MD5", 9)) {
+		ast_log(LOG_ERROR, "Authentication algorithm not supported. Please fix!\n");
+		return -EINVAL;
+	}
+	if (!!pj_strncmp2(&auth_hdr->challenge.digest.algorithm, "AKAv1-MD5", 9)) {
+		ast_log(LOG_ERROR, "Authentication algorithm not supported.\n");
+		return -EINVAL;
+	}
+
+	status = my_hex_to_octet_string("OPC", opc_str, opc, sizeof(opc));
+	if (status)
+		return status;
+	status = my_hex_to_octet_string("K", k_str, k, sizeof(k));
+	if (status)
+		return status;
+	status = my_hex_to_octet_string("SQN", sqn_str, sqn, sizeof(sqn));
+	if (status)
+		return status;
+
+	ast_base64decode(rand_auth, auth_hdr->challenge.digest.nonce.ptr, sizeof(rand_auth));
+hexdump(LOG_DEBUG, "nonce", rand_auth, 32);
+
+	rc = milenage_check(opc, k, sqn, rand, autn, out_ik, out_ck, out_res, &out_res_len, out_auts);
+printf("mileange rc=%d\n", rc);
+	if (rc == -1) {
+		ast_log(LOG_ERROR, "Milenage authentication failed.\n");
+		return -EINVAL;
+	}
+hexdump(LOG_DEBUG, "IK", out_ik, 16);
+hexdump(LOG_DEBUG, "CK", out_ck, 16);
+
+	if (rc == -2) {
+		/* Tell the caller to perform resync process. */
+		return -EAGAIN;
+	}
+
+	return PJ_SUCCESS;
+}

res/res_pjsip_outbound_registration/volte.h

@@ -2,10 +2,47 @@
 
 #include "netlink_xfrm.h"
 
-void volte_cleanup_xfrm(void);
-pj_status_t volte_alloc_spi(void);
-pj_status_t volte_set_xfrm(const pj_str_t *alg, const pj_str_t *ealg, uint8_t *ik);
+struct volte_states {
+	pj_sockaddr	local_addr_c, remote_addr_s;
+	pj_sockaddr	remote_addr_c, local_addr_s;
+	uint32_t	local_spi_c, remote_spi_s;
+	uint32_t	remote_spi_c, local_spi_s;
+	pj_bool_t	local_sa_c_set, remote_sa_s_set;
+	pj_bool_t	remote_sa_c_set, local_sa_s_set;
+	pj_bool_t	local_sp_c_set, remote_sp_s_set;
+	pj_bool_t	remote_sp_c_set, local_sp_s_set;
+
+	char		security_server[1024];
+};
+
+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;
+};
+
+pj_status_t g_volte_init(void);
+void g_volte_exit(void);
+
+void volte_cleanup_xfrm(struct volte_states *volte);
+pj_status_t volte_alloc_spi(struct volte_states *volte);
 
 pj_status_t volte_add_sec_agree(pjsip_tx_data *tdata);
 pj_status_t volte_init_authorization(pjsip_tx_data *tdata, const char *fromdomain, const char *username);
 pj_status_t volte_del_authorization(pjsip_tx_data *tdata);
+pj_status_t volte_reset_transport(struct volte_states *volte, pjsip_tx_data *tdata);
+pj_status_t volte_add_security_client(struct volte_states *volte, pjsip_tx_data *tdata, int port_c, int port_s);
+pj_status_t volte_set_transport(struct volte_states *volte, pjsip_tx_data *tdata, const pj_str_t *alg,
+				const pj_str_t *ealg, uint8_t *ik, uint32_t remote_spi_c, uint32_t remote_spi_s,
+				uint16_t remote_port_c, uint16_t remote_port_s);
+pj_status_t volte_get_security_server(struct volte_states *volte, pjsip_rx_data *rdata, struct security_server *sec);
+pj_status_t volte_add_security_verify(struct volte_states *volte, pjsip_tx_data *tdata);
+pj_status_t volte_authenticate(struct volte_states *volte, pjsip_rx_data *rdata, pjsip_hdr_e auth_type,
+			       const char *opc_str, const char *k_str, const char *sqn_str, uint8_t *out_res,
+			       uint8_t *out_ik, uint8_t *out_ck, uint8_t *out_auts);