stun: link attribute when calling stun_parse_body() with an existing list
[sip-router] / src / modules / stun / kam_stun.c
1 /*
2  * Portions Copyright (C) 2013 Crocodile RCS Ltd
3  *
4  * Based on "ser_stun.c". Copyright (C) 2001-2003 FhG Fokus
5  *
6  * This file is part of Kamailio, a free SIP server.
7  *
8  * Kamailio is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version
12  *
13  * Kamailio is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License 
19  * along with this program; if not, write to the Free Software 
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
21  *
22  */
23 /*!
24  * \file
25  * \brief STUN :: Configuration
26  * \ingroup stun
27  */
28 /*!
29  * \defgroup stun STUN Nat traversal support
30  * 
31  */
32
33 #include <arpa/inet.h>
34 #include "kam_stun.h"
35 #include "../../core/forward.h"
36
37 /*
38  * ****************************************************************************
39  *                     Declaration of functions                               *
40  * ****************************************************************************
41  */
42 static int stun_parse_header(struct stun_msg* req, USHORT_T* error_code);
43 static int stun_parse_body(
44                                 struct stun_msg* req,
45                                 struct stun_unknown_att** unknown,
46                                 USHORT_T* error_code);
47 static void stun_delete_unknown_attrs(struct stun_unknown_att* unknown);
48 static struct stun_unknown_att* stun_alloc_unknown_attr(USHORT_T type);
49 static int stun_add_address_attr(struct stun_msg* res, 
50                                                 UINT_T          af,
51                                                 USHORT_T        port,
52                                                 UINT_T*         ip_addr,
53                                                 USHORT_T        type,
54                                                 int do_xor);
55 static int add_unknown_attr(struct stun_msg* res, struct stun_unknown_att* unknown);
56 static int add_error_code(struct stun_msg* res, USHORT_T error_code);
57 static int copy_str_to_buffer(struct stun_msg* res, const char* data, UINT_T pad);
58 static int reallock_buffer(struct stun_buffer* buffer, UINT_T len);
59 static int buf_copy(struct stun_buffer* msg, void* source, UINT_T len);
60 static void clean_memory(struct stun_msg* req,
61                                 struct stun_msg* res,   struct stun_unknown_att* unknown);
62 static int stun_create_response(
63                                                 struct stun_msg* req,
64                                                 struct stun_msg* res,
65                                                 struct receive_info* ri,
66                                                 struct stun_unknown_att* unknown, 
67                                                 UINT_T error_code);
68 static int stun_add_common_text_attr(struct stun_msg* res, USHORT_T type, char* value, 
69                                                         USHORT_T pad);
70
71
72 /*
73  * ****************************************************************************
74  *                      Definition of functions                               *
75  * ****************************************************************************
76  */
77  
78 /* 
79  * process_stun_msg(): 
80  *                      buf - incoming message
81  *                      len - length of incoming message
82  *                      ri  - information about socket that received a message and 
83  *                also information about sender (its IP, port, protocol)
84  * 
85  * This function ensures processing of incoming message. It's common for both
86  * TCP and UDP protocol. There is no other function as an interface.
87  * 
88  * Return value:        0       if there is no environment error
89  *                                      -1      if there is some enviroment error such as insufficiency
90  *                                              of memory
91  * 
92  */
93 int process_stun_msg(char* buf, unsigned len, struct receive_info* ri)
94 {
95         struct stun_msg                         msg_req;
96         struct stun_msg                         msg_res;
97         struct dest_info                        dst;
98         struct stun_unknown_att*        unknown;
99         USHORT_T                                        error_code;
100          
101         memset(&msg_req, 0, sizeof(msg_req));
102         memset(&msg_res, 0, sizeof(msg_res));
103         
104         msg_req.msg.buf.s = buf;
105         msg_req.msg.buf.len = len;      
106         unknown = NULL;
107         error_code = RESPONSE_OK;
108         
109         if (stun_parse_header(&msg_req, &error_code) != 0) {
110                 goto error;
111         }
112         
113         if (error_code == RESPONSE_OK) {
114                 if (stun_parse_body(&msg_req, &unknown, &error_code) != 0) {
115                         goto error;
116                 }
117         }
118         
119         if (stun_create_response(&msg_req, &msg_res, ri,  
120                                                         unknown, error_code) != 0) {
121                 goto error;
122         }
123         
124         init_dst_from_rcv(&dst, ri);
125
126 #ifdef EXTRA_DEBUG      
127         struct ip_addr ip;
128         su2ip_addr(&ip, &dst.to);
129         LOG(L_DBG, "DEBUG: process_stun_msg: decoded request from (%s:%d)\n", ip_addr2a(&ip), 
130                 su_getport(&dst.to));
131 #endif
132         
133         /* send STUN response */
134         if (msg_send(&dst, msg_res.msg.buf.s, msg_res.msg.buf.len) != 0) {
135                 goto error;
136         }
137         
138 #ifdef EXTRA_DEBUG
139         LOG(L_DBG, "DEBUG: process_stun_msg: send response\n");
140 #endif
141         clean_memory(&msg_req, &msg_res, unknown);
142         return 0;
143         
144 error:
145 #ifdef EXTRA_DEBUG
146         LOG(L_DBG, "DEBUG: process_stun_msg: failed to decode request\n");
147 #endif
148         clean_memory(&msg_req, &msg_res, unknown);
149         return FATAL_ERROR;
150 }
151
152 /*
153  * stun_parse_header():
154  *                      - req: request from host that should be processed
155  *                      - error_code: indication of any protocol error
156  * 
157  * This function ensures parsing of incoming header.
158  * 
159  * Return value:        0       if there is no environment error
160  *                                      -1      if there is some enviroment error such as insufficiency
161  *                                              of memory
162  */
163
164 static int stun_parse_header(struct stun_msg* req, USHORT_T* error_code)
165 {
166         
167         if (sizeof(req->hdr) > req->msg.buf.len) {
168                 /* the received message does not contain whole header */
169                 LOG(L_INFO, "INFO: stun_parse_header: incomplete header of STUN message\n");
170                 /* Any better solution? IMHO it's not possible to send error response
171                  * because the transaction ID is not available.
172                  */
173                 return FATAL_ERROR;
174         }
175         
176         memcpy(&req->hdr, req->msg.buf.s, sizeof(struct stun_hdr));
177         req->hdr.type = ntohs(req->hdr.type);
178         
179         /* the SER supports only Binding Request right now */ 
180         if (req->hdr.type != BINDING_REQUEST) {
181                 LOG(L_INFO, "INFO: stun_parse_header: unsupported type of STUN message: %x\n", 
182                                         req->hdr.type);
183                 /* resending of same message is not welcome */
184                 *error_code = GLOBAL_FAILURE_ERR;
185         }
186         
187         req->hdr.len = ntohs(req->hdr.len);
188         
189         /* check if there is correct magic cookie */
190         req->old = (req->hdr.id.magic_cookie == htonl(MAGIC_COOKIE)) ? 0 : 1;
191
192 #ifdef EXTRA_DEBUG
193         LOG(L_DBG, "DEBUG: stun_parse_header: request is old: %i\n", req->old);
194 #endif
195         return 0;
196 }
197
198 /*
199  * stun_parse_body():
200  *                      - req: request from host that should be processed
201  *                      - unknown: this is a link list header of attributes 
202  *                                         that are unknown to SER; defaul value is NULL
203  *                      - error_code: indication of any protocol error
204  * 
205  * Return value:        0       if there is no environment error
206  *                                      -1      if there is some enviroment error such as insufficiency
207  *                                              of memory
208  */
209 static int stun_parse_body(
210                                 struct stun_msg* req,
211                                 struct stun_unknown_att** unknown,
212                                 USHORT_T* error_code)
213 {
214         int not_parsed;
215         struct stun_attr attr;
216         USHORT_T attr_size;
217         UINT_T padded_len;
218         struct stun_unknown_att*        tmp_unknown;
219         struct stun_unknown_att*        body;
220         char*   buf;
221         
222         attr_size = sizeof(struct stun_attr);
223         buf = &req->msg.buf.s[sizeof(struct stun_hdr)];
224         
225         /* 
226          * Mark the body lenght as unparsed.
227          */
228         not_parsed = req->msg.buf.len - sizeof(struct stun_hdr);
229         
230         if (not_parsed != req->hdr.len) {
231 #ifdef EXTRA_DEBUG
232                 LOG(L_DBG, "DEBUG: stun_parse_body: body too short to be valid\n");
233 #endif
234                 *error_code = BAD_REQUEST_ERR;
235                 return 0; 
236         }
237         
238         tmp_unknown = *unknown;
239         body = NULL;
240         
241         while (not_parsed > 0 && *error_code == RESPONSE_OK) {
242                 memset(&attr, 0, attr_size);
243                 
244                 /* check if there are 4 bytes for attribute type and its value */
245                 if (not_parsed < 4) {
246 #ifdef EXTRA_DEBUG
247                         LOG(L_DBG, "DEBUG: stun_parse_body: attribute header short to be valid\n");
248 #endif
249                         *error_code = BAD_REQUEST_ERR;
250                         continue;
251                 }
252                 
253                 memcpy(&attr, buf, attr_size);
254                 
255                 buf += attr_size;
256                 not_parsed -= attr_size;
257                 
258                 /* check if there is enought unparsed space for attribute's value */
259                 if (not_parsed < ntohs(attr.len)) {
260 #ifdef EXTRA_DEBUG
261                         LOG(L_DBG, "DEBUG: stun_parse_body: remaining message is shorter then attribute length\n");
262 #endif
263                         *error_code = BAD_REQUEST_ERR;
264                         continue;
265                 }
266                 
267                 /* check if the attribute is known to the server */
268                 switch (ntohs(attr.type)) {                     
269                         case REALM_ATTR:
270                         case NONCE_ATTR:
271                         case MAPPED_ADDRESS_ATTR:
272                         case XOR_MAPPED_ADDRESS_ATTR:
273                         case ALTERNATE_SERVER_ATTR:
274                         case RESPONSE_ADDRESS_ATTR:
275                         case SOURCE_ADDRESS_ATTR:
276                         case REFLECTED_FROM_ATTR:               
277                         case CHANGE_REQUEST_ATTR:
278                         case CHANGED_ADDRESS_ATTR:
279                                 padded_len = ntohs(attr.len);
280 #ifdef EXTRA_DEBUG
281                                 LOG(L_DBG, "DEBUG: stun_parse_body: known attributes\n");
282 #endif
283                                 break;
284                         
285                         /* following attributes must be padded to 4 bytes */
286                         case USERNAME_ATTR:
287                         case ERROR_CODE_ATTR:
288                         case UNKNOWN_ATTRIBUTES_ATTR:
289                         case SOFTWARE_ATTR:
290                                 padded_len = PADDED_TO_FOUR(ntohs(attr.len));
291 #ifdef EXTRA_DEBUG
292                                 LOG(L_DBG, "DEBUG: stun_parse_body: padded to four\n");
293 #endif
294                                 break;
295
296                         /* MESSAGE_INTEGRITY must be padded to sixty four bytes*/
297                         case MESSAGE_INTEGRITY_ATTR:
298 #ifdef EXTRA_DEBUG
299                                 LOG(L_DBG, "DEBUG: stun_parse_body: message integrity attribute found\n");
300 #endif
301                                 padded_len = PADDED_TO_SIXTYFOUR(ntohs(attr.len));
302                                 break;
303                         
304                         case FINGERPRINT_ATTR:
305 #ifdef EXTRA_DEBUG
306                                 LOG(L_DBG, "DEBUG: stun_parse_body: fingerprint attribute found\n");
307 #endif
308                                 padded_len = SHA_DIGEST_LENGTH;
309
310                                 if (not_parsed > SHA_DIGEST_LENGTH) {
311 #ifdef EXTRA_DEBUG
312                                         LOG(L_DBG, "DEBUG: stun_parse_body: fingerprint is not the last attribute\n");
313 #endif
314                                         /* fingerprint must be last parameter in request */
315                                         *error_code = BAD_REQUEST_ERR;
316                                         continue;
317                                 }
318                                 break;
319                         
320                         default:
321                                 /* 
322                                  * the attribute is uknnown to the server
323                                  * let see if it's necessary to generate error response 
324                                  */
325 #ifdef EXTRA_DEBUG
326                                 LOG(L_DBG, "DEBUG: low endian: attr - 0x%x   const - 0x%x\n", ntohs(attr.type), MANDATORY_ATTR);
327                     LOG(L_DBG, "DEBUG: big endian: attr - 0x%x   const - 0x%x\n", attr.type, htons(MANDATORY_ATTR));
328 #endif
329                                 if (ntohs(attr.type) <= MANDATORY_ATTR) {
330 #ifdef EXTRA_DEBUG
331                                 LOG(L_DBG, "DEBUG: stun_parse_body: mandatory unknown attribute found - 0x%x\n", ntohs(attr.type));
332 #endif          
333                                         tmp_unknown = stun_alloc_unknown_attr(attr.type);
334                                         if (tmp_unknown == NULL) {
335                                                 return FATAL_ERROR;
336                                         }
337                                         if (*unknown == NULL) {
338                                                 *unknown = body = tmp_unknown;
339                                         } else {
340                                                 if(body==NULL) {
341                                                         body = *unknown;
342                                                 }
343                                                 body->next = tmp_unknown;
344                                                 body = body->next;
345                                         }
346                                 }
347 #ifdef EXTRA_DEBUG
348         else {
349                                   LOG(L_DBG, "DEBUG: stun_parse_body: optional unknown attribute found - 0x%x\n", ntohs(attr.type));
350         }
351 #endif
352                                 padded_len = ntohs(attr.len);
353                                 break;
354                 }
355                 
356                 /* check if there is enough unparsed space for the padded attribute
357                    (the padded length might be greater then the attribute length)
358                  */
359                 if (not_parsed < padded_len) {
360                         break;
361                 }
362                 buf += padded_len;
363                 not_parsed -= padded_len;
364         }  /* while */
365         
366         /*
367          * The unknown attribute error code must set after parsing of whole body
368          * because it's necessary to obtain all of unknown attributes! 
369          */
370         if (*error_code == RESPONSE_OK && *unknown != NULL) {
371                 *error_code = UNKNOWN_ATTRIBUTE_ERR;
372         } 
373         
374         return 0;
375 }
376
377 /*
378  * stun_create_response():
379  *                      - req: original request from host
380  *                      - res: this will represent response to host
381  *                      - ri: information about request, necessary because of IP 
382  *                                address and port 
383  *                      - unknown: link list of unknown attributes
384  *                      - error_code: indication of any protocol error
385  * 
386  * The function stun_create_response ensures creating response to a host.
387  * The type of response depends on value of error_code parameter.
388  * 
389  * Return value:        0       if there is no environment error
390  *                                      -1      if there is some enviroment error such as insufficiency
391  *                                              of memory  
392  */
393
394 static int stun_create_response(
395                                                 struct stun_msg* req,
396                                                 struct stun_msg* res,
397                                                 struct receive_info* ri,
398                                                 struct stun_unknown_att* unknown, 
399                                                 UINT_T error_code)
400 {
401         /*
402          * Alloc some space for response.
403          * Optimalization? - maybe it would be better to use biggish static array.
404          */
405         res->msg.buf.s = (char *) pkg_malloc(sizeof(char)*STUN_MSG_LEN);
406         if (res->msg.buf.s == NULL) {
407                 LOG(L_ERR, "ERROR: STUN: out of memory\n");
408                 return FATAL_ERROR;
409         }
410         
411         memset(res->msg.buf.s, 0, sizeof(char)*STUN_MSG_LEN); 
412         res->msg.buf.len = 0;
413         res->msg.empty = STUN_MSG_LEN;
414         
415         /* use magic cookie and transaction ID from request */
416         memcpy(&res->hdr.id, &req->hdr.id, sizeof(struct transaction_id));
417         /* the correct body length will be added ASAP it will be known */ 
418         res->hdr.len = htons(0);
419         
420         if (error_code == RESPONSE_OK) {
421 #ifdef EXTRA_DEBUG
422                 LOG(L_DBG, "DEBUG: stun_create_response: creating normal response\n");
423 #endif
424                 res->hdr.type = htons(BINDING_RESPONSE);
425                 
426                 /* copy header into msg buffer */
427                 if (buf_copy(&res->msg, (void *) &res->hdr, 
428                                         sizeof(struct stun_hdr)) != 0) {
429 #ifdef EXTRA_DEBUG
430                         LOG(L_DBG, "DEBUG: stun_create_response: failed to copy buffer\n");
431 #endif
432                         return FATAL_ERROR;
433                 }
434
435                 /* 
436                  * If the SER received message in old format, then the body will 
437                  * contain MAPPED-ADDRESS attribute instead of XOR-MAPPED-ADDRESS
438                  * attribute.
439                  */             
440                 if (req->old == 0) {
441                         if (stun_add_address_attr(res, ri->src_ip.af, ri->src_port, 
442                                                   ri->src_ip.u.addr32, XOR_MAPPED_ADDRESS_ATTR, 
443                                                   XOR) != 0) {
444 #ifdef EXTRA_DEBUG
445                                 LOG(L_DBG, "DEBUG: stun_create_response: failed to add address\n");
446 #endif
447                                 return FATAL_ERROR;
448                         }
449                 }
450                 else {
451                         if (stun_add_address_attr(res, ri->src_ip.af, ri->src_port, 
452                                                 ri->src_ip.u.addr32, MAPPED_ADDRESS_ATTR, !XOR) != 0) {
453 #ifdef EXTRA_DEBUG
454                                 LOG(L_DBG, "DEBUG: stun_create_response: failed to add address\n");
455 #endif
456                                 return FATAL_ERROR;
457                         }
458                 }
459         }
460         else {
461 #ifdef EXTRA_DEBUG
462                 LOG(L_DBG, "DEBUG: stun_create_response: creating error response\n");
463 #endif
464                 res->hdr.type = htons(BINDING_ERROR_RESPONSE);
465                 
466                 if (buf_copy(&res->msg, (void *) &res->hdr, 
467                                                                 sizeof(struct stun_hdr)) != 0) {
468 #ifdef EXTRA_DEBUG
469                         LOG(L_DBG, "DEBUG: stun_create_response: failed to copy buffer\n");
470 #endif
471                         return FATAL_ERROR;
472                 }
473                 
474                 if (add_error_code(res, error_code) != 0) {
475 #ifdef EXTRA_DEBUG
476                         LOG(L_DBG, "DEBUG: stun_create_response: failed to add error code\n");
477 #endif
478                         return FATAL_ERROR;
479                 }
480                 
481                 if (unknown != NULL) {
482                         if (add_unknown_attr(res, unknown) != 0) {
483 #ifdef EXTRA_DEBUG
484                                 LOG(L_DBG, "DEBUG: stun_create_response: failed to add unknown attribute\n");
485 #endif
486                                 return FATAL_ERROR;
487                         }
488                 } 
489         }
490         
491         if (req->old == 0) {
492                 /* 
493                  * add optional information about server; attribute SOFTWARE is part of 
494                  * rfc5389.txt
495                  * */
496                 if (stun_add_common_text_attr(res, SOFTWARE_ATTR, SERVER_HDR, PAD4)!=0) {
497 #ifdef EXTRA_DEBUG
498                         LOG(L_DBG, "DEBUG: stun_create_response: failed to add common text attribute\n");
499 #endif
500                         return FATAL_ERROR;
501                 }
502         }       
503         
504         res->hdr.len = htons(res->msg.buf.len - sizeof(struct stun_hdr));
505         memcpy(&res->msg.buf.s[sizeof(USHORT_T)], (void *) &res->hdr.len,
506                sizeof(USHORT_T));
507         
508         return 0;
509 }
510
511 /*
512  * add_unknown_attr()
513  *                      - res: response representation
514  *                      - unknown: link list of unknown attributes
515  * 
516  * The function add_unknown_attr ensures copy of link list of unknown 
517  * attributes into response buffer.
518  * 
519  * Return value:        0       if there is no environment error
520  *                                      -1      if there is some enviroment error such as insufficiency
521  *                                              of memory
522  * 
523  */
524 static int add_unknown_attr(struct stun_msg* res, struct stun_unknown_att* unknown)
525 {
526         struct stun_attr attr;
527         struct stun_unknown_att* tmp_unknown;
528         UINT_T          counter;
529         USHORT_T        orig_len;
530
531         counter = 0;
532         orig_len = res->msg.buf.len;
533         tmp_unknown = unknown;
534         
535         attr.type = htons(UNKNOWN_ATTRIBUTES_ATTR);
536         attr.len = htons(0);
537         
538         if (buf_copy(&res->msg, (void *) &attr, sizeof(struct stun_attr)) != 0) {
539 #ifdef EXTRA_DEBUG
540                 LOG(L_DBG, "DEBUG: add_unknown_attr: failed to copy buffer\n");
541 #endif
542                 return FATAL_ERROR;
543         }
544         
545         while (tmp_unknown != NULL) {
546                 if (buf_copy(&res->msg, (void *)&tmp_unknown->type, 
547                                                                 sizeof(USHORT_T)) != 0) {
548 #ifdef EXTRA_DEBUG
549                         LOG(L_DBG, "DEBUG: add_unknown_attr: failed to copy unknown attribute\n");
550 #endif
551                         return FATAL_ERROR;
552                 }
553                 tmp_unknown = tmp_unknown->next;
554                 ++counter;
555         }
556         
557         attr.len = htons(res->msg.buf.len - orig_len);
558         memcpy(&res->msg.buf.s[orig_len], (void *)&attr, sizeof(struct stun_attr));
559         
560         /* check if there is an odd number of unknown attributes and if yes, 
561          * repeat one of them because of padding to 32
562          */
563         if (counter/2 != 0 && unknown != NULL) {
564                 if (buf_copy(&res->msg, (void *)&unknown->type, sizeof(USHORT_T))!=0) {
565 #ifdef EXTRA_DEBUG
566                         LOG(L_DBG, "DEBUG: add_unknown_attr: failed to padd\n");
567 #endif
568                         return FATAL_ERROR;
569                 }
570         }       
571         
572         return 0;
573 }
574
575 /*
576  * add_error_code()
577  *                      - res: response representation
578  *                      - error_code: value of error type
579  * 
580  * The function add_error_code ensures copy of link list of unknown 
581  * attributes into response buffer.
582  * 
583  * Return value:        0       if there is no environment error
584  *                                      -1      if there is some enviroment error such as insufficiency
585  *                                              of memory
586  */
587 static int add_error_code(struct stun_msg* res, USHORT_T error_code)
588 {
589         struct stun_attr attr;
590         USHORT_T        orig_len;
591         USHORT_T        two_bytes;
592         int                     text_pad;
593         char            err[2];
594         
595         orig_len = res->msg.buf.len;
596         text_pad = 0;
597         
598         /* the type and length will be copy as last one because of unknown length*/
599         if (res->msg.buf.len < sizeof(struct stun_attr)) {
600                 if (reallock_buffer(&res->msg, sizeof(struct stun_attr)) != 0) {
601 #ifdef EXTRA_DEBUG
602                         LOG(L_DBG, "DEBUG: add_error_code: failed to reallocate buffer\n");
603 #endif
604                         return FATAL_ERROR;
605                 }
606         }
607         res->msg.buf.len += sizeof(struct stun_attr);
608         res->msg.empty -= sizeof(struct stun_attr);
609         
610         /* first two bytes are empty */
611         two_bytes = 0x0000;
612         
613         if (buf_copy(&res->msg, (void *) &two_bytes, sizeof(USHORT_T)) != 0) {
614 #ifdef EXTRA_DEBUG
615                 LOG(L_DBG, "DEBUG: add_error_code: failed to copy buffer\n");
616 #endif
617                 return FATAL_ERROR;
618         }
619         
620         err[0] = error_code / 100;
621         err[1] = error_code % 100;
622         if (buf_copy(&res->msg, (void *) err, sizeof(UCHAR_T)*2) != 0) {
623                 return FATAL_ERROR;
624         }
625         
626         switch (error_code) {
627                 case TRY_ALTERNATE_ERR:
628                         text_pad = copy_str_to_buffer(res, TRY_ALTERNATE_TXT, PAD4); 
629                         break;
630                 case BAD_REQUEST_ERR:
631                         text_pad = copy_str_to_buffer(res, BAD_REQUEST_TXT, PAD4); 
632                         break;
633                 case UNAUTHORIZED_ERR:
634                         text_pad = copy_str_to_buffer(res, UNAUTHORIZED_TXT, PAD4); 
635                         break;
636                 case UNKNOWN_ATTRIBUTE_ERR:
637                         text_pad = copy_str_to_buffer(res, UNKNOWN_ATTRIBUTE_TXT, PAD4);
638                         break;
639                 case STALE_CREDENTIALS_ERR:
640                         text_pad = copy_str_to_buffer(res, STALE_CREDENTIALS_TXT, PAD4); 
641                         break;
642                 case INTEGRITY_CHECK_ERR:
643                         text_pad = copy_str_to_buffer(res, INTEGRITY_CHECK_TXT, PAD4); 
644                         break;
645                 case MISSING_USERNAME_ERR:
646                         text_pad = copy_str_to_buffer(res, MISSING_USERNAME_TXT, PAD4); 
647                         break;
648                 case USE_TLS_ERR:
649                         text_pad = copy_str_to_buffer(res, USE_TLS_TXT, PAD4); 
650                         break;
651                 case MISSING_REALM_ERR:
652                         text_pad = copy_str_to_buffer(res, MISSING_REALM_TXT, PAD4); 
653                         break;
654                 case MISSING_NONCE_ERR:
655                         text_pad = copy_str_to_buffer(res, MISSING_NONCE_TXT, PAD4); 
656                         break;
657                 case UNKNOWN_USERNAME_ERR:
658                         text_pad = copy_str_to_buffer(res, UNKNOWN_USERNAME_TXT, PAD4); 
659                         break;
660                 case STALE_NONCE_ERR:
661                         text_pad = copy_str_to_buffer(res, STALE_NONCE_TXT, PAD4);
662                         break;
663                 case SERVER_ERROR_ERR:
664                         text_pad = copy_str_to_buffer(res, SERVER_ERROR_TXT, PAD4); 
665                         break;
666                 case GLOBAL_FAILURE_ERR:
667                         text_pad = copy_str_to_buffer(res, GLOBAL_FAILURE_TXT, PAD4); 
668                         break;
669                 default:
670                         LOG(L_ERR, "ERROR: STUN: Unknown error code.\n");
671                         break;
672         }
673         if (text_pad < 0) {
674 #ifdef EXTRA_DEBUG
675                 LOG(L_DBG, "DEBUG: add_error_code: text_pad is negative\n");
676 #endif
677                 goto error;
678         }
679         attr.type = htons(ERROR_CODE_ATTR);
680         /* count length of "value" field -> without type and lehgth field */
681         attr.len = htons(res->msg.buf.len - orig_len - 
682                                          text_pad - sizeof(struct stun_attr));
683         memcpy(&res->msg.buf.s[orig_len], (void *)&attr, sizeof(struct stun_attr));
684         
685         return 0;
686
687 error:
688         return FATAL_ERROR;
689 }
690
691 /*
692  * copy_str_to_buffer()
693  *                      - res: response representation
694  *                      - data: text data, in our case almost text representation of error
695  *                      - pad: the size of pad (for how much bytes the string should be 
696  *                                 padded
697  * 
698  * The function copy_str_to_buffer ensures copy of text buffer into response
699  * buffer.
700  * 
701  * Return value:        0       if there is no environment error
702  *                                      -1      if there is some enviroment error such as insufficiency
703  *                                              of memory
704  */
705 static int copy_str_to_buffer(struct stun_msg* res, const char* data, UINT_T pad)
706 {
707         USHORT_T        pad_len;
708         UINT_T          data_len;
709         UCHAR_T         empty[pad];
710         
711         data_len = strlen(data);
712         memset(&empty, 0, pad);
713         
714         pad_len = (pad - data_len%pad) % pad;
715         
716         if (buf_copy(&res->msg, (void *) data, sizeof(UCHAR_T)*data_len) != 0) {
717 #ifdef EXTRA_DEBUG
718                 LOG(L_DBG, "DEBUG: copy_str_to_buffer: failed to copy buffer\n");
719 #endif
720                 return FATAL_ERROR;
721         }
722         
723         if (pad_len != 0) {
724                 if (buf_copy(&res->msg, &empty, pad_len) != 0) {
725 #ifdef EXTRA_DEBUG
726                         LOG(L_DBG, "DEBUG: copy_str_to_buffer: failed to pad\n");
727 #endif
728                         return FATAL_ERROR;
729                 }       
730         }
731
732         return pad_len; 
733 }
734
735 /*
736  * stun_add_address_attr()
737  *                      - res: response representation
738  *                      - af: address family
739  *                      - port: port
740  *                      - ip_addr: represent both IPv4 and IPv6, the differences is in 
741  *                      length  
742  *                      - type: type of attribute
743  *                      - do_xor: if the port should be XOR-ed or not.
744  * 
745  * The function stun_add_address_attr ensures copy of any IP attribute into
746  * response buffer.
747  * 
748  * Return value:        0       if there is no environment error
749  *                                      -1      if there is some enviroment error such as insufficiency
750  *                                              of memory
751  */
752 static int stun_add_address_attr(struct stun_msg* res, 
753                                                 UINT_T          af,
754                                                 USHORT_T        port,
755                                                 UINT_T*         ip_addr,
756                                                 USHORT_T        type,
757                                                 int do_xor)
758 {
759         struct stun_attr attr;
760         int              ip_struct_len;
761         UINT_T   id[IP_ADDR];
762         int i;
763         
764         ip_struct_len = 0;
765         attr.type = htons(type);
766         res->ip_addr.port = htons((do_xor) ? (port ^ MAGIC_COOKIE_2B) : port);
767         switch(af) {
768                 case AF_INET:
769                         ip_struct_len = sizeof(struct stun_ip_addr) - 3*sizeof(UINT_T);
770                         res->ip_addr.family = htons(IPV4_FAMILY);
771                         memcpy(res->ip_addr.ip, ip_addr, IPV4_LEN);
772                         res->ip_addr.ip[0] = (do_xor) ? 
773                                 res->ip_addr.ip[0] ^ htonl(MAGIC_COOKIE) : res->ip_addr.ip[0];
774                         break;
775                 case AF_INET6:
776                         ip_struct_len = sizeof(struct stun_ip_addr);
777                         res->ip_addr.family = htons(IPV6_FAMILY);
778                         memcpy(&res->ip_addr.ip, ip_addr, IPV6_LEN);
779                         memcpy(id, &res->hdr.id, sizeof(struct transaction_id));
780                         for (i=0; i<IP_ADDR; i++) {
781                                 res->ip_addr.ip[i] = (do_xor) ? 
782                                                         res->ip_addr.ip[i] ^ id[i] : res->ip_addr.ip[i];
783                         }
784                         break;
785                 default:
786                         break;
787         }
788         
789         attr.len = htons(ip_struct_len);
790         
791         /* copy type and attribute's length */ 
792         if (buf_copy(&res->msg, (void *) &attr, sizeof(struct stun_attr)) != 0) {
793                 return FATAL_ERROR;
794         }
795         
796         /* copy family, port and IP */
797         if (buf_copy(&res->msg, (void *) &res->ip_addr, ip_struct_len) != 0) {
798                 return FATAL_ERROR;
799         }
800
801         return 0;
802 }
803
804 /*
805  * stun_alloc_unknown_attr()
806  *                      - type: type of unknown attribute
807  * 
808  * The function stun_alloc_unknown_attr ensures allocationg new element for
809  * the link list of unknown attributes.
810  * 
811  * Return value: pointer to new element of link list in positive case
812  *                               NULL if there is some enviroment error such as insufficiency
813  *                                              of memory
814  */
815 static struct stun_unknown_att* stun_alloc_unknown_attr(USHORT_T type)
816 {
817         struct stun_unknown_att* attr;
818
819         attr = (struct stun_unknown_att *) pkg_malloc(sizeof(struct stun_unknown_att));
820         if (attr == NULL) {
821                 LOG(L_ERR, "ERROR: STUN: out of memory\n");
822                 return NULL;
823         }
824         
825         attr->type = type;
826         attr->next = NULL;
827         
828         return attr;
829 }
830
831 /*
832  * stun_delete_unknown_attrs()
833  *                      - unknown: link list of unknown attributes
834  * 
835  * The function stun_delete_unknown_attrs ensures deleting of link list
836  * 
837  * Return value: none
838  */
839 static void stun_delete_unknown_attrs(struct stun_unknown_att* unknown)
840 {
841         struct stun_unknown_att* tmp_unknown;
842         
843         if (unknown == NULL) {
844                 return;
845         }
846         
847         while(unknown->next) {
848                 tmp_unknown = unknown->next;
849                 unknown->next = tmp_unknown->next;
850                 pkg_free(tmp_unknown);          
851         }
852         pkg_free(unknown);
853 }
854
855 /*
856  * buf_copy()
857  *                      - msg: buffer where the data will be copy to
858  *                      - source: source data buffer
859  *                      - len: number of bytes that should be copied
860  * 
861  * The function buf_copy copies "len" bytes from source into msg buffer
862  * 
863  * Return value:        0       if there is no environment error
864  *                                      -1      if there is some enviroment error such as insufficiency
865  *                                              of memory
866  */
867 static int buf_copy(struct stun_buffer* msg, void* source, UINT_T len)
868 {
869         if (msg->empty < len) {
870                 if (reallock_buffer(msg, len) != 0) {
871                         return FATAL_ERROR;
872                 }
873         }
874         
875         memcpy(&msg->buf.s[msg->buf.len], source, len);
876         msg->buf.len += len;
877         msg->empty -= len;
878         
879         return 0;
880 }
881
882 /*
883  * reallock_buffer()
884  *                      - buffer: original buffer
885  *                      - len: represents minimum of bytes that must be available after 
886  *                                      reallocation
887  * 
888  * The function reallock_buffer reallocks buffer. New buffer's length will be 
889  * original length plus bigger from len and STUN_MSG_LEN constant.
890  * 
891  * Return value:        0       if there is no environment error
892  *                                      -1      if there is some enviroment error such as insufficiency
893  *                                              of memory
894  */
895 static int reallock_buffer(struct stun_buffer* buffer, UINT_T len)
896 {
897         char*   tmp_buf;
898         UINT_T  new_len;
899         
900         new_len = (STUN_MSG_LEN < len) ? STUN_MSG_LEN+len : STUN_MSG_LEN;
901         
902         tmp_buf = (char *) pkg_realloc(buffer->buf.s, 
903                                                                    buffer->buf.len + buffer->empty + new_len);
904         if (tmp_buf == 0) {
905                 LOG(L_ERR, "ERROR: STUN: out of memory\n");
906                 return FATAL_ERROR;
907         }
908         
909         buffer->buf.s = tmp_buf;
910         buffer->empty += new_len;
911
912         return 0;
913 }
914
915 /*
916  * clean_memory()
917  *                      - res: structure representing response message
918  *                      - unknown: link list of unknown attributes
919  * 
920  * The function clean_memory should free dynamic allocated memory.
921  * 
922  * Return value: none
923  */
924 static void clean_memory(struct stun_msg* req,
925                                 struct stun_msg* res,   struct stun_unknown_att* unknown)
926 {
927 #ifdef DYN_BUF
928         pkg_free(req->msg.buf.s);
929 #endif
930
931         if (res->msg.buf.s != NULL) {
932                 pkg_free(res->msg.buf.s);
933         }
934         stun_delete_unknown_attrs(unknown);
935 }
936
937 /*
938  * stun_add_common_text_attr()
939  *                      - res: structure representing response
940  *                      - type: type of attribute
941  *                      - value: attribute's value
942  *                      - pad: size of pad
943  * 
944  * The function stun_add_common_text_attr copy attribute with string value 
945  * into response buffer.
946  * 
947  * Return value:        0       if there is no environment error
948  *                                      -1      if there is some enviroment error such as insufficiency
949  *                                              of memory
950  */
951 static int stun_add_common_text_attr(struct stun_msg* res, 
952                                                           USHORT_T type, 
953                                                           char* value, 
954                                                           USHORT_T pad)
955 {
956         struct stun_attr attr;
957         
958         if (value == NULL) {
959                 LOG(L_INFO, "INFO: stun_add_common_text_attr: value is NULL\n");
960                 return 0;
961         }
962         
963         attr.type = htons(type);
964         attr.len = htons(strlen(value));
965         
966         if (buf_copy(&res->msg, (void *) &attr, sizeof(struct stun_attr)) != 0) {
967                 return FATAL_ERROR;
968         }
969         
970         if (copy_str_to_buffer(res, value, pad) < 0) {
971                 return FATAL_ERROR;
972         }
973         
974         return 0;
975         
976 }