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