Wed Mar 12 07:00:01 CET 2003
[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 RTP 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 = 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 };
401
402 void rtp_pt_init(struct ast_rtp* rtp) {
403   int i;
404
405   for (i = 0; i < MAX_RTP_PT; ++i) {
406     rtp->current_RTP_PT[i].isAstFormat = 0;
407     rtp->current_RTP_PT[i].code = 0;
408   }
409
410   rtp->rtp_lookup_code_cache_isAstFormat = 0;
411   rtp->rtp_lookup_code_cache_code = 0;
412   rtp->rtp_lookup_code_cache_result = 0;
413 }
414
415 // Make a note of a RTP payload type that was seen in a SDP "m=" line.
416 // By default, use the well-known value for this type (although it may
417 // still be set to a different value by a subsequent "a=rtpmap:" line):
418 void rtp_set_m_type(struct ast_rtp* rtp, int pt) {
419   if (pt < 0 || pt > MAX_RTP_PT) return; // bogus payload type
420
421   if (static_RTP_PT[pt].code != 0) {
422     rtp->current_RTP_PT[pt] = static_RTP_PT[pt];
423   }
424
425
426 // Make a note of a RTP payload type (with MIME type) that was seen in
427 // a SDP "a=rtpmap:" line.
428 void rtp_set_rtpmap_type(struct ast_rtp* rtp, int pt,
429                          char* mimeType, char* mimeSubtype) {
430   int i;
431
432   if (pt < 0 || pt > MAX_RTP_PT) return; // bogus payload type
433
434   for (i = 0; i < sizeof mimeTypes/sizeof mimeTypes[0]; ++i) {
435     if (strcmp(mimeSubtype, mimeTypes[i].subtype) == 0 &&
436         strcmp(mimeType, mimeTypes[i].type) == 0) {
437       rtp->current_RTP_PT[pt] = mimeTypes[i].payloadType;
438       return;
439     }
440   }
441
442
443 // Return the union of all of the codecs that were set by rtp_set...() calls
444 // They're returned as two distinct sets: AST_FORMATs, and AST_RTPs
445 void rtp_get_current_formats(struct ast_rtp* rtp,
446                              int* astFormats, int* nonAstFormats) {
447   int pt;
448
449   *astFormats = *nonAstFormats = 0;
450   for (pt = 0; pt < MAX_RTP_PT; ++pt) {
451     if (rtp->current_RTP_PT[pt].isAstFormat) {
452       *astFormats |= rtp->current_RTP_PT[pt].code;
453     } else {
454       *nonAstFormats |= rtp->current_RTP_PT[pt].code;
455     }
456   }
457 }
458
459 struct rtpPayloadType rtp_lookup_pt(struct ast_rtp* rtp, int pt) {
460   if (pt < 0 || pt > MAX_RTP_PT) {
461     struct rtpPayloadType result;
462     result.isAstFormat = result.code = 0;
463     return result; // bogus payload type
464   }
465   return rtp->current_RTP_PT[pt];
466 }
467
468 int rtp_lookup_code(struct ast_rtp* rtp, int isAstFormat, int code) {
469   int pt;
470
471   if (isAstFormat == rtp->rtp_lookup_code_cache_isAstFormat &&
472       code == rtp->rtp_lookup_code_cache_code) {
473     // Use our cached mapping, to avoid the overhead of the loop below
474     return rtp->rtp_lookup_code_cache_result;
475   }
476
477   for (pt = 0; pt < MAX_RTP_PT; ++pt) {
478     if (rtp->current_RTP_PT[pt].code == code &&
479         rtp->current_RTP_PT[pt].isAstFormat == isAstFormat) {
480       rtp->rtp_lookup_code_cache_isAstFormat = isAstFormat;
481       rtp->rtp_lookup_code_cache_code = code;
482       rtp->rtp_lookup_code_cache_result = pt;
483       return pt;
484     }
485   }
486   return -1;
487 }
488
489 char* rtp_lookup_mime_subtype(int isAstFormat, int code) {
490   int i;
491
492   for (i = 0; i < sizeof mimeTypes/sizeof mimeTypes[0]; ++i) {
493     if (mimeTypes[i].payloadType.code == code &&
494         mimeTypes[i].payloadType.isAstFormat == isAstFormat) {
495       return mimeTypes[i].subtype;
496     }
497   }
498   return "";
499 }
500
501 struct ast_rtp *ast_rtp_new(struct sched_context *sched, struct io_context *io)
502 {
503         struct ast_rtp *rtp;
504         int x;
505         int flags;
506         rtp = malloc(sizeof(struct ast_rtp));
507         if (!rtp)
508                 return NULL;
509         memset(rtp, 0, sizeof(struct ast_rtp));
510         rtp->them.sin_family = AF_INET;
511         rtp->us.sin_family = AF_INET;
512         rtp->s = socket(AF_INET, SOCK_DGRAM, 0);
513         rtp->ssrc = rand();
514         rtp->seqno = rand() & 0xffff;
515         if (rtp->s < 0) {
516                 free(rtp);
517                 ast_log(LOG_WARNING, "Unable to allocate socket: %s\n", strerror(errno));
518                 return NULL;
519         }
520         flags = fcntl(rtp->s, F_GETFL);
521         fcntl(rtp->s, F_SETFL, flags | O_NONBLOCK);
522         for (;;) {
523                 /* Find us a place */
524                 x = (rand() % (65000-1025)) + 1025;
525                 /* Must be an even port number by RTP spec */
526                 x = x & ~1;
527                 rtp->us.sin_port = htons(x);
528                 if (!bind(rtp->s, &rtp->us, sizeof(rtp->us)))
529                         break;
530                 if (errno != EADDRINUSE) {
531                         ast_log(LOG_WARNING, "Unexpected bind error: %s\n", strerror(errno));
532                         close(rtp->s);
533                         free(rtp);
534                         return NULL;
535                 }
536         }
537         if (io && sched) {
538                 /* Operate this one in a callback mode */
539                 rtp->sched = sched;
540                 rtp->io = io;
541                 rtp->ioid = ast_io_add(rtp->io, rtp->s, rtpread, AST_IO_IN, rtp);
542         }
543         return rtp;
544 }
545
546 int ast_rtp_settos(struct ast_rtp *rtp, int tos)
547 {
548         int res;
549         if ((res = setsockopt(rtp->s, SOL_IP, IP_TOS, &tos, sizeof(tos)))) 
550                 ast_log(LOG_WARNING, "Unable to set TOS to %d\n", tos);
551         return res;
552 }
553
554 void ast_rtp_set_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
555 {
556         rtp->them.sin_port = them->sin_port;
557         rtp->them.sin_addr = them->sin_addr;
558 }
559
560 void ast_rtp_get_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
561 {
562         them->sin_family = AF_INET;
563         them->sin_port = rtp->them.sin_port;
564         them->sin_addr = rtp->them.sin_addr;
565 }
566
567 void ast_rtp_get_us(struct ast_rtp *rtp, struct sockaddr_in *us)
568 {
569         memcpy(us, &rtp->us, sizeof(rtp->us));
570 }
571
572 void ast_rtp_destroy(struct ast_rtp *rtp)
573 {
574         if (rtp->smoother)
575                 ast_smoother_free(rtp->smoother);
576         if (rtp->ioid)
577                 ast_io_remove(rtp->io, rtp->ioid);
578         if (rtp->s > -1)
579                 close(rtp->s);
580         free(rtp);
581 }
582
583 static unsigned int calc_txstamp(struct ast_rtp *rtp)
584 {
585         struct timeval now;
586         unsigned int ms;
587         if (!rtp->txcore.tv_sec && !rtp->txcore.tv_usec) {
588                 gettimeofday(&rtp->txcore, NULL);
589         }
590         gettimeofday(&now, NULL);
591         ms = (now.tv_sec - rtp->txcore.tv_sec) * 1000;
592         ms += (now.tv_usec - rtp->txcore.tv_usec) / 1000;
593         /* Use what we just got for next time */
594         rtp->txcore.tv_sec = now.tv_sec;
595         rtp->txcore.tv_usec = now.tv_usec;
596         return ms;
597 }
598
599 int ast_rtp_senddigit(struct ast_rtp *rtp, char digit)
600 {
601         unsigned int *rtpheader;
602         int hdrlen = 12;
603         int res;
604         int ms;
605         int pred;
606         int x;
607         char data[256];
608
609         if ((digit <= '9') && (digit >= '0'))
610                 digit -= '0';
611         else if (digit == '*')
612                 digit = 10;
613         else if (digit == '#')
614                 digit = 11;
615         else if ((digit >= 'A') && (digit <= 'D')) 
616                 digit = digit - 'A' + 12;
617         else if ((digit >= 'a') && (digit <= 'd')) 
618                 digit = digit - 'a' + 12;
619         else {
620                 ast_log(LOG_WARNING, "Don't know how to represent '%c'\n", digit);
621                 return -1;
622         }
623         
624
625         /* If we have no peer, return immediately */    
626         if (!rtp->them.sin_addr.s_addr)
627                 return 0;
628
629         ms = calc_txstamp(rtp);
630         /* Default prediction */
631         pred = rtp->lastts + ms * 8;
632         
633         /* Get a pointer to the header */
634         rtpheader = (unsigned int *)data;
635         rtpheader[0] = htonl((2 << 30) | (1 << 23) | (101 << 16) | (rtp->seqno++));
636         rtpheader[1] = htonl(rtp->lastts);
637         rtpheader[2] = htonl(rtp->ssrc); 
638         rtpheader[3] = htonl((digit << 24) | (0xa << 16) | (0));
639         for (x=0;x<4;x++) {
640                 if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
641                         res = sendto(rtp->s, (void *)rtpheader, hdrlen + 4, 0, &rtp->them, sizeof(rtp->them));
642                         if (res <0) 
643                                 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));
644         #if 0
645                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
646         #endif          
647                 }
648                 if (x ==0) {
649                         /* Clear marker bit and increment seqno */
650                         rtpheader[0] = htonl((2 << 30)  | (101 << 16) | (rtp->seqno++));
651                         /* Make duration 240 */
652                         rtpheader[3] |= htonl((240));
653                         /* Set the End bit for the last 3 */
654                         rtpheader[3] |= htonl((1 << 23));
655                 }
656         }
657         return 0;
658 }
659
660 static int ast_rtp_raw_write(struct ast_rtp *rtp, struct ast_frame *f, int codec)
661 {
662         unsigned int *rtpheader;
663         int hdrlen = 12;
664         int res;
665         int ms;
666         int pred;
667
668         ms = calc_txstamp(rtp);
669         /* Default prediction */
670         pred = rtp->lastts + ms * 8;
671         
672         switch(f->subclass) {
673         case AST_FORMAT_ULAW:
674         case AST_FORMAT_ALAW:
675                 /* If we're within +/- 20ms from when where we
676                    predict we should be, use that */
677                 pred = rtp->lastts + f->datalen;
678                 break;
679         case AST_FORMAT_G729A:
680                 pred = rtp->lastts + f->datalen * 8;
681                 break;
682         case AST_FORMAT_GSM:
683                 pred = rtp->lastts + (f->datalen * 160 / 33);
684                 break;
685         case AST_FORMAT_G723_1:
686                 pred = rtp->lastts + g723_samples(f->data, f->datalen);
687                 break;
688         case AST_FORMAT_SPEEX:
689                 pred = rtp->lastts + 160;
690                 // assumes that the RTP packet contains one Speex frame
691                 break;
692         default:
693                 ast_log(LOG_WARNING, "Not sure about timestamp format for codec format %d\n", f->subclass);
694         }
695
696         /* Re-calculate last TS */
697         rtp->lastts = rtp->lastts + ms * 8;
698         /* If it's close to ou prediction, go for it */
699         if (abs(rtp->lastts - pred) < 640)
700                 rtp->lastts = pred;
701         else
702                 ast_log(LOG_DEBUG, "Difference is %d, ms is %d\n", abs(rtp->lastts - pred), ms);
703         /* Get a pointer to the header */
704         rtpheader = (unsigned int *)(f->data - hdrlen);
705         rtpheader[0] = htonl((2 << 30) | (codec << 16) | (rtp->seqno++));
706         rtpheader[1] = htonl(rtp->lastts);
707         rtpheader[2] = htonl(rtp->ssrc); 
708         if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
709                 res = sendto(rtp->s, (void *)rtpheader, f->datalen + hdrlen, 0, &rtp->them, sizeof(rtp->them));
710                 if (res <0) 
711                         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));
712 #if 0
713                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
714 #endif          
715         }
716         return 0;
717 }
718
719 int ast_rtp_write(struct ast_rtp *rtp, struct ast_frame *_f)
720 {
721         struct ast_frame *f;
722         int codec;
723         int hdrlen = 12;
724         
725
726         /* If we have no peer, return immediately */    
727         if (!rtp->them.sin_addr.s_addr)
728                 return 0;
729         
730         /* Make sure we have enough space for RTP header */
731         if (_f->frametype != AST_FRAME_VOICE) {
732                 ast_log(LOG_WARNING, "RTP can only send voice\n");
733                 return -1;
734         }
735
736         codec = rtp_lookup_code(rtp, 1, _f->subclass);
737         if (codec < 0) {
738                 ast_log(LOG_WARNING, "Don't know how to send format %d packets with RTP\n", _f->subclass);
739                 return -1;
740         }
741
742         if (rtp->lasttxformat !=  _f->subclass) {
743                 /* New format, reset the smoother */
744                 ast_log(LOG_DEBUG, "Ooh, format changed from %d to %d\n", rtp->lasttxformat, _f->subclass);
745                 rtp->lasttxformat = _f->subclass;
746                 if (rtp->smoother)
747                         ast_smoother_free(rtp->smoother);
748                 rtp->smoother = NULL;
749         }
750
751
752         switch(_f->subclass) {
753         case AST_FORMAT_ULAW:
754         case AST_FORMAT_ALAW:
755                 if (!rtp->smoother) {
756                         rtp->smoother = ast_smoother_new(160);
757                 }
758                 if (!rtp->smoother) {
759                         ast_log(LOG_WARNING, "Unable to create smoother :(\n");
760                         return -1;
761                 }
762                 ast_smoother_feed(rtp->smoother, _f);
763                 
764                 while((f = ast_smoother_read(rtp->smoother)))
765                         ast_rtp_raw_write(rtp, f, codec);
766                 break;
767         case AST_FORMAT_G729A:
768                 if (!rtp->smoother) {
769                         rtp->smoother = ast_smoother_new(20);
770                 }
771                 if (!rtp->smoother) {
772                         ast_log(LOG_WARNING, "Unable to create g729 smoother :(\n");
773                         return -1;
774                 }
775                 ast_smoother_feed(rtp->smoother, _f);
776                 
777                 while((f = ast_smoother_read(rtp->smoother)))
778                         ast_rtp_raw_write(rtp, f, codec);
779                 break;
780         case AST_FORMAT_GSM:
781                 if (!rtp->smoother) {
782                         rtp->smoother = ast_smoother_new(33);
783                 }
784                 if (!rtp->smoother) {
785                         ast_log(LOG_WARNING, "Unable to create GSM smoother :(\n");
786                         return -1;
787                 }
788                 ast_smoother_feed(rtp->smoother, _f);
789                 while((f = ast_smoother_read(rtp->smoother)))
790                         ast_rtp_raw_write(rtp, f, codec);
791                 break;
792         default:        
793                 ast_log(LOG_WARNING, "Not sure about sending format %d packets\n", _f->subclass);
794                 // fall through to...
795         case AST_FORMAT_SPEEX:
796                 // Don't buffer outgoing frames; send them one-per-packet:
797                 if (_f->offset < hdrlen) {
798                         f = ast_frdup(_f);
799                 } else {
800                         f = _f;
801                 }
802                 ast_rtp_raw_write(rtp, f, codec);
803         }
804                 
805         return 0;
806 }
807
808 void ast_rtp_proto_unregister(struct ast_rtp_protocol *proto)
809 {
810         struct ast_rtp_protocol *cur, *prev;
811         cur = protos;
812         prev = NULL;
813         while(cur) {
814                 if (cur == proto) {
815                         if (prev)
816                                 prev->next = proto->next;
817                         else
818                                 protos = proto->next;
819                         return;
820                 }
821                 prev = cur;
822                 cur = cur->next;
823         }
824 }
825
826 int ast_rtp_proto_register(struct ast_rtp_protocol *proto)
827 {
828         struct ast_rtp_protocol *cur;
829         cur = protos;
830         while(cur) {
831                 if (cur->type == proto->type) {
832                         ast_log(LOG_WARNING, "Tried to register same protocol '%s' twice\n", cur->type);
833                         return -1;
834                 }
835                 cur = cur->next;
836         }
837         proto->next = protos;
838         protos = proto;
839         return 0;
840 }
841
842 static struct ast_rtp_protocol *get_proto(struct ast_channel *chan)
843 {
844         struct ast_rtp_protocol *cur;
845         cur = protos;
846         while(cur) {
847                 if (cur->type == chan->type) {
848                         return cur;
849                 }
850                 cur = cur->next;
851         }
852         return NULL;
853 }
854
855 int ast_rtp_bridge(struct ast_channel *c0, struct ast_channel *c1, int flags, struct ast_frame **fo, struct ast_channel **rc)
856 {
857         struct ast_frame *f;
858         struct ast_channel *who, *cs[3];
859         struct ast_rtp *p0, *p1;
860         struct ast_rtp_protocol *pr0, *pr1;
861         void *pvt0, *pvt1;
862         int to;
863
864         /* XXX Wait a half a second for things to settle up 
865                         this really should be fixed XXX */
866         ast_autoservice_start(c0);
867         ast_autoservice_start(c1);
868         usleep(500000);
869         ast_autoservice_stop(c0);
870         ast_autoservice_stop(c1);
871
872         /* if need DTMF, cant native bridge */
873         if (flags & (AST_BRIDGE_DTMF_CHANNEL_0 | AST_BRIDGE_DTMF_CHANNEL_1))
874                 return -2;
875         ast_pthread_mutex_lock(&c0->lock);
876         ast_pthread_mutex_lock(&c1->lock);
877         pr0 = get_proto(c0);
878         pr1 = get_proto(c1);
879         if (!pr0) {
880                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c0->name);
881                 ast_pthread_mutex_unlock(&c0->lock);
882                 ast_pthread_mutex_unlock(&c1->lock);
883                 return -1;
884         }
885         if (!pr1) {
886                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c1->name);
887                 ast_pthread_mutex_unlock(&c0->lock);
888                 ast_pthread_mutex_unlock(&c1->lock);
889                 return -1;
890         }
891         pvt0 = c0->pvt->pvt;
892         pvt1 = c1->pvt->pvt;
893         p0 = pr0->get_rtp_info(c0);
894         p1 = pr1->get_rtp_info(c1);
895         if (!p0 || !p1) {
896                 /* Somebody doesn't want to play... */
897                 ast_pthread_mutex_unlock(&c0->lock);
898                 ast_pthread_mutex_unlock(&c1->lock);
899                 return -2;
900         }
901         if (pr0->set_rtp_peer(c0, p1)) 
902                 ast_log(LOG_WARNING, "Channel '%s' failed to talk to '%s'\n", c0->name, c1->name);
903         if (pr1->set_rtp_peer(c1, p0)) 
904                 ast_log(LOG_WARNING, "Channel '%s' failed to talk back to '%s'\n", c1->name, c0->name);
905         ast_pthread_mutex_unlock(&c0->lock);
906         ast_pthread_mutex_unlock(&c1->lock);
907         cs[0] = c0;
908         cs[1] = c1;
909         cs[2] = NULL;
910         for (;;) {
911                 if ((c0->pvt->pvt != pvt0)  ||
912                         (c1->pvt->pvt != pvt1) ||
913                         (c0->masq || c0->masqr || c1->masq || c1->masqr)) {
914                                 ast_log(LOG_DEBUG, "Oooh, something is weird, backing out\n");
915                                 if (c0->pvt->pvt == pvt0) {
916                                         if (pr0->set_rtp_peer(c0, NULL)) 
917                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
918                                 }
919                                 if (c1->pvt->pvt == pvt1) {
920                                         if (pr1->set_rtp_peer(c1, NULL)) 
921                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
922                                 }
923                                 /* Tell it to try again later */
924                                 return -3;
925                 }
926                 to = -1;
927                 who = ast_waitfor_n(cs, 2, &to);
928                 if (!who) {
929                         ast_log(LOG_DEBUG, "Ooh, empty read...\n");
930                         continue;
931                 }
932                 f = ast_read(who);
933                 if (!f || ((f->frametype == AST_FRAME_DTMF) &&
934                                    (((who == c0) && (flags & AST_BRIDGE_DTMF_CHANNEL_0)) || 
935                                ((who == c1) && (flags & AST_BRIDGE_DTMF_CHANNEL_1))))) {
936                         *fo = f;
937                         *rc = who;
938                         ast_log(LOG_DEBUG, "Oooh, got a %s\n", f ? "digit" : "hangup");
939                         if ((c0->pvt->pvt == pvt0) && (!c0->_softhangup)) {
940                                 if (pr0->set_rtp_peer(c0, NULL)) 
941                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
942                         }
943                         if ((c1->pvt->pvt == pvt1) && (!c1->_softhangup)) {
944                                 if (pr1->set_rtp_peer(c1, NULL)) 
945                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
946                         }
947                         /* That's all we needed */
948                         return 0;
949                 } else 
950                         ast_frfree(f);
951                 /* Swap priority not that it's a big deal at this point */
952                 cs[2] = cs[0];
953                 cs[0] = cs[1];
954                 cs[1] = cs[2];
955                 
956         }
957         return -1;
958 }