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