faa39c74172aeb121c3c023daff75ec62a7cedba
[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_destroy(struct ast_rtp *rtp)
590 {
591         if (rtp->smoother)
592                 ast_smoother_free(rtp->smoother);
593         if (rtp->ioid)
594                 ast_io_remove(rtp->io, rtp->ioid);
595         if (rtp->s > -1)
596                 close(rtp->s);
597         free(rtp);
598 }
599
600 static unsigned int calc_txstamp(struct ast_rtp *rtp)
601 {
602         struct timeval now;
603         unsigned int ms;
604         if (!rtp->txcore.tv_sec && !rtp->txcore.tv_usec) {
605                 gettimeofday(&rtp->txcore, NULL);
606         }
607         gettimeofday(&now, NULL);
608         ms = (now.tv_sec - rtp->txcore.tv_sec) * 1000;
609         ms += (now.tv_usec - rtp->txcore.tv_usec) / 1000;
610         /* Use what we just got for next time */
611         rtp->txcore.tv_sec = now.tv_sec;
612         rtp->txcore.tv_usec = now.tv_usec;
613         return ms;
614 }
615
616 int ast_rtp_senddigit(struct ast_rtp *rtp, char digit)
617 {
618         unsigned int *rtpheader;
619         int hdrlen = 12;
620         int res;
621         int ms;
622         int pred;
623         int x;
624         char data[256];
625
626         if ((digit <= '9') && (digit >= '0'))
627                 digit -= '0';
628         else if (digit == '*')
629                 digit = 10;
630         else if (digit == '#')
631                 digit = 11;
632         else if ((digit >= 'A') && (digit <= 'D')) 
633                 digit = digit - 'A' + 12;
634         else if ((digit >= 'a') && (digit <= 'd')) 
635                 digit = digit - 'a' + 12;
636         else {
637                 ast_log(LOG_WARNING, "Don't know how to represent '%c'\n", digit);
638                 return -1;
639         }
640         
641
642         /* If we have no peer, return immediately */    
643         if (!rtp->them.sin_addr.s_addr)
644                 return 0;
645
646         ms = calc_txstamp(rtp);
647         /* Default prediction */
648         pred = rtp->lastts + ms * 8;
649         
650         /* Get a pointer to the header */
651         rtpheader = (unsigned int *)data;
652         rtpheader[0] = htonl((2 << 30) | (1 << 23) | (101 << 16) | (rtp->seqno++));
653         rtpheader[1] = htonl(rtp->lastts);
654         rtpheader[2] = htonl(rtp->ssrc); 
655         rtpheader[3] = htonl((digit << 24) | (0xa << 16) | (0));
656         for (x=0;x<4;x++) {
657                 if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
658                         res = sendto(rtp->s, (void *)rtpheader, hdrlen + 4, 0, &rtp->them, sizeof(rtp->them));
659                         if (res <0) 
660                                 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));
661         #if 0
662                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
663         #endif          
664                 }
665                 if (x ==0) {
666                         /* Clear marker bit and increment seqno */
667                         rtpheader[0] = htonl((2 << 30)  | (101 << 16) | (rtp->seqno++));
668                         /* Make duration 240 */
669                         rtpheader[3] |= htonl((240));
670                         /* Set the End bit for the last 3 */
671                         rtpheader[3] |= htonl((1 << 23));
672                 }
673         }
674         return 0;
675 }
676
677 static int ast_rtp_raw_write(struct ast_rtp *rtp, struct ast_frame *f, int codec)
678 {
679         unsigned int *rtpheader;
680         int hdrlen = 12;
681         int res;
682         int ms;
683         int pred;
684
685         ms = calc_txstamp(rtp);
686         /* Default prediction */
687         pred = rtp->lastts + ms * 8;
688         
689         switch(f->subclass) {
690         case AST_FORMAT_ULAW:
691         case AST_FORMAT_ALAW:
692                 /* If we're within +/- 20ms from when where we
693                    predict we should be, use that */
694                 pred = rtp->lastts + f->datalen;
695                 break;
696         case AST_FORMAT_G729A:
697                 pred = rtp->lastts + f->datalen * 8;
698                 break;
699         case AST_FORMAT_GSM:
700                 pred = rtp->lastts + (f->datalen * 160 / 33);
701                 break;
702         case AST_FORMAT_G723_1:
703                 pred = rtp->lastts + g723_samples(f->data, f->datalen);
704                 break;
705         case AST_FORMAT_SPEEX:
706                 pred = rtp->lastts + 160;
707                 // assumes that the RTP packet contains one Speex frame
708                 break;
709         default:
710                 ast_log(LOG_WARNING, "Not sure about timestamp format for codec format %d\n", f->subclass);
711         }
712
713         /* Re-calculate last TS */
714         rtp->lastts = rtp->lastts + ms * 8;
715         /* If it's close to ou prediction, go for it */
716         if (abs(rtp->lastts - pred) < 640)
717                 rtp->lastts = pred;
718         else
719                 ast_log(LOG_DEBUG, "Difference is %d, ms is %d\n", abs(rtp->lastts - pred), ms);
720         /* Get a pointer to the header */
721         rtpheader = (unsigned int *)(f->data - hdrlen);
722         rtpheader[0] = htonl((2 << 30) | (codec << 16) | (rtp->seqno++));
723         rtpheader[1] = htonl(rtp->lastts);
724         rtpheader[2] = htonl(rtp->ssrc); 
725         if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
726                 res = sendto(rtp->s, (void *)rtpheader, f->datalen + hdrlen, 0, &rtp->them, sizeof(rtp->them));
727                 if (res <0) 
728                         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));
729 #if 0
730                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
731 #endif          
732         }
733         return 0;
734 }
735
736 int ast_rtp_write(struct ast_rtp *rtp, struct ast_frame *_f)
737 {
738         struct ast_frame *f;
739         int codec;
740         int hdrlen = 12;
741         
742
743         /* If we have no peer, return immediately */    
744         if (!rtp->them.sin_addr.s_addr)
745                 return 0;
746         
747         /* Make sure we have enough space for RTP header */
748         if (_f->frametype != AST_FRAME_VOICE) {
749                 ast_log(LOG_WARNING, "RTP can only send voice\n");
750                 return -1;
751         }
752
753         codec = ast_rtp_lookup_code(rtp, 1, _f->subclass);
754         if (codec < 0) {
755                 ast_log(LOG_WARNING, "Don't know how to send format %d packets with RTP\n", _f->subclass);
756                 return -1;
757         }
758
759         if (rtp->lasttxformat !=  _f->subclass) {
760                 /* New format, reset the smoother */
761                 ast_log(LOG_DEBUG, "Ooh, format changed from %d to %d\n", rtp->lasttxformat, _f->subclass);
762                 rtp->lasttxformat = _f->subclass;
763                 if (rtp->smoother)
764                         ast_smoother_free(rtp->smoother);
765                 rtp->smoother = NULL;
766         }
767
768
769         switch(_f->subclass) {
770         case AST_FORMAT_ULAW:
771         case AST_FORMAT_ALAW:
772                 if (!rtp->smoother) {
773                         rtp->smoother = ast_smoother_new(160);
774                 }
775                 if (!rtp->smoother) {
776                         ast_log(LOG_WARNING, "Unable to create smoother :(\n");
777                         return -1;
778                 }
779                 ast_smoother_feed(rtp->smoother, _f);
780                 
781                 while((f = ast_smoother_read(rtp->smoother)))
782                         ast_rtp_raw_write(rtp, f, codec);
783                 break;
784         case AST_FORMAT_G729A:
785                 if (!rtp->smoother) {
786                         rtp->smoother = ast_smoother_new(20);
787                 }
788                 if (!rtp->smoother) {
789                         ast_log(LOG_WARNING, "Unable to create g729 smoother :(\n");
790                         return -1;
791                 }
792                 ast_smoother_feed(rtp->smoother, _f);
793                 
794                 while((f = ast_smoother_read(rtp->smoother)))
795                         ast_rtp_raw_write(rtp, f, codec);
796                 break;
797         case AST_FORMAT_GSM:
798                 if (!rtp->smoother) {
799                         rtp->smoother = ast_smoother_new(33);
800                 }
801                 if (!rtp->smoother) {
802                         ast_log(LOG_WARNING, "Unable to create GSM smoother :(\n");
803                         return -1;
804                 }
805                 ast_smoother_feed(rtp->smoother, _f);
806                 while((f = ast_smoother_read(rtp->smoother)))
807                         ast_rtp_raw_write(rtp, f, codec);
808                 break;
809         default:        
810                 ast_log(LOG_WARNING, "Not sure about sending format %d packets\n", _f->subclass);
811                 // fall through to...
812         case AST_FORMAT_SPEEX:
813                 // Don't buffer outgoing frames; send them one-per-packet:
814                 if (_f->offset < hdrlen) {
815                         f = ast_frdup(_f);
816                 } else {
817                         f = _f;
818                 }
819                 ast_rtp_raw_write(rtp, f, codec);
820         }
821                 
822         return 0;
823 }
824
825 void ast_rtp_proto_unregister(struct ast_rtp_protocol *proto)
826 {
827         struct ast_rtp_protocol *cur, *prev;
828         cur = protos;
829         prev = NULL;
830         while(cur) {
831                 if (cur == proto) {
832                         if (prev)
833                                 prev->next = proto->next;
834                         else
835                                 protos = proto->next;
836                         return;
837                 }
838                 prev = cur;
839                 cur = cur->next;
840         }
841 }
842
843 int ast_rtp_proto_register(struct ast_rtp_protocol *proto)
844 {
845         struct ast_rtp_protocol *cur;
846         cur = protos;
847         while(cur) {
848                 if (cur->type == proto->type) {
849                         ast_log(LOG_WARNING, "Tried to register same protocol '%s' twice\n", cur->type);
850                         return -1;
851                 }
852                 cur = cur->next;
853         }
854         proto->next = protos;
855         protos = proto;
856         return 0;
857 }
858
859 static struct ast_rtp_protocol *get_proto(struct ast_channel *chan)
860 {
861         struct ast_rtp_protocol *cur;
862         cur = protos;
863         while(cur) {
864                 if (cur->type == chan->type) {
865                         return cur;
866                 }
867                 cur = cur->next;
868         }
869         return NULL;
870 }
871
872 int ast_rtp_bridge(struct ast_channel *c0, struct ast_channel *c1, int flags, struct ast_frame **fo, struct ast_channel **rc)
873 {
874         struct ast_frame *f;
875         struct ast_channel *who, *cs[3];
876         struct ast_rtp *p0, *p1;
877         struct ast_rtp_protocol *pr0, *pr1;
878         void *pvt0, *pvt1;
879         int to;
880
881         /* XXX Wait a half a second for things to settle up 
882                         this really should be fixed XXX */
883         ast_autoservice_start(c0);
884         ast_autoservice_start(c1);
885         usleep(500000);
886         ast_autoservice_stop(c0);
887         ast_autoservice_stop(c1);
888
889         /* if need DTMF, cant native bridge */
890         if (flags & (AST_BRIDGE_DTMF_CHANNEL_0 | AST_BRIDGE_DTMF_CHANNEL_1))
891                 return -2;
892         ast_pthread_mutex_lock(&c0->lock);
893         ast_pthread_mutex_lock(&c1->lock);
894         pr0 = get_proto(c0);
895         pr1 = get_proto(c1);
896         if (!pr0) {
897                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c0->name);
898                 ast_pthread_mutex_unlock(&c0->lock);
899                 ast_pthread_mutex_unlock(&c1->lock);
900                 return -1;
901         }
902         if (!pr1) {
903                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c1->name);
904                 ast_pthread_mutex_unlock(&c0->lock);
905                 ast_pthread_mutex_unlock(&c1->lock);
906                 return -1;
907         }
908         pvt0 = c0->pvt->pvt;
909         pvt1 = c1->pvt->pvt;
910         p0 = pr0->get_rtp_info(c0);
911         p1 = pr1->get_rtp_info(c1);
912         if (!p0 || !p1) {
913                 /* Somebody doesn't want to play... */
914                 ast_pthread_mutex_unlock(&c0->lock);
915                 ast_pthread_mutex_unlock(&c1->lock);
916                 return -2;
917         }
918         if (pr0->set_rtp_peer(c0, p1)) 
919                 ast_log(LOG_WARNING, "Channel '%s' failed to talk to '%s'\n", c0->name, c1->name);
920         if (pr1->set_rtp_peer(c1, p0)) 
921                 ast_log(LOG_WARNING, "Channel '%s' failed to talk back to '%s'\n", c1->name, c0->name);
922         ast_pthread_mutex_unlock(&c0->lock);
923         ast_pthread_mutex_unlock(&c1->lock);
924         cs[0] = c0;
925         cs[1] = c1;
926         cs[2] = NULL;
927         for (;;) {
928                 if ((c0->pvt->pvt != pvt0)  ||
929                         (c1->pvt->pvt != pvt1) ||
930                         (c0->masq || c0->masqr || c1->masq || c1->masqr)) {
931                                 ast_log(LOG_DEBUG, "Oooh, something is weird, backing out\n");
932                                 if (c0->pvt->pvt == pvt0) {
933                                         if (pr0->set_rtp_peer(c0, NULL)) 
934                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
935                                 }
936                                 if (c1->pvt->pvt == pvt1) {
937                                         if (pr1->set_rtp_peer(c1, NULL)) 
938                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
939                                 }
940                                 /* Tell it to try again later */
941                                 return -3;
942                 }
943                 to = -1;
944                 who = ast_waitfor_n(cs, 2, &to);
945                 if (!who) {
946                         ast_log(LOG_DEBUG, "Ooh, empty read...\n");
947                         continue;
948                 }
949                 f = ast_read(who);
950                 if (!f || ((f->frametype == AST_FRAME_DTMF) &&
951                                    (((who == c0) && (flags & AST_BRIDGE_DTMF_CHANNEL_0)) || 
952                                ((who == c1) && (flags & AST_BRIDGE_DTMF_CHANNEL_1))))) {
953                         *fo = f;
954                         *rc = who;
955                         ast_log(LOG_DEBUG, "Oooh, got a %s\n", f ? "digit" : "hangup");
956                         if ((c0->pvt->pvt == pvt0) && (!c0->_softhangup)) {
957                                 if (pr0->set_rtp_peer(c0, NULL)) 
958                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
959                         }
960                         if ((c1->pvt->pvt == pvt1) && (!c1->_softhangup)) {
961                                 if (pr1->set_rtp_peer(c1, NULL)) 
962                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
963                         }
964                         /* That's all we needed */
965                         return 0;
966                 } else {
967                         if ((f->frametype == AST_FRAME_DTMF) || (f->frametype == AST_FRAME_VOICE)) {
968                                 /* Forward voice or DTMF frames if they happen upon us */
969                                 if (who == c0) {
970                                         ast_write(c1, f);
971                                 } else if (who == c1) {
972                                         ast_write(c0, f);
973                                 }
974                         }
975                         ast_frfree(f);
976                 }
977                 /* Swap priority not that it's a big deal at this point */
978                 cs[2] = cs[0];
979                 cs[0] = cs[1];
980                 cs[1] = cs[2];
981                 
982         }
983         return -1;
984 }