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