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