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