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