1 //#define UDPTL_DEBUG
2 /*
3 * SpanDSP - a series of DSP components for telephony
4 *
5 * udptl.c - An implementation of the UDPTL protocol defined in T.38,
6 * less the packet exchange part
7 *
8 * Written by Steve Underwood <steveu@coppice.org>
9 *
10 * Copyright (C) 2005, 2009, 2012 Steve Underwood
11 *
12 * All rights reserved.
13 *
14 * This program is free software; you can redistribute it and/or modify
15 * it under the terms of the GNU General Public License version 2, as
16 * published by the Free Software Foundation.
17 *
18 * This program is distributed in the hope that it will be useful,
19 * but WITHOUT ANY WARRANTY; without even the implied warranty of
20 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21 * GNU General Public License for more details.
22 *
23 * You should have received a copy of the GNU General Public License
24 * along with this program; if not, write to the Free Software
25 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
26 */
27
28 #if defined(HAVE_CONFIG_H)
29 #include "config.h"
30 #endif
31
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <sys/types.h>
35 #include <inttypes.h>
36 #include <memory.h>
37 #if defined(HAVE_STDBOOL_H)
38 #include <stdbool.h>
39 #else
40 #include <spandsp/stdbool.h>
41 #endif
42
43 #include "udptl.h"
44
decode_length(const uint8_t * buf,int limit,int * len,int * pvalue)45 static int decode_length(const uint8_t *buf, int limit, int *len, int *pvalue)
46 {
47 if (*len >= limit)
48 return -1;
49 if ((buf[*len] & 0x80) == 0)
50 {
51 *pvalue = buf[(*len)++];
52 return 0;
53 }
54 if ((buf[*len] & 0x40) == 0)
55 {
56 if (*len >= limit - 1)
57 return -1;
58 *pvalue = (buf[(*len)++] & 0x3F) << 8;
59 *pvalue |= buf[(*len)++];
60 return 0;
61 }
62 *pvalue = (buf[(*len)++] & 0x3F) << 14;
63 /* Indicate that we have a fragment */
64 return 1;
65 }
66 /*- End of function --------------------------------------------------------*/
67
decode_open_type(const uint8_t * buf,int limit,int * len,const uint8_t ** p_object,int * p_num_octets)68 static int decode_open_type(const uint8_t *buf, int limit, int *len, const uint8_t **p_object, int *p_num_octets)
69 {
70 int octet_cnt;
71 #if 0
72 int octet_idx;
73 int stat;
74 const uint8_t **pbuf;
75
76 *p_num_octets = 0;
77 for (octet_idx = 0; ; octet_idx += octet_cnt)
78 {
79 if ((stat = decode_length(buf, limit, len, &octet_cnt)) < 0)
80 return -1;
81 if (octet_cnt > 0)
82 {
83 *p_num_octets += octet_cnt;
84
85 pbuf = &p_object[octet_idx];
86 /* Make sure the buffer contains at least the number of bits requested */
87 if ((*len + octet_cnt) > limit)
88 return -1;
89
90 *pbuf = &buf[*len];
91 *len += octet_cnt;
92 }
93 if (stat == 0)
94 break;
95 }
96 #else
97 /* We do not deal with fragments, so there is no point in looping through them. Just say that something
98 fragmented is bad. */
99 if (decode_length(buf, limit, len, &octet_cnt) != 0)
100 return -1;
101 *p_num_octets = octet_cnt;
102 if (octet_cnt > 0)
103 {
104 /* Make sure the buffer contains at least the number of bits requested */
105 if ((*len + octet_cnt) > limit)
106 return -1;
107 *p_object = &buf[*len];
108 *len += octet_cnt;
109 }
110 #endif
111 return 0;
112 }
113 /*- End of function --------------------------------------------------------*/
114
encode_length(uint8_t * buf,int * len,int value)115 static int encode_length(uint8_t *buf, int *len, int value)
116 {
117 int multiplier;
118
119 if (value < 0x80)
120 {
121 /* 1 octet */
122 buf[(*len)++] = value;
123 return value;
124 }
125 if (value < 0x4000)
126 {
127 /* 2 octets */
128 /* Set the first bit of the first octet */
129 buf[(*len)++] = ((0x8000 | value) >> 8) & 0xFF;
130 buf[(*len)++] = value & 0xFF;
131 return value;
132 }
133 /* Fragmentation */
134 multiplier = (value < 0x10000) ? (value >> 14) : 4;
135 /* Set the first 2 bits of the octet */
136 buf[(*len)++] = 0xC0 | multiplier;
137 return multiplier << 14;
138 }
139 /*- End of function --------------------------------------------------------*/
140
encode_open_type(uint8_t * buf,int * len,const uint8_t * data,int num_octets)141 static int encode_open_type(uint8_t *buf, int *len, const uint8_t *data, int num_octets)
142 {
143 int enclen;
144 int octet_idx;
145 uint8_t zero_byte;
146
147 /* If open type is of zero length, add a single zero byte (10.1) */
148 if (num_octets == 0)
149 {
150 zero_byte = 0;
151 data = &zero_byte;
152 num_octets = 1;
153 }
154 /* Encode the open type */
155 for (octet_idx = 0; ; num_octets -= enclen, octet_idx += enclen)
156 {
157 if ((enclen = encode_length(buf, len, num_octets)) < 0)
158 return -1;
159 if (enclen > 0)
160 {
161 memcpy(&buf[*len], &data[octet_idx], enclen);
162 *len += enclen;
163 }
164 if (enclen >= num_octets)
165 break;
166 }
167
168 return 0;
169 }
170 /*- End of function --------------------------------------------------------*/
171
udptl_rx_packet(udptl_state_t * s,const uint8_t buf[],int len)172 int udptl_rx_packet(udptl_state_t *s, const uint8_t buf[], int len)
173 {
174 int stat;
175 int i;
176 int j;
177 int k;
178 int l;
179 int m;
180 int x;
181 int limit;
182 int which;
183 int ptr;
184 int count;
185 int total_count;
186 int seq_no;
187 const uint8_t *msg;
188 const uint8_t *data;
189 int msg_len;
190 int repaired[16];
191 const uint8_t *bufs[16];
192 int lengths[16];
193 int span;
194 int entries;
195
196 ptr = 0;
197 /* Decode seq_number */
198 if (ptr + 2 > len)
199 return -1;
200 seq_no = (buf[0] << 8) | buf[1];
201 ptr += 2;
202 /* Break out the primary packet */
203 if ((stat = decode_open_type(buf, len, &ptr, &msg, &msg_len)) != 0)
204 return -1;
205 /* Decode error_recovery */
206 if (ptr + 1 > len)
207 return -1;
208 /* Our buffers cannot tolerate overlength packets */
209 if (msg_len > LOCAL_FAX_MAX_DATAGRAM)
210 return -1;
211 /* Update any missed slots in the buffer */
212 for (i = s->rx_seq_no; seq_no > i; i++)
213 {
214 x = i & UDPTL_BUF_MASK;
215 s->rx[x].buf_len = -1;
216 s->rx[x].fec_len[0] = 0;
217 s->rx[x].fec_span = 0;
218 s->rx[x].fec_entries = 0;
219 }
220 /* Save the new packet. Pure redundancy mode won't use this, but some systems will switch
221 into FEC mode after sending some redundant packets. */
222 x = seq_no & UDPTL_BUF_MASK;
223 if (msg_len > 0)
224 memcpy(s->rx[x].buf, msg, msg_len);
225 s->rx[x].buf_len = msg_len;
226 s->rx[x].fec_len[0] = 0;
227 s->rx[x].fec_span = 0;
228 s->rx[x].fec_entries = 0;
229 if ((buf[ptr++] & 0x80) == 0)
230 {
231 /* Secondary packet mode for error recovery */
232 /* We might have the packet we want, but we need to check through
233 the redundant stuff, and verify the integrity of the UDPTL.
234 This greatly reduces our chances of accepting garbage. */
235 total_count = 0;
236 do
237 {
238 if ((stat = decode_length(buf, len, &ptr, &count)) < 0)
239 return -1;
240 if ((total_count + count) >= 16)
241 {
242 /* There is too much stuff here to be real, and it would overflow the bufs array
243 if we continue */
244 return -1;
245 }
246 for (i = 0; i < count; i++)
247 {
248 if (decode_open_type(buf, len, &ptr, &bufs[total_count + i], &lengths[total_count + i]) != 0)
249 return -1;
250 }
251 total_count += count;
252 }
253 while (stat > 0);
254 /* We should now be exactly at the end of the packet. If not, this is a fault. */
255 if (ptr != len)
256 return -1;
257 if (seq_no > s->rx_seq_no)
258 {
259 /* We received a later packet than we expected, so we need to check if we can fill in the gap from the
260 secondary packets. */
261 /* Step through in reverse order, so we go oldest to newest */
262 for (i = total_count; i > 0; i--)
263 {
264 if (seq_no - i >= s->rx_seq_no)
265 {
266 /* This one wasn't seen before */
267 /* Process the secondary packet */
268 #if defined(UDPTL_DEBUG)
269 fprintf(stderr, "Secondary %d, len %d\n", seq_no - i, lengths[i - 1]);
270 #endif
271 /* Save the new packet. Redundancy mode won't use this, but some systems will switch into
272 FEC mode after sending some redundant packets, and this may then be important. */
273 x = (seq_no - i) & UDPTL_BUF_MASK;
274 if (lengths[i - 1] > 0)
275 memcpy(s->rx[x].buf, bufs[i - 1], lengths[i - 1]);
276 s->rx[x].buf_len = lengths[i - 1];
277 s->rx[x].fec_len[0] = 0;
278 s->rx[x].fec_span = 0;
279 s->rx[x].fec_entries = 0;
280 if (s->rx_packet_handler(s->user_data, bufs[i - 1], lengths[i - 1], seq_no - i) < 0)
281 fprintf(stderr, "Bad IFP\n");
282 }
283 }
284 }
285 }
286 else
287 {
288 /* FEC mode for error recovery */
289
290 /* Decode the FEC packets */
291 /* The span is defined as an unconstrained integer, but will never be more
292 than a small value. */
293 if (ptr + 2 > len)
294 return -1;
295 if (buf[ptr++] != 1)
296 return -1;
297 span = buf[ptr++];
298
299 x = seq_no & UDPTL_BUF_MASK;
300
301 s->rx[x].fec_span = span;
302
303 memset(repaired, 0, sizeof(repaired));
304 repaired[x] = true;
305
306 /* The number of entries is defined as a length, but will only ever be a small
307 value. Treat it as such. */
308 if (ptr + 1 > len)
309 return -1;
310 entries = buf[ptr++];
311 s->rx[x].fec_entries = entries;
312
313 /* Decode the elements */
314 for (i = 0; i < entries; i++)
315 {
316 if ((stat = decode_open_type(buf, len, &ptr, &data, &s->rx[x].fec_len[i])) != 0)
317 return -1;
318 if (s->rx[x].fec_len[i] > LOCAL_FAX_MAX_DATAGRAM)
319 return -1;
320
321 /* Save the new FEC data */
322 if (s->rx[x].fec_len[i])
323 memcpy(s->rx[x].fec[i], data, s->rx[x].fec_len[i]);
324 #if 0
325 fprintf(stderr, "FEC: ");
326 for (j = 0; j < s->rx[x].fec_len[i]; j++)
327 fprintf(stderr, "%02X ", data[j]);
328 fprintf(stderr, "\n");
329 #endif
330 }
331 /* We should now be exactly at the end of the packet. If not, this is a fault. */
332 if (ptr != len)
333 return -1;
334 /* See if we can reconstruct anything which is missing */
335 /* TODO: this does not comprehensively hunt back and repair everything that is possible */
336 for (l = x; l != ((x - (16 - span*entries)) & UDPTL_BUF_MASK); l = (l - 1) & UDPTL_BUF_MASK)
337 {
338 if (s->rx[l].fec_len[0] <= 0)
339 continue;
340 for (m = 0; m < s->rx[l].fec_entries; m++)
341 {
342 limit = (l + m) & UDPTL_BUF_MASK;
343 for (which = -1, k = (limit - s->rx[l].fec_span*s->rx[l].fec_entries) & UDPTL_BUF_MASK;
344 k != limit;
345 k = (k + s->rx[l].fec_entries) & UDPTL_BUF_MASK)
346 {
347 if (s->rx[k].buf_len <= 0)
348 which = (which == -1) ? k : -2;
349 }
350 if (which >= 0)
351 {
352 /* Repairable */
353 for (j = 0; j < s->rx[l].fec_len[m]; j++)
354 {
355 s->rx[which].buf[j] = s->rx[l].fec[m][j];
356 for (k = (limit - s->rx[l].fec_span*s->rx[l].fec_entries) & UDPTL_BUF_MASK;
357 k != limit;
358 k = (k + s->rx[l].fec_entries) & UDPTL_BUF_MASK)
359 {
360 s->rx[which].buf[j] ^= (s->rx[k].buf_len > j) ? s->rx[k].buf[j] : 0;
361 }
362 }
363 s->rx[which].buf_len = s->rx[l].fec_len[m];
364 repaired[which] = true;
365 }
366 }
367 }
368 /* Now play any new packets forwards in time */
369 for (l = (x + 1) & UDPTL_BUF_MASK, j = seq_no - UDPTL_BUF_MASK; l != x; l = (l + 1) & UDPTL_BUF_MASK, j++)
370 {
371 if (repaired[l])
372 {
373 #if defined(UDPTL_DEBUG)
374 fprintf(stderr, "Fixed packet %d, len %d\n", j, l);
375 #endif
376 if (s->rx_packet_handler(s->user_data, s->rx[l].buf, s->rx[l].buf_len, j) < 0)
377 fprintf(stderr, "Bad IFP\n");
378 }
379 }
380 }
381 /* If packets are received out of sequence, we may have already processed this packet
382 from the error recovery information in a packet already received. */
383 if (seq_no >= s->rx_seq_no)
384 {
385 /* Decode the primary packet */
386 #if defined(UDPTL_DEBUG)
387 fprintf(stderr, "Primary packet %d, len %d\n", seq_no, msg_len);
388 #endif
389 if (s->rx_packet_handler(s->user_data, msg, msg_len, seq_no) < 0)
390 fprintf(stderr, "Bad IFP\n");
391 }
392
393 s->rx_seq_no = (seq_no + 1) & 0xFFFF;
394 return 0;
395 }
396 /*- End of function --------------------------------------------------------*/
397
udptl_build_packet(udptl_state_t * s,uint8_t buf[],const uint8_t msg[],int msg_len)398 int udptl_build_packet(udptl_state_t *s, uint8_t buf[], const uint8_t msg[], int msg_len)
399 {
400 uint8_t fec[LOCAL_FAX_MAX_DATAGRAM];
401 int i;
402 int j;
403 int seq;
404 int entry;
405 int entries;
406 int span;
407 int m;
408 int len;
409 int limit;
410 int high_tide;
411 int len_before_entries;
412 int previous_len;
413
414 /* UDPTL cannot cope with zero length messages, and our buffering for redundancy limits their
415 maximum length. */
416 if (msg_len < 1 || msg_len > LOCAL_FAX_MAX_DATAGRAM)
417 return -1;
418 seq = s->tx_seq_no & 0xFFFF;
419
420 /* Map the sequence number to an entry in the circular buffer */
421 entry = seq & UDPTL_BUF_MASK;
422
423 /* We save the message in a circular buffer, for generating FEC or
424 redundancy sets later on. */
425 s->tx[entry].buf_len = msg_len;
426 memcpy(s->tx[entry].buf, msg, msg_len);
427
428 /* Build the UDPTL packet */
429
430 len = 0;
431 /* Encode the sequence number */
432 buf[len++] = (seq >> 8) & 0xFF;
433 buf[len++] = seq & 0xFF;
434
435 /* Encode the primary packet */
436 if (encode_open_type(buf, &len, msg, msg_len) < 0)
437 return -1;
438
439 /* Encode the appropriate type of error recovery information */
440 switch (s->error_correction_scheme)
441 {
442 case UDPTL_ERROR_CORRECTION_NONE:
443 /* Encode the error recovery type */
444 buf[len++] = 0x00;
445 /* The number of entries will always be zero, so it is pointless allowing
446 for the fragmented case here. */
447 if (encode_length(buf, &len, 0) < 0)
448 return -1;
449 break;
450 case UDPTL_ERROR_CORRECTION_REDUNDANCY:
451 /* Encode the error recovery type */
452 buf[len++] = 0x00;
453 if (s->tx_seq_no > s->error_correction_entries)
454 entries = s->error_correction_entries;
455 else
456 entries = s->tx_seq_no;
457 len_before_entries = len;
458 /* The number of entries will always be small, so it is pointless allowing
459 for the fragmented case here. */
460 if (encode_length(buf, &len, entries) < 0)
461 return -1;
462 /* Encode the elements */
463 for (m = 0; m < entries; m++)
464 {
465 previous_len = len;
466 j = (entry - m - 1) & UDPTL_BUF_MASK;
467 if (encode_open_type(buf, &len, s->tx[j].buf, s->tx[j].buf_len) < 0)
468 return -1;
469
470 /* If we have exceeded the far end's max datagram size, don't include this last chunk,
471 and stop trying to add more. */
472 if (len > s->far_max_datagram_size)
473 {
474 len = previous_len;
475 if (encode_length(buf, &len_before_entries, m) < 0)
476 return -1;
477 break;
478 }
479 }
480 break;
481 case UDPTL_ERROR_CORRECTION_FEC:
482 span = s->error_correction_span;
483 entries = s->error_correction_entries;
484 if (seq < s->error_correction_span*s->error_correction_entries)
485 {
486 /* In the initial stages, wind up the FEC smoothly */
487 entries = seq/s->error_correction_span;
488 if (seq < s->error_correction_span)
489 span = 0;
490 }
491 /* Encode the error recovery type */
492 buf[len++] = 0x80;
493 /* Span is defined as an inconstrained integer, which it dumb. It will only
494 ever be a small value. Treat it as such. */
495 buf[len++] = 1;
496 buf[len++] = span;
497 /* The number of entries is defined as a length, but will only ever be a small
498 value. Treat it as such. */
499 len_before_entries = len;
500 buf[len++] = entries;
501 for (m = 0; m < entries; m++)
502 {
503 previous_len = len;
504 /* Make an XOR'ed entry the maximum length */
505 limit = (entry + m) & UDPTL_BUF_MASK;
506 high_tide = 0;
507 for (i = (limit - span*entries) & UDPTL_BUF_MASK; i != limit; i = (i + entries) & UDPTL_BUF_MASK)
508 {
509 if (high_tide < s->tx[i].buf_len)
510 {
511 for (j = 0; j < high_tide; j++)
512 fec[j] ^= s->tx[i].buf[j];
513 for ( ; j < s->tx[i].buf_len; j++)
514 fec[j] = s->tx[i].buf[j];
515 high_tide = s->tx[i].buf_len;
516 }
517 else
518 {
519 for (j = 0; j < s->tx[i].buf_len; j++)
520 fec[j] ^= s->tx[i].buf[j];
521 }
522 }
523 if (encode_open_type(buf, &len, fec, high_tide) < 0)
524 return -1;
525
526 /* If we have exceeded the far end's max datagram size, don't include this last chunk,
527 and stop trying to add more. */
528 if (len > s->far_max_datagram_size)
529 {
530 len = previous_len;
531 buf[len_before_entries] = (uint8_t) m;
532 break;
533 }
534 }
535 break;
536 }
537
538 if (s->verbose)
539 fprintf(stderr, "\n");
540 s->tx_seq_no++;
541 return len;
542 }
543 /*- End of function --------------------------------------------------------*/
544
udptl_set_error_correction(udptl_state_t * s,int ec_scheme,int span,int entries)545 int udptl_set_error_correction(udptl_state_t *s,
546 int ec_scheme,
547 int span,
548 int entries)
549 {
550 switch (ec_scheme)
551 {
552 case UDPTL_ERROR_CORRECTION_FEC:
553 case UDPTL_ERROR_CORRECTION_REDUNDANCY:
554 case UDPTL_ERROR_CORRECTION_NONE:
555 s->error_correction_scheme = ec_scheme;
556 break;
557 case -1:
558 /* Just don't change the scheme */
559 break;
560 default:
561 return -1;
562 }
563 if (span >= 0)
564 s->error_correction_span = span;
565 if (entries >= 0)
566 s->error_correction_entries = entries;
567 return 0;
568 }
569 /*- End of function --------------------------------------------------------*/
570
udptl_get_error_correction(udptl_state_t * s,int * ec_scheme,int * span,int * entries)571 int udptl_get_error_correction(udptl_state_t *s, int *ec_scheme, int *span, int *entries)
572 {
573 if (ec_scheme)
574 *ec_scheme = s->error_correction_scheme;
575 if (span)
576 *span = s->error_correction_span;
577 if (entries)
578 *entries = s->error_correction_entries;
579 return 0;
580 }
581 /*- End of function --------------------------------------------------------*/
582
udptl_set_local_max_datagram(udptl_state_t * s,int max_datagram)583 int udptl_set_local_max_datagram(udptl_state_t *s, int max_datagram)
584 {
585 s->local_max_datagram_size = max_datagram;
586 return 0;
587 }
588 /*- End of function --------------------------------------------------------*/
589
udptl_get_local_max_datagram(udptl_state_t * s)590 int udptl_get_local_max_datagram(udptl_state_t *s)
591 {
592 return s->local_max_datagram_size;
593 }
594 /*- End of function --------------------------------------------------------*/
595
udptl_set_far_max_datagram(udptl_state_t * s,int max_datagram)596 int udptl_set_far_max_datagram(udptl_state_t *s, int max_datagram)
597 {
598 s->far_max_datagram_size = max_datagram;
599 return 0;
600 }
601 /*- End of function --------------------------------------------------------*/
602
udptl_get_far_max_datagram(udptl_state_t * s)603 int udptl_get_far_max_datagram(udptl_state_t *s)
604 {
605 return s->far_max_datagram_size;
606 }
607 /*- End of function --------------------------------------------------------*/
608
udptl_init(udptl_state_t * s,int ec_scheme,int span,int entries,udptl_rx_packet_handler_t rx_packet_handler,void * user_data)609 udptl_state_t *udptl_init(udptl_state_t *s,
610 int ec_scheme,
611 int span,
612 int entries,
613 udptl_rx_packet_handler_t rx_packet_handler,
614 void *user_data)
615 {
616 int i;
617
618 if (rx_packet_handler == NULL)
619 return NULL;
620
621 if (s == NULL)
622 {
623 if ((s = (udptl_state_t *) malloc(sizeof(*s))) == NULL)
624 return NULL;
625 }
626 memset(s, 0, sizeof(*s));
627
628 s->error_correction_scheme = ec_scheme;
629 s->error_correction_span = span;
630 s->error_correction_entries = entries;
631
632 s->far_max_datagram_size = LOCAL_FAX_MAX_DATAGRAM;
633 s->local_max_datagram_size = LOCAL_FAX_MAX_DATAGRAM;
634
635 memset(&s->rx, 0, sizeof(s->rx));
636 memset(&s->tx, 0, sizeof(s->tx));
637 for (i = 0; i <= UDPTL_BUF_MASK; i++)
638 {
639 s->rx[i].buf_len = -1;
640 s->tx[i].buf_len = -1;
641 }
642
643 s->rx_packet_handler = rx_packet_handler;
644 s->user_data = user_data;
645
646 return s;
647 }
648 /*- End of function --------------------------------------------------------*/
649
udptl_release(udptl_state_t * s)650 int udptl_release(udptl_state_t *s)
651 {
652 return 0;
653 }
654 /*- End of function --------------------------------------------------------*/
655 /*- End of file ------------------------------------------------------------*/
656