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