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