Don't send empty RTP frames
[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         /* If there is no data length, return immediately */
754         if (!_f->datalen) 
755                 return 0;
756         
757         /* Make sure we have enough space for RTP header */
758         if (_f->frametype != AST_FRAME_VOICE) {
759                 ast_log(LOG_WARNING, "RTP can only send voice\n");
760                 return -1;
761         }
762
763
764         codec = ast_rtp_lookup_code(rtp, 1, _f->subclass);
765         if (codec < 0) {
766                 ast_log(LOG_WARNING, "Don't know how to send format %d packets with RTP\n", _f->subclass);
767                 return -1;
768         }
769
770         if (rtp->lasttxformat !=  _f->subclass) {
771                 /* New format, reset the smoother */
772                 ast_log(LOG_DEBUG, "Ooh, format changed from %d to %d\n", rtp->lasttxformat, _f->subclass);
773                 rtp->lasttxformat = _f->subclass;
774                 if (rtp->smoother)
775                         ast_smoother_free(rtp->smoother);
776                 rtp->smoother = NULL;
777         }
778
779
780         switch(_f->subclass) {
781         case AST_FORMAT_ULAW:
782         case AST_FORMAT_ALAW:
783                 if (!rtp->smoother) {
784                         rtp->smoother = ast_smoother_new(160);
785                 }
786                 if (!rtp->smoother) {
787                         ast_log(LOG_WARNING, "Unable to create smoother :(\n");
788                         return -1;
789                 }
790                 ast_smoother_feed(rtp->smoother, _f);
791                 
792                 while((f = ast_smoother_read(rtp->smoother)))
793                         ast_rtp_raw_write(rtp, f, codec);
794                 break;
795         case AST_FORMAT_G729A:
796                 if (!rtp->smoother) {
797                         rtp->smoother = ast_smoother_new(20);
798                 }
799                 if (!rtp->smoother) {
800                         ast_log(LOG_WARNING, "Unable to create g729 smoother :(\n");
801                         return -1;
802                 }
803                 ast_smoother_feed(rtp->smoother, _f);
804                 
805                 while((f = ast_smoother_read(rtp->smoother)))
806                         ast_rtp_raw_write(rtp, f, codec);
807                 break;
808         case AST_FORMAT_GSM:
809                 if (!rtp->smoother) {
810                         rtp->smoother = ast_smoother_new(33);
811                 }
812                 if (!rtp->smoother) {
813                         ast_log(LOG_WARNING, "Unable to create GSM smoother :(\n");
814                         return -1;
815                 }
816                 ast_smoother_feed(rtp->smoother, _f);
817                 while((f = ast_smoother_read(rtp->smoother)))
818                         ast_rtp_raw_write(rtp, f, codec);
819                 break;
820         default:        
821                 ast_log(LOG_WARNING, "Not sure about sending format %d packets\n", _f->subclass);
822                 // fall through to...
823         case AST_FORMAT_SPEEX:
824                 // Don't buffer outgoing frames; send them one-per-packet:
825                 if (_f->offset < hdrlen) {
826                         f = ast_frdup(_f);
827                 } else {
828                         f = _f;
829                 }
830                 ast_rtp_raw_write(rtp, f, codec);
831         }
832                 
833         return 0;
834 }
835
836 void ast_rtp_proto_unregister(struct ast_rtp_protocol *proto)
837 {
838         struct ast_rtp_protocol *cur, *prev;
839         cur = protos;
840         prev = NULL;
841         while(cur) {
842                 if (cur == proto) {
843                         if (prev)
844                                 prev->next = proto->next;
845                         else
846                                 protos = proto->next;
847                         return;
848                 }
849                 prev = cur;
850                 cur = cur->next;
851         }
852 }
853
854 int ast_rtp_proto_register(struct ast_rtp_protocol *proto)
855 {
856         struct ast_rtp_protocol *cur;
857         cur = protos;
858         while(cur) {
859                 if (cur->type == proto->type) {
860                         ast_log(LOG_WARNING, "Tried to register same protocol '%s' twice\n", cur->type);
861                         return -1;
862                 }
863                 cur = cur->next;
864         }
865         proto->next = protos;
866         protos = proto;
867         return 0;
868 }
869
870 static struct ast_rtp_protocol *get_proto(struct ast_channel *chan)
871 {
872         struct ast_rtp_protocol *cur;
873         cur = protos;
874         while(cur) {
875                 if (cur->type == chan->type) {
876                         return cur;
877                 }
878                 cur = cur->next;
879         }
880         return NULL;
881 }
882
883 int ast_rtp_bridge(struct ast_channel *c0, struct ast_channel *c1, int flags, struct ast_frame **fo, struct ast_channel **rc)
884 {
885         struct ast_frame *f;
886         struct ast_channel *who, *cs[3];
887         struct ast_rtp *p0, *p1;
888         struct ast_rtp_protocol *pr0, *pr1;
889         void *pvt0, *pvt1;
890         int to;
891
892         /* XXX Wait a half a second for things to settle up 
893                         this really should be fixed XXX */
894         ast_autoservice_start(c0);
895         ast_autoservice_start(c1);
896         usleep(500000);
897         ast_autoservice_stop(c0);
898         ast_autoservice_stop(c1);
899
900         /* if need DTMF, cant native bridge */
901         if (flags & (AST_BRIDGE_DTMF_CHANNEL_0 | AST_BRIDGE_DTMF_CHANNEL_1))
902                 return -2;
903         ast_pthread_mutex_lock(&c0->lock);
904         ast_pthread_mutex_lock(&c1->lock);
905         pr0 = get_proto(c0);
906         pr1 = get_proto(c1);
907         if (!pr0) {
908                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c0->name);
909                 ast_pthread_mutex_unlock(&c0->lock);
910                 ast_pthread_mutex_unlock(&c1->lock);
911                 return -1;
912         }
913         if (!pr1) {
914                 ast_log(LOG_WARNING, "Can't find native functions for channel '%s'\n", c1->name);
915                 ast_pthread_mutex_unlock(&c0->lock);
916                 ast_pthread_mutex_unlock(&c1->lock);
917                 return -1;
918         }
919         pvt0 = c0->pvt->pvt;
920         pvt1 = c1->pvt->pvt;
921         p0 = pr0->get_rtp_info(c0);
922         p1 = pr1->get_rtp_info(c1);
923         if (!p0 || !p1) {
924                 /* Somebody doesn't want to play... */
925                 ast_pthread_mutex_unlock(&c0->lock);
926                 ast_pthread_mutex_unlock(&c1->lock);
927                 return -2;
928         }
929         if (pr0->set_rtp_peer(c0, p1)) 
930                 ast_log(LOG_WARNING, "Channel '%s' failed to talk to '%s'\n", c0->name, c1->name);
931         if (pr1->set_rtp_peer(c1, p0)) 
932                 ast_log(LOG_WARNING, "Channel '%s' failed to talk back to '%s'\n", c1->name, c0->name);
933         ast_pthread_mutex_unlock(&c0->lock);
934         ast_pthread_mutex_unlock(&c1->lock);
935         cs[0] = c0;
936         cs[1] = c1;
937         cs[2] = NULL;
938         for (;;) {
939                 if ((c0->pvt->pvt != pvt0)  ||
940                         (c1->pvt->pvt != pvt1) ||
941                         (c0->masq || c0->masqr || c1->masq || c1->masqr)) {
942                                 ast_log(LOG_DEBUG, "Oooh, something is weird, backing out\n");
943                                 if (c0->pvt->pvt == pvt0) {
944                                         if (pr0->set_rtp_peer(c0, NULL)) 
945                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
946                                 }
947                                 if (c1->pvt->pvt == pvt1) {
948                                         if (pr1->set_rtp_peer(c1, NULL)) 
949                                                 ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
950                                 }
951                                 /* Tell it to try again later */
952                                 return -3;
953                 }
954                 to = -1;
955                 who = ast_waitfor_n(cs, 2, &to);
956                 if (!who) {
957                         ast_log(LOG_DEBUG, "Ooh, empty read...\n");
958                         continue;
959                 }
960                 f = ast_read(who);
961                 if (!f || ((f->frametype == AST_FRAME_DTMF) &&
962                                    (((who == c0) && (flags & AST_BRIDGE_DTMF_CHANNEL_0)) || 
963                                ((who == c1) && (flags & AST_BRIDGE_DTMF_CHANNEL_1))))) {
964                         *fo = f;
965                         *rc = who;
966                         ast_log(LOG_DEBUG, "Oooh, got a %s\n", f ? "digit" : "hangup");
967                         if ((c0->pvt->pvt == pvt0) && (!c0->_softhangup)) {
968                                 if (pr0->set_rtp_peer(c0, NULL)) 
969                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert\n", c0->name);
970                         }
971                         if ((c1->pvt->pvt == pvt1) && (!c1->_softhangup)) {
972                                 if (pr1->set_rtp_peer(c1, NULL)) 
973                                         ast_log(LOG_WARNING, "Channel '%s' failed to revert back\n", c1->name);
974                         }
975                         /* That's all we needed */
976                         return 0;
977                 } else {
978                         if ((f->frametype == AST_FRAME_DTMF) || (f->frametype == AST_FRAME_VOICE)) {
979                                 /* Forward voice or DTMF frames if they happen upon us */
980                                 if (who == c0) {
981                                         ast_write(c1, f);
982                                 } else if (who == c1) {
983                                         ast_write(c0, f);
984                                 }
985                         }
986                         ast_frfree(f);
987                 }
988                 /* Swap priority not that it's a big deal at this point */
989                 cs[2] = cs[0];
990                 cs[0] = cs[1];
991                 cs[1] = cs[2];
992                 
993         }
994         return -1;
995 }