Version 0.2.0 from FTP
[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 static int dtmftimeout = 300;   /* 300 samples */
35
36 struct ast_rtp {
37         int s;
38         char resp;
39         struct ast_frame f;
40         unsigned char rawdata[1024 + AST_FRIENDLY_OFFSET];
41         unsigned int ssrc;
42         unsigned int lastts;
43         unsigned int lastrxts;
44         int dtmfcount;
45         struct sockaddr_in us;
46         struct sockaddr_in them;
47         struct timeval rxcore;
48         struct timeval txcore;
49         struct ast_smoother *smoother;
50         int *ioid;
51         unsigned short seqno;
52         struct sched_context *sched;
53         struct io_context *io;
54         void *data;
55         ast_rtp_callback callback;
56 };
57
58
59 void ast_rtp_set_data(struct ast_rtp *rtp, void *data)
60 {
61         rtp->data = data;
62 }
63
64 void ast_rtp_set_callback(struct ast_rtp *rtp, ast_rtp_callback callback)
65 {
66         rtp->callback = callback;
67 }
68
69 static void send_dtmf(struct ast_rtp *rtp)
70 {
71         printf("Sending dtmf: %d (%c)\n", rtp->resp, rtp->resp);
72         rtp->f.frametype = AST_FRAME_DTMF;
73         rtp->f.subclass = rtp->resp;
74         rtp->f.datalen = 0;
75         rtp->f.timelen = 0;
76         rtp->f.mallocd = 0;
77         rtp->f.src = "RTP";
78         rtp->resp = 0;
79         if (rtp->callback)
80                 rtp->callback(rtp, &rtp->f, rtp->data);
81         
82 }
83
84 static void process_rfc2833(struct ast_rtp *rtp, unsigned char *data, int len)
85 {
86         unsigned int event;
87         char resp = 0;
88         event = ntohl(*((unsigned int *)(data)));
89         event >>= 24;
90 #if 0
91         printf("Event: %08x (len = %d)\n", event, len);
92 #endif  
93         if (event < 10) {
94                 resp = '0' + event;
95         } else if (event < 11) {
96                 resp = '*';
97         } else if (event < 12) {
98                 resp = '#';
99         } else if (event < 16) {
100                 resp = 'A' + (event - 12);
101         }
102         if (rtp->resp && (rtp->resp != resp)) {
103                 send_dtmf(rtp);
104         }
105         rtp->resp = resp;
106         rtp->dtmfcount = dtmftimeout;
107 }
108
109 static void process_type121(struct ast_rtp *rtp, unsigned char *data, int len)
110 {
111         char resp = 0;
112         
113         unsigned char b0,b1,b2,b3,b4,b5,b6,b7;
114         
115         b0=*(data+0);b1=*(data+1);b2=*(data+2);b3=*(data+3);
116         b4=*(data+4);b5=*(data+5);b6=*(data+6);b7=*(data+7);
117 //      printf("%u %u %u %u %u %u %u %u\n",b0,b1,b2,b3,b4,b5,b6,b7);
118         if (b2==32) {
119 //              printf("Start %d\n",b3);
120                 if (b4==0) {
121 //                      printf("Detection point for DTMF %d\n",b3);
122                         if (b3<10) {
123                                 resp='0'+b3;
124                         } else if (b3<11) {
125                                 resp='*';
126                         } else if (b3<12) {
127                                 resp='#';
128                         } else if (b3<16) {
129                                 resp='A'+(b3-12);
130                         }
131                         rtp->resp=resp;
132                         send_dtmf(rtp);
133                 }
134         }
135         if (b2==3) {
136 //              printf("Stop(3) %d\n",b3);
137         }
138         if (b2==0) {
139 //              printf("Stop(0) %d\n",b3);
140         }
141 }
142
143 static int rtpread(int *id, int fd, short events, void *cbdata)
144 {
145         struct ast_rtp *rtp = cbdata;
146         int res;
147         struct sockaddr_in sin;
148         int len;
149         unsigned int seqno;
150         int payloadtype;
151         int hdrlen = 12;
152         unsigned int timestamp;
153         unsigned int *rtpheader;
154         
155         len = sizeof(sin);
156         
157         res = recvfrom(rtp->s, rtp->rawdata + AST_FRIENDLY_OFFSET, sizeof(rtp->rawdata) - AST_FRIENDLY_OFFSET,
158                                         0, (struct sockaddr *)&sin, &len);
159
160         rtpheader = (unsigned int *)(rtp->rawdata + AST_FRIENDLY_OFFSET);
161         if (res < 0) {
162                 ast_log(LOG_WARNING, "RTP Read error: %s\n", strerror(errno));
163                 if (errno == EBADF)
164                         CRASH;
165                 return 1;
166         }
167         if (res < hdrlen) {
168                 ast_log(LOG_WARNING, "RTP Read too short\n");
169                 return 1;
170         }
171         /* Get fields */
172         seqno = ntohl(rtpheader[0]);
173         payloadtype = (seqno & 0x7f0000) >> 16;
174         seqno &= 0xffff;
175         timestamp = ntohl(rtpheader[1]);
176 #if 0
177         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);
178 #endif  
179         rtp->f.frametype = AST_FRAME_VOICE;
180         rtp->f.subclass = rtp2ast(payloadtype);
181         if (rtp->f.subclass < 0) {
182                 if (payloadtype == 101) {
183                         /* It's special -- rfc2833 process it */
184                         process_rfc2833(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
185                 } else if (payloadtype == 121) {
186                         /* CISCO proprietary DTMF bridge */
187                         process_type121(rtp, rtp->rawdata + AST_FRIENDLY_OFFSET + hdrlen, res - hdrlen);
188                 } else {
189                         ast_log(LOG_NOTICE, "Unknown RTP codec %d received\n", payloadtype);
190                 }
191                 return 1;
192         }
193
194         if (!rtp->lastrxts)
195                 rtp->lastrxts = timestamp;
196
197         if (rtp->dtmfcount) {
198 #if 0
199                 printf("dtmfcount was %d\n", rtp->dtmfcount);
200 #endif          
201                 rtp->dtmfcount -= (timestamp - rtp->lastrxts);
202                 if (rtp->dtmfcount < 0)
203                         rtp->dtmfcount = 0;
204 #if 0
205                 if (dtmftimeout != rtp->dtmfcount)
206                         printf("dtmfcount is %d\n", rtp->dtmfcount);
207 #endif
208         }
209         rtp->lastrxts = timestamp;
210
211         /* Send any pending DTMF */
212         if (rtp->resp && !rtp->dtmfcount) {
213                 send_dtmf(rtp);
214                 /* Setup the voice frame again */
215                 rtp->f.frametype = AST_FRAME_VOICE;
216                 rtp->f.subclass = rtp2ast(payloadtype);
217         }
218         rtp->f.mallocd = 0;
219         rtp->f.datalen = res - hdrlen;
220         rtp->f.data = rtp->rawdata + hdrlen + AST_FRIENDLY_OFFSET;
221         rtp->f.offset = hdrlen + AST_FRIENDLY_OFFSET;
222         switch(rtp->f.subclass) {
223         case AST_FORMAT_ULAW:
224         case AST_FORMAT_ALAW:
225                 rtp->f.timelen = rtp->f.datalen / 8;
226                 break;
227         case AST_FORMAT_SLINEAR:
228                 rtp->f.timelen = rtp->f.datalen / 16;
229                 break;
230         case AST_FORMAT_GSM:
231                 rtp->f.timelen = 20 * (rtp->f.datalen / 33);
232                 break;
233         case AST_FORMAT_ADPCM:
234                 rtp->f.timelen = rtp->f.datalen / 4;
235                 break;
236         case AST_FORMAT_G729A:
237                 rtp->f.timelen = rtp->f.datalen;
238                 break;
239         default:
240                 ast_log(LOG_NOTICE, "Unable to calculate timelen for format %d\n", rtp->f.subclass);
241                 break;
242         }
243         rtp->f.src = "RTP";
244         if (rtp->callback)
245                 rtp->callback(rtp, &rtp->f, rtp->data);
246         return 1;
247 }
248
249 static struct {
250         int rtp;
251         int ast;
252         char *label;
253 } cmap[] = {
254         { 0, AST_FORMAT_ULAW, "PCMU" },
255         { 3, AST_FORMAT_GSM, "GSM" },
256         { 4, AST_FORMAT_G723_1, "G723" },
257         { 5, AST_FORMAT_ADPCM, "ADPCM" },
258         { 8, AST_FORMAT_ALAW, "PCMA" },
259         { 18, AST_FORMAT_G729A, "G729" },
260 };
261
262 int rtp2ast(int id)
263 {
264         int x;
265         for (x=0;x<sizeof(cmap) / sizeof(cmap[0]); x++) {
266                 if (cmap[x].rtp == id)
267                         return cmap[x].ast;
268         }
269         return -1;
270 }
271
272 int ast2rtp(int id)
273 {
274         int x;
275         for (x=0;x<sizeof(cmap) / sizeof(cmap[0]); x++) {
276                 if (cmap[x].ast == id)
277                         return cmap[x].rtp;
278         }
279         return -1;
280 }
281
282 char *ast2rtpn(int id)
283 {
284         int x;
285         for (x=0;x<sizeof(cmap) / sizeof(cmap[0]); x++) {
286                 if (cmap[x].ast == id)
287                         return cmap[x].label;
288         }
289         return "";
290 }
291 struct ast_rtp *ast_rtp_new(struct sched_context *sched, struct io_context *io)
292 {
293         struct ast_rtp *rtp;
294         int x;
295         int flags;
296         rtp = malloc(sizeof(struct ast_rtp));
297         if (!rtp)
298                 return NULL;
299         memset(rtp, 0, sizeof(struct ast_rtp));
300         rtp->them.sin_family = AF_INET;
301         rtp->us.sin_family = AF_INET;
302         rtp->s = socket(AF_INET, SOCK_DGRAM, 0);
303         rtp->ssrc = rand();
304         rtp->seqno = rand() & 0xffff;
305         if (rtp->s < 0) {
306                 free(rtp);
307                 ast_log(LOG_WARNING, "Unable to allocate socket: %s\n", strerror(errno));
308                 return NULL;
309         }
310         flags = fcntl(rtp->s, F_GETFL);
311         fcntl(rtp->s, F_SETFL, flags | O_NONBLOCK);
312         for (;;) {
313                 /* Find us a place */
314                 x = (rand() % (65000-1025)) + 1025;
315                 /* Must be an even port number by RTP spec */
316                 x = x & ~1;
317                 rtp->us.sin_port = htons(x);
318                 if (!bind(rtp->s, &rtp->us, sizeof(rtp->us)))
319                         break;
320                 if (errno != EADDRINUSE) {
321                         ast_log(LOG_WARNING, "Unexpected bind error: %s\n", strerror(errno));
322                         close(rtp->s);
323                         free(rtp);
324                         return NULL;
325                 }
326         }
327         rtp->io = io;
328         rtp->sched = sched;
329         rtp->ioid = ast_io_add(rtp->io, rtp->s, rtpread, AST_IO_IN, rtp);
330         return rtp;
331 }
332
333 void ast_rtp_set_peer(struct ast_rtp *rtp, struct sockaddr_in *them)
334 {
335         rtp->them.sin_port = them->sin_port;
336         rtp->them.sin_addr = them->sin_addr;
337 }
338
339 void ast_rtp_get_us(struct ast_rtp *rtp, struct sockaddr_in *us)
340 {
341         memcpy(us, &rtp->us, sizeof(rtp->us));
342 }
343
344 void ast_rtp_destroy(struct ast_rtp *rtp)
345 {
346         if (rtp->smoother)
347                 ast_smoother_free(rtp->smoother);
348         if (rtp->ioid)
349                 ast_io_remove(rtp->io, rtp->ioid);
350         if (rtp->s > -1)
351                 close(rtp->s);
352         free(rtp);
353 }
354
355 static unsigned int calc_txstamp(struct ast_rtp *rtp)
356 {
357         struct timeval now;
358         unsigned int ms;
359         if (!rtp->txcore.tv_sec && !rtp->txcore.tv_usec) {
360                 gettimeofday(&rtp->txcore, NULL);
361         }
362         gettimeofday(&now, NULL);
363         ms = (now.tv_sec - rtp->txcore.tv_sec) * 1000;
364         ms += (now.tv_usec - rtp->txcore.tv_usec) / 1000;
365         return ms;
366 }
367
368 static int ast_rtp_raw_write(struct ast_rtp *rtp, struct ast_frame *f, int codec)
369 {
370         unsigned int *rtpheader;
371         int hdrlen = 12;
372         int res;
373         int ms;
374         int pred;
375
376         ms = calc_txstamp(rtp);
377         /* Default prediction */
378         pred = ms * 8;
379         
380         switch(f->subclass) {
381         case AST_FORMAT_ULAW:
382         case AST_FORMAT_ALAW:
383                 /* If we're within +/- 20ms from when where we
384                    predict we should be, use that */
385                 pred = rtp->lastts + f->datalen;
386                 break;
387         case AST_FORMAT_G729A:
388                 pred = rtp->lastts + f->datalen * 8;
389                 break;
390         default:
391                 ast_log(LOG_WARNING, "Not sure about timestamp format for codec format %d\n", f->subclass);
392         }
393
394         /* Re-calculate last TS */
395         rtp->lastts = ms * 8;
396         
397         /* If it's close to ou prediction, go for it */
398         if (abs(rtp->lastts - pred) < 640)
399                 rtp->lastts = pred;
400 #if 0
401         else
402                 printf("Difference is %d, ms is %d\n", abs(rtp->lastts - pred), ms);
403 #endif                  
404         /* Get a pointer to the header */
405         rtpheader = (unsigned int *)(f->data - hdrlen);
406         rtpheader[0] = htonl((2 << 30) | (codec << 16) | (rtp->seqno++));
407         rtpheader[1] = htonl(rtp->lastts);
408         rtpheader[2] = htonl(rtp->ssrc); 
409         if (rtp->them.sin_port && rtp->them.sin_addr.s_addr) {
410                 res = sendto(rtp->s, (void *)rtpheader, f->datalen + hdrlen, 0, &rtp->them, sizeof(rtp->them));
411                 if (res <0) 
412                         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));
413 #if 0
414                 printf("Sent %d bytes of RTP data to %s:%d\n", res, inet_ntoa(rtp->them.sin_addr), ntohs(rtp->them.sin_port));
415 #endif          
416         }
417         return 0;
418 }
419
420 int ast_rtp_write(struct ast_rtp *rtp, struct ast_frame *_f)
421 {
422         struct ast_frame *f;
423         int codec;
424         int hdrlen = 12;
425         
426         /* Make sure we have enough space for RTP header */
427         
428         if (_f->frametype != AST_FRAME_VOICE) {
429                 ast_log(LOG_WARNING, "RTP can only send voice\n");
430                 return -1;
431         }
432
433         codec = ast2rtp(_f->subclass);
434         if (codec < 0) {
435                 ast_log(LOG_WARNING, "Don't know how to send format %d packets with RTP\n", _f->subclass);
436                 return -1;
437         }
438
439
440         switch(_f->subclass) {
441         case AST_FORMAT_ULAW:
442         case AST_FORMAT_ALAW:
443                 if (!rtp->smoother) {
444                         rtp->smoother = ast_smoother_new(160);
445                 }
446                 if (!rtp->smoother) {
447                         ast_log(LOG_WARNING, "Unable to create smoother :(\n");
448                         return -1;
449                 }
450                 ast_smoother_feed(rtp->smoother, _f);
451                 
452                 while((f = ast_smoother_read(rtp->smoother)))
453                         ast_rtp_raw_write(rtp, f, codec);
454                 break;
455         case AST_FORMAT_G729A:
456                 if (!rtp->smoother) {
457                         rtp->smoother = ast_smoother_new(20);
458                 }
459                 if (!rtp->smoother) {
460                         ast_log(LOG_WARNING, "Unable to create g729 smoother :(\n");
461                         return -1;
462                 }
463                 ast_smoother_feed(rtp->smoother, _f);
464                 
465                 while((f = ast_smoother_read(rtp->smoother)))
466                         ast_rtp_raw_write(rtp, f, codec);
467                 break;
468                 
469         default:        
470                 ast_log(LOG_WARNING, "Not sure about sending format %d packets\n", _f->subclass);
471                 if (_f->offset < hdrlen) {
472                         f = ast_frdup(_f);
473                 } else {
474                         f = _f;
475                 }
476                 ast_rtp_raw_write(rtp, f, codec);
477         }
478                 
479         return 0;
480 }