ims_ipsec_pcscf: fixed possible use of uninitialized value in ipsec_forward()
[sip-router] / src / modules / ims_ipsec_pcscf / cmd.c
1 /*
2  * Copyright (C) 2012 Smile Communications, jason.penton@smilecoms.com
3  * Copyright (C) 2012 Smile Communications, richard.good@smilecoms.com
4  *
5  * The initial version of this code was written by Dragos Vingarzan
6  * (dragos(dot)vingarzan(at)fokus(dot)fraunhofer(dot)de and the
7  * Fruanhofer Institute. It was and still is maintained in a separate
8  * branch of the original SER. We are therefore migrating it to
9  * Kamailio/SR and look forward to maintaining it from here on out.
10  * 2011/2012 Smile Communications, Pty. Ltd.
11  * ported/maintained/improved by
12  * Jason Penton (jason(dot)penton(at)smilecoms.com and
13  * Richard Good (richard(dot)good(at)smilecoms.com) as part of an
14  * effort to add full IMS support to Kamailio/SR using a new and
15  * improved architecture
16  *
17  * NB: Alot of this code was originally part of OpenIMSCore,
18  * FhG Fokus.
19  * Copyright (C) 2004-2006 FhG Fokus
20  * Thanks for great work! This is an effort to
21  * break apart the various CSCF functions into logically separate
22  * components. We hope this will drive wider use. We also feel
23  * that in this way the architecture is more complete and thereby easier
24  * to manage in the Kamailio/SR environment
25  *
26  * This file is part of Kamailio, a free SIP server.
27  *
28  * Kamailio is free software; you can redistribute it and/or modify
29  * it under the terms of the GNU General Public License as published by
30  * the Free Software Foundation; either version 2 of the License, or
31  * (at your option) any later version
32  *
33  * Kamailio is distributed in the hope that it will be useful,
34  * but WITHOUT ANY WARRANTY; without even the implied warranty of
35  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
36  * GNU General Public License for more details.
37  *
38  * You should have received a copy of the GNU General Public License
39  * along with this program; if not, write to the Free Software
40  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
41  *
42  */
43
44 #include "../../core/parser/msg_parser.h"
45 #include "../../core/socket_info.h"
46 #include "../../lib/ims/ims_getters.h"
47 #include "../../modules/tm/tm_load.h"
48 #include "../ims_usrloc_pcscf/usrloc.h"
49
50 #include "ipsec.h"
51 #include "spi_gen.h"
52
53 #include <stdlib.h>
54 #include <string.h>
55
56 #include <arpa/inet.h>
57
58
59 extern str ipsec_listen_addr;
60 extern str ipsec_listen_addr6;
61 extern short ipsec_listen_port;
62 extern short ipsec_server_port;
63 extern short ipsec_client_port;
64
65 // check http://www.asipto.com/pub/kamailio-devel-guide/#c16return_values
66 const int IPSEC_CMD_FAIL = -1;
67 const int IPSEC_CMD_SUCCESS = 1;
68
69 extern usrloc_api_t ul;
70 extern struct tm_binds tmb;
71
72
73 static str get_www_auth_param(const char* param_name, str www_auth)
74 {
75     str val = {0,0};
76     int i = 0;
77     int param_len = strlen(param_name);
78     int start = 0;
79     int end = 0;
80
81     for(i = 0; i < www_auth.len; i++) {
82         if (www_auth.s[i] == ' ') continue;
83
84         if(strncasecmp(www_auth.s+i, param_name, param_len) == 0) {
85             i += param_len;
86
87             //find first double quote
88             while(www_auth.s[i] != '"' && i < www_auth.len) i++;
89             i++; //and skip it
90
91             if (i == www_auth.len)
92                 return val; //error
93             start = i;
94             i++;
95
96             //find second double quote
97             while(www_auth.s[i] != '"' && i < www_auth.len) i++;
98             if (i == www_auth.len)
99                 return val; //error
100             end = i;
101             i++;
102
103             val.s = www_auth.s + start;
104             val.len = end - start;
105             break;
106         }
107
108         //parameter not relevant - fast forward
109         do { i++; } while (www_auth.s[i] != ',' && i < www_auth.len);
110     }
111
112     return val;
113 }
114
115 static int fill_contact(struct pcontact_info* ci, struct sip_msg* m)
116 {
117     contact_body_t* cb = NULL;
118     struct via_body* vb = NULL;
119     struct sip_msg* req = NULL;
120
121     if(!ci) {
122         LM_ERR("called with null ptr\n");
123         return -1;
124     }
125
126     memset(ci, 0, sizeof(struct pcontact_info));
127
128     if(m->first_line.type == SIP_REQUEST) {
129         struct sip_uri uri;
130         memset(&uri, 0, sizeof(struct sip_uri));
131
132         if(parse_uri(m->first_line.u.request.uri.s, m->first_line.u.request.uri.len, &uri)) {
133             LM_ERR("Can't parse the request URI from first line\n");
134             return -1;
135         }
136
137         // populate host,port, aor in CI
138         ci->via_host = uri.host;
139         ci->via_port = uri.port_no ? uri.port_no : 5060;
140         ci->via_prot = 0;
141         ci->aor = m->first_line.u.request.uri;
142
143         req = m;
144     }
145     else if(m->first_line.type == SIP_REPLY) {
146         struct cell *t = tmb.t_gett();
147         if (!t || t == (void*) -1) {
148             LM_ERR("Reply without transaction\n");
149             return -1;
150         }
151
152         req = t->uas.request;
153
154         cb = cscf_parse_contacts(req);
155         if (!cb || (!cb->contacts)) {
156             LM_ERR("Reply No contact headers\n");
157             return -1;
158         }
159
160         vb = cscf_get_ue_via(m);
161         if (!vb) {
162             LM_ERR("Reply No via body headers\n");
163             return -1;
164         }
165
166         // populate CI with bare minimum
167         ci->via_host = vb->host;
168         ci->via_port = vb->port;
169         ci->via_prot = vb->proto;
170         ci->aor = cb->contacts->uri;
171     }
172     else {
173         LM_ERR("Unknown first line type: %d\n", m->first_line.type);
174         return -1;
175     }
176
177
178     char* srcip = NULL;
179     if((srcip = pkg_malloc(50)) == NULL) {
180         LM_ERR("Error allocating memory for source IP address\n");
181         return -1;
182     }
183
184     ci->received_host.len = ip_addr2sbuf(&req->rcv.src_ip, srcip, 50);
185     ci->received_host.s = srcip;
186     ci->received_port = req->rcv.src_port;
187     ci->received_proto = req->rcv.proto;
188
189     LM_DBG("SIP %s fill contact with AOR [%.*s], VIA [%d://%.*s:%d], received_host [%d://%.*s:%d]\n",
190             m->first_line.type == SIP_REQUEST ? "REQUEST" : "REPLY",
191             ci->aor.len, ci->aor.s, ci->via_prot, ci->via_host.len, ci->via_host.s, ci->via_port,
192             ci->received_proto, ci->received_host.len, ci->received_host.s, ci->received_port);
193
194     // Set to default, if not set:
195     if (ci->received_port == 0)
196         ci->received_port = 5060;
197
198
199     return 0;
200 }
201
202 // Get CK and IK from WWW-Authenticate
203 static int get_ck_ik(const struct sip_msg* m, str* ck, str* ik)
204 {
205     struct hdr_field *www_auth_hdr = NULL;
206     str www_auth;
207     memset(&www_auth, 0, sizeof(str));
208
209     www_auth = cscf_get_authenticate((sip_msg_t*)m, &www_auth_hdr);
210
211     *ck = get_www_auth_param("ck", www_auth);
212     if (ck->len == 0) {
213         LM_ERR("Error getting CK\n");
214         return -1;
215     }
216
217     *ik = get_www_auth_param("ik", www_auth);
218     if (ck->len == 0) {
219         LM_ERR("Error getting IK\n");
220         return -1;
221     }
222
223     return 0;
224 }
225
226 static int update_contact_ipsec_params(ipsec_t* s, const struct sip_msg* m)
227 {
228     // Get CK and IK
229     str ck, ik;
230     if(get_ck_ik(m, &ck, &ik) != 0) {
231         return -1;
232     }
233
234     // Save CK and IK in the contact
235     s->ck.s = shm_malloc(ck.len);
236     if(s->ck.s == NULL) {
237         LM_ERR("Error allocating memory for CK\n");
238         return -1;
239     }
240     memcpy(s->ck.s, ck.s, ck.len);
241     s->ck.len = ck.len;
242
243     s->ik.s = shm_malloc(ik.len);
244     if(s->ik.s == NULL) {
245         LM_ERR("Error allocating memory for IK\n");
246         shm_free(s->ck.s);
247         s->ck.s = NULL; s->ck.len = 0;
248         s->ik.s = NULL; s->ik.len = 0;
249         return -1;
250     }
251     memcpy(s->ik.s, ik.s, ik.len);
252     s->ik.len = ik.len;
253
254     // Generate SPI
255     if((s->spi_pc = acquire_spi()) == 0) {
256         LM_ERR("Error generating client SPI for IPSEC tunnel creation\n");
257         shm_free(s->ck.s);
258         s->ck.s = NULL; s->ck.len = 0;
259         shm_free(s->ik.s);
260         s->ik.s = NULL; s->ik.len = 0;
261         return -1;
262     }
263
264     if((s->spi_ps = acquire_spi()) == 0) {
265         LM_ERR("Error generating server SPI for IPSEC tunnel creation\n");
266         shm_free(s->ck.s);
267         s->ck.s = NULL; s->ck.len = 0;
268         shm_free(s->ik.s);
269         s->ik.s = NULL; s->ik.len = 0;
270         return -1;
271     }
272
273     return 0;
274 }
275
276 static int create_ipsec_tunnel(const struct ip_addr *remote_addr, unsigned short proto, ipsec_t* s)
277 {
278     struct mnl_socket* sock = init_mnl_socket();
279     if (sock == NULL) {
280         return -1;
281     }
282
283     //Convert ipsec address from str to struct ip_addr
284         ip_addr_t ipsec_addr;
285
286     if(remote_addr->af == AF_INET){
287         if(str2ipbuf(&ipsec_listen_addr, &ipsec_addr) < 0){
288             LM_ERR("Unable to convert ipsec addr4 [%.*s]\n", ipsec_listen_addr.len, ipsec_listen_addr.s);
289             return 0;
290         }
291     } else if(remote_addr->af == AF_INET6){
292         if(str2ip6buf(&ipsec_listen_addr6, &ipsec_addr) < 0){
293             LM_ERR("Unable to convert ipsec addr6 [%.*s]\n", ipsec_listen_addr6.len, ipsec_listen_addr6.s);
294             return 0;
295         }
296     } else {
297         LM_ERR("Unsupported AF %d\n", remote_addr->af);
298         return 0;
299     }
300
301     //Convert to char* for logging
302     char remote_addr_str[128];
303     memset(remote_addr_str, 0, sizeof(remote_addr_str));
304     if(inet_ntop(remote_addr->af, remote_addr->u.addr, remote_addr_str, sizeof(remote_addr_str)) == NULL) {
305         LM_CRIT("Error converting remote IP address: %s\n", strerror(errno));
306         return -1;
307     }
308
309     LM_DBG("Creating security associations: Local IP: %.*s client port: %d server port: %d; UE IP: %s; client port %d server port %d; spi_ps %u, spi_pc %u, spi_us %u, spi_uc %u\n",
310             remote_addr->af == AF_INET ? ipsec_listen_addr.len : ipsec_listen_addr6.len,
311             remote_addr->af == AF_INET ? ipsec_listen_addr.s : ipsec_listen_addr6.s,
312             ipsec_client_port, ipsec_server_port, remote_addr_str, s->port_uc, s->port_us, s->spi_ps, s->spi_pc, s->spi_us, s->spi_uc);
313
314     // SA1 UE client to P-CSCF server
315     //                      src adrr     dst addr     src port    dst port
316     add_sa    (sock, proto, remote_addr, &ipsec_addr, s->port_uc, ipsec_server_port, s->spi_ps, s->ck, s->ik, s->r_alg);
317     add_policy(sock, proto, remote_addr, &ipsec_addr, s->port_uc, ipsec_server_port, s->spi_ps, IPSEC_POLICY_DIRECTION_IN);
318
319     // SA2 P-CSCF client to UE server
320     //                      src adrr     dst addr     src port           dst port
321     add_sa    (sock, proto, &ipsec_addr, remote_addr, ipsec_client_port, s->port_us, s->spi_us, s->ck, s->ik, s->r_alg);
322     add_policy(sock, proto, &ipsec_addr, remote_addr, ipsec_client_port, s->port_us, s->spi_us, IPSEC_POLICY_DIRECTION_OUT);
323
324     // SA3 P-CSCF server to UE client
325     //                      src adrr     dst addr     src port           dst port
326     add_sa    (sock, proto, &ipsec_addr, remote_addr, ipsec_server_port, s->port_uc, s->spi_uc, s->ck, s->ik, s->r_alg);
327     add_policy(sock, proto, &ipsec_addr, remote_addr, ipsec_server_port, s->port_uc, s->spi_uc, IPSEC_POLICY_DIRECTION_OUT);
328
329     // SA4 UE server to P-CSCF client
330     //                      src adrr     dst addr     src port    dst port
331     add_sa    (sock, proto, remote_addr, &ipsec_addr, s->port_us, ipsec_client_port, s->spi_pc, s->ck, s->ik, s->r_alg);
332     add_policy(sock, proto, remote_addr, &ipsec_addr, s->port_us, ipsec_client_port, s->spi_pc, IPSEC_POLICY_DIRECTION_IN);
333
334     close_mnl_socket(sock);
335
336     return 0;
337 }
338
339 static int destroy_ipsec_tunnel(const str remote_addr, unsigned short proto, ipsec_t* s)
340 {
341     struct mnl_socket* sock = init_mnl_socket();
342     if (sock == NULL) {
343         return -1;
344     }
345
346     // TODO: pass ipsec listen address v4 or v6 to destroy the tunnel
347
348     LM_DBG("Destroying security associations: Local IP: %.*s client port: %d server port: %d; UE IP: %.*s; client port %d server port %d\n",
349             ipsec_listen_addr.len, ipsec_listen_addr.s, ipsec_client_port, ipsec_server_port,
350             remote_addr.len, remote_addr.s, s->port_uc, s->port_us);
351
352     // SA1 UE client to P-CSCF server
353     remove_sa    (sock, remote_addr, ipsec_listen_addr, s->port_uc, ipsec_server_port, s->spi_ps);
354     remove_policy(sock, proto, remote_addr, ipsec_listen_addr, s->port_uc, ipsec_server_port, s->spi_ps, IPSEC_POLICY_DIRECTION_IN);
355
356     // SA2 P-CSCF client to UE server
357     remove_sa    (sock, ipsec_listen_addr, remote_addr, ipsec_client_port, s->port_us, s->spi_us);
358     remove_policy(sock, proto, ipsec_listen_addr, remote_addr, ipsec_client_port, s->port_us, s->spi_us, IPSEC_POLICY_DIRECTION_OUT);
359
360     // SA3 P-CSCF server to UE client
361     remove_sa    (sock, ipsec_listen_addr, remote_addr, ipsec_server_port, s->port_uc, s->spi_uc);
362     remove_policy(sock, proto, ipsec_listen_addr, remote_addr, ipsec_server_port, s->port_uc, s->spi_uc, IPSEC_POLICY_DIRECTION_OUT);
363
364     // SA4 UE server to P-CSCF client
365     remove_sa    (sock, remote_addr, ipsec_listen_addr, s->port_us, ipsec_client_port, s->spi_pc);
366     remove_policy(sock, proto, remote_addr, ipsec_listen_addr, s->port_us, ipsec_client_port, s->spi_pc, IPSEC_POLICY_DIRECTION_IN);
367
368     // Release SPIs
369     release_spi(s->spi_uc);
370     release_spi(s->spi_us);
371
372     close_mnl_socket(sock);
373     return 0;
374 }
375
376 static void on_expire(struct pcontact *c, int type, void *param)
377 {
378     if(type != PCSCF_CONTACT_EXPIRE && type != PCSCF_CONTACT_DELETE) {
379         LM_ERR("Unexpected event type %d\n", type);
380         return;
381     }
382
383
384     if(c->security_temp == NULL) {
385         LM_ERR("No security parameters found in contact\n");
386         return;
387     }
388
389     //get security parameters
390     if(c->security_temp->type != SECURITY_IPSEC ) {
391         LM_ERR("Unsupported security type: %d\n", c->security_temp->type);
392         return;
393     }
394
395     destroy_ipsec_tunnel(c->received_host, c->received_proto, c->security_temp->data.ipsec);
396 }
397
398 int add_supported_secagree_header(struct sip_msg* m)
399 {
400     // Add sec-agree header in the reply
401     const char* supported_sec_agree = "Supported: sec-agree\r\n";
402     const int supported_sec_agree_len = 22;
403
404     str* supported = NULL;
405     if((supported = pkg_malloc(sizeof(str))) == NULL) {
406         LM_ERR("Error allocating pkg memory for supported header\n");
407         return -1;
408     }
409
410     if((supported->s = pkg_malloc(supported_sec_agree_len)) == NULL) {
411         LM_ERR("Error allcationg pkg memory for supported header str\n");
412         pkg_free(supported);
413         return -1;
414     }
415     memcpy(supported->s, supported_sec_agree, supported_sec_agree_len);
416     supported->len = supported_sec_agree_len;
417
418     if(cscf_add_header(m, supported, HDR_SUPPORTED_T) != 1) {
419                 pkg_free(supported->s);
420                 pkg_free(supported);
421         LM_ERR("Error adding security header to reply!\n");
422         return -1;
423     }
424     pkg_free(supported);
425
426     return 0;
427 }
428
429 int add_security_server_header(struct sip_msg* m, ipsec_t* s)
430 {
431     // allocate memory for the header itself
432     str* sec_header = NULL;
433     if((sec_header = pkg_malloc(sizeof(str))) == NULL) {
434         LM_ERR("Error allocating pkg memory for security header\n");
435         return -1;
436     }
437     memset(sec_header, 0, sizeof(str));
438
439     // create a temporary buffer and set the value in it
440     char sec_hdr_buf[1024];
441     memset(sec_hdr_buf, 0, sizeof(sec_hdr_buf));
442     sec_header->len = snprintf(sec_hdr_buf, sizeof(sec_hdr_buf) - 1,
443                                 "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",
444                                 s->spi_pc, s->spi_ps, ipsec_client_port, ipsec_server_port,
445                                 s->r_alg.len, s->r_alg.s,
446                                 s->r_ealg.len, s->r_ealg.s
447                               );
448
449     // copy to the header and add
450     if((sec_header->s = pkg_malloc(sec_header->len)) == NULL) {
451         LM_ERR("Error allocating pkg memory for security header payload\n");
452         pkg_free(sec_header);
453         return -1;
454     }
455     memcpy(sec_header->s, sec_hdr_buf, sec_header->len);
456
457     // add security-server header in reply
458     if(cscf_add_header(m, sec_header, HDR_OTHER_T) != 1) {
459         LM_ERR("Error adding security header to reply!\n");
460         pkg_free(sec_header->s);
461         pkg_free(sec_header);
462         return -1;
463     }
464
465     pkg_free(sec_header);
466
467     return 0;
468 }
469
470 int ipsec_create(struct sip_msg* m, udomain_t* d)
471 {
472     pcontact_t* pcontact = NULL;
473     struct pcontact_info ci;
474     int ret = IPSEC_CMD_FAIL;   // FAIL by default
475
476     // Find the contact
477     if(fill_contact(&ci, m) != 0) {
478         LM_ERR("Error filling in contact data\n");
479         return ret;
480     }
481
482     ul.lock_udomain(d, &ci.via_host, ci.via_port, ci.via_prot);
483
484     if (ul.get_pcontact(d, &ci, &pcontact) != 0) {
485         LM_ERR("Contact doesn't exist\n");
486         goto cleanup;
487     }
488
489     // Get security parameters
490     if(pcontact->security_temp == NULL) {
491         LM_ERR("No security parameters found in contact\n");
492         goto cleanup;
493     }
494
495     if(pcontact->security_temp->type != SECURITY_IPSEC ) {
496         LM_ERR("Unsupported security type: %d\n", pcontact->security_temp->type);
497         goto cleanup;
498     }
499
500     ipsec_t* s = pcontact->security_temp->data.ipsec;
501
502     if(update_contact_ipsec_params(s, m) != 0) {
503         goto cleanup;
504     }
505
506     // Get request from reply
507     struct cell *t = tmb.t_gett();
508     if (!t || t == (void*) -1) {
509         LM_ERR("fill_contact(): Reply without transaction\n");
510         return -1;
511     }
512
513     struct sip_msg* req = t->uas.request;
514     ////
515
516     if(create_ipsec_tunnel(&req->rcv.src_ip, ci.received_proto, s) != 0) {
517         goto cleanup;
518     }
519
520     // TODO: Save security_tmp to security!!!!!
521
522     if (ul.update_pcontact(d, &ci, pcontact) != 0) {
523         LM_ERR("Error updating contact\n");
524         goto cleanup;
525     }
526
527     // Update temp security parameters
528     if(ul.update_temp_security(d, pcontact->security_temp->type, pcontact->security_temp, pcontact) != 0)
529     {
530         LM_ERR("Error updating temp security\n");
531     }
532
533     // Destroy the tunnel, if the contact expires
534     if(ul.register_ulcb(pcontact, PCSCF_CONTACT_EXPIRE|PCSCF_CONTACT_DELETE, on_expire, NULL) != 1) {
535         LM_ERR("Error subscribing for contact\n");
536         goto cleanup;
537     }
538
539
540     if(add_supported_secagree_header(m) != 0) {
541         goto cleanup;
542     }
543
544     if(add_security_server_header(m, s) != 0) {
545         goto cleanup;
546     }
547
548     ret = IPSEC_CMD_SUCCESS;    // all good, set ret to SUCCESS, and exit
549
550 cleanup:
551     // Do not free str* sec_header! It will be freed in data_lump.c -> free_lump()
552     ul.unlock_udomain(d, &ci.via_host, ci.via_port, ci.via_prot);
553     pkg_free(ci.received_host.s);
554     return ret;
555 }
556
557
558 int ipsec_forward(struct sip_msg* m, udomain_t* d)
559 {
560     struct pcontact_info ci;
561     pcontact_t* pcontact = NULL;
562     int ret = IPSEC_CMD_FAIL; // FAIL by default
563     unsigned char dst_proto = PROTO_UDP;
564     unsigned short dst_port = 0;
565     unsigned short src_port = 0;
566     ip_addr_t via_host;
567     
568     struct sip_msg* req = NULL;
569     if(m->first_line.type == SIP_REPLY) {
570         // Get request from reply
571         struct cell *t = tmb.t_gett();
572         if (!t) {
573             LM_ERR("Error getting transaction\n");
574             return ret;
575         }
576
577         req = t->uas.request;
578     } else {
579         req = m;
580     }
581
582     //
583     // Find the contact
584     //
585     if(fill_contact(&ci, m) != 0) {
586         LM_ERR("Error filling in contact data\n");
587         return ret;
588     }
589
590     ul.lock_udomain(d, &ci.via_host, ci.via_port, ci.via_prot);
591
592     if (ul.get_pcontact(d, &ci, &pcontact) != 0) {
593         LM_ERR("Contact doesn't exist\n");
594         goto cleanup;
595     }
596
597     if(str2ipxbuf(&ci.via_host, &via_host) < 0){
598         LM_ERR("Error getting AF from ci.via_host\n");
599         goto cleanup;
600     }
601
602     if(pcontact->security_temp == NULL) {
603         LM_ERR("No security parameters found in contact\n");
604         goto cleanup;
605     }
606
607     //get security parameters
608     if(pcontact->security_temp->type != SECURITY_IPSEC ) {
609         LM_ERR("Unsupported security type: %d\n", pcontact->security_temp->type);
610         goto cleanup;
611     }
612
613     ipsec_t* s = pcontact->security_temp->data.ipsec;
614
615     // Update the destination
616     //
617     //       from sec-agree
618     //            v
619     // sip:host:port
620     //       ^
621     //    from URI
622     //int uri_len = 4 /* strlen("sip:") */ + ci.via_host.len + 5 /* max len of port number */ ;
623
624     if(m->dst_uri.s) {
625         pkg_free(m->dst_uri.s);
626         m->dst_uri.s = NULL;
627         m->dst_uri.len = 0;
628     }
629
630     char buf[1024];
631     if(m->first_line.type == SIP_REPLY){
632         // for Reply get the dest proto from the received request
633         dst_proto = req->rcv.proto;
634
635         // for Reply and TCP sends from P-CSCF server port, for Reply and UDP sends from P-CSCF client port
636         src_port = dst_proto == PROTO_TCP ? ipsec_server_port : ipsec_client_port;
637
638         // for Reply and TCP sends to UE client port, for Reply and UDP sends to UE server port
639         dst_port = dst_proto == PROTO_TCP ? s->port_uc : s->port_us;
640     }else{
641         // for Request get the dest proto from the saved contact
642         dst_proto = pcontact->received_proto;
643
644         // for Request sends from P-CSCF client port
645         src_port = ipsec_client_port;
646         
647         // for Request sends to UE server port
648         dst_port = s->port_us;
649     }
650
651     int buf_len = snprintf(buf, sizeof(buf) - 1, "sip:%.*s:%d", ci.via_host.len, ci.via_host.s, dst_port);
652
653     if((m->dst_uri.s = pkg_malloc(buf_len)) == NULL) {
654         LM_ERR("Error allocating memory for dst_uri\n");
655         goto cleanup;
656     }
657
658     memcpy(m->dst_uri.s, buf, buf_len);
659     m->dst_uri.len = buf_len;
660
661     // Set send socket
662     struct socket_info * client_sock = grep_sock_info(via_host.af == AF_INET ? &ipsec_listen_addr : &ipsec_listen_addr6, src_port, dst_proto);
663     if(!client_sock) {
664         LM_ERR("Error calling grep_sock_info() for ipsec client port\n");
665         goto cleanup;
666     }
667     m->force_send_socket = client_sock;
668
669    // Set destination info
670     struct dest_info dst_info;
671     dst_info.send_sock = client_sock;
672 #ifdef USE_DNS_FAILOVER
673     if (!uri2dst(NULL, &dst_info, m, &m->dst_uri, dst_proto)) {
674 #else
675     if (!uri2dst(&dst_info, m, &m->dst_uri, dst_proto)) {
676 #endif
677         LM_ERR("Error converting dst_uri (%.*s) to struct dst_info\n", m->dst_uri.len, m->dst_uri.s);
678         goto cleanup;
679     }
680
681     // Update dst_info in message
682     if(m->first_line.type == SIP_REPLY) {
683         struct cell *t = tmb.t_gett();
684         if (!t) {
685             LM_ERR("Error getting transaction\n");
686             goto cleanup;
687         }
688         t->uas.response.dst = dst_info;
689     }
690
691     LM_DBG("Destination changed to [%d://%.*s]\n", dst_info.proto, m->dst_uri.len, m->dst_uri.s);
692
693     ret = IPSEC_CMD_SUCCESS; // all good, return SUCCESS
694
695     if(add_supported_secagree_header(m) != 0) {
696         goto cleanup;
697     }
698
699     if(add_security_server_header(m, s) != 0) {
700         goto cleanup;
701     }
702
703     ret = IPSEC_CMD_SUCCESS;    // all good, set ret to SUCCESS, and exit
704
705 cleanup:
706     ul.unlock_udomain(d, &ci.via_host, ci.via_port, ci.via_prot);
707     pkg_free(ci.received_host.s);
708     return ret;
709 }
710
711
712 int ipsec_destroy(struct sip_msg* m, udomain_t* d)
713 {
714     struct pcontact_info ci;
715     pcontact_t* pcontact = NULL;
716     int ret = IPSEC_CMD_FAIL; // FAIL by default
717
718     //
719     // Find the contact
720     //
721     if(fill_contact(&ci, m) != 0) {
722         LM_ERR("Error filling in contact data\n");
723         return ret;
724     }
725
726     ul.lock_udomain(d, &ci.via_host, ci.via_port, ci.via_prot);
727
728     if (ul.get_pcontact(d, &ci, &pcontact) != 0) {
729         LM_ERR("Contact doesn't exist\n");
730         goto cleanup;
731     }
732
733
734     if(pcontact->security_temp == NULL) {
735         LM_ERR("No security parameters found in contact\n");
736         goto cleanup;
737     }
738
739     //get security parameters
740     if(pcontact->security_temp->type != SECURITY_IPSEC ) {
741         LM_ERR("Unsupported security type: %d\n", pcontact->security_temp->type);
742         goto cleanup;
743     }
744
745     destroy_ipsec_tunnel(ci.received_host, ci.received_proto, pcontact->security_temp->data.ipsec);
746
747     ret = IPSEC_CMD_SUCCESS;    // all good, set ret to SUCCESS, and exit
748
749 cleanup:
750     ul.unlock_udomain(d, &ci.via_host, ci.via_port, ci.via_prot);
751     pkg_free(ci.received_host.s);
752     return ret;
753 }
754
755 int ipsec_cleanall()
756 {
757     struct mnl_socket* nlsock = init_mnl_socket();
758     if(!nlsock) {
759         return -1;
760     }
761
762     if(clean_sa(nlsock) != 0) {
763         LM_WARN("Error cleaning IPSec Security associations during startup.\n");
764     }
765
766     if(clean_policy(nlsock) != 0) {
767         LM_WARN("Error cleaning IPSec Policies during startup.\n");
768     }
769
770     close_mnl_socket(nlsock);
771
772     return 0;
773 }