2a015b6e4a15441e801eff070e3a516c64087c80
[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
34 #define TYPE_SILENCE     0x2
35 #define TYPE_HIGH        0x0
36 #define TYPE_LOW         0x1
37 #define TYPE_MASK        0x3
38
39 static int dtmftimeout = 300;   /* 300 samples */
40
41 struct ast_rtp {
42         int s;
43         char resp;
44         struct ast_frame f;
45         unsigned char rawdata[1024 + AST_FRIENDLY_OFFSET];
46         unsigned int ssrc;
47         unsigned int lastts;
48         unsigned int lastrxts;
49         int lasttxformat;
50         int dtmfcount;
51         struct sockaddr_in us;
52         struct sockaddr_in them;
53         struct timeval rxcore;
54         struct timeval txcore;
55         struct ast_smoother *smoother;
56         int *ioid;
57         unsigned short seqno;
58         struct sched_context *sched;
59         struct io_context *io;
60         void *data;
61         ast_rtp_callback callback;
62 };
63
64
65 static int g723_len(unsigned char buf)
66 {
67         switch(buf & TYPE_MASK) {
68         case TYPE_MASK:
69         case TYPE_SILENCE:
70                 return 4;
71                 break;
72         case TYPE_HIGH:
73                 return 24;
74                 break;
75         case TYPE_LOW:
76                 return 20;
77                 break;
78         default:
79                 ast_log(LOG_WARNING, "Badly encoded frame (%d)\n", buf & TYPE_MASK);
80         }
81         return -1;
82 }
83
84 static int g723_samples(unsigned char *buf, int maxlen)
85 {
86         int pos = 0;
87         int samples = 0;
88         int res;
89         while(pos < maxlen) {
90                 res = g723_len(buf[pos]);
91                 if (res < 0)
92                         break;
93                 samples += 240;
94                 pos += res;
95         }
96         return samples;
97 }
98
99 void ast_rtp_set_data(struct ast_rtp *rtp, void *data)
100 {
101         rtp->data = data;
102 }
103
104 void ast_rtp_set_callback(struct ast_rtp *rtp, ast_rtp_callback callback)
105 {
106         rtp->callback = callback;
107 }
108
109 static void send_dtmf(struct ast_rtp *rtp)
110 {
111         printf("Sending dtmf: %d (%c)\n", rtp->resp, rtp->resp);
112         rtp->f.frametype = AST_FRAME_DTMF;
113         rtp->f.subclass = rtp->resp;
114         rtp->f.datalen = 0;
115         rtp->f.samples = 0;
116         rtp->f.mallocd = 0;
117         rtp->f.src = "RTP";
118         rtp->resp = 0;
119         if (rtp->callback)
120                 rtp->callback(rtp, &rtp->f, rtp->data);
121         
122 }
123
124 static void process_rfc2833(struct ast_rtp *rtp, unsigned char *data, int len)
125 {
126         unsigned int event;
127         char resp = 0;
128         event = ntohl(*((unsigned int *)(data)));
129         event >>= 24;
130 #if 0
131         printf("Event: %08x (len = %d)\n", event, len);
132 #endif  
133         if (event < 10) {
134                 resp = '0' + event;
135         } else if (event < 11) {
136                 resp = '*';
137         } else if (event < 12) {
138                 resp = '#';
139         } else if (event < 16) {
140                 resp = 'A' + (event - 12);
141         }
142         if (rtp->resp && (rtp->resp != resp)) {
143                 send_dtmf(rtp);
144         }
145         rtp->resp = resp;
146         rtp->dtmfcount = dtmftimeout;
147 }
148
149 static void process_type121(struct ast_rtp *rtp, unsigned char *data, int len)
150 {
151         char resp = 0;
152         
153         unsigned char b0,b1,b2,b3,b4,b5,b6,b7;
154         
155         b0=*(data+0);b1=*(data+1);b2=*(data+2);b3=*(data+3);
156         b4=*(data+4);b5=*(data+5);b6=*(data+6);b7=*(data+7);
157 //      printf("%u %u %u %u %u %u %u %u\n",b0,b1,b2,b3,b4,b5,b6,b7);
158         if (b2==32) {
159 //              printf("Start %d\n",b3);
160                 if (b4==0) {
161 //                      printf("Detection point for DTMF %d\n",b3);
162                         if (b3<10) {
163                                 resp='0'+b3;
164                         } else if (b3<11) {
165                                 resp='*';
166                         } else if (b3<12) {
167                                 resp='#';
168                         } else if (b3<16) {
169                                 resp='A'+(b3-12);
170                         }
171                         rtp->resp=resp;
172                         send_dtmf(rtp);
173                 }
174         }
175         if (b2==3) {
176 //              printf("Stop(3) %d\n",b3);
177         }
178         if (b2==0) {
179 //              printf("Stop(0) %d\n",b3);
180         }
181 }
182
183 static int rtpread(int *id, int fd, short events, void *cbdata)
184 {
185         struct ast_rtp *rtp = cbdata;
186         int res;
187         struct sockaddr_in sin;
188         int len;
189         unsigned int seqno;
190         int payloadtype;
191         int hdrlen = 12;
192         unsigned int timestamp;
193         unsigned int *rtpheader;
194         
195         len = sizeof(sin);
196         
197         res = recvfrom(rtp->s, rtp->rawdata + AST_FRIENDLY_OFFSET, sizeof(rtp->rawdata) - AST_FRIENDLY_OFFSET,
198                                         0, (struct sockaddr *)&sin, &len);
199
200         rtpheader = (unsigned int *)(rtp->rawdata + AST_FRIENDLY_OFFSET);
201         if (res < 0) {
202                 ast_log(LOG_WARNING, "RTP Read error: %s\n", strerror(errno));
203                 if (errno == EBADF)
204                         CRASH;
205                 return 1;
206         }
207         if (res < hdrlen) {
208                 ast_log(LOG_WARNING, "RTP Read too short\n");
209                 return 1;
210         }
211         /* Get fields */
212         seqno = ntohl(rtpheader[0]);
213         payloadtype = (seqno & 0x7f0000) >> 16;
214         seqno &= 0xffff;
215         timestamp = ntohl(rtpheader[1]);
216 #if 0
217         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);
218 #endif  
219         rtp->f.frametype = AST_FRAME_VOICE;
220         rtp->f.subclass = rtp2ast(payloadtype);
221         if (rtp->f.subclass < 0) {
222                 if (payloadtype == 101) {
223                         /* It's special -- rfc2833 process it */
224                         process_rfc2833(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
225                 } else if (payloadtype == 121) {
226                         /* CISCO proprietary DTMF bridge */
227                         process_type121(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
228                 } else if (payloadtype == 100) {
229                         /* CISCO's notso proprietary DTMF bridge */
230                         process_rfc2833(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
231                 } else {
232                         ast_log(LOG_NOTICE, "Unknown RTP codec %d received\n", payloadtype);
233                 }
234                 return 1;
235         }
236
237         if (!rtp->lastrxts)
238                 rtp->lastrxts = timestamp;
239
240         if (rtp->dtmfcount) {
241 #if 0
242                 printf("dtmfcount was %d\n", rtp->dtmfcount);
243 #endif          
244                 rtp->dtmfcount -= (timestamp - rtp->lastrxts);
245                 if (rtp->dtmfcount < 0)
246                         rtp->dtmfcount = 0;
247 #if 0
248                 if (dtmftimeout != rtp->dtmfcount)
249                         printf("dtmfcount is %d\n", rtp->dtmfcount);
250 #endif
251         }
252         rtp->lastrxts = timestamp;
253
254         /* Send any pending DTMF */
255         if (rtp->resp && !rtp->dtmfcount) {
256                 send_dtmf(rtp);
257                 /* Setup the voice frame again */
258                 rtp->f.frametype = AST_FRAME_VOICE;
259                 rtp->f.subclass = rtp2ast(payloadtype);
260         }
261         rtp->f.mallocd = 0;
262         rtp->f.datalen = res - hdrlen;
263         rtp->f.data = rtp->rawdata + hdrlen + AST_FRIENDLY_OFFSET;
264         rtp->f.offset = hdrlen + AST_FRIENDLY_OFFSET;
265         switch(rtp->f.subclass) {
266         case AST_FORMAT_ULAW:
267         case AST_FORMAT_ALAW:
268                 rtp->f.samples = rtp->f.datalen;
269                 break;
270         case AST_FORMAT_SLINEAR:
271                 rtp->f.samples = rtp->f.datalen / 2;
272                 break;
273         case AST_FORMAT_GSM:
274                 rtp->f.samples = 160 * (rtp->f.datalen / 33);
275                 break;
276         case AST_FORMAT_ADPCM:
277                 rtp->f.samples = rtp->f.datalen * 2;
278                 break;
279         case AST_FORMAT_G729A:
280                 rtp->f.samples = rtp->f.datalen * 8;
281                 break;
282         case AST_FORMAT_G723_1:
283                 rtp->f.samples = g723_samples(rtp->f.data, rtp->f.datalen);
284                 break;
285         default:
286                 ast_log(LOG_NOTICE, "Unable to calculate samples for format %d\n", rtp->f.subclass);
287                 break;
288         }
289         rtp->f.src = "RTP";
290         if (rtp->callback)
291                 rtp->callback(rtp, &rtp->f, rtp->data);
292         return 1;
293 }
294
295 static struct {
296         int rtp;
297         int ast;
298         char *label;
299 } cmap[] = {
300         { 0, AST_FORMAT_ULAW, "PCMU" },
301         { 3, AST_FORMAT_GSM, "GSM" },
302         { 4, AST_FORMAT_G723_1, "G723" },
303         { 5, AST_FORMAT_ADPCM, "ADPCM" },
304         { 8, AST_FORMAT_ALAW, "PCMA" },
305         { 18, AST_FORMAT_G729A, "G729" },
306 };
307
308 int rtp2ast(int id)
309 {
310         int x;
311         for (x=0;x<sizeof(cmap) / sizeof(cmap[0]); x++) {
312                 if (cmap[x].rtp == id)
313                         return cmap[x].ast;
314         }
315         return -1;
316 }
317
318 int ast2rtp(int id)
319 {
320         int x;
321         for (x=0;x<sizeof(cmap) / sizeof(cmap[0]); x++) {
322                 if (cmap[x].ast == id)
323                         return cmap[x].rtp;
324         }
325         return -1;
326 }
327
328 char *ast2rtpn(int id)
329 {
330         int x;
331         for (x=0;x<sizeof(cmap) / sizeof(cmap[0]); x++) {
332                 if (cmap[x].ast == id)
333                         return cmap[x].label;
334         }
335         return "";
336 }
337 struct ast_rtp *ast_rtp_new(struct sched_context *sched, struct io_context *io)
338 {
339         struct ast_rtp *rtp;
340         int x;
341         int flags;
342         rtp = malloc(sizeof(struct ast_rtp));
343         if (!rtp)
344                 return NULL;
345         memset(rtp, 0, sizeof(struct ast_rtp));
346         rtp->them.sin_family = AF_INET;
347         rtp->us.sin_family = AF_INET;
348         rtp->s = socket(AF_INET, SOCK_DGRAM, 0);
349         rtp->ssrc = rand();
350         rtp->seqno = rand() & 0xffff;
351         if (rtp->s < 0) {
352                 free(rtp);
353                 ast_log(LOG_WARNING, "Unable to allocate socket: %s\n", strerror(errno));
354                 return NULL;
355         }
356         flags = fcntl(rtp->s, F_GETFL);
357         fcntl(rtp->s, F_SETFL, flags | O_NONBLOCK);
358         for (;;) {
359                 /* Find us a place */
360                 x = (rand() % (65000-1025)) + 1025;
361                 /* Must be an even port number by RTP spec */
362                 x = x & ~1;
363                 rtp->us.sin_port = htons(x);
364                 if (!bind(rtp->s, &rtp->us, sizeof(rtp->us)))
365                         break;
366                 if (errno != EADDRINUSE) {
367                         ast_log(LOG_WARNING, "Unexpected bind error: %s\n", strerror(errno));
368                         close(rtp->s);
369                         free(rtp);
370                         return NULL;
371                 }
372         }
373         rtp->io = io;
374         rtp->sched = sched;
375         rtp->ioid = ast_io_add(rtp->io, rtp->s, rtpread, AST_IO_IN, rtp);
376         return rtp;
377 }
378
379 int ast_rtp_settos(struct ast_rtp *rtp, int tos)
380 {
381         int res;
382         if ((res = setsockopt(rtp->s, SOL_IP, IP_TOS, &tos, sizeof(tos)))) 
383                 ast_log(LOG_WARNING, "Unable to set TOS to %d\n", tos);
384         return res;
385 }
386
387 void ast_rtp_set_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
388 {
389         rtp->them.sin_port = them->sin_port;
390         rtp->them.sin_addr = them->sin_addr;
391 }
392
393 void ast_rtp_get_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
394 {
395         them->sin_family = AF_INET;
396         them->sin_port = rtp->them.sin_port;
397         them->sin_addr = rtp->them.sin_addr;
398 }
399
400 void ast_rtp_get_us(struct ast_rtp *rtp, struct sockaddr_in *us)
401 {
402         memcpy(us, &rtp->us, sizeof(rtp->us));
403 }
404
405 void ast_rtp_destroy(struct ast_rtp *rtp)
406 {
407         if (rtp->smoother)
408                 ast_smoother_free(rtp->smoother);
409         if (rtp->ioid)
410                 ast_io_remove(rtp->io, rtp->ioid);
411         if (rtp->s > -1)
412                 close(rtp->s);
413         free(rtp);
414 }
415
416 static unsigned int calc_txstamp(struct ast_rtp *rtp)
417 {
418         struct timeval now;
419         unsigned int ms;
420         if (!rtp->txcore.tv_sec && !rtp->txcore.tv_usec) {
421                 gettimeofday(&rtp->txcore, NULL);
422         }
423         gettimeofday(&now, NULL);
424         ms = (now.tv_sec - rtp->txcore.tv_sec) * 1000;
425         ms += (now.tv_usec - rtp->txcore.tv_usec) / 1000;
426         return ms;
427 }
428
429 int ast_rtp_senddigit(struct ast_rtp *rtp, char digit)
430 {
431         unsigned int *rtpheader;
432         int hdrlen = 12;
433         int res;
434         int ms;
435         int pred;
436         int x;
437         char data[256];
438
439         if ((digit <= '9') && (digit >= '0'))
440                 digit -= '0';
441         else if (digit == '*')
442                 digit = 10;
443         else if (digit == '#')
444                 digit = 11;
445         else if ((digit >= 'A') && (digit <= 'D')) 
446                 digit = digit - 'A' + 12;
447         else if ((digit >= 'a') && (digit <= 'd')) 
448                 digit = digit - 'a' + 12;
449         else {
450                 ast_log(LOG_WARNING, "Don't know how to represent '%c'\n", digit);
451                 return -1;
452         }
453         
454
455         /* If we have no peer, return immediately */    
456         if (!rtp->them.sin_addr.s_addr)
457                 return 0;
458
459         ms = calc_txstamp(rtp);
460         /* Default prediction */
461         pred = ms * 8;
462         
463         /* Get a pointer to the header */
464         rtpheader = (unsigned int *)data;
465         rtpheader[0] = htonl((2 << 30) | (1 << 23) | (101 << 16) | (rtp->seqno++));
466         rtpheader[1] = htonl(rtp->lastts);
467         rtpheader[2] = htonl(rtp->ssrc); 
468         rtpheader[3] = htonl((digit << 24) | (0xa << 16) | (0));
469         for (x=0;x<4;x++) {
470                 if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
471                         res = sendto(rtp->s, (void *)rtpheader, hdrlen + 4, 0, &rtp->them, sizeof(rtp->them));
472                         if (res <0) 
473                                 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));
474         #if 0
475                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
476         #endif          
477                 }
478                 if (x ==0) {
479                         /* Clear marker bit and increment seqno */
480                         rtpheader[0] = htonl((2 << 30)  | (101 << 16) | (rtp->seqno++));
481                         /* Make duration 240 */
482                         rtpheader[3] |= htonl((240));
483                         /* Set the End bit for the last 3 */
484                         rtpheader[3] |= htonl((1 << 23));
485                 }
486         }
487         return 0;
488 }
489
490 static int ast_rtp_raw_write(struct ast_rtp *rtp, struct ast_frame *f, int codec)
491 {
492         unsigned int *rtpheader;
493         int hdrlen = 12;
494         int res;
495         int ms;
496         int pred;
497
498         ms = calc_txstamp(rtp);
499         /* Default prediction */
500         pred = ms * 8;
501         
502         switch(f->subclass) {
503         case AST_FORMAT_ULAW:
504         case AST_FORMAT_ALAW:
505                 /* If we're within +/- 20ms from when where we
506                    predict we should be, use that */
507                 pred = rtp->lastts + f->datalen;
508                 break;
509         case AST_FORMAT_G729A:
510                 pred = rtp->lastts + f->datalen * 8;
511                 break;
512         case AST_FORMAT_GSM:
513                 pred = rtp->lastts + f->datalen * 20 / 33;
514                 break;
515         case AST_FORMAT_G723_1:
516                 pred = rtp->lastts + g723_samples(f->data, f->datalen);
517                 break;
518         default:
519                 ast_log(LOG_WARNING, "Not sure about timestamp format for codec format %d\n", f->subclass);
520         }
521
522         /* Re-calculate last TS */
523         rtp->lastts = ms * 8;
524         
525         /* If it's close to ou prediction, go for it */
526         if (abs(rtp->lastts - pred) < 640)
527                 rtp->lastts = pred;
528 #if 0
529         else
530                 printf("Difference is %d, ms is %d\n", abs(rtp->lastts - pred), ms);
531 #endif                  
532         /* Get a pointer to the header */
533         rtpheader = (unsigned int *)(f->data - hdrlen);
534         rtpheader[0] = htonl((2 << 30) | (codec << 16) | (rtp->seqno++));
535         rtpheader[1] = htonl(rtp->lastts);
536         rtpheader[2] = htonl(rtp->ssrc); 
537         if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
538                 res = sendto(rtp->s, (void *)rtpheader, f->datalen + hdrlen, 0, &rtp->them, sizeof(rtp->them));
539                 if (res <0) 
540                         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));
541 #if 0
542                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
543 #endif          
544         }
545         return 0;
546 }
547
548 int ast_rtp_write(struct ast_rtp *rtp, struct ast_frame *_f)
549 {
550         struct ast_frame *f;
551         int codec;
552         int hdrlen = 12;
553         
554
555         /* If we have no peer, return immediately */    
556         if (!rtp->them.sin_addr.s_addr)
557                 return 0;
558         
559         /* Make sure we have enough space for RTP header */
560         if (_f->frametype != AST_FRAME_VOICE) {
561                 ast_log(LOG_WARNING, "RTP can only send voice\n");
562                 return -1;
563         }
564
565         codec = ast2rtp(_f->subclass);
566         if (codec < 0) {
567                 ast_log(LOG_WARNING, "Don't know how to send format %d packets with RTP\n", _f->subclass);
568                 return -1;
569         }
570
571         if (rtp->lasttxformat !=  _f->subclass) {
572                 /* New format, reset the smoother */
573                 ast_log(LOG_DEBUG, "Ooh, format changed from %d to %d\n", rtp->lasttxformat, _f->subclass);
574                 rtp->lasttxformat = _f->subclass;
575                 if (rtp->smoother)
576                         ast_smoother_free(rtp->smoother);
577                 rtp->smoother = NULL;
578         }
579
580
581         switch(_f->subclass) {
582         case AST_FORMAT_ULAW:
583         case AST_FORMAT_ALAW:
584                 if (!rtp->smoother) {
585                         rtp->smoother = ast_smoother_new(160);
586                 }
587                 if (!rtp->smoother) {
588                         ast_log(LOG_WARNING, "Unable to create smoother :(\n");
589                         return -1;
590                 }
591                 ast_smoother_feed(rtp->smoother, _f);
592                 
593                 while((f = ast_smoother_read(rtp->smoother)))
594                         ast_rtp_raw_write(rtp, f, codec);
595                 break;
596         case AST_FORMAT_G729A:
597                 if (!rtp->smoother) {
598                         rtp->smoother = ast_smoother_new(20);
599                 }
600                 if (!rtp->smoother) {
601                         ast_log(LOG_WARNING, "Unable to create g729 smoother :(\n");
602                         return -1;
603                 }
604                 ast_smoother_feed(rtp->smoother, _f);
605                 
606                 while((f = ast_smoother_read(rtp->smoother)))
607                         ast_rtp_raw_write(rtp, f, codec);
608                 break;
609         case AST_FORMAT_GSM:
610                 if (!rtp->smoother) {
611                         rtp->smoother = ast_smoother_new(33);
612                 }
613                 if (!rtp->smoother) {
614                         ast_log(LOG_WARNING, "Unable to create GSM smoother :(\n");
615                         return -1;
616                 }
617                 ast_smoother_feed(rtp->smoother, _f);
618                 while((f = ast_smoother_read(rtp->smoother)))
619                         ast_rtp_raw_write(rtp, f, codec);
620                 break;
621         default:        
622                 ast_log(LOG_WARNING, "Not sure about sending format %d packets\n", _f->subclass);
623                 if (_f->offset < hdrlen) {
624                         f = ast_frdup(_f);
625                 } else {
626                         f = _f;
627                 }
628                 ast_rtp_raw_write(rtp, f, codec);
629         }
630                 
631         return 0;
632 }