c2aa090d5e5268cd15cabfa1fd8ebd8677fabed2
[asterisk/asterisk.git] / codecs / codec_adpcm.c
1 /* codec_adpcm.c - translate between signed linear and Dialogic ADPCM
2  * 
3  * Asterisk -- A telephony toolkit for Linux.
4  *
5  * Based on frompcm.c and topcm.c from the Emiliano MIPL browser/
6  * interpreter.  See http://www.bsdtelephony.com.mx
7  *
8  * Copyright (c) 2001 Linux Support Services, Inc.  All rights reserved.
9  *
10  * Karl Sackett <krs@linux-support.net>, 2001-3-21
11  *
12  * This program is free software, distributed under the terms of
13  * the GNU General Public License
14  */
15
16 #include <asterisk/logger.h>
17 #include <asterisk/module.h>
18 #include <asterisk/translate.h>
19 #include <asterisk/channel.h>
20 #include <fcntl.h>
21 #include <netinet/in.h>
22 #include <pthread.h>
23 #include <stdio.h>
24 #include <stdlib.h>
25 #include <string.h>
26 #include <unistd.h>
27
28 #define BUFFER_SIZE   8096      /* size for the translation buffers */
29
30 static pthread_mutex_t localuser_lock = PTHREAD_MUTEX_INITIALIZER;
31 static int localusecnt = 0;
32
33 static char *tdesc = "Adaptive Differential PCM Coder/Decoder";
34
35 /* Sample frame data */
36
37 #include "slin_adpcm_ex.h"
38 #include "adpcm_slin_ex.h"
39
40 /*
41  * Step size index shift table 
42  */
43
44 static short indsft[8] = { -1, -1, -1, -1, 2, 4, 6, 8 };
45
46 /*
47  * Step size table, where stpsz[i]=floor[16*(11/10)^i]
48  */
49
50 static short stpsz[49] = {
51   16, 17, 19, 21, 23, 25, 28, 31, 34, 37, 41, 45, 50, 55, 60, 66, 73,
52   80, 88, 97, 107, 118, 130, 143, 157, 173, 190, 209, 230, 253, 279,
53   307, 337, 371, 408, 449, 494, 544, 598, 658, 724, 796, 876, 963,
54   1060, 1166, 1282, 1411, 1552
55 };
56
57 /* 
58  * Nibble to bit map
59  */
60
61 static short nbl2bit[16][4] = {
62   {1, 0, 0, 0}, {1, 0, 0, 1}, {1, 0, 1, 0}, {1, 0, 1, 1},
63   {1, 1, 0, 0}, {1, 1, 0, 1}, {1, 1, 1, 0}, {1, 1, 1, 1},
64   {-1, 0, 0, 0}, {-1, 0, 0, 1}, {-1, 0, 1, 0}, {-1, 0, 1, 1},
65   {-1, 1, 0, 0}, {-1, 1, 0, 1}, {-1, 1, 1, 0}, {-1, 1, 1, 1}
66 };
67
68 /*
69  * Decode(encoded)
70  *  Decodes the encoded nibble from the adpcm file.
71  *
72  * Results:
73  *  Returns the encoded difference.
74  *
75  * Side effects:
76  *  Sets the index to the step size table for the next encode.
77  */
78
79 static inline short
80 decode (unsigned char encoded, short *ssindex)
81 {
82   short diff, step;
83   step = stpsz[*ssindex];
84   diff = nbl2bit[encoded][0] * (step * nbl2bit[encoded][1] +
85                                 (step >> 1) * nbl2bit[encoded][2] +
86                                 (step >> 2) * nbl2bit[encoded][3] +
87                                 (step >> 3));
88   *ssindex = *ssindex + indsft[(encoded & 7)];
89   if (*ssindex < 0)
90     *ssindex = 0;
91   else if (*ssindex > 48)
92     *ssindex = 48;
93   return (diff);
94 }
95
96 /*
97  * Adpcm
98  *  Takes a signed linear signal and encodes it as ADPCM
99  *  For more information see http://support.dialogic.com/appnotes/adpcm.pdf
100  *
101  * Results:
102  *  Foo.
103  *
104  * Side effects:
105  *  signal gets updated with each pass.
106  */
107
108 static inline unsigned char
109 adpcm (short csig, short *ssindex, short *signal)
110 {
111   short diff, step;
112   unsigned char encoded;
113   step = stpsz[*ssindex];
114   /* 
115    * Clip csig if too large or too small
116    */
117    
118   csig >>= 4;
119
120   diff = csig - *signal;
121   
122   if (diff < 0)
123     {
124       encoded = 8;
125       diff = -diff;
126     }
127   else
128     encoded = 0;
129   if (diff >= step)
130     {
131       encoded |= 4;
132       diff -= step;
133     }
134   step >>= 1;
135   if (diff >= step)
136     {
137       encoded |= 2;
138       diff -= step;
139     }
140   step >>= 1;
141   if (diff >= step)
142     encoded |= 1;
143     
144   *signal += decode (encoded, ssindex);
145   return (encoded);
146 }
147
148 /*
149  * Private workspace for translating signed linear signals to ADPCM.
150  */
151
152 struct adpcm_encoder_pvt
153 {
154   struct ast_frame f;
155   char offset[AST_FRIENDLY_OFFSET];   /* Space to build offset */
156   short inbuf[BUFFER_SIZE];           /* Unencoded signed linear values */
157   unsigned char outbuf[BUFFER_SIZE];  /* Encoded ADPCM, two nibbles to a word */
158   short ssindex;
159   short signal;
160   int tail;
161 };
162
163 /*
164  * Private workspace for translating ADPCM signals to signed linear.
165  */
166
167 struct adpcm_decoder_pvt
168 {
169   struct ast_frame f;
170   char offset[AST_FRIENDLY_OFFSET];     /* Space to build offset */
171   short outbuf[BUFFER_SIZE];    /* Decoded signed linear values */
172   int tail;
173 };
174
175 /*
176  * AdpcmToLin_New
177  *  Create a new instance of adpcm_decoder_pvt.
178  *
179  * Results:
180  *  Returns a pointer to the new instance.
181  *
182  * Side effects:
183  *  None.
184  */
185
186 static struct ast_translator_pvt *
187 adpcmtolin_new ()
188 {
189   struct adpcm_decoder_pvt *tmp;
190   tmp = malloc (sizeof (struct adpcm_decoder_pvt));
191   if (tmp)
192     {
193           memset(tmp, 0, sizeof(*tmp));
194       tmp->tail = 0;
195       localusecnt++;
196       ast_update_use_count ();
197     }
198   return (struct ast_translator_pvt *) tmp;
199 }
200
201 /*
202  * LinToAdpcm_New
203  *  Create a new instance of adpcm_encoder_pvt.
204  *
205  * Results:
206  *  Returns a pointer to the new instance.
207  *
208  * Side effects:
209  *  None.
210  */
211
212 static struct ast_translator_pvt *
213 lintoadpcm_new ()
214 {
215   struct adpcm_encoder_pvt *tmp;
216   tmp = malloc (sizeof (struct adpcm_encoder_pvt));
217   if (tmp)
218     {
219           memset(tmp, 0, sizeof(*tmp));
220       localusecnt++;
221       ast_update_use_count ();
222       tmp->tail = 0;
223     }
224   return (struct ast_translator_pvt *) tmp;
225 }
226
227 /*
228  * AdpcmToLin_FrameIn
229  *  Fill an input buffer with packed 4-bit ADPCM values if there is room
230  *  left.
231  *
232  * Results:
233  *  Foo
234  *
235  * Side effects:
236  *  tmp->tail is the number of packed values in the buffer.
237  */
238
239 static int
240 adpcmtolin_framein (struct ast_translator_pvt *pvt, struct ast_frame *f)
241 {
242   struct adpcm_decoder_pvt *tmp = (struct adpcm_decoder_pvt *) pvt;
243   int x;
244   short signal;
245   short ssindex;
246   unsigned char *b;
247
248   if (f->datalen < 3) {
249         ast_log(LOG_WARNING, "Didn't have at least three bytes of input\n");
250         return -1;
251   }
252
253   if ((f->datalen - 3) * 4 > sizeof(tmp->outbuf)) {
254         ast_log(LOG_WARNING, "Out of buffer space\n");
255         return -1;
256   }
257
258   /* Reset ssindex and signal to frame's specified values */
259   b = f->data;
260   ssindex = b[0];
261   if (ssindex < 0)
262         ssindex = 0;
263   if (ssindex > 48)
264     ssindex = 48;
265
266   signal = (b[1] << 8) | b[2]; 
267   
268   for (x=3;x<f->datalen;x++) {
269         signal += decode(b[x] >> 4, &ssindex);
270         if (signal > 2047)
271                 signal = 2047;
272         if (signal < -2048)
273                 signal = -2048;
274     tmp->outbuf[tmp->tail++] = signal << 4;
275         signal +=  decode(b[x] & 0x0f, &ssindex);
276         if (signal > 2047)
277                 signal = 2047;
278         if (signal < -2048)
279                 signal = -2048;
280     tmp->outbuf[tmp->tail++] = signal << 4;
281   }
282
283   return 0;
284 }
285
286 /*
287  * AdpcmToLin_FrameOut
288  *  Convert 4-bit ADPCM encoded signals to 16-bit signed linear.
289  *
290  * Results:
291  *  Converted signals are placed in tmp->f.data, tmp->f.datalen
292  *  and tmp->f.timelen are calculated.
293  *
294  * Side effects:
295  *  None.
296  */
297
298 static struct ast_frame *
299 adpcmtolin_frameout (struct ast_translator_pvt *pvt)
300 {
301   struct adpcm_decoder_pvt *tmp = (struct adpcm_decoder_pvt *) pvt;
302
303   if (!tmp->tail)
304     return NULL;
305
306   tmp->f.frametype = AST_FRAME_VOICE;
307   tmp->f.subclass = AST_FORMAT_SLINEAR;
308   tmp->f.datalen = tmp->tail *2;
309   tmp->f.timelen = tmp->tail / 8;
310   tmp->f.mallocd = 0;
311   tmp->f.offset = AST_FRIENDLY_OFFSET;
312   tmp->f.src = __PRETTY_FUNCTION__;
313   tmp->f.data = tmp->outbuf;
314   tmp->tail = 0;
315   return &tmp->f;
316 }
317
318 /*
319  * LinToAdpcm_FrameIn
320  *  Fill an input buffer with 16-bit signed linear PCM values.
321  *
322  * Results:
323  *  None.
324  *
325  * Side effects:
326  *  tmp->tail is number of signal values in the input buffer.
327  */
328
329 static int
330 lintoadpcm_framein (struct ast_translator_pvt *pvt, struct ast_frame *f)
331 {
332   struct adpcm_encoder_pvt *tmp = (struct adpcm_encoder_pvt *) pvt;
333
334   if ((tmp->tail + f->datalen / 2) < (sizeof (tmp->inbuf) / 2))
335     {
336       memcpy (&tmp->inbuf[tmp->tail], f->data, f->datalen);
337       tmp->tail += f->datalen / 2;
338     }
339   else
340     {
341       ast_log (LOG_WARNING, "Out of buffer space\n");
342       return -1;
343     }
344   return 0;
345 }
346
347 /*
348  * LinToAdpcm_FrameOut
349  *  Convert a buffer of raw 16-bit signed linear PCM to a buffer
350  *  of 4-bit ADPCM packed two to a byte (Big Endian).
351  *
352  * Results:
353  *  Foo
354  *
355  * Side effects:
356  *  Leftover inbuf data gets packed, tail gets updated.
357  */
358
359 static struct ast_frame *
360 lintoadpcm_frameout (struct ast_translator_pvt *pvt)
361 {
362   struct adpcm_encoder_pvt *tmp = (struct adpcm_encoder_pvt *) pvt;
363   unsigned char adpcm0, adpcm1;
364   int i_max, i;
365   
366   if (tmp->tail < 2) return NULL;
367
368
369   i_max = (tmp->tail / 2) * 2;
370
371   tmp->outbuf[0] = tmp->ssindex & 0xff;
372   tmp->outbuf[1] = (tmp->signal >> 8) & 0xff;
373   tmp->outbuf[2] = (tmp->signal & 0xff);
374
375   for (i = 0; i < i_max; i+=2)
376   {
377     adpcm0 = adpcm(tmp->inbuf[i], &tmp->ssindex, &tmp->signal);
378     adpcm1 = adpcm(tmp->inbuf[i+1], &tmp->ssindex, &tmp->signal);
379     tmp->outbuf[i/2 + 3] = (adpcm0 << 4) | adpcm1;
380   };
381
382
383   tmp->f.frametype = AST_FRAME_VOICE;
384   tmp->f.subclass = AST_FORMAT_ADPCM;
385   tmp->f.timelen = i_max / 8;
386   tmp->f.mallocd = 0;
387   tmp->f.offset = AST_FRIENDLY_OFFSET;
388   tmp->f.src = __PRETTY_FUNCTION__;
389   tmp->f.data = tmp->outbuf;
390   tmp->f.datalen = i_max / 2 + 3;
391
392   /*
393    * If there is a signal left over (there should be no more than
394    * one) move it to the beginning of the input buffer.
395    */
396
397   if (tmp->tail == i_max)
398     tmp->tail = 0;
399   else
400     {
401       tmp->inbuf[0] = tmp->inbuf[tmp->tail];
402       tmp->tail = 1;
403     }
404   return &tmp->f;
405 }
406
407
408 /*
409  * AdpcmToLin_Sample
410  */
411
412 static struct ast_frame *
413 adpcmtolin_sample ()
414 {
415   static struct ast_frame f;
416   f.frametype = AST_FRAME_VOICE;
417   f.subclass = AST_FORMAT_ADPCM;
418   f.datalen = sizeof (adpcm_slin_ex);
419   f.timelen = sizeof(adpcm_slin_ex) / 4;
420   f.mallocd = 0;
421   f.offset = 0;
422   f.src = __PRETTY_FUNCTION__;
423   f.data = adpcm_slin_ex;
424   return &f;
425 }
426
427 /*
428  * LinToAdpcm_Sample
429  */
430
431 static struct ast_frame *
432 lintoadpcm_sample ()
433 {
434   static struct ast_frame f;
435   f.frametype = AST_FRAME_VOICE;
436   f.subclass = AST_FORMAT_SLINEAR;
437   f.datalen = sizeof (slin_adpcm_ex);
438   /* Assume 8000 Hz */
439   f.timelen = sizeof (slin_adpcm_ex) / 16;
440   f.mallocd = 0;
441   f.offset = 0;
442   f.src = __PRETTY_FUNCTION__;
443   f.data = slin_adpcm_ex;
444   return &f;
445 }
446
447 /*
448  * Adpcm_Destroy
449  *  Destroys a private workspace.
450  *
451  * Results:
452  *  It's gone!
453  *
454  * Side effects:
455  *  None.
456  */
457
458 static void
459 adpcm_destroy (struct ast_translator_pvt *pvt)
460 {
461   free (pvt);
462   localusecnt--;
463   ast_update_use_count ();
464 }
465
466 /*
467  * The complete translator for ADPCMToLin.
468  */
469
470 static struct ast_translator adpcmtolin = {
471   "adpcmtolin",
472   AST_FORMAT_ADPCM,
473   AST_FORMAT_SLINEAR,
474   adpcmtolin_new,
475   adpcmtolin_framein,
476   adpcmtolin_frameout,
477   adpcm_destroy,
478   /* NULL */
479   adpcmtolin_sample
480 };
481
482 /*
483  * The complete translator for LinToADPCM.
484  */
485
486 static struct ast_translator lintoadpcm = {
487   "lintoadpcm",
488   AST_FORMAT_SLINEAR,
489   AST_FORMAT_ADPCM,
490   lintoadpcm_new,
491   lintoadpcm_framein,
492   lintoadpcm_frameout,
493   adpcm_destroy,
494   /* NULL */
495   lintoadpcm_sample
496 };
497
498 int
499 unload_module (void)
500 {
501   int res;
502   ast_pthread_mutex_lock (&localuser_lock);
503   res = ast_unregister_translator (&lintoadpcm);
504   if (!res)
505     res = ast_unregister_translator (&adpcmtolin);
506   if (localusecnt)
507     res = -1;
508   ast_pthread_mutex_unlock (&localuser_lock);
509   return res;
510 }
511
512 int
513 load_module (void)
514 {
515   int res;
516   res = ast_register_translator (&adpcmtolin);
517   if (!res)
518     res = ast_register_translator (&lintoadpcm);
519   else
520     ast_unregister_translator (&adpcmtolin);
521   return res;
522 }
523
524 /*
525  * Return a description of this module.
526  */
527
528 char *
529 description (void)
530 {
531   return tdesc;
532 }
533
534 int
535 usecount (void)
536 {
537   int res;
538   STANDARD_USECOUNT (res);
539   return res;
540 }
541
542 char *
543 key ()
544 {
545   return ASTERISK_GPL_KEY;
546 }