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