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