Fix CNG issues in G.723.1
[asterisk/asterisk.git] / rtp.c
1 /*
2  * Asterisk -- A telephony toolkit for Linux.
3  *
4  * Real-time Protocol Support
5  * 
6  * Copyright (C) 1999, Mark Spencer
7  *
8  * Mark Spencer <markster@linux-support.net>
9  *
10  * This program is free software, distributed under the terms of
11  * the GNU General Public License
12  */
13
14 #include <stdio.h>
15 #include <stdlib.h>
16 #include <pthread.h>
17 #include <string.h>
18 #include <sys/time.h>
19 #include <signal.h>
20 #include <errno.h>
21 #include <unistd.h>
22 #include <netinet/in.h>
23 #include <sys/time.h>
24 #include <sys/socket.h>
25 #include <arpa/inet.h>
26 #include <fcntl.h>
27
28 #include <asterisk/rtp.h>
29 #include <asterisk/frame.h>
30 #include <asterisk/logger.h>
31 #include <asterisk/options.h>
32 #include <asterisk/channel.h>
33 #include <asterisk/acl.h>
34 #include <asterisk/channel_pvt.h>
35
36 #define TYPE_HIGH        0x0
37 #define TYPE_LOW         0x1
38 #define TYPE_SILENCE     0x2
39 #define TYPE_DONTSEND    0x3
40 #define TYPE_MASK        0x3
41
42 static int dtmftimeout = 300;   /* 300 samples */
43
44 // The value of each payload format mapping:
45 struct rtpPayloadType {
46   int isAstFormat; // whether the following code is an AST_FORMAT
47   int code;
48 };
49
50 #define MAX_RTP_PT 256
51
52 struct ast_rtp {
53         int s;
54         char resp;
55         struct ast_frame f;
56         unsigned char rawdata[1024 + AST_FRIENDLY_OFFSET];
57         unsigned int ssrc;
58         unsigned int lastts;
59         unsigned int lastrxts;
60         int lasttxformat;
61         int lastrxformat;
62         int dtmfcount;
63         int nat;
64         struct sockaddr_in us;
65         struct sockaddr_in them;
66         struct timeval rxcore;
67         struct timeval txcore;
68         struct ast_smoother *smoother;
69         int *ioid;
70         unsigned short seqno;
71         struct sched_context *sched;
72         struct io_context *io;
73         void *data;
74         ast_rtp_callback callback;
75     struct rtpPayloadType current_RTP_PT[MAX_RTP_PT];
76     // a cache for the result of rtp_lookup_code():
77     int rtp_lookup_code_cache_isAstFormat;
78     int rtp_lookup_code_cache_code;
79     int rtp_lookup_code_cache_result;
80 };
81
82 static struct ast_rtp_protocol *protos = NULL;
83
84 int ast_rtp_fd(struct ast_rtp *rtp)
85 {
86         return rtp->s;
87 }
88
89 static int g723_len(unsigned char buf)
90 {
91         switch(buf & TYPE_MASK) {
92         case TYPE_DONTSEND:
93                 return 0;
94                 break;
95         case TYPE_SILENCE:
96                 return 4;
97                 break;
98         case TYPE_HIGH:
99                 return 24;
100                 break;
101         case TYPE_LOW:
102                 return 20;
103                 break;
104         default:
105                 ast_log(LOG_WARNING, "Badly encoded frame (%d)\n", buf & TYPE_MASK);
106         }
107         return -1;
108 }
109
110 static int g723_samples(unsigned char *buf, int maxlen)
111 {
112         int pos = 0;
113         int samples = 0;
114         int res;
115         while(pos < maxlen) {
116                 res = g723_len(buf[pos]);
117                 if (res < 0)
118                         break;
119                 samples += 240;
120                 pos += res;
121         }
122         return samples;
123 }
124
125 void ast_rtp_set_data(struct ast_rtp *rtp, void *data)
126 {
127         rtp->data = data;
128 }
129
130 void ast_rtp_set_callback(struct ast_rtp *rtp, ast_rtp_callback callback)
131 {
132         rtp->callback = callback;
133 }
134
135 void ast_rtp_setnat(struct ast_rtp *rtp, int nat)
136 {
137         rtp->nat = nat;
138 }
139
140 static struct ast_frame *send_dtmf(struct ast_rtp *rtp)
141 {
142         ast_log(LOG_DEBUG, "Sending dtmf: %d (%c)\n", rtp->resp, rtp->resp);
143         rtp->f.frametype = AST_FRAME_DTMF;
144         rtp->f.subclass = rtp->resp;
145         rtp->f.datalen = 0;
146         rtp->f.samples = 0;
147         rtp->f.mallocd = 0;
148         rtp->f.src = "RTP";
149         rtp->resp = 0;
150         return &rtp->f;
151         
152 }
153
154 static struct ast_frame *process_rfc2833(struct ast_rtp *rtp, unsigned char *data, int len)
155 {
156         unsigned int event;
157         char resp = 0;
158         struct ast_frame *f = NULL;
159         event = ntohl(*((unsigned int *)(data)));
160         event >>= 24;
161 #if 0
162         printf("Event: %08x (len = %d)\n", event, len);
163 #endif  
164         if (event < 10) {
165                 resp = '0' + event;
166         } else if (event < 11) {
167                 resp = '*';
168         } else if (event < 12) {
169                 resp = '#';
170         } else if (event < 16) {
171                 resp = 'A' + (event - 12);
172         }
173         if (rtp->resp && (rtp->resp != resp)) {
174                 f = send_dtmf(rtp);
175         }
176         rtp->resp = resp;
177         rtp->dtmfcount = dtmftimeout;
178         return f;
179 }
180
181 static struct ast_frame *process_rfc3389(struct ast_rtp *rtp, unsigned char *data, int len)
182 {
183         struct ast_frame *f = NULL;
184         /* Convert comfort noise into audio with various codecs.  Unfortunately this doesn't
185            totally help us out becuase we don't have an engine to keep it going and we are not
186            guaranteed to have it every 20ms or anything */
187 #if 0
188         printf("RFC3389: %d bytes, format is %d\n", len, rtp->lastrxformat);
189 #endif  
190         ast_log(LOG_NOTICE, "RFC3389 support incomplete.  Turn off on client if possible\n");
191         if (!rtp->lastrxformat)
192                 return  NULL;
193         switch(rtp->lastrxformat) {
194         case AST_FORMAT_ULAW:
195                 rtp->f.frametype = AST_FRAME_VOICE;
196                 rtp->f.subclass = AST_FORMAT_ULAW;
197                 rtp->f.datalen = 160;
198                 rtp->f.samples = 160;
199                 memset(rtp->f.data, 0x7f, rtp->f.datalen);
200                 f = &rtp->f;
201                 break;
202         case AST_FORMAT_ALAW:
203                 rtp->f.frametype = AST_FRAME_VOICE;
204                 rtp->f.subclass = AST_FORMAT_ALAW;
205                 rtp->f.datalen = 160;
206                 rtp->f.samples = 160;
207                 memset(rtp->f.data, 0x7e, rtp->f.datalen); /* XXX Is this right? XXX */
208                 f = &rtp->f;
209                 break;
210         case AST_FORMAT_SLINEAR:
211                 rtp->f.frametype = AST_FRAME_VOICE;
212                 rtp->f.subclass = AST_FORMAT_SLINEAR;
213                 rtp->f.datalen = 320;
214                 rtp->f.samples = 160;
215                 memset(rtp->f.data, 0x00, rtp->f.datalen);
216                 f = &rtp->f;
217                 break;
218         default:
219                 ast_log(LOG_NOTICE, "Don't know how to handle RFC3389 for receive codec %d\n", rtp->lastrxformat);
220         }
221         return f;
222 }
223
224 static int rtpread(int *id, int fd, short events, void *cbdata)
225 {
226         struct ast_rtp *rtp = cbdata;
227         struct ast_frame *f;
228         f = ast_rtp_read(rtp);
229         if (f) {
230                 if (rtp->callback)
231                         rtp->callback(rtp, f, rtp->data);
232         }
233         return 1;
234 }
235
236 struct ast_frame *ast_rtp_read(struct ast_rtp *rtp)
237 {
238         int res;
239         struct sockaddr_in sin;
240         int len;
241         unsigned int seqno;
242         int payloadtype;
243         int hdrlen = 12;
244         unsigned int timestamp;
245         unsigned int *rtpheader;
246         static struct ast_frame *f, null_frame = { AST_FRAME_NULL, };
247         struct rtpPayloadType rtpPT;
248         
249         len = sizeof(sin);
250         
251         res = recvfrom(rtp->s, rtp->rawdata + AST_FRIENDLY_OFFSET, sizeof(rtp->rawdata) - AST_FRIENDLY_OFFSET,
252                                         0, (struct sockaddr *)&sin, &len);
253
254
255         rtpheader = (unsigned int *)(rtp->rawdata + AST_FRIENDLY_OFFSET);
256         if (res < 0) {
257                 ast_log(LOG_WARNING, "RTP Read error: %s\n", strerror(errno));
258                 if (errno == EBADF)
259                         CRASH;
260                 return &null_frame;
261         }
262         if (res < hdrlen) {
263                 ast_log(LOG_WARNING, "RTP Read too short\n");
264                 return &null_frame;
265         }
266         if (rtp->nat) {
267                 /* Send to whoever sent to us */
268                 if ((rtp->them.sin_addr.s_addr != sin.sin_addr.s_addr) ||
269                     (rtp->them.sin_port != sin.sin_port)) {
270                         memcpy(&rtp->them, &sin, sizeof(rtp->them));
271                         ast_log(LOG_DEBUG, "RTP NAT: Using address %s:%d\n", inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
272                 }
273         }
274         /* Get fields */
275         seqno = ntohl(rtpheader[0]);
276         payloadtype = (seqno & 0x7f0000) >> 16;
277         seqno &= 0xffff;
278         timestamp = ntohl(rtpheader[1]);
279 #if 0
280         printf("Got RTP packet from %s:%d (type %d, seq %d, ts %d, len = %d)\n", inet_ntoa(sin.sin_addr), ntohs(sin.sin_port), payloadtype, seqno, timestamp,res - hdrlen);
281 #endif  
282         rtp->f.frametype = AST_FRAME_VOICE;
283         rtpPT = ast_rtp_lookup_pt(rtp, payloadtype);
284         if (!rtpPT.isAstFormat) {
285           // This is special in-band data that's not one of our codecs
286           if (rtpPT.code == AST_RTP_DTMF) {
287             /* It's special -- rfc2833 process it */
288             f = process_rfc2833(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
289             if (f) return f; else return &null_frame;
290           } else if (rtpPT.code == AST_RTP_CN) {
291             /* Comfort Noise */
292             f = process_rfc3389(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
293             if (f) return f; else return &null_frame;
294           } else {
295             ast_log(LOG_NOTICE, "Unknown RTP codec %d received\n", payloadtype);
296             return &null_frame;
297           }
298         }
299         rtp->f.subclass = rtpPT.code;
300         rtp->lastrxformat = rtp->f.subclass;
301
302         if (!rtp->lastrxts)
303                 rtp->lastrxts = timestamp;
304
305         if (rtp->dtmfcount) {
306 #if 0
307                 printf("dtmfcount was %d\n", rtp->dtmfcount);
308 #endif          
309                 rtp->dtmfcount -= (timestamp - rtp->lastrxts);
310                 if (rtp->dtmfcount < 0)
311                         rtp->dtmfcount = 0;
312 #if 0
313                 if (dtmftimeout != rtp->dtmfcount)
314                         printf("dtmfcount is %d\n", rtp->dtmfcount);
315 #endif
316         }
317         rtp->lastrxts = timestamp;
318
319         /* Send any pending DTMF */
320         if (rtp->resp && !rtp->dtmfcount) {
321                 ast_log(LOG_DEBUG, "Sending pending DTMF\n");
322                 return send_dtmf(rtp);
323         }
324         rtp->f.mallocd = 0;
325         rtp->f.datalen = res - hdrlen;
326         rtp->f.data = rtp->rawdata + hdrlen + AST_FRIENDLY_OFFSET;
327         rtp->f.offset = hdrlen + AST_FRIENDLY_OFFSET;
328         switch(rtp->f.subclass) {
329         case AST_FORMAT_ULAW:
330         case AST_FORMAT_ALAW:
331                 rtp->f.samples = rtp->f.datalen;
332                 break;
333         case AST_FORMAT_SLINEAR:
334                 rtp->f.samples = rtp->f.datalen / 2;
335                 break;
336         case AST_FORMAT_GSM:
337                 rtp->f.samples = 160 * (rtp->f.datalen / 33);
338                 break;
339         case AST_FORMAT_ILBC:
340                 rtp->f.samples = 240 * (rtp->f.datalen / 50);
341                 break;
342         case AST_FORMAT_ADPCM:
343                 rtp->f.samples = rtp->f.datalen * 2;
344                 break;
345         case AST_FORMAT_G729A:
346                 rtp->f.samples = rtp->f.datalen * 8;
347                 break;
348         case AST_FORMAT_G723_1:
349                 rtp->f.samples = g723_samples(rtp->f.data, rtp->f.datalen);
350                 break;
351         case AST_FORMAT_SPEEX:
352                 rtp->f.samples = 160;
353                 // assumes that the RTP packet contained one Speex frame
354                 break;
355         default:
356                 ast_log(LOG_NOTICE, "Unable to calculate samples for format %d\n", rtp->f.subclass);
357                 break;
358         }
359         rtp->f.src = "RTP";
360         return &rtp->f;
361 }
362
363 // The following array defines the MIME type (and subtype) for each
364 // of our codecs, or RTP-specific data type.
365 static struct {
366   struct rtpPayloadType payloadType;
367   char* type;
368   char* subtype;
369 } mimeTypes[] = {
370   {{1, AST_FORMAT_G723_1}, "audio", "G723"},
371   {{1, AST_FORMAT_GSM}, "audio", "GSM"},
372   {{1, AST_FORMAT_ULAW}, "audio", "PCMU"},
373   {{1, AST_FORMAT_ALAW}, "audio", "PCMA"},
374   {{1, AST_FORMAT_MP3}, "audio", "MPA"},
375   {{1, AST_FORMAT_ADPCM}, "audio", "DVI4"},
376   {{1, AST_FORMAT_SLINEAR}, "audio", "L16"},
377   {{1, AST_FORMAT_LPC10}, "audio", "LPC"},
378   {{1, AST_FORMAT_G729A}, "audio", "G729"},
379   {{1, AST_FORMAT_SPEEX}, "audio", "SPEEX"},
380   {{1, AST_FORMAT_ILBC}, "audio", "iLBC"},
381   {{0, AST_RTP_DTMF}, "audio", "telephone-event"},
382   {{0, AST_RTP_CN}, "audio", "CN"},
383   {{1, AST_FORMAT_JPEG}, "video", "JPEG"},
384   {{1, AST_FORMAT_PNG}, "video", "PNG"},
385   {{1, AST_FORMAT_H261}, "video", "H261"},
386   {{1, AST_FORMAT_H263}, "video", "H263"},
387 };
388
389 /* Static (i.e., well-known) RTP payload types for our "AST_FORMAT..."s:
390    also, our own choices for dynamic payload types.  This is our master
391    table for transmission */
392 static struct rtpPayloadType static_RTP_PT[MAX_RTP_PT] = {
393   [0] = {1, AST_FORMAT_ULAW},
394   [3] = {1, AST_FORMAT_GSM},
395   [4] = {1, AST_FORMAT_G723_1},
396   [5] = {1, AST_FORMAT_ADPCM}, // 8 kHz
397   [6] = {1, AST_FORMAT_ADPCM}, // 16 kHz
398   [7] = {1, AST_FORMAT_LPC10},
399   [8] = {1, AST_FORMAT_ALAW},
400   [10] = {1, AST_FORMAT_SLINEAR}, // 2 channels
401   [11] = {1, AST_FORMAT_SLINEAR}, // 1 channel
402   [13] = {0, AST_RTP_CN},
403   [14] = {1, AST_FORMAT_MP3},
404   [16] = {1, AST_FORMAT_ADPCM}, // 11.025 kHz
405   [17] = {1, AST_FORMAT_ADPCM}, // 22.050 kHz
406   [18] = {1, AST_FORMAT_G729A},
407   [26] = {1, AST_FORMAT_JPEG},
408   [31] = {1, AST_FORMAT_H261},
409   [34] = {1, AST_FORMAT_H263},
410   [97] = {1, AST_FORMAT_ILBC},
411   [101] = {0, AST_RTP_DTMF},
412   [110] = {1, AST_FORMAT_SPEEX},
413 };
414
415 void ast_rtp_pt_clear(struct ast_rtp* rtp) 
416 {
417   int i;
418
419   for (i = 0; i < MAX_RTP_PT; ++i) {
420     rtp->current_RTP_PT[i].isAstFormat = 0;
421     rtp->current_RTP_PT[i].code = 0;
422   }
423
424   rtp->rtp_lookup_code_cache_isAstFormat = 0;
425   rtp->rtp_lookup_code_cache_code = 0;
426   rtp->rtp_lookup_code_cache_result = 0;
427 }
428
429 void ast_rtp_pt_default(struct ast_rtp* rtp) 
430 {
431   int i;
432   /* Initialize to default payload types */
433   for (i = 0; i < MAX_RTP_PT; ++i) {
434     rtp->current_RTP_PT[i].isAstFormat = static_RTP_PT[i].isAstFormat;
435     rtp->current_RTP_PT[i].code = static_RTP_PT[i].code;
436   }
437
438   rtp->rtp_lookup_code_cache_isAstFormat = 0;
439   rtp->rtp_lookup_code_cache_code = 0;
440   rtp->rtp_lookup_code_cache_result = 0;
441 }
442
443 // Make a note of a RTP payload type that was seen in a SDP "m=" line.
444 // By default, use the well-known value for this type (although it may
445 // still be set to a different value by a subsequent "a=rtpmap:" line):
446 void ast_rtp_set_m_type(struct ast_rtp* rtp, int pt) {
447   if (pt < 0 || pt > MAX_RTP_PT) return; // bogus payload type
448
449   if (static_RTP_PT[pt].code != 0) {
450     rtp->current_RTP_PT[pt] = static_RTP_PT[pt];
451   }
452
453
454 // Make a note of a RTP payload type (with MIME type) that was seen in
455 // a SDP "a=rtpmap:" line.
456 void ast_rtp_set_rtpmap_type(struct ast_rtp* rtp, int pt,
457                          char* mimeType, char* mimeSubtype) {
458   int i;
459
460   if (pt < 0 || pt > MAX_RTP_PT) return; // bogus payload type
461
462   for (i = 0; i < sizeof mimeTypes/sizeof mimeTypes[0]; ++i) {
463     if (strcasecmp(mimeSubtype, mimeTypes[i].subtype) == 0 &&
464         strcasecmp(mimeType, mimeTypes[i].type) == 0) {
465       rtp->current_RTP_PT[pt] = mimeTypes[i].payloadType;
466       return;
467     }
468   }
469
470
471 // Return the union of all of the codecs that were set by rtp_set...() calls
472 // They're returned as two distinct sets: AST_FORMATs, and AST_RTPs
473 void ast_rtp_get_current_formats(struct ast_rtp* rtp,
474                              int* astFormats, int* nonAstFormats) {
475   int pt;
476
477   *astFormats = *nonAstFormats = 0;
478   for (pt = 0; pt < MAX_RTP_PT; ++pt) {
479     if (rtp->current_RTP_PT[pt].isAstFormat) {
480       *astFormats |= rtp->current_RTP_PT[pt].code;
481     } else {
482       *nonAstFormats |= rtp->current_RTP_PT[pt].code;
483     }
484   }
485 }
486
487 struct rtpPayloadType ast_rtp_lookup_pt(struct ast_rtp* rtp, int pt) {
488   if (pt < 0 || pt > MAX_RTP_PT) {
489     struct rtpPayloadType result;
490     result.isAstFormat = result.code = 0;
491     return result; // bogus payload type
492   }
493   /* Gotta use our static one, since that's what we sent against */
494   return static_RTP_PT[pt];
495 }
496
497 int ast_rtp_lookup_code(struct ast_rtp* rtp, int isAstFormat, int code) {
498   int pt;
499
500   /* Looks up an RTP code out of our *static* outbound list */
501
502   if (isAstFormat == rtp->rtp_lookup_code_cache_isAstFormat &&
503       code == rtp->rtp_lookup_code_cache_code) {
504     // Use our cached mapping, to avoid the overhead of the loop below
505     return rtp->rtp_lookup_code_cache_result;
506   }
507
508   for (pt = 0; pt < MAX_RTP_PT; ++pt) {
509     if (static_RTP_PT[pt].code == code &&
510                 static_RTP_PT[pt].isAstFormat == isAstFormat) {
511       rtp->rtp_lookup_code_cache_isAstFormat = isAstFormat;
512       rtp->rtp_lookup_code_cache_code = code;
513       rtp->rtp_lookup_code_cache_result = pt;
514       return pt;
515     }
516   }
517   return -1;
518 }
519
520 char* ast_rtp_lookup_mime_subtype(int isAstFormat, int code) {
521   int i;
522
523   for (i = 0; i < sizeof mimeTypes/sizeof mimeTypes[0]; ++i) {
524     if (mimeTypes[i].payloadType.code == code &&
525         mimeTypes[i].payloadType.isAstFormat == isAstFormat) {
526       return mimeTypes[i].subtype;
527     }
528   }
529   return "";
530 }
531
532 struct ast_rtp *ast_rtp_new(struct sched_context *sched, struct io_context *io)
533 {
534         struct ast_rtp *rtp;
535         int x;
536         int flags;
537         rtp = malloc(sizeof(struct ast_rtp));
538         if (!rtp)
539                 return NULL;
540         memset(rtp, 0, sizeof(struct ast_rtp));
541         rtp->them.sin_family = AF_INET;
542         rtp->us.sin_family = AF_INET;
543         rtp->s = socket(AF_INET, SOCK_DGRAM, 0);
544         rtp->ssrc = rand();
545         rtp->seqno = rand() & 0xffff;
546         if (rtp->s < 0) {
547                 free(rtp);
548                 ast_log(LOG_WARNING, "Unable to allocate socket: %s\n", strerror(errno));
549                 return NULL;
550         }
551         flags = fcntl(rtp->s, F_GETFL);
552         fcntl(rtp->s, F_SETFL, flags | O_NONBLOCK);
553         for (;;) {
554                 /* Find us a place */
555                 x = (rand() % (65000-1025)) + 1025;
556                 /* Must be an even port number by RTP spec */
557                 x = x & ~1;
558                 rtp->us.sin_port = htons(x);
559                 if (!bind(rtp->s, &rtp->us, sizeof(rtp->us)))
560                         break;
561                 if (errno != EADDRINUSE) {
562                         ast_log(LOG_WARNING, "Unexpected bind error: %s\n", strerror(errno));
563                         close(rtp->s);
564                         free(rtp);
565                         return NULL;
566                 }
567         }
568         if (io && sched) {
569                 /* Operate this one in a callback mode */
570                 rtp->sched = sched;
571                 rtp->io = io;
572                 rtp->ioid = ast_io_add(rtp->io, rtp->s, rtpread, AST_IO_IN, rtp);
573         }
574         ast_rtp_pt_default(rtp);
575         return rtp;
576 }
577
578 int ast_rtp_settos(struct ast_rtp *rtp, int tos)
579 {
580         int res;
581         if ((res = setsockopt(rtp->s, IPPROTO_IP, IP_TOS, &tos, sizeof(tos)))) 
582                 ast_log(LOG_WARNING, "Unable to set TOS to %d\n", tos);
583         return res;
584 }
585
586 void ast_rtp_set_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
587 {
588         rtp->them.sin_port = them->sin_port;
589         rtp->them.sin_addr = them->sin_addr;
590 }
591
592 void ast_rtp_get_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
593 {
594         them->sin_family = AF_INET;
595         them->sin_port = rtp->them.sin_port;
596         them->sin_addr = rtp->them.sin_addr;
597 }
598
599 void ast_rtp_get_us(struct ast_rtp *rtp, struct sockaddr_in *us)
600 {
601         memcpy(us, &rtp->us, sizeof(rtp->us));
602 }
603
604 void ast_rtp_stop(struct ast_rtp *rtp)
605 {
606         memset(&rtp->them.sin_addr, 0, sizeof(rtp->them.sin_addr));
607         memset(&rtp->them.sin_port, 0, sizeof(rtp->them.sin_port));
608 }
609
610 void ast_rtp_destroy(struct ast_rtp *rtp)
611 {
612         if (rtp->smoother)
613                 ast_smoother_free(rtp->smoother);
614         if (rtp->ioid)
615                 ast_io_remove(rtp->io, rtp->ioid);
616         if (rtp->s > -1)
617                 close(rtp->s);
618         free(rtp);
619 }
620
621 static unsigned int calc_txstamp(struct ast_rtp *rtp)
622 {
623         struct timeval now;
624         unsigned int ms;
625         if (!rtp->txcore.tv_sec && !rtp->txcore.tv_usec) {
626                 gettimeofday(&rtp->txcore, NULL);
627         }
628         gettimeofday(&now, NULL);
629         ms = (now.tv_sec - rtp->txcore.tv_sec) * 1000;
630         ms += (now.tv_usec - rtp->txcore.tv_usec) / 1000;
631         /* Use what we just got for next time */
632         rtp->txcore.tv_sec = now.tv_sec;
633         rtp->txcore.tv_usec = now.tv_usec;
634         return ms;
635 }
636
637 int ast_rtp_senddigit(struct ast_rtp *rtp, char digit)
638 {
639         unsigned int *rtpheader;
640         int hdrlen = 12;
641         int res;
642         int ms;
643         int pred;
644         int x;
645         char data[256];
646
647         if ((digit <= '9') && (digit >= '0'))
648                 digit -= '0';
649         else if (digit == '*')
650                 digit = 10;
651         else if (digit == '#')
652                 digit = 11;
653         else if ((digit >= 'A') && (digit <= 'D')) 
654                 digit = digit - 'A' + 12;
655         else if ((digit >= 'a') && (digit <= 'd')) 
656                 digit = digit - 'a' + 12;
657         else {
658                 ast_log(LOG_WARNING, "Don't know how to represent '%c'\n", digit);
659                 return -1;
660         }
661         
662
663         /* If we have no peer, return immediately */    
664         if (!rtp->them.sin_addr.s_addr)
665                 return 0;
666
667         ms = calc_txstamp(rtp);
668         /* Default prediction */
669         pred = rtp->lastts + ms * 8;
670         
671         /* Get a pointer to the header */
672         rtpheader = (unsigned int *)data;
673         rtpheader[0] = htonl((2 << 30) | (1 << 23) | (101 << 16) | (rtp->seqno++));
674         rtpheader[1] = htonl(rtp->lastts);
675         rtpheader[2] = htonl(rtp->ssrc); 
676         rtpheader[3] = htonl((digit << 24) | (0xa << 16) | (0));
677         for (x=0;x<4;x++) {
678                 if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
679                         res = sendto(rtp->s, (void *)rtpheader, hdrlen + 4, 0, &rtp->them, sizeof(rtp->them));
680                         if (res <0) 
681                                 ast_log(LOG_NOTICE, "RTP Transmission error to %s:%d: %s\n", inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port), strerror(errno));
682         #if 0
683                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
684         #endif          
685                 }
686                 if (x ==0) {
687                         /* Clear marker bit and increment seqno */
688                         rtpheader[0] = htonl((2 << 30)  | (101 << 16) | (rtp->seqno++));
689                         /* Make duration 240 */
690                         rtpheader[3] |= htonl((240));
691                         /* Set the End bit for the last 3 */
692                         rtpheader[3] |= htonl((1 << 23));
693                 }
694         }
695         return 0;
696 }
697
698 static int ast_rtp_raw_write(struct ast_rtp *rtp, struct ast_frame *f, int codec)
699 {
700         unsigned int *rtpheader;
701         int hdrlen = 12;
702         int res;
703         int ms;
704         int pred;
705
706         ms = calc_txstamp(rtp);
707         /* Default prediction */
708         pred = rtp->lastts + ms * 8;
709         
710         switch(f->subclass) {
711         case AST_FORMAT_ULAW:
712         case AST_FORMAT_ALAW:
713                 /* If we're within +/- 20ms from when where we
714                    predict we should be, use that */
715                 pred = rtp->lastts + f->datalen;
716                 break;
717         case AST_FORMAT_G729A:
718                 pred = rtp->lastts + f->datalen * 8;
719                 break;
720         case AST_FORMAT_GSM:
721                 pred = rtp->lastts + (f->datalen * 160 / 33);
722                 break;
723         case AST_FORMAT_ILBC:
724                 pred = rtp->lastts + (f->datalen * 240 / 50);
725                 break;
726         case AST_FORMAT_G723_1:
727                 pred = rtp->lastts + g723_samples(f->data, f->datalen);
728                 break;
729         case AST_FORMAT_SPEEX:
730                 pred = rtp->lastts + 160;
731                 // assumes that the RTP packet contains one Speex frame
732                 break;
733         default:
734                 ast_log(LOG_WARNING, "Not sure about timestamp format for codec format %d\n", f->subclass);
735         }
736
737         /* Re-calculate last TS */
738         rtp->lastts = rtp->lastts + ms * 8;
739         /* If it's close to ou prediction, go for it */
740         if (abs(rtp->lastts - pred) < 640)
741                 rtp->lastts = pred;
742         else
743                 ast_log(LOG_DEBUG, "Difference is %d, ms is %d\n", abs(rtp->lastts - pred), ms);
744         /* Get a pointer to the header */
745         rtpheader = (unsigned int *)(f->data - hdrlen);
746         rtpheader[0] = htonl((2 << 30) | (codec << 16) | (rtp->seqno++));
747         rtpheader[1] = htonl(rtp->lastts);
748         rtpheader[2] = htonl(rtp->ssrc); 
749         if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
750                 res = sendto(rtp->s, (void *)rtpheader, f->datalen + hdrlen, 0, &rtp->them, sizeof(rtp->them));
751                 if (res <0) 
752                         ast_log(LOG_NOTICE, "RTP Transmission error to %s:%d: %s\n", inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port), strerror(errno));
753 #if 0
754                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
755 #endif          
756         }
757         return 0;
758 }
759
760 int ast_rtp_write(struct ast_rtp *rtp, struct ast_frame *_f)
761 {
762         struct ast_frame *f;
763         int codec;
764         int hdrlen = 12;
765         
766
767         /* If we have no peer, return immediately */    
768         if (!rtp->them.sin_addr.s_addr)
769                 return 0;
770
771         /* If there is no data length, return immediately */
772         if (!_f->datalen) 
773                 return 0;
774         
775         /* Make sure we have enough space for RTP header */
776         if (_f->frametype != AST_FRAME_VOICE) {
777                 ast_log(LOG_WARNING, "RTP can only send voice\n");
778                 return -1;
779         }
780
781
782         codec = ast_rtp_lookup_code(rtp, 1, _f->subclass);
783         if (codec < 0) {
784                 ast_log(LOG_WARNING, "Don't know how to send format %d packets with RTP\n", _f->subclass);
785                 return -1;
786         }
787
788         if (rtp->lasttxformat !=  _f->subclass) {
789                 /* New format, reset the smoother */
790                 ast_log(LOG_DEBUG, "Ooh, format changed from %d to %d\n", rtp->lasttxformat, _f->subclass);
791                 rtp->lasttxformat = _f->subclass;
792                 if (rtp->smoother)
793                         ast_smoother_free(rtp->smoother);
794                 rtp->smoother = NULL;
795         }
796
797
798         switch(_f->subclass) {
799         case AST_FORMAT_ULAW:
800         case AST_FORMAT_ALAW:
801                 if (!rtp->smoother) {
802                         rtp->smoother = ast_smoother_new(160);
803                 }
804                 if (!rtp->smoother) {
805                         ast_log(LOG_WARNING, "Unable to create smoother :(\n");
806                         return -1;
807                 }
808                 ast_smoother_feed(rtp->smoother, _f);
809                 
810                 while((f = ast_smoother_read(rtp->smoother)))
811                         ast_rtp_raw_write(rtp, f, codec);
812                 break;
813         case AST_FORMAT_G729A:
814                 if (!rtp->smoother) {
815                         rtp->smoother = ast_smoother_new(20);
816                 }
817                 if (!rtp->smoother) {
818                         ast_log(LOG_WARNING, "Unable to create g729 smoother :(\n");
819                         return -1;
820                 }
821                 ast_smoother_feed(rtp->smoother, _f);
822                 
823                 while((f = ast_smoother_read(rtp->smoother)))
824                         ast_rtp_raw_write(rtp, f, codec);
825                 break;
826         case AST_FORMAT_GSM:
827                 if (!rtp->smoother) {
828                         rtp->smoother = ast_smoother_new(33);
829                 }
830                 if (!rtp->smoother) {
831                         ast_log(LOG_WARNING, "Unable to create GSM smoother :(\n");
832                         return -1;
833                 }
834                 ast_smoother_feed(rtp->smoother, _f);
835                 while((f = ast_smoother_read(rtp->smoother)))
836                         ast_rtp_raw_write(rtp, f, codec);
837                 break;
838         case AST_FORMAT_ILBC:
839                 if (!rtp->smoother) {
840                         rtp->smoother = ast_smoother_new(50);
841                 }
842                 if (!rtp->smoother) {
843                         ast_log(LOG_WARNING, "Unable to create ILBC smoother :(\n");
844                         return -1;
845                 }
846                 ast_smoother_feed(rtp->smoother, _f);
847                 while((f = ast_smoother_read(rtp->smoother)))
848                         ast_rtp_raw_write(rtp, f, codec);
849                 break;
850         default:        
851                 ast_log(LOG_WARNING, "Not sure about sending format %d packets\n", _f->subclass);
852                 // fall through to...
853         case AST_FORMAT_SPEEX:
854                 // Don't buffer outgoing frames; send them one-per-packet:
855                 if (_f->offset < hdrlen) {
856                         f = ast_frdup(_f);
857                 } else {
858                         f = _f;
859                 }
860                 ast_rtp_raw_write(rtp, f, codec);
861         }
862                 
863         return 0;
864 }
865
866 void ast_rtp_proto_unregister(struct ast_rtp_protocol *proto)
867 {
868         struct ast_rtp_protocol *cur, *prev;
869         cur = protos;
870         prev = NULL;
871         while(cur) {
872                 if (cur == proto) {
873                         if (prev)
874                                 prev->next = proto->next;
875                         else
876                                 protos = proto->next;
877                         return;
878                 }
879                 prev = cur;
880                 cur = cur->next;
881         }
882 }
883
884 int ast_rtp_proto_register(struct ast_rtp_protocol *proto)
885 {
886         struct ast_rtp_protocol *cur;
887         cur = protos;
888         while(cur) {
889                 if (cur->type == proto->type) {
890                         ast_log(LOG_WARNING, "Tried to register same protocol '%s' twice\n", cur->type);
891                         return -1;
892                 }
893                 cur = cur->next;
894         }
895         proto->next = protos;
896         protos = proto;
897         return 0;
898 }
899
900 static struct ast_rtp_protocol *get_proto(struct ast_channel *chan)
901 {
902         struct ast_rtp_protocol *cur;
903         cur = protos;
904         while(cur) {
905                 if (cur->type == chan->type) {
906                         return cur;
907                 }
908                 cur = cur->next;
909         }
910         return NULL;
911 }
912
913 int ast_rtp_bridge(struct ast_channel *c0, struct ast_channel *c1, int flags, struct ast_frame **fo, struct ast_channel **rc)
914 {
915         struct ast_frame *f;
916         struct ast_channel *who, *cs[3];
917         struct ast_rtp *p0, *p1;
918         struct ast_rtp_protocol *pr0, *pr1;
919         struct sockaddr_in ac0, ac1;
920         struct sockaddr_in t0, t1;
921         
922         void *pvt0, *pvt1;
923         int to;
924
925         /* XXX Wait a half a second for things to settle up 
926                         this really should be fixed XXX */
927         ast_autoservice_start(c0);
928         ast_autoservice_start(c1);
929         usleep(500000);
930         ast_autoservice_stop(c0);
931         ast_autoservice_stop(c1);
932
933         /* if need DTMF, cant native bridge */
934         if (flags & (AST_BRIDGE_DTMF_CHANNEL_0 | AST_BRIDGE_DTMF_CHANNEL_1))
935                 return -2;
936         ast_pthread_mutex_lock(&c0->lock);
937         ast_pthread_mutex_lock(&c1->lock);
938         pr0 = get_proto(c0);
939         pr1 = get_proto(c1);
940         if (!pr0) {
941                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c0->name);
942                 ast_pthread_mutex_unlock(&c0->lock);
943                 ast_pthread_mutex_unlock(&c1->lock);
944                 return -1;
945         }
946         if (!pr1) {
947                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c1->name);
948                 ast_pthread_mutex_unlock(&c0->lock);
949                 ast_pthread_mutex_unlock(&c1->lock);
950                 return -1;
951         }
952         pvt0 = c0->pvt->pvt;
953         pvt1 = c1->pvt->pvt;
954         p0 = pr0->get_rtp_info(c0);
955         p1 = pr1->get_rtp_info(c1);
956         if (!p0 || !p1) {
957                 /* Somebody doesn't want to play... */
958                 ast_pthread_mutex_unlock(&c0->lock);
959                 ast_pthread_mutex_unlock(&c1->lock);
960                 return -2;
961         }
962         if (pr0->set_rtp_peer(c0, p1)) 
963                 ast_log(LOG_WARNING, "Channel '%s' failed to talk to '%s'\n", c0->name, c1->name);
964         else {
965                 /* Store RTP peer */
966                 ast_rtp_get_peer(p1, &ac1);
967         }
968         if (pr1->set_rtp_peer(c1, p0))
969                 ast_log(LOG_WARNING, "Channel '%s' failed to talk back to '%s'\n", c1->name, c0->name);
970         else {
971                 /* Store RTP peer */
972                 ast_rtp_get_peer(p0, &ac0);
973         }
974         ast_pthread_mutex_unlock(&c0->lock);
975         ast_pthread_mutex_unlock(&c1->lock);
976         cs[0] = c0;
977         cs[1] = c1;
978         cs[2] = NULL;
979         for (;;) {
980                 if ((c0->pvt->pvt != pvt0)  ||
981                         (c1->pvt->pvt != pvt1) ||
982                         (c0->masq || c0->masqr || c1->masq || c1->masqr)) {
983                                 ast_log(LOG_DEBUG, "Oooh, something is weird, backing out\n");
984                                 if (c0->pvt->pvt == pvt0) {
985                                         if (pr0->set_rtp_peer(c0, NULL)) 
986                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
987                                 }
988                                 if (c1->pvt->pvt == pvt1) {
989                                         if (pr1->set_rtp_peer(c1, NULL)) 
990                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
991                                 }
992                                 /* Tell it to try again later */
993                                 return -3;
994                 }
995                 to = -1;
996                 ast_rtp_get_peer(p1, &t1);
997                 ast_rtp_get_peer(p0, &t0);
998                 if (inaddrcmp(&t1, &ac1)) {
999                         ast_log(LOG_DEBUG, "Oooh, '%s' changed end address\n", c1->name);
1000                         if (pr0->set_rtp_peer(c0, p1)) 
1001                                 ast_log(LOG_WARNING, "Channel '%s' failed to update to '%s'\n", c0->name, c1->name);
1002                         memcpy(&ac1, &t1, sizeof(ac1));
1003                 }
1004                 if (inaddrcmp(&t0, &ac0)) {
1005                         ast_log(LOG_DEBUG, "Oooh, '%s' changed end address\n", c0->name);
1006                         if (pr1->set_rtp_peer(c1, p0))
1007                                 ast_log(LOG_WARNING, "Channel '%s' failed to update to '%s'\n", c1->name, c0->name);
1008                         memcpy(&ac0, &t0, sizeof(ac0));
1009                 }
1010                 who = ast_waitfor_n(cs, 2, &to);
1011                 if (!who) {
1012                         ast_log(LOG_DEBUG, "Ooh, empty read...\n");
1013                         continue;
1014                 }
1015                 f = ast_read(who);
1016                 if (!f || ((f->frametype == AST_FRAME_DTMF) &&
1017                                    (((who == c0) && (flags & AST_BRIDGE_DTMF_CHANNEL_0)) || 
1018                                ((who == c1) && (flags & AST_BRIDGE_DTMF_CHANNEL_1))))) {
1019                         *fo = f;
1020                         *rc = who;
1021                         ast_log(LOG_DEBUG, "Oooh, got a %s\n", f ? "digit" : "hangup");
1022                         if ((c0->pvt->pvt == pvt0) && (!c0->_softhangup)) {
1023                                 if (pr0->set_rtp_peer(c0, NULL)) 
1024                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
1025                         }
1026                         if ((c1->pvt->pvt == pvt1) && (!c1->_softhangup)) {
1027                                 if (pr1->set_rtp_peer(c1, NULL)) 
1028                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
1029                         }
1030                         /* That's all we needed */
1031                         return 0;
1032                 } else {
1033                         if ((f->frametype == AST_FRAME_DTMF) || (f->frametype == AST_FRAME_VOICE)) {
1034                                 /* Forward voice or DTMF frames if they happen upon us */
1035                                 if (who == c0) {
1036                                         ast_write(c1, f);
1037                                 } else if (who == c1) {
1038                                         ast_write(c0, f);
1039                                 }
1040                         }
1041                         ast_frfree(f);
1042                 }
1043                 /* Swap priority not that it's a big deal at this point */
1044                 cs[2] = cs[0];
1045                 cs[0] = cs[1];
1046                 cs[1] = cs[2];
1047                 
1048         }
1049         return -1;
1050 }