diff --git a/src/modules/cdp/peerstatemachine.c b/src/modules/cdp/peerstatemachine.c
index a4520d797d1..eb875831d33 100644
--- a/src/modules/cdp/peerstatemachine.c
+++ b/src/modules/cdp/peerstatemachine.c
@@ -128,8 +128,10 @@ int sm_process(peer *p,peer_event_t event,AAAMessage *msg,int peer_locked,int so
 		case Wait_Conn_Ack:
 			switch(event){
 				case I_Rcv_Conn_Ack:
-					I_Snd_CER(p);
 					p->state = Wait_I_CEA;
+					I_Snd_CER(p);
+					if (p->state == Closed)
+						sm_process(p, Start, 0, 1, 0);
 					break;
 				case I_Rcv_Conn_NAck:
 					Cleanup(p,p->I_sock);
@@ -624,15 +626,23 @@ void I_Snd_CER(peer *p)
 	} addr_u ;
 	socklen_t addrlen;
 	char x[18];
+	int ret = 0;
 
 	cer = AAANewMessage(Code_CE,0,0,0);
 	if (!cer) return;
 	cer->hopbyhopId = next_hopbyhop();
 	cer->endtoendId = next_endtoend();
 	addrlen = sizeof(addr_u);
-	if (getsockname(p->I_sock,&(addr_u.addr), &addrlen) == -1) {
+	if ((ret = getsockname(p->I_sock,&(addr_u.addr), &addrlen)) == -1) {
 		LM_ERR("I_Snd_CER(): Error on finding local host address > %s\n",strerror(errno));
-	}else{
+
+		Cleanup(p,p->I_sock);
+		p->state = Closed;
+		AAAFreeMessage(&cer);
+		return;
+	}
+
+	if (ret != -1) {
 		switch(addr_u.addr.sa_family){
 			case AF_INET:
 				set_2bytes(x,1);
diff --git a/src/modules/ims_ipsec_pcscf/cmd.c b/src/modules/ims_ipsec_pcscf/cmd.c
index 0651d4226f5..f920ab166f7 100644
--- a/src/modules/ims_ipsec_pcscf/cmd.c
+++ b/src/modules/ims_ipsec_pcscf/cmd.c
@@ -82,6 +82,24 @@ extern struct tm_binds tmb;
 
 #define IPSEC_CREATE_DELETE_UNUSED_TUNNELS	0x01 /* if set - delete unused tunnels before every registration */
 
+static int pv_t_copy_msg(struct sip_msg *src, struct sip_msg *dst)
+{
+        dst->id = src->id;
+        dst->rcv = src->rcv;
+        dst->set_global_address = src->set_global_address;
+        dst->set_global_port = src->set_global_port;
+        dst->flags = src->flags;
+        dst->fwd_send_flags = src->fwd_send_flags;
+        dst->rpl_send_flags = src->rpl_send_flags;
+        dst->force_send_socket = src->force_send_socket;
+
+        if (parse_msg(dst->buf, dst->len, dst) != 0) {
+                LM_ERR("parse msg failed\n");
+                return -1;
+        }
+        return 0;
+}
+
 int bind_ipsec_pcscf(ipsec_pcscf_api_t* api) {
 	if(!api){
 		LM_ERR("invalid parameter value\n");
@@ -146,6 +164,8 @@ static int fill_contact(struct pcontact_info* ci, struct sip_msg* m, tm_cell_t *
     struct via_body* vb = NULL;
     struct sip_msg* req = NULL;
 	char* srcip = NULL;
+    str aor, f_uri;
+    int i = 0;
 
     if(!ci) {
         LM_ERR("called with null ptr\n");
@@ -170,9 +190,17 @@ static int fill_contact(struct pcontact_info* ci, struct sip_msg* m, tm_cell_t *
         ci->via_host = uri.host;
         ci->via_port = uri.port_no ? uri.port_no : 5060;
         ci->via_prot = 0;
-        ci->aor = m->first_line.u.request.uri;
+        //ci->aor = m->first_line.u.request.uri;
         ci->searchflag = SEARCH_NORMAL;
 
+        aor.s = pkg_malloc(m->first_line.u.request.uri.len);
+        if (aor.s == NULL) {
+            LM_ERR("memory allocation failure\n");
+            return -1;
+        }
+        memcpy(aor.s, m->first_line.u.request.uri.s, m->first_line.u.request.uri.len);
+        aor.len = m->first_line.u.request.uri.len;
+
 		if(ci->via_host.s == NULL || ci->via_host.len == 0){
 			// no host included in RURI
 			vb = cscf_get_ue_via(m);
@@ -251,15 +279,37 @@ static int fill_contact(struct pcontact_info* ci, struct sip_msg* m, tm_cell_t *
 
         req = t->uas.request;
 
-        cb = cscf_parse_contacts(req);
-        if (!cb || (!cb->contacts)) {
-            LM_ERR("Reply No contact headers\n");
+        /* Do not use t->uas.request for getting contacts - it has garbage */
+        struct sip_msg req_msg;
+        memset(&req_msg, 0, sizeof(struct sip_msg));
+        req_msg.buf =
+            (char*) pkg_malloc((t->uas.request->len + 1) * sizeof(char));
+        memcpy(req_msg.buf, t->uas.request->buf, t->uas.request->len);
+        req_msg.buf[t->uas.request->len] = '\0';
+        req_msg.len = t->uas.request->len;
+        if (pv_t_copy_msg(t->uas.request, &req_msg) != 0) {
+            pkg_free(req_msg.buf);
+            req_msg.buf = NULL;
             return -1;
         }
+        if (req_msg.contact && ((contact_body_t *) req_msg.contact->parsed)) {
+            free_contact((contact_body_t **) &req_msg.contact->parsed);
+        }
+
+        cb = cscf_parse_contacts(&req_msg);
+        if (!cb || (!cb->contacts)) {
+            LM_ERR("Reply No contact headers. Will try with request's From URI\n");
+            if (!cscf_get_from_uri(&req_msg, &f_uri)) {
+                LM_INFO("From URI in request <%.*s>\n", f_uri.len, f_uri.s);
+                pkg_free(req_msg.buf);
+                return -1;
+            }
+        }
 
         vb = cscf_get_ue_via(m);
         if (!vb) {
             LM_ERR("Reply No via body headers\n");
+            pkg_free(req_msg.buf);
             return -1;
         }
 
@@ -267,9 +317,18 @@ static int fill_contact(struct pcontact_info* ci, struct sip_msg* m, tm_cell_t *
         ci->via_host = vb->host;
         ci->via_port = vb->port;
         ci->via_prot = vb->proto;
-        ci->aor = cb->contacts->uri;
+        //ci->aor = cb->contacts->uri;
         ci->searchflag = SEARCH_RECEIVED;
 
+        aor.s = pkg_malloc((cb && cb->contacts) ? cb->contacts->uri.len : f_uri.len);
+        if (aor.s == NULL) {
+            LM_ERR("memory allocation failure\n");
+            pkg_free(req_msg.buf);
+            return -1;
+        }
+        memcpy(aor.s, (cb && cb->contacts) ? cb->contacts->uri.s : f_uri.s, (cb && cb->contacts) ? cb->contacts->uri.len : f_uri.len);
+        aor.len = (cb && cb->contacts) ? cb->contacts->uri.len : f_uri.len;
+
 		if((srcip = pkg_malloc(50)) == NULL) {
 			LM_ERR("Error allocating memory for source IP address\n");
 			return -1;
@@ -279,12 +338,23 @@ static int fill_contact(struct pcontact_info* ci, struct sip_msg* m, tm_cell_t *
 		ci->received_host.s = srcip;
 		ci->received_port = req->rcv.src_port;
 		ci->received_proto = req->rcv.proto;
+
+        pkg_free(req_msg.buf);
     }
     else {
         LM_ERR("Unknown first line type: %d\n", m->first_line.type);
         return -1;
     }
 
+    for (i = 4; i < aor.len; i++)
+        if (aor.s[i] == ';') {
+            aor.len = i;
+            break;
+        }
+
+    LM_DBG("AOR <%.*s>\n", aor.len, aor.s);
+    ci->aor = aor;
+
     LM_DBG("SIP %s fill contact with AOR [%.*s], VIA [%d://%.*s:%d], received_host [%d://%.*s:%d]\n",
             m->first_line.type == SIP_REQUEST ? "REQUEST" : "REPLY",
             ci->aor.len, ci->aor.s, ci->via_prot, ci->via_host.len, ci->via_host.s, ci->via_port,
@@ -371,17 +441,22 @@ static int update_contact_ipsec_params(ipsec_t* s, const struct sip_msg* m, ipse
         return -1;
     }
 
-    if((s->port_pc = acquire_cport()) == 0){
-        LM_ERR("No free client port for IPSEC tunnel creation\n");
-		shm_free(s->ck.s);
-		s->ck.s = NULL; s->ck.len = 0;
-		shm_free(s->ik.s);
-		s->ik.s = NULL; s->ik.len = 0;
+	// use the same P-CSCF client port if it is present
+	if(s_old){
+		s->port_pc = s_old->port_pc;
+	}else{
+		if((s->port_pc = acquire_cport()) == 0){
+			LM_ERR("No free client port for IPSEC tunnel creation\n");
+			shm_free(s->ck.s);
+			s->ck.s = NULL; s->ck.len = 0;
+			shm_free(s->ik.s);
+			s->ik.s = NULL; s->ik.len = 0;
 
-		release_spi(s->spi_pc);
-		release_spi(s->spi_ps);
-        return -1;
-    }
+			release_spi(s->spi_pc);
+			release_spi(s->spi_ps);
+			return -1;
+		}
+	}
 
 	// use the same P-CSCF server port if it is present
 	if(s_old){
@@ -394,7 +469,7 @@ static int update_contact_ipsec_params(ipsec_t* s, const struct sip_msg* m, ipse
 			shm_free(s->ik.s);
 			s->ik.s = NULL; s->ik.len = 0;
 
-			release_cport(s->port_pc);
+			// release_cport(s->port_pc);
 
 			release_spi(s->spi_pc);
 			release_spi(s->spi_ps);
@@ -459,12 +534,34 @@ static int create_ipsec_tunnel(const struct ip_addr *remote_addr, ipsec_t* s)
     add_sa    (sock, remote_addr, ipsec_addr, s->port_us, s->port_pc, s->spi_pc, s->ck, s->ik, s->r_alg, s->r_ealg);
     add_policy(sock, remote_addr, ipsec_addr, s->port_us, s->port_pc, s->spi_pc, IPSEC_POLICY_DIRECTION_IN);
 
+    /* Fix for some broken In-Dialog routing */
+
+    // SA5 UE client to P-CSCF client
+    //               src adrr     dst addr     src port    dst port
+    add_sa    (sock, remote_addr, ipsec_addr, s->port_uc, s->port_pc, s->spi_ps, s->ck, s->ik, s->r_alg, s->r_ealg);
+    add_policy(sock, remote_addr, ipsec_addr, s->port_uc, s->port_pc, s->spi_ps, IPSEC_POLICY_DIRECTION_IN);
+
+    // SA6 P-CSCF client to UE client
+    //               src adrr     dst addr     src port    dst port
+    add_sa    (sock, ipsec_addr, remote_addr, s->port_pc, s->port_uc, s->spi_us, s->ck, s->ik, s->r_alg, s->r_ealg);
+    add_policy(sock, ipsec_addr, remote_addr, s->port_pc, s->port_uc, s->spi_us, IPSEC_POLICY_DIRECTION_OUT);
+
+    // SA7 P-CSCF server to UE server
+    //               src adrr     dst addr     src port    dst port
+    add_sa    (sock, ipsec_addr, remote_addr, s->port_ps, s->port_us, s->spi_uc, s->ck, s->ik, s->r_alg, s->r_ealg);
+    add_policy(sock, ipsec_addr, remote_addr, s->port_ps, s->port_us, s->spi_uc, IPSEC_POLICY_DIRECTION_OUT);
+
+    // SA8 UE server to P-CSCF server
+    //               src adrr     dst addr     src port    dst port
+    add_sa    (sock, remote_addr, ipsec_addr, s->port_us, s->port_ps, s->spi_pc, s->ck, s->ik, s->r_alg, s->r_ealg);
+    add_policy(sock, remote_addr, ipsec_addr, s->port_us, s->port_ps, s->spi_pc, IPSEC_POLICY_DIRECTION_IN);
+
     close_mnl_socket(sock);
 
     return 0;
 }
 
-static int destroy_ipsec_tunnel(str remote_addr, ipsec_t* s, unsigned short received_port)
+static int destroy_ipsec_tunnel(str remote_addr, ipsec_t* s, unsigned short received_port, int release_proxy_ports)
 {
     struct mnl_socket* sock = init_mnl_socket();
     if (sock == NULL) {
@@ -507,13 +604,34 @@ static int destroy_ipsec_tunnel(str remote_addr, ipsec_t* s, unsigned short rece
     remove_sa    (sock, remote_addr, ipsec_addr, s->port_us, s->port_pc, s->spi_pc, ip_addr.af);
     remove_policy(sock, remote_addr, ipsec_addr, s->port_us, s->port_pc, s->spi_pc, ip_addr.af, IPSEC_POLICY_DIRECTION_IN);
 
+    /* Fix for some broken In-Dialog routing */
+
+    // SA5 UE client to P-CSCF client
+    remove_sa    (sock, remote_addr, ipsec_addr, s->port_uc, s->port_pc, s->spi_ps, ip_addr.af);
+    remove_policy(sock, remote_addr, ipsec_addr, s->port_uc, s->port_pc, s->spi_ps, ip_addr.af, IPSEC_POLICY_DIRECTION_IN);
+
+    // SA6 P-CSCF client to UE client
+    remove_sa    (sock, ipsec_addr, remote_addr, s->port_pc, s->port_uc, s->spi_us, ip_addr.af);
+    remove_policy(sock, ipsec_addr, remote_addr, s->port_pc, s->port_uc, s->spi_us, ip_addr.af, IPSEC_POLICY_DIRECTION_OUT);
+
+    // SA7 P-CSCF server to UE server
+    remove_sa    (sock, ipsec_addr, remote_addr, s->port_ps, s->port_us, s->spi_uc, ip_addr.af);
+    remove_policy(sock, ipsec_addr, remote_addr, s->port_ps, s->port_us, s->spi_uc, ip_addr.af, IPSEC_POLICY_DIRECTION_OUT);
+
+    // SA8 UE server to P-CSCF server
+    remove_sa    (sock, remote_addr, ipsec_addr, s->port_us, s->port_ps, s->spi_pc, ip_addr.af);
+    remove_policy(sock, remote_addr, ipsec_addr, s->port_us, s->port_ps, s->spi_pc, ip_addr.af, IPSEC_POLICY_DIRECTION_IN);
+
     // Release SPIs
     release_spi(s->spi_pc);
     release_spi(s->spi_ps);
 
-    // Release the client and the server ports
-    release_cport(s->port_pc);
-    release_sport(s->port_ps);
+    if (release_proxy_ports) {
+        // Release the client and the server ports
+        // Do not release proxy IPSec ports at all just remove SA and Policies
+        // release_cport(s->port_pc);
+        // release_sport(s->port_ps);
+    }
 
     close_mnl_socket(sock);
     return 0;
@@ -538,7 +656,7 @@ void ipsec_on_expire(struct pcontact *c, int type, void *param)
         return;
     }
 
-    destroy_ipsec_tunnel(c->received_host, c->security_temp->data.ipsec, c->contact_port);
+    destroy_ipsec_tunnel(c->received_host, c->security_temp->data.ipsec, c->contact_port, 1);
 }
 
 int add_supported_secagree_header(struct sip_msg* m)
@@ -618,7 +736,7 @@ int add_security_server_header(struct sip_msg* m, ipsec_t* s)
     char sec_hdr_buf[1024];
     memset(sec_hdr_buf, 0, sizeof(sec_hdr_buf));
     sec_header->len = snprintf(sec_hdr_buf, sizeof(sec_hdr_buf) - 1,
-                                "Security-Server: ipsec-3gpp;prot=esp;mod=trans;spi-c=%d;spi-s=%d;port-c=%d;port-s=%d;alg=%.*s;ealg=%.*s\r\n",
+                                "Security-Server: ipsec-3gpp;q=0.1;prot=esp;mod=trans;spi-c=%d;spi-s=%d;port-c=%d;port-s=%d;alg=%.*s;ealg=%.*s\r\n",
                                 s->spi_pc, s->spi_ps, s->port_pc, s->port_ps,
                                 s->r_alg.len, s->r_alg.s,
                                 s->r_ealg.len, s->r_ealg.s
@@ -693,18 +811,83 @@ int ipsec_create(struct sip_msg* m, udomain_t* d, int _cflags)
     struct sip_msg* req = t->uas.request;
 
     // Update contacts only for initial registration, for re-registration the existing contacts shouldn't be updated.
-    if(ci.via_port == SIP_PORT){
+    if((ci.via_port == SIP_PORT) ||
+        (pcontact->security_temp->data.ipsec->port_ps == 0  &&
+        pcontact->security_temp->data.ipsec->port_pc == 0)){
         LM_DBG("Registration for contact with AOR [%.*s], VIA [%d://%.*s:%d], received_host [%d://%.*s:%d]\n",
                 ci.aor.len, ci.aor.s, ci.via_prot, ci.via_host.len, ci.via_host.s, ci.via_port,
                 ci.received_proto, ci.received_host.len, ci.received_host.s, ci.received_port);
 
-        ipsec_t* s = pcontact->security_temp->data.ipsec;
+        //ipsec_t* s = pcontact->security_temp->data.ipsec;
+        ipsec_t* s = NULL;
+        security_t* sec_params = NULL;
 
-		// for initial Registration use a new P-CSCF server port
-        if(update_contact_ipsec_params(s, m, NULL) != 0) {
+        // Parse security parameters from the REGISTER request and get some data for the tunnels
+        if((sec_params = cscf_get_security(req)) == NULL) {
+            LM_CRIT("No security parameters in REGISTER request\n");
             goto cleanup;
         }
 
+        if (sec_params->data.ipsec->port_uc != pcontact->security_temp->data.ipsec->port_uc ||
+            sec_params->data.ipsec->port_us != pcontact->security_temp->data.ipsec->port_us ||
+            sec_params->data.ipsec->spi_uc != pcontact->security_temp->data.ipsec->spi_uc ||
+            sec_params->data.ipsec->spi_us != pcontact->security_temp->data.ipsec->spi_us) {
+
+            // Backup the Proxy Server and Client port - we re-use them
+            ipsec_t ipsec_ps;
+            ipsec_ps.port_ps = pcontact->security_temp->data.ipsec->port_ps;
+            ipsec_ps.port_pc = pcontact->security_temp->data.ipsec->port_pc;
+
+            // Destroy privously existing IPSec tunnels but dont release proxy ports
+            destroy_ipsec_tunnel(ci.received_host,
+                pcontact->security_temp->data.ipsec, pcontact->contact_port, 0);
+
+            if(pcontact->security_temp->sec_header.s)
+                shm_free(pcontact->security_temp->sec_header.s);
+
+            if(pcontact->security_temp->data.ipsec){
+                if(pcontact->security_temp->data.ipsec->ealg.s)
+                    shm_free(pcontact->security_temp->data.ipsec->ealg.s);
+                if(pcontact->security_temp->data.ipsec->r_ealg.s)
+                    shm_free(pcontact->security_temp->data.ipsec->r_ealg.s);
+                if(pcontact->security_temp->data.ipsec->ck.s)
+                    shm_free(pcontact->security_temp->data.ipsec->ck.s);
+                if(pcontact->security_temp->data.ipsec->alg.s)
+                    shm_free(pcontact->security_temp->data.ipsec->alg.s);
+                if(pcontact->security_temp->data.ipsec->r_alg.s)
+                    shm_free(pcontact->security_temp->data.ipsec->r_alg.s);
+                if(pcontact->security_temp->data.ipsec->ik.s)
+                    shm_free(pcontact->security_temp->data.ipsec->ik.s);
+                if(pcontact->security_temp->data.ipsec->prot.s)
+                    shm_free(pcontact->security_temp->data.ipsec->prot.s);
+                if(pcontact->security_temp->data.ipsec->mod.s)
+                    shm_free(pcontact->security_temp->data.ipsec->mod.s);
+
+                shm_free(pcontact->security_temp->data.ipsec);
+            }
+            shm_free(pcontact->security_temp);
+
+            if(ul.update_temp_security(d, sec_params->type, sec_params, pcontact) != 0){
+                LM_ERR("Error updating temp security\n");
+                goto cleanup;
+            }
+
+            s = pcontact->security_temp->data.ipsec;
+
+            // Restore the backed up Proxy Server and Client port
+            if(update_contact_ipsec_params(s, m, &ipsec_ps) != 0) {
+                goto cleanup;
+            }
+        } else {
+
+            s = pcontact->security_temp->data.ipsec;
+
+            // for initial Registration use a new P-CSCF server port
+            if(update_contact_ipsec_params(s, m, NULL) != 0) {
+                goto cleanup;
+            }
+        }
+
         if(create_ipsec_tunnel(&req->rcv.src_ip, s) != 0){
             goto cleanup;
         }
@@ -749,6 +932,10 @@ int ipsec_create(struct sip_msg* m, udomain_t* d, int _cflags)
             goto cleanup;
         }
 
+        // Restore Proxy Server and Client port
+        req_sec_params->data.ipsec->port_ps = pcontact->security_temp->data.ipsec->port_ps;
+        req_sec_params->data.ipsec->port_pc = pcontact->security_temp->data.ipsec->port_pc;
+
         if(create_ipsec_tunnel(&req->rcv.src_ip, req_sec_params->data.ipsec) != 0){
             goto cleanup;
         }
@@ -784,13 +971,14 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
     unsigned short dst_port = 0;
     unsigned short src_port = 0;
     ip_addr_t via_host;
+    struct via_body *vb;
     struct sip_msg* req = NULL;
     struct cell *t = NULL;
 
     if(m->first_line.type == SIP_REPLY) {
         // Get request from reply
         t = tmb.t_gett();
-        if (!t) {
+        if (!t || t == (void*) -1) {
             LM_ERR("Error getting transaction\n");
             return ret;
         }
@@ -843,22 +1031,33 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
     //    from URI
     //int uri_len = 4 /* strlen("sip:") */ + ci.via_host.len + 5 /* max len of port number */ ;
 
-    if(m->dst_uri.s) {
-        pkg_free(m->dst_uri.s);
-        m->dst_uri.s = NULL;
-        m->dst_uri.len = 0;
-    }
+    vb = cscf_get_last_via(m);
 
     char buf[1024];
     if(m->first_line.type == SIP_REPLY){
-        // for Reply get the dest proto from the received request
-        dst_proto = req->rcv.proto;
+        dst_proto = vb ? vb->proto : req->rcv.proto;
+
+        // As per ETSI TS 133 203 V11.2.0, 7.1 Security association parameters
+        // https://tools.ietf.org/html/rfc3261#section-18
 
         // for Reply and TCP sends from P-CSCF server port, for Reply and UDP sends from P-CSCF client port
-        src_port = dst_proto == PROTO_TCP ? s->port_ps : s->port_pc;
+        // src_port = dst_proto == PROTO_TCP ? s->port_ps : s->port_pc;
 
         // for Reply and TCP sends to UE client port, for Reply and UDP sends to UE server port
-        dst_port = dst_proto == PROTO_TCP ? s->port_uc : s->port_us;
+        // dst_port = dst_proto == PROTO_TCP ? s->port_uc : s->port_us;
+
+        // From Reply and TCP send via the same ports Request was recevied.
+        if (dst_proto == PROTO_TCP) {
+            src_port = req->rcv.dst_port;
+            dst_port = req->rcv.src_port;
+        } else {
+            src_port =  s->port_pc;
+            if (vb && ((vb->port == s->port_uc) || (vb->port == s->port_us))) {
+                dst_port = vb->port;
+            } else {
+                dst_port = s->port_us;
+            }
+        }
 
         // Check send socket
         struct socket_info * client_sock = grep_sock_info(via_host.af == AF_INET ? &ipsec_listen_addr : &ipsec_listen_addr6, src_port, dst_proto);
@@ -867,8 +1066,18 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
             dst_port = s->port_us;
         }
     }else{
-        // for Request get the dest proto from the saved contact
-        dst_proto = pcontact->received_proto;
+        if (req->first_line.u.request.method_value == METHOD_REGISTER) {
+            // for Request get the dest proto from the saved contact
+            dst_proto = pcontact->received_proto;
+        } else {
+			if (strstr(m->dst_uri.s, ";transport=tcp") != NULL) {
+				dst_proto = PROTO_TCP;
+			} else if (strstr(m->dst_uri.s, ";transport=tls") != NULL) {
+				dst_proto = PROTO_TLS;
+			} else {
+				dst_proto = m->rcv.proto;
+			}
+        }
 
         // for Request sends from P-CSCF client port
         src_port = s->port_pc;
@@ -877,7 +1086,27 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
         dst_port = s->port_us;
     }
 
-    int buf_len = snprintf(buf, sizeof(buf) - 1, "sip:%.*s:%d", ci.via_host.len, ci.via_host.s, dst_port);
+    // Try for send socket
+    struct socket_info * client_sock = grep_sock_info(via_host.af == AF_INET ? &ipsec_listen_addr : &ipsec_listen_addr6, src_port, dst_proto);
+    if(!client_sock && dst_proto == PROTO_UDP) {
+        LM_ERR("UDP socket not found for IPSec forward, trying for TCP\n");
+        dst_proto = PROTO_TCP;
+    }
+
+    int buf_len = 0;
+    if (dst_proto == PROTO_TCP) {
+        buf_len = snprintf(buf, sizeof(buf) - 1, "sip:%.*s:%d;transport=tcp", ci.via_host.len, ci.via_host.s, dst_port);
+    } else if (dst_proto == PROTO_TLS) {
+        buf_len = snprintf(buf, sizeof(buf) - 1, "sip:%.*s:%d;transport=tls", ci.via_host.len, ci.via_host.s, dst_port);
+    } else {
+        buf_len = snprintf(buf, sizeof(buf) - 1, "sip:%.*s:%d", ci.via_host.len, ci.via_host.s, dst_port);
+    }
+
+	if(m->dst_uri.s) {
+		pkg_free(m->dst_uri.s);
+		m->dst_uri.s = NULL;
+		m->dst_uri.len = 0;
+	}
 
     if((m->dst_uri.s = pkg_malloc(buf_len + 1)) == NULL) {
         LM_ERR("Error allocating memory for dst_uri\n");
@@ -889,7 +1118,7 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
     m->dst_uri.s[m->dst_uri.len] = '\0';
 
     // Set send socket
-    struct socket_info * client_sock = grep_sock_info(via_host.af == AF_INET ? &ipsec_listen_addr : &ipsec_listen_addr6, src_port, dst_proto);
+    client_sock = grep_sock_info(via_host.af == AF_INET ? &ipsec_listen_addr : &ipsec_listen_addr6, src_port, dst_proto);
     if(!client_sock) {
         LM_ERR("Error calling grep_sock_info() for ipsec client port\n");
         goto cleanup;
@@ -899,10 +1128,15 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
    // Set destination info
     struct dest_info dst_info;
     dst_info.send_sock = client_sock;
+#if 1
 	if(m->first_line.type == SIP_REQUEST && (_cflags & IPSEC_SEND_FORCE_SOCKET)){
 		dst_info.send_flags.f |= SND_F_FORCE_SOCKET;
 		m->fwd_send_flags.f |= SND_F_FORCE_SOCKET;
 	}
+#else
+	// commit c08ae85661b35a93bf98d8112982e5fcf7ff1ae8 from https://github.com/herlesupreeth/kamailio
+	set_force_socket(m, client_sock);
+#endif
 #ifdef USE_DNS_FAILOVER
     if (!uri2dst(NULL, &dst_info, m, &m->dst_uri, dst_proto)) {
 #else
@@ -915,14 +1149,14 @@ int ipsec_forward(struct sip_msg* m, udomain_t* d, int _cflags)
     // Update dst_info in message
     if(m->first_line.type == SIP_REPLY) {
         struct cell *t = tmb.t_gett();
-        if (!t) {
+        if (!t || t == (void*) -1) {
             LM_ERR("Error getting transaction\n");
             goto cleanup;
         }
         t->uas.response.dst = dst_info;
     }
 
-	LM_DBG("Destination changed to [%d://%.*s], from [%d:%d]\n", dst_info.proto, m->dst_uri.len, m->dst_uri.s,
+	LM_INFO("Destination changed to [%d://%.*s], from [%d:%d]\n", dst_info.proto, m->dst_uri.len, m->dst_uri.s,
 			dst_info.send_sock->proto, dst_info.send_sock->port_no);
 
     ret = IPSEC_CMD_SUCCESS; // all good, return SUCCESS
@@ -985,7 +1219,7 @@ int ipsec_destroy(struct sip_msg* m, udomain_t* d)
         goto cleanup;
     }
 
-    destroy_ipsec_tunnel(ci.received_host, pcontact->security_temp->data.ipsec, pcontact->contact_port);
+    destroy_ipsec_tunnel(ci.received_host, pcontact->security_temp->data.ipsec, pcontact->contact_port, 1);
 
     ret = IPSEC_CMD_SUCCESS;    // all good, set ret to SUCCESS, and exit
 
@@ -998,6 +1232,62 @@ int ipsec_destroy(struct sip_msg* m, udomain_t* d)
     return ret;
 }
 
+int ipsec_destroy_by_contact(udomain_t* _d, str * uri, str * received_host, int received_port) {
+
+    pcontact_t* pcontact = NULL;
+    int ret = IPSEC_CMD_FAIL; // FAIL by default
+
+    pcontact_info_t search_ci;
+    memset(&search_ci, 0, sizeof(struct pcontact_info));
+
+    sip_uri_t contact_uri;
+    if (parse_uri(uri->s, uri->len, &contact_uri) != 0) {
+        LM_WARN("Failed to parse aor [%.*s]\n", uri->len, uri->s);
+        return ret;
+    }
+
+    search_ci.received_host.s = received_host->s;
+    search_ci.received_host.len = received_host->len;
+    search_ci.received_port = received_port;
+    search_ci.received_proto = contact_uri.proto? contact_uri.proto : PROTO_UDP;
+    search_ci.searchflag = SEARCH_RECEIVED;
+    search_ci.via_host.s = received_host->s;
+    search_ci.via_host.len = received_host->len;
+    search_ci.via_port = received_port;
+    search_ci.via_prot = search_ci.received_proto;
+    search_ci.aor.s = uri->s;
+    search_ci.aor.len = uri->len;
+    search_ci.reg_state = PCONTACT_ANY;
+
+    if (ul.get_pcontact(_d, &search_ci, &pcontact, 0) != 0 || pcontact==NULL) {
+        LM_ERR("Contact doesn't exist\n");
+        return ret;
+    }
+
+    /* Lock this record while working with the data: */
+    ul.lock_udomain(_d, &pcontact->via_host, pcontact->via_port, pcontact->via_proto);
+
+    if(pcontact->security_temp == NULL) {
+        LM_ERR("No security parameters found in contact\n");
+        goto cleanup;
+    }
+
+    //get security parameters
+    if(pcontact->security_temp->type != SECURITY_IPSEC ) {
+        LM_ERR("Unsupported security type: %d\n", pcontact->security_temp->type);
+        goto cleanup;
+    }
+
+    destroy_ipsec_tunnel(search_ci.received_host, pcontact->security_temp->data.ipsec, pcontact->contact_port, 1);
+
+    ret = IPSEC_CMD_SUCCESS;    // all good, set ret to SUCCESS, and exit
+
+cleanup:
+    /* Unlock domain */
+    ul.unlock_udomain(_d, &pcontact->via_host, pcontact->via_port, pcontact->via_proto);
+    return ret;
+}
+
 int ipsec_reconfig()
 {
 	if(ul.get_number_of_contacts() != 0){
@@ -1005,7 +1295,7 @@ int ipsec_reconfig()
 	}
 
 	clean_spi_list();
-	clean_port_lists();
+	// clean_port_lists();
 
 	LM_DBG("Clean all ipsec tunnels\n");
 
diff --git a/src/modules/ims_ipsec_pcscf/cmd.h b/src/modules/ims_ipsec_pcscf/cmd.h
index 122330dbb80..680fb2eeeb1 100644
--- a/src/modules/ims_ipsec_pcscf/cmd.h
+++ b/src/modules/ims_ipsec_pcscf/cmd.h
@@ -68,5 +68,6 @@ int ipsec_destroy(struct sip_msg* m, udomain_t* d);
 int ipsec_cleanall();
 int ipsec_reconfig();
 void ipsec_on_expire(pcontact_t* c, int type, void* param);
+int ipsec_destroy_by_contact(udomain_t* _d, str * uri, str * received_host, int received_port);
 
 #endif /* IPSEC_CMD_H */
diff --git a/src/modules/ims_ipsec_pcscf/ims_ipsec_pcscf_mod.c b/src/modules/ims_ipsec_pcscf/ims_ipsec_pcscf_mod.c
index b2a19f332ec..be5f1babdfd 100644
--- a/src/modules/ims_ipsec_pcscf/ims_ipsec_pcscf_mod.c
+++ b/src/modules/ims_ipsec_pcscf/ims_ipsec_pcscf_mod.c
@@ -57,12 +57,15 @@ static void mod_destroy(void);
 static int w_create(struct sip_msg* _m, char* _d, char* _cflags);
 static int w_forward(struct sip_msg* _m, char* _d, char* _cflags);
 static int w_destroy(struct sip_msg* _m, char* _d, char* _cflags);
+static int w_destroy_by_contact(struct sip_msg* _m, char* _d, char* _aor, char* _received_host, char* _received_port, char* _cflags);
 
 /*! \brief Fixup functions */
 static int domain_fixup(void** param, int param_no);
 static int save_fixup2(void** param, int param_no);
 static int free_uint_fixup(void** param, int param_no);
 
+static int unregister_fixup(void ** param, int param_no);
+
 extern int bind_ipsec_pcscf(usrloc_api_t* api);
 
 int init_flag = 0;
@@ -76,6 +79,7 @@ static cmd_export_t cmds[] = {
 	{"ipsec_forward", (cmd_function)w_forward, 1, save_fixup2, 0, REQUEST_ROUTE | ONREPLY_ROUTE },
 	{"ipsec_forward", (cmd_function)w_forward, 2, save_fixup2, free_uint_fixup, REQUEST_ROUTE | ONREPLY_ROUTE },
 	{"ipsec_destroy", (cmd_function)w_destroy, 1, save_fixup2, 0, REQUEST_ROUTE | ONREPLY_ROUTE },
+	{"ipsec_destroy_by_contact", (cmd_function)w_destroy_by_contact, 4, unregister_fixup, 0, ANY_ROUTE},
 	{"bind_ims_ipsec_pcscf", (cmd_function)bind_ipsec_pcscf, 1, 0, 0, 0},
 	{0, 0, 0, 0, 0, 0}
 };
@@ -401,6 +405,32 @@ static int save_fixup2(void** param, int param_no)
 	return 0;
 }
 
+static int unregister_fixup(void ** param, int param_no) {
+	if (param_no == 1) {
+		return domain_fixup(param,param_no);
+	} else {
+		pv_elem_t *model=NULL;
+		str s;
+
+		/* convert to str */
+		s.s = (char*)*param;
+		s.len = strlen(s.s);
+
+		model = NULL;
+		if(s.len==0) {
+			LM_ERR("no param!\n");
+			return E_CFG;
+		}
+		if(pv_parse_format(&s, &model)<0 || model==NULL) {
+			LM_ERR("wrong format [%s]!\n", s.s);
+			return E_CFG;
+		}
+		*param = (void*)model;
+		return 0;
+	}
+	return E_CFG;
+}
+
 
 /*! \brief
  * Wrapper to ipsec functions
@@ -425,3 +455,44 @@ static int w_destroy(struct sip_msg* _m, char* _d, char* _cflags)
 {
 	return ipsec_destroy(_m, (udomain_t*)_d);
 }
+
+static int w_destroy_by_contact(struct sip_msg* _m, char* _d, char* _aor, char* _received_host, char* _received_port, char* _cflags)
+{
+	pv_elem_t *model;
+	str aor;
+	str received_host;
+	str received_port;
+	int port = 0;
+
+	if ((_aor == NULL) || (_received_host == NULL) || (_received_port == NULL)) {
+		LM_ERR("error - bad parameters\n");
+		return -1;
+	}
+
+	model = (pv_elem_t*)_aor;
+	if (pv_printf_s(_m, model, &aor)<0) {
+		LM_ERR("error - cannot print the format\n");
+		return -1;
+	}
+	LM_DBG("URI: %.*s\n", aor.len, aor.s);
+
+	model = (pv_elem_t*)_received_host;
+	if (pv_printf_s(_m, model, &received_host)<0) {
+		LM_ERR("error - cannot print the format\n");
+		return -1;
+	}
+	LM_DBG("Received-Host: %.*s\n", received_host.len, received_host.s);
+
+	model = (pv_elem_t*)_received_port;
+	if (pv_printf_s(_m, model, &received_port)<0) {
+		LM_ERR("error - cannot print the format\n");
+		return -1;
+	}
+	LM_DBG("Received-Port: %.*s\n", received_port.len, received_port.s);
+	if (str2sint(&received_port, &port) != 0) {
+		LM_ERR("error - cannot convert %.*s to an int!\n", received_port.len, received_port.s);
+		return -1;
+	}
+
+	return ipsec_destroy_by_contact((udomain_t*)_d, &aor, &received_host, port);
+}
diff --git a/src/modules/ims_ipsec_pcscf/port_gen.c b/src/modules/ims_ipsec_pcscf/port_gen.c
index d722eefd2be..a841ce33580 100644
--- a/src/modules/ims_ipsec_pcscf/port_gen.c
+++ b/src/modules/ims_ipsec_pcscf/port_gen.c
@@ -93,34 +93,41 @@ uint32_t acquire_port(spi_list_t* used_ports, pthread_mutex_t* port_mut, uint32_
         return ret;
     }
 
-    while(1){
-        if(spi_in_list(used_ports, *port_val) == 0) {
-            ret = *port_val;
-            (*port_val)++;
-
-			if(*port_val >= max_port) { //reached the top of the range - reset
-				*port_val = min_port;
-			}
-
-            break;
-        }
-
-        (*port_val)++; //the current server port is not available - increment
-
-        if(*port_val >= max_port) { //reached the top of the range - reset
-            *port_val = min_port;
-        }
-
-        if(*port_val == initial_val) { //there are no free server ports
-            pthread_mutex_unlock(port_mut);
-            return ret;
-        }
-    }
-
-    // found unused server port - add it to the used list
-    if(spi_add(used_ports, ret) != 0) {
-        ret = 0;
-    }
+	if(*port_val >= max_port) { //reached the top of the range - reset
+		*port_val = min_port;
+	}
+	ret = *port_val;
+	(*port_val)++;
+
+	// The below code is commented because we want to re-use the ports but with different SA and Policies
+    // while(1){
+    //     if(spi_in_list(used_ports, *port_val) == 0) {
+    //         ret = *port_val;
+    //         (*port_val)++;
+
+	// 		if(*port_val >= max_port) { //reached the top of the range - reset
+	// 			*port_val = min_port;
+	// 		}
+
+    //         break;
+    //     }
+
+    //     (*port_val)++; //the current server port is not available - increment
+
+    //     if(*port_val >= max_port) { //reached the top of the range - reset
+    //         *port_val = min_port;
+    //     }
+
+    //     if(*port_val == initial_val) { //there are no free server ports
+    //         pthread_mutex_unlock(port_mut);
+    //         return ret;
+    //     }
+    // }
+
+    // // found unused server port - add it to the used list
+    // if(spi_add(used_ports, ret) != 0) {
+    //     ret = 0;
+    // }
 
     pthread_mutex_unlock(port_mut);
     return ret;
diff --git a/src/modules/ims_qos/ims_qos_mod.c b/src/modules/ims_qos/ims_qos_mod.c
index bcf8266853a..4bd622f8e22 100644
--- a/src/modules/ims_qos/ims_qos_mod.c
+++ b/src/modules/ims_qos/ims_qos_mod.c
@@ -1300,8 +1300,12 @@ static int w_rx_aar_register(struct sip_msg *msg, char* route, char* str1, char*
 		}
 
 		//we use the received IP address for the framed_ip_address 
-		recv_ip.s = ip_addr2a(&msg->rcv.src_ip);
-		recv_ip.len = strlen(ip_addr2a(&msg->rcv.src_ip));
+		memset(&recv_ip, 0, sizeof(str));
+		char buff[IP_ADDR_MAX_STR_SIZE];
+		memcpy(&buff, vb->host.s, vb->host.len);
+		buff[vb->host.len]=0;
+		recv_ip.s = buff;
+		recv_ip.len = strlen(buff);
 
 		ip_version = check_ip_version(recv_ip);
 		if (!ip_version) {
@@ -1309,8 +1313,8 @@ static int w_rx_aar_register(struct sip_msg *msg, char* route, char* str1, char*
 				goto error;
 		}
 
-		recv_port = msg->rcv.src_port;
-		recv_proto = msg->rcv.proto;
+		recv_port = via_port;
+		recv_proto = via_proto;
 
 		LM_DBG("Message received IP address is: [%.*s]\n", recv_ip.len, recv_ip.s);
 		LM_DBG("Message via is [%d://%.*s:%d]\n", vb->proto, vb->host.len, vb->host.s, via_port);
diff --git a/src/modules/ims_qos/rx_aar.c b/src/modules/ims_qos/rx_aar.c
index 287df6d9894..9bf1d1012d9 100644
--- a/src/modules/ims_qos/rx_aar.c
+++ b/src/modules/ims_qos/rx_aar.c
@@ -1049,7 +1049,7 @@ int rx_send_aar_register(struct sip_msg *msg, AAASession* auth, saved_transactio
                &port_from, ip_version == AF_INET ? &af_signaling_ip : &af_signaling_ip6,
                &port_to, &flow_protocol,
                &raw_stream,
-               &raw_stream, DLG_MOBILE_REGISTER, AVP_EPC_Flow_Usage_AF_Signaling);
+               &raw_stream, DLG_MOBILE_ORIGINATING, AVP_EPC_Flow_Usage_AF_Signaling);
 
     /* Add specific action AVP's */
     rx_add_specific_action_avp(aar, 1); // CHARGING_CORRELATION_EXCHANGE
diff --git a/src/modules/ims_registrar_pcscf/notify.c b/src/modules/ims_registrar_pcscf/notify.c
index ca5391c7c1a..e046a9b4c68 100644
--- a/src/modules/ims_registrar_pcscf/notify.c
+++ b/src/modules/ims_registrar_pcscf/notify.c
@@ -203,8 +203,13 @@ int process_contact(udomain_t * _d, int expires, str contact_uri, int contact_st
         if (contact_state == STATE_TERMINATED) {
             //delete contact
             LM_DBG("This contact <%.*s> is in state terminated and is in usrloc so removing it from usrloc\n", contact_uri.len, contact_uri.s);
-            if (ul.delete_pcontact(_d, pcontact) != 0) {
-                LM_DBG("failed to delete pcscf contact <%.*s> - not a problem this may have been removed by de registration", contact_uri.len, contact_uri.s);
+            // if (ul.delete_pcontact(_d, pcontact) != 0) {
+            //     LM_DBG("failed to delete pcscf contact <%.*s> - not a problem this may have been removed by de registration", contact_uri.len, contact_uri.s);
+            // }
+            // Rather than delete update the pcontact with expire value of 10 seconds
+            ci.expires = local_time_now + 10;
+            if (ul.update_pcontact(_d, &ci, pcontact) != 0) {
+                LM_DBG("failed to update pcscf contact on de-register\n");
             }
             /*TODO_LATEST - put this back */
         } else {//state is active
diff --git a/src/modules/ims_registrar_pcscf/save.c b/src/modules/ims_registrar_pcscf/save.c
index b33a596cd2d..20266229e74 100644
--- a/src/modules/ims_registrar_pcscf/save.c
+++ b/src/modules/ims_registrar_pcscf/save.c
@@ -218,8 +218,13 @@ static inline int update_contacts(struct sip_msg *req,struct sip_msg *rpl, udoma
 					LM_DBG("contact already exists and is in state (%d) : [%s]\n",pcontact->reg_state, reg_state_to_string(pcontact->reg_state));
 					if ((expires-local_time_now)<=0) { //remove contact - de-register
 						LM_DBG("This is a de-registration for contact <%.*s>\n", c->uri.len, c->uri.s);
-						if (ul.delete_pcontact(_d, pcontact) != 0) {
-							LM_ERR("failed to delete pcscf contact <%.*s>\n", c->uri.len, c->uri.s);
+						// if (ul.delete_pcontact(_d, pcontact) != 0) {
+						// 	LM_ERR("failed to delete pcscf contact <%.*s>\n", c->uri.len, c->uri.s);
+						// }
+						// Rather than delete update the pcontact with expire value of 10 seconds
+						ci.expires = local_time_now + 10;
+						if (ul.update_pcontact(_d, &ci, pcontact) != 0) {
+							LM_DBG("failed to update pcscf contact on de-register\n");
 						}
                                                 //TODO_LATEST replace above
 					} else { //update contact
diff --git a/src/modules/ims_usrloc_scscf/contact_dlg_handlers.c b/src/modules/ims_usrloc_scscf/contact_dlg_handlers.c
index e4c5a2a130f..e9241a17d34 100644
--- a/src/modules/ims_usrloc_scscf/contact_dlg_handlers.c
+++ b/src/modules/ims_usrloc_scscf/contact_dlg_handlers.c
@@ -44,35 +44,65 @@ void contact_dlg_create_handler(struct dlg_cell* dlg, int cb_types, struct dlg_c
  * @return 0 on success, anything else on failure
  */
 static inline int find_contact_from_impu(impurecord_t* impu, str* search_aor, ucontact_t** scontact) {
-	impu_contact_t *impucontact;
-    short i_searchlen;
+    impu_contact_t *impucontact;
+    short i_searchlen, c_searchlen, alias_searchlen;
     char *s_term;
+    char *c_term;
+    char *alias_term;
     
     if (!search_aor)
         return 1;
-    
+
     LM_DBG("Looking for contact [%.*s] for IMPU [%.*s]\n", search_aor->len, search_aor->s, impu->public_identity.len, impu->public_identity.s);
+
+    /* Filter out sip: and anything before @ from search URI */
+    s_term = strstr(search_aor->s, "@");
+    if (!s_term) {
+        s_term = strstr(search_aor->s, ":");
+    }
+    s_term += 1;
+    if (s_term-search_aor->s >= search_aor->len) {
+        goto error;
+    }
+    i_searchlen = search_aor->len - (s_term-search_aor->s);
+
+    /* Compare the entire contact including alias, if not until alias IP */
+    alias_term = strstr(s_term, "~");
+    if (!alias_term) {
+        alias_searchlen = i_searchlen;
+    } else {
+        alias_term += 1;
+        alias_searchlen = alias_term - s_term;
+    }
     
-    s_term = memchr(search_aor->s,'@',search_aor->len);
-    if (!s_term)
-    {
-        LM_DBG("Malformed contact...bailing search\n"); 
-        return 1;
-    }        
-    i_searchlen = s_term - search_aor->s;
-	
-	impucontact = impu->linked_contacts.head;
-	
+    impucontact = impu->linked_contacts.head;
+
     while (impucontact) {
-		if (impucontact->contact && impucontact->contact->aor.s[i_searchlen] == '@'
-			&& (memcmp(impucontact->contact->aor.s, search_aor->s, i_searchlen) == 0)) {
-			*scontact = impucontact->contact;
-			return 0;
-		}
-		if (impucontact->contact)
-			LM_DBG("Skipping %.*s\n", impucontact->contact->aor.len, impucontact->contact->aor.s);
-		impucontact = impucontact->next;
-	}    
+        
+        if (impucontact->contact) {
+
+            c_term = strstr(impucontact->contact->c.s, "@");
+            if (!c_term) {
+                c_term = strstr(impucontact->contact->c.s, ":");
+            }
+            c_term += 1;
+            c_searchlen = impucontact->contact->c.len - (c_term-impucontact->contact->c.s);
+
+            LM_DBG("Comparing [%.*s] and [%.*s]\n", i_searchlen, s_term, c_searchlen, c_term);
+            LM_DBG("Comparing [%.*s] and [%.*s]\n", alias_searchlen, s_term, c_searchlen, c_term);
+            if ((strncmp(c_term, s_term, i_searchlen) == 0) ||
+                (strncmp(c_term, s_term, alias_searchlen) == 0)) {
+                *scontact = impucontact->contact;
+                return 0;
+            }
+        }
+        if (impucontact->contact)
+            LM_DBG("Skipping %.*s\n", impucontact->contact->c.len, impucontact->contact->c.s);
+        impucontact = impucontact->next;
+    }
+
+error:
+    LM_DBG("Malformed contact...bailing search\n");
     return 1;
 }
 
diff --git a/src/modules/siputils/checks.c b/src/modules/siputils/checks.c
index 1bfaae23300..f290de1b5a5 100644
--- a/src/modules/siputils/checks.c
+++ b/src/modules/siputils/checks.c
@@ -575,89 +575,324 @@ int w_uri_param_rm(struct sip_msg* _msg, char* _param, char* _str2)
  * tel URI.  Returns -1, if conversion failed.  Takes SIP URI hostpart from
  * second parameter and (if needed) writes the result to third paramater.
  */
-int tel2sip(struct sip_msg* _msg, char* _uri, char* _hostpart, char* _res)
-{
-	str uri, hostpart, tel_uri, sip_uri;
-	char *at;
-	int i, j, in_tel_parameters = 0;
-	pv_spec_t *res;
-	pv_value_t res_val;
+// int tel2sip(struct sip_msg* _msg, char* _uri, char* _hostpart, char* _res)
+// {
+// 	str uri, hostpart, tel_uri, sip_uri;
+// 	char *at;
+// 	int i, j, in_tel_parameters = 0;
+// 	pv_spec_t *res;
+// 	pv_value_t res_val;
+
+// 	/* get parameters */
+// 	if (get_str_fparam(&uri, _msg, (fparam_t*)_uri) < 0) {
+// 		LM_ERR("failed to get uri value\n");
+// 		return -1;
+// 	}
+// 	if (get_str_fparam(&hostpart, _msg, (fparam_t*)_hostpart) < 0) {
+// 		LM_ERR("failed to get hostpart value\n");
+// 		return -1;
+// 	}
+// 	res = (pv_spec_t *)_res;
+
+// 	/* check if anything needs to be done */
+// 	if (uri.len < 4) return 2;
+// 	if (strncasecmp(uri.s, "tel:", 4) != 0) return 2;
+
+// 	/* reserve memory for clean tel uri */
+// 	tel_uri.s = pkg_malloc(uri.len+1);
+// 	if (tel_uri.s == 0) {
+// 		LM_ERR("no more pkg memory\n");
+// 		return -1;
+// 	}
+
+// 	/* Remove visual separators before converting to SIP URI. Don't remove
+// 	 * visual separators in TEL URI parameters (after the first ";") */
+// 	for (i=0, j=0; i < uri.len; i++) {
+// 		if (in_tel_parameters == 0) {
+// 			if (uri.s[i] == ';')
+// 				in_tel_parameters = 1;
+// 		}
+// 		if (in_tel_parameters == 0) {
+// 			if ((uri.s[i] != '-') && (uri.s[i] != '.') &&
+// 					(uri.s[i] != '(') && (uri.s[i] != ')'))
+// 				tel_uri.s[j++] = tolower(uri.s[i]);
+// 		} else {
+// 			tel_uri.s[j++] = tolower(uri.s[i]);
+// 		}
+// 	}
+// 	tel_uri.s[j] = '\0';
+// 	tel_uri.len = strlen(tel_uri.s);
+
+// 	/* reserve memory for resulting sip uri */
+// 	sip_uri.len = 4 + tel_uri.len - 4 + 1 + hostpart.len + 1 + 10;
+// 	sip_uri.s = pkg_malloc(sip_uri.len+1);
+// 	if (sip_uri.s == 0) {
+// 		LM_ERR("no more pkg memory\n");
+// 		pkg_free(tel_uri.s);
+// 		return -1;
+// 	}
+
+// 	/* create resulting sip uri */
+// 	at = sip_uri.s;
+// 	append_str(at, "sip:", 4);
+// 	append_str(at, tel_uri.s + 4, tel_uri.len - 4);
+// 	append_chr(at, '@');
+// 	append_str(at, hostpart.s, hostpart.len);
+// 	append_chr(at, ';');
+// 	append_str(at, "user=phone", 10);
+
+// 	/* tel_uri is not needed anymore */
+// 	pkg_free(tel_uri.s);
+
+// 	/* set result pv value and write sip uri to result pv */
+// 	res_val.rs = sip_uri;
+// 	res_val.flags = PV_VAL_STR;
+// 	if (res->setf(_msg, &res->pvp, (int)EQ_T, &res_val) != 0) {
+// 		LM_ERR("failed to set result pvar\n");
+// 		pkg_free(sip_uri.s);
+// 		return -1;
+// 	}
+
+// 	/* free allocated pkg memory and return */
+// 	pkg_free(sip_uri.s);
+// 	return 1;
+// }
 
-	/* get parameters */
-	if (get_str_fparam(&uri, _msg, (fparam_t*)_uri) < 0) {
-		LM_ERR("failed to get uri value\n");
-		return -1;
-	}
-	if (get_str_fparam(&hostpart, _msg, (fparam_t*)_hostpart) < 0) {
-		LM_ERR("failed to get hostpart value\n");
-		return -1;
-	}
-	res = (pv_spec_t *)_res;
-
-	/* check if anything needs to be done */
-	if (uri.len < 4) return 2;
-	if (strncasecmp(uri.s, "tel:", 4) != 0) return 2;
-
-	/* reserve memory for clean tel uri */
-	tel_uri.s = pkg_malloc(uri.len+1);
-	if (tel_uri.s == 0) {
-		LM_ERR("no more pkg memory\n");
-		return -1;
-	}
-
-	/* Remove visual separators before converting to SIP URI. Don't remove
-	 * visual separators in TEL URI parameters (after the first ";") */
-	for (i=0, j=0; i < uri.len; i++) {
-		if (in_tel_parameters == 0) {
-			if (uri.s[i] == ';')
-				in_tel_parameters = 1;
-		}
-		if (in_tel_parameters == 0) {
-			if ((uri.s[i] != '-') && (uri.s[i] != '.') &&
-					(uri.s[i] != '(') && (uri.s[i] != ')'))
-				tel_uri.s[j++] = tolower(uri.s[i]);
-		} else {
-			tel_uri.s[j++] = tolower(uri.s[i]);
-		}
-	}
-	tel_uri.s[j] = '\0';
-	tel_uri.len = strlen(tel_uri.s);
-
-	/* reserve memory for resulting sip uri */
-	sip_uri.len = 4 + tel_uri.len - 4 + 1 + hostpart.len + 1 + 10;
-	sip_uri.s = pkg_malloc(sip_uri.len+1);
-	if (sip_uri.s == 0) {
-		LM_ERR("no more pkg memory\n");
-		pkg_free(tel_uri.s);
-		return -1;
-	}
-
-	/* create resulting sip uri */
-	at = sip_uri.s;
-	append_str(at, "sip:", 4);
-	append_str(at, tel_uri.s + 4, tel_uri.len - 4);
-	append_chr(at, '@');
-	append_str(at, hostpart.s, hostpart.len);
-	append_chr(at, ';');
-	append_str(at, "user=phone", 10);
+/*
+ * Compare function to sort tel: uri options acording to standard
+ * before inserting into sip: uri
+ *
+ * See "RFC 3261 SIP: Session Initiation Protocol June 2002"  
+ *     19.1.6 Relating SIP URIs and tel URLs
+ */
+typedef struct
+{
+        char *name;
+        char *value;
+} tel_param_t;
+#define MAX_TEL_PARAMS (10)
 
-	/* tel_uri is not needed anymore */
-	pkg_free(tel_uri.s);
+int compare_tel_options(const void *v1, const void *v2)
+{
+        tel_param_t *p1 = (tel_param_t *) v1;
+        tel_param_t *p2 = (tel_param_t *) v2;
+
+        if (0 == strcasecmp(p1->name, "isdn-subaddress"))
+        {
+                return -1;
+        }
+        else if (0 == strcasecmp(p2->name, "isdn-subaddress"))
+        {
+                return 1;
+        }
+        else if (0 == strcasecmp(p1->name, "post-dial"))
+        {
+                return -1;
+        }
+        else if (0 == strcasecmp(p2->name, "post-dial"))
+        {
+                return 1;
+        }
+        else
+        {
+                return strcasecmp(p1->name, p2->name);
+        }
+}
 
-	/* set result pv value and write sip uri to result pv */
-	res_val.rs = sip_uri;
-	res_val.flags = PV_VAL_STR;
-	if (res->setf(_msg, &res->pvp, (int)EQ_T, &res_val) != 0) {
-		LM_ERR("failed to set result pvar\n");
-		pkg_free(sip_uri.s);
-		return -1;
-	}
+/*
+ *   Remove visual separators from the phone number 
+ *   Assume it has been validated as a number containing 
+ *   ONLY leading '+', digits, and visual separators.
+ */
+static void remove_visual_separators_from_phone(char *p)
+{
+        char *p2;
+        p2 = p;
+        while (*p != '\0')
+        {
+                 /* Skip all visual separators */
+                 while ((*p != '\0') && ((*p == '.') || (*p == '-') || (*p == '(') || (*p == ')')))
+                 {
+                         p++; 
+                 }
+                 *p2 = *p; /* Until the first visual separator, these both point to  the same place. */
+                           /* but, more efficient to just do it than an if statement each time. */
+                 /* If we arrived at a terminator in the inner loop, time to exit */
+                 if (*p == '\0') return;
+                 /* Now we increment both pointers. */
+                 p++;
+                 p2++;
+        }
+        *p2 = '\0';  /* Make sure that the string is terminated after the last valid digit. */
+}
 
-	/* free allocated pkg memory and return */
-	pkg_free(sip_uri.s);
-	return 1;
+/*
+ * Check if this is a phone number.
+ *   Assume possible leading '+'
+ *   Assume separators '.', '-', '(', or ')' could be present.
+ */
+static int is_number(const char *p)
+{
+        if (*p == '+') p++;
+        while (*p != '\0')
+        {
+                 if ((!isdigit(*p)) && (*p != '.') && (*p != '-') && (*p != '(') && (*p != ')')) return 0;
+                 p++;
+        }
+        return 1;
 }
 
+/*
+ * Converts URI, if it is tel URI, to SIP URI.  Returns 1, if
+ * conversion succeeded or if no conversion was needed, i.e., URI was not
+ * tel URI.  Returns -1, if conversion failed.  Takes SIP URI hostpart from
+ * second parameter and (if needed) writes the result to third parameter.
+ * This one attempts to be standards compliant and sort tel: uri parameters
+ * copied to the sip: uri in the manner defined in the standard. It also 
+ * deletes the "phone-context" parameter if it is a domain, and, takes visual
+ * separators from the "phone-context" parameter if it is a telephone number.
+ */
+int tel2sip(struct sip_msg* _msg, char* _uri, char* _hostpart, char* _res)
+{
+        str uri, hostpart, tel_uri, sip_uri;
+        char *at;
+        int i, j, in_tel_parameters = 0;
+        pv_spec_t *res;
+        pv_value_t res_val;
+
+        /* get parameters */
+        if (get_str_fparam(&uri, _msg, (fparam_t*)_uri) < 0) {
+                LM_ERR("failed to get uri value\n");
+			return -1;
+        }
+        if (get_str_fparam(&hostpart, _msg, (fparam_t*)_hostpart) < 0) {
+                LM_ERR("failed to get hostpart value\n");
+			return -1;
+        }
+        res = (pv_spec_t *)_res;
+
+        /* check if anything needs to be done */
+        if (uri.len < 4) return 1;
+        if (strncasecmp(uri.s, "tel:", 4) != 0) return 1;
+
+        /* reserve memory for clean tel uri */
+        tel_uri.s = pkg_malloc(uri.len+1);
+        if (tel_uri.s == 0) {
+                LM_ERR("no more pkg memory\n");
+                return -1;
+        }
+
+        /* Remove visual separators before converting to SIP URI. Don't remove
+         * visual separators in TEL URI parameters (after the first ";") */
+        for (i=0, j=0; i < uri.len; i++) {
+                if (in_tel_parameters == 0) {
+                        if (uri.s[i] == ';')
+                                in_tel_parameters = 1;
+                }
+                if (in_tel_parameters == 0) {
+                        if ((uri.s[i] != '-') && (uri.s[i] != '.') &&
+                                        (uri.s[i] != '(') && (uri.s[i] != ')'))
+                                tel_uri.s[j++] = tolower(uri.s[i]);
+                } else {
+                        tel_uri.s[j++] = tolower(uri.s[i]);
+                }
+        }
+        tel_uri.s[j] = '\0';
+        tel_uri.len = strlen(tel_uri.s);
+
+        /*** Start Code to sort tel: params *******/
+        tel_param_t params[MAX_TEL_PARAMS];
+        char *tmp_ptr = tel_uri.s + 4; // skip tel:
+
+        int n_tel_params = 0;
+        for (int i=0; i < MAX_TEL_PARAMS; i++)
+        {
+           tmp_ptr = strchr(tmp_ptr, ';');
+           if (tmp_ptr == NULL)
+           {
+             break;
+           }
+           *tmp_ptr = '\0';
+           tmp_ptr++;
+           n_tel_params++;
+           params[i].name = tmp_ptr;
+        }
+        for (int i=0; i < n_tel_params; i++)
+        {
+          tmp_ptr = strchr(params[i].name, '=');
+          if (tmp_ptr == NULL)
+          {
+            params[i].value = "";
+          }
+          else
+          {
+             *tmp_ptr = '\0';
+             tmp_ptr++;
+             params[i].value = tmp_ptr;
+          }
+          if ((0 == strcasecmp(params[i].name, "phone-context")) && (is_number(params[i].value)))
+          {
+            remove_visual_separators_from_phone(params[i].value);
+          }
+           
+        }
+        if (n_tel_params > 1)
+        {
+          qsort(&params[0], n_tel_params, sizeof(tel_param_t), compare_tel_options);
+        }
+        /*** End Code to sort tel: params ******/
+
+        /* reserve memory for resulting sip uri */
+        sip_uri.len = 4 + tel_uri.len - 4 + 1 + hostpart.len + 1 + 10;
+        sip_uri.s = pkg_malloc(sip_uri.len+1);
+        if (sip_uri.s == 0) {
+                LM_ERR("no more pkg memory\n");
+                pkg_free(tel_uri.s);
+                return -1;
+        }
+
+        /* create resulting sip uri */
+        at = sip_uri.s;
+        append_str(at, "sip:", 4);
+        /** Original code tel: parameters NOT sorted 
+        append_str(at, tel_uri.s + 4, tel_uri.len - 4);
+        *****/
+        /***** Start Changed Code for sorted tel: parameters ****/
+        append_str(at, tel_uri.s + 4, strlen(tel_uri.s + 4)); /* This string was terminated after the number */
+        /** Now we need to insert sorted tel: parameters **/
+        for (int i=0; i < n_tel_params; i++)
+        {
+          /* If the phone context is a domain, it has already been extracted and is in the "host part" */
+          if ((0 != strcasecmp(params[i].name, "phone-context")) || (is_number(params[i].value)))
+          {
+            append_chr(at, ';');
+            append_str(at, params[i].name, strlen(params[i].name));
+            append_chr(at, '=');
+            append_str(at, params[i].value, strlen(params[i].value));
+          }
+        }
+        /***** End Changed Code for sort tel: parameters ****/
+        append_chr(at, '@');
+        append_str(at, hostpart.s, hostpart.len);
+        append_chr(at, ';');
+        append_str(at, "user=phone", 10);
+
+        /* tel_uri is not needed anymore */
+        pkg_free(tel_uri.s);
+
+        /* set result pv value and write sip uri to result pv */
+        res_val.rs = sip_uri;
+        res_val.flags = PV_VAL_STR;
+        if (res->setf(_msg, &res->pvp, (int)EQ_T, &res_val) != 0) {
+                LM_ERR("failed to set result pvar\n");
+                pkg_free(sip_uri.s);
+                return -1;
+        }
+
+        /* free allocated pkg memory and return */
+        pkg_free(sip_uri.s);
+        return 1;
+}
 
 /*
  * Check if parameter is an e164 number.
diff --git a/utils/kamctl/mysql/ims_usrloc_scscf-create.sql b/utils/kamctl/mysql/ims_usrloc_scscf-create.sql
index ae7bd93a545..21c816c34e9 100644
--- a/utils/kamctl/mysql/ims_usrloc_scscf-create.sql
+++ b/utils/kamctl/mysql/ims_usrloc_scscf-create.sql
@@ -49,7 +49,7 @@ CREATE TABLE `subscriber` (
   `call_id` varchar(50) NOT NULL,
   `from_tag` varchar(50) NOT NULL,
   `to_tag` varchar(50) NOT NULL,
-  `record_route` varchar(50) NOT NULL,
+  `record_route` TEXT NOT NULL,
   `sockinfo_str` varchar(50) NOT NULL,
   PRIMARY KEY (`id`),
   UNIQUE KEY `watcher_uri` (`event`,`watcher_contact`,`presentity_uri`)