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