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