modules/sipt: fix bug in setting NAI of called party
[sip-router] / src / modules / sipt / ss7_parser.c
1 /*
2  *
3  * Copyright (C) 2013 Voxbone SA
4  * 
5  * Parsing code derrived from libss7 Copyright (C) Digium
6  *
7  *
8  * This file is part of SIP-Router, a free SIP server.
9  *
10  * SIP-Router is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version
14  *
15  * SIP-Router is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18  * GNU General Public License for more details.
19  *
20  * You should have received a copy of the GNU General Public License 
21  * along with this program; if not, write to the Free Software 
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
23  *
24  * 
25  */
26
27 #include "ss7.h"
28 #include <string.h>
29 #include <stddef.h>
30
31 static char char2digit(char localchar)
32 {
33         switch (localchar) {
34                 case '0':
35                         return 0;
36                 case '1':
37                         return 1;
38                 case '2':
39                         return 2;
40                 case '3':
41                         return 3;
42                 case '4':
43                         return 4;
44                 case '5':
45                         return 5;
46                 case '6':
47                         return 6;
48                 case '7':
49                         return 7;
50                 case '8':
51                         return 8;
52                 case '9':
53                         return 9;
54                 case 'A':
55                         return 0xa;
56                 case 'B':
57                         return 0xb;
58                 case 'C':
59                         return 0xc;
60                 case 'D':
61                         return 0xd;
62                 case '*':
63                         return 0xe;
64                 case '#':
65                 case 'F':
66                         return 0xf;
67                 default:
68                         return 0;
69         }
70 }
71
72 static void isup_put_number(unsigned char *dest, char *src, int *len, int *oddeven)
73 {
74         int i = 0;
75         int numlen = strlen(src);
76
77         if (numlen % 2) {
78                 *oddeven = 1;
79                 *len = numlen/2 + 1;
80         } else {
81                 *oddeven = 0;
82                 *len = numlen/2;
83         }
84         
85
86         while (i < numlen) {
87                 if (!(i % 2))
88                         dest[i/2] = char2digit(src[i]) & 0xf;
89                 else
90                 {
91                         dest[i/2] |= (char2digit(src[i]) << 4) & 0xf0;
92                 }
93                 i++;
94         }
95 }
96
97 static int encode_called_party(char * number, unsigned char * flags, int nai, unsigned char * buf, int len)
98 {
99         int numlen, oddeven;
100         buf[0] = flags[0]&0x7F;
101         buf[1] = flags[1];
102
103         isup_put_number(&buf[2], number, &numlen, &oddeven);
104
105         if(oddeven)
106         {
107                 buf[0] |= 0x80;
108         }
109         
110         if(nai)
111         {
112                 buf[0] &= 0x80;
113                 buf[0] |= (unsigned char)(nai&0x7F);
114         }
115
116         return numlen + 2;
117 }
118
119 static int encode_calling_party(char * number, int nai, int presentation, int screening, unsigned char * buf, int len) 
120 {
121         int oddeven, datalen;
122
123         if (!number[0] && presentation != SS7_PRESENTATION_ADDR_NOT_AVAILABLE)
124                 return 0;
125
126         if (number[0] && presentation != SS7_PRESENTATION_ADDR_NOT_AVAILABLE)
127         {
128                 isup_put_number(&buf[2], number, &datalen, &oddeven);
129         }
130         else 
131         {
132                 datalen = 0;
133                 oddeven = 0;
134                 nai = 0;
135         }
136
137         buf[0] = (oddeven << 7) | nai;      /* Nature of Address Indicator */
138          /* Assume E.164 ISDN numbering plan, calling number complete */
139         buf[1] = ((presentation == SS7_PRESENTATION_ADDR_NOT_AVAILABLE) ? 0 : (1 << 4)) |
140                 ((presentation & 0x3) << 2) |
141                 (screening & 0x3);
142
143         return datalen + 2;
144 }
145
146 // returns start of specified optional header of IAM or CPG, otherwise return -1
147 static int get_optional_header(unsigned char header, unsigned char *buf, int len)
148 {
149         int offset = 0;
150         int res;
151         union isup_msg * message = (union isup_msg*)buf;
152         unsigned char optional_pointer = 0;
153
154
155         if(message->type == ISUP_IAM)
156         {
157                 len -= offsetof(struct isup_iam_fixed, optional_pointer);
158                 offset += offsetof(struct isup_iam_fixed, optional_pointer);
159                 optional_pointer = message->iam.optional_pointer;
160         }
161         else if(message->type == ISUP_ACM || message->type == ISUP_COT)
162         {
163                 len -= offsetof(struct isup_acm_fixed, optional_pointer);
164                 offset += offsetof(struct isup_acm_fixed, optional_pointer);
165                 optional_pointer = message->acm.optional_pointer;
166         }
167         else if(message->type == ISUP_CPG)
168         {
169                 len -= offsetof(struct isup_cpg_fixed, optional_pointer);
170                 offset += offsetof(struct isup_cpg_fixed, optional_pointer);
171                 optional_pointer = message->cpg.optional_pointer;
172         }
173         else
174         {
175                 // don't recognize the type? do nothing
176                 return -1;
177         }
178
179
180         if (len < 1)
181                 return -1;
182
183         offset += optional_pointer;
184         len -= optional_pointer;
185
186         if (len < 1 )
187                 return -1;
188
189         /* Optional paramter parsing code */
190         if (optional_pointer) {
191                 while ((len > 0) && (buf[offset] != 0)) {
192                         struct isup_parm_opt *optparm = (struct isup_parm_opt *)(buf + offset);
193
194                         res = optparm->len+2;
195                         if(optparm->type == header)
196                         {
197                                 return offset;
198                         }
199
200                         len -= res;
201                         offset += res;
202                 }
203         }
204         return -1;
205 }
206
207 int isup_get_hop_counter(unsigned char *buf, int len)
208 {
209         int  offset = get_optional_header(ISUP_PARM_HOP_COUNTER, buf, len);
210
211         if(offset != -1 && len-offset-2 > 0)
212         {
213                 return buf[offset+2] & 0x1F;
214         }
215         return -1;
216 }
217
218 int isup_get_event_info(unsigned char *buf, int len)
219 {
220         struct isup_cpg_fixed * message = (struct isup_cpg_fixed*)buf;
221
222         // not a CPG? do nothing
223         if(message->type != ISUP_CPG)
224         {
225                 return -1;
226         }
227
228         /* Message Type = 1 */
229         len -= offsetof(struct isup_cpg_fixed, event_info);
230
231         if (len < 1)
232                 return -1;
233
234         return (int)message->event_info;
235 }
236
237 int isup_get_cpc(unsigned char *buf, int len)
238 {
239         struct isup_iam_fixed * message = (struct isup_iam_fixed*)buf;
240
241         // not an iam? do nothing
242         if(message->type != ISUP_IAM)
243         {
244                 return -1;
245         }
246
247         /* Message Type = 1 */
248         len -= offsetof(struct isup_iam_fixed, calling_party_category);
249
250         if (len < 1)
251                 return -1;
252
253         return (int)message->calling_party_category;
254 }
255
256
257 int isup_get_calling_party_nai(unsigned char *buf, int len)
258 {
259         int  offset = get_optional_header(ISUP_PARM_CALLING_PARTY_NUM, buf, len);
260
261         if(offset != -1 && len-offset-2 > 0)
262         {
263                 return buf[offset+2] & 0x7F;
264         }
265         return -1;
266 }
267
268 int isup_get_screening(unsigned char *buf, int len)
269 {
270         int  offset = get_optional_header(ISUP_PARM_CALLING_PARTY_NUM, buf, len);
271
272         if(offset != -1 && len-offset-3 > 0)
273         {
274                 return buf[offset+3] & 0x03;
275         }
276         return -1;
277 }
278
279 int isup_get_presentation(unsigned char *buf, int len)
280 {
281         int  offset = get_optional_header(ISUP_PARM_CALLING_PARTY_NUM, buf, len);
282
283         if(offset != -1 && len-offset-3 > 0)
284         {
285                 return (buf[offset+3]>>2) & 0x03;
286         }
287         return -1;
288 }
289
290 int isup_get_called_party_nai(unsigned char *buf, int len)
291 {
292         struct isup_iam_fixed * message = (struct isup_iam_fixed*)buf;
293
294         // not an iam? do nothing
295         if(message->type != ISUP_IAM)
296         {
297                 return -1;
298         }
299
300         /* Message Type = 1 */
301         len -= offsetof(struct isup_iam_fixed, called_party_number);
302
303         if (len < 1)
304                 return -1;
305         return message->called_party_number[1]&0x7F;
306 }
307
308
309 int isup_get_charging_indicator(unsigned char *buf, int len) {
310         struct isup_acm_fixed * orig_message = (struct isup_acm_fixed*)buf;
311
312         // not an acm or cot? do nothing
313         if(orig_message->type != ISUP_ACM && orig_message->type != ISUP_COT)
314         {
315                 return -1;
316         }
317
318         // add minus 1 because the optinal pointer is optional
319         if (len < sizeof(struct isup_acm_fixed) -1 )
320                 return -1;
321
322         return (orig_message->backwards_call_ind[0] & 0x03);
323 }
324
325 int isup_update_bci_1(struct sdp_mangler * mangle, int charge_indicator, int called_status, int called_category, int e2e_indicator, unsigned char *buf, int len)
326 {
327         struct isup_acm_fixed * orig_message = (struct isup_acm_fixed*)buf;
328         unsigned char bci;
329
330         // not an acm or cot? do nothing
331         if(orig_message->type != ISUP_ACM && orig_message->type != ISUP_COT)
332         {
333                 return 1;
334         }
335
336         // add minus 1 because the optinal pointer is optional
337         if (len < sizeof(struct isup_acm_fixed) -1 )
338                 return -1;
339
340         bci = (charge_indicator & 0x3) | ((called_status & 0x3)<<2) |
341                 ((called_category & 0x3)<<4) | ((e2e_indicator & 0x3)<<6);
342
343         replace_body_segment(mangle, offsetof(struct isup_acm_fixed, backwards_call_ind), 1, &bci, 1);
344
345         return sizeof(struct isup_acm_fixed);
346 }
347
348 int isup_update_destination(struct sdp_mangler * mangle, char * dest, int hops, int nai, unsigned char *buf, int len)
349 {
350         int offset = 0;
351         int res, res2;
352         struct isup_iam_fixed * orig_message = (struct isup_iam_fixed*)buf;
353         unsigned char tmp_buf[255];
354
355
356         // not an iam? do nothing
357         if(orig_message->type != ISUP_IAM)
358         {
359                 return 1;
360         }
361
362         // bounds checking
363         if(hops > 31)
364         {
365                 hops = 31;
366         }
367
368
369
370         /* Copy the fixed parms */
371         len -= 6;
372         offset += 6;
373
374         if (len < 1)
375                 return -1;
376
377         /* IAM has one Fixed variable param, Called party number, we need to modify this */
378
379         // pointer to fixed part (2)
380         offset++;
381
382         //pointer to optional part (to update later)
383         offset++;
384         len--;
385
386
387         // modify the mandatory fixed header
388         res2 = encode_called_party(dest, buf+offset+1, nai, tmp_buf+2, 255-1);
389         tmp_buf[1] = (char)res2;
390         res = buf[offset]+1;
391
392         // set the new optional part pointer
393         tmp_buf[0] = (char)res2+2;
394         
395         // replace the mandatory fixed header + optional pointer
396         replace_body_segment(mangle, offset - 1,res+1,tmp_buf, res2+2);
397
398         offset += res;
399         len -= res;
400         
401         if (len < 1 )
402                 return -1;
403
404
405         /* Optional paramter parsing code */
406         if (orig_message->optional_pointer) {
407
408                 bool has_hops = 0;
409                 
410                 while ((len > 0) && (buf[offset] != 0)) {
411                         struct isup_parm_opt *optparm = (struct isup_parm_opt *)(buf + offset);
412
413
414                         res = optparm->len+2;
415                         switch(optparm->type)
416                         {
417                                 case ISUP_PARM_HOP_COUNTER:
418                                         tmp_buf[0] = ISUP_PARM_HOP_COUNTER;
419                                         tmp_buf[1] = 1;
420                                         tmp_buf[2] = ((optparm->data[0]&0x1F)-1)&0x1F;
421                                         replace_body_segment(mangle, offset, res, tmp_buf, 3);
422                                         has_hops = 1;
423                                         break;
424                                 default:
425                                         break;
426                         }
427
428                         len -= res;
429                         offset += res;
430                 }
431
432                 // add missing headers
433                 if(!has_hops && len >= 0)
434                 {
435                         tmp_buf[0] = ISUP_PARM_HOP_COUNTER;
436                         tmp_buf[1] = 1;
437                         tmp_buf[2] = hops & 0x1F;
438                         has_hops = 1;
439                         add_body_segment(mangle, offset,tmp_buf,3);
440                 }
441         }
442
443         return offset;
444 }
445
446 int isup_update_calling(struct sdp_mangler * mangle, char * origin, int nai, int presentation, int screening, unsigned char * buf, int len)
447 {
448         int offset = 0;
449         int res;
450         struct isup_iam_fixed * orig_message = (struct isup_iam_fixed*)buf;
451
452         // not an iam? do nothing
453         if(orig_message->type != ISUP_IAM)
454         {
455                 return 1;
456         }
457
458         /* Copy the fixed parms */
459         len -= offsetof(struct isup_iam_fixed, called_party_number);
460         offset += offsetof(struct isup_iam_fixed, called_party_number);
461
462         if (len < 1)
463                 return -1;
464
465
466         /* IAM has one Fixed variable param, Called party number, we need to modify this */
467
468
469         // add the new mandatory fixed header
470         res = buf[offset];
471         offset += res+1;
472         len -= res+1;
473         
474         if (len < 1 )
475                 return -1;
476
477
478         /* Optional paramter parsing code */
479         if (orig_message->optional_pointer) {
480
481                 bool has_calling = 0;
482                 
483                 while ((len > 0) && (buf[offset] != 0)) {
484                         int res2 = 0;
485                         struct isup_parm_opt *optparm = (struct isup_parm_opt *)(buf + offset);
486                         unsigned char new_party[255];
487
488
489                         res = optparm->len+2;
490                         switch(optparm->type)
491                         {
492                                 case ISUP_PARM_CALLING_PARTY_NUM:
493                                         res2 = encode_calling_party(origin, nai, presentation, screening, &new_party[1], 255-1);
494                                         new_party[0] = (char)res2;
495                                         replace_body_segment(mangle, offset+1,(int)buf[offset+1]+1,new_party, res2+1);
496
497                                         has_calling = 1;
498                                         break;
499                                 default:
500                                         break;
501                         }
502
503                         len -= res;
504                         offset += res;
505                 }
506
507
508                 // add missing headers
509                 if(!has_calling && len >= 0)
510                 {
511                         unsigned char new_party[255];
512                         new_party[0] = ISUP_PARM_CALLING_PARTY_NUM;
513                         res = encode_calling_party(origin, nai, presentation, screening, new_party+2, 255-2);
514                         new_party[1] = (char)res;
515
516                         add_body_segment(mangle, offset,new_party, res+2);
517                 }
518
519         }
520
521         return offset;
522 }