OSDN Git Service

Added missing diagnostics
[android-x86/external-alsa-lib.git] / src / pcm / pcm_adpcm.c
1 /*
2  *  PCM - Ima-ADPC conversion
3  *  Copyright (c) 2000 by Abramo Bagnara <abramo@alsa-project.org>
4  *  Copyright (c) 1999 by Uros Bizjak <uros@kss-loka.si>
5  *                        Jaroslav Kysela <perex@suse.cz>
6  *
7  *  Based on Version 1.2, 18-Dec-92 implementation of Intel/DVI ADPCM code
8  *  by Jack Jansen, CWI, Amsterdam <Jack.Jansen@cwi.nl>, Copyright 1992
9  *  by Stichting Mathematisch Centrum, Amsterdam, The Netherlands.
10  *
11  *   This library is free software; you can redistribute it and/or modify
12  *   it under the terms of the GNU Library General Public License as
13  *   published by the Free Software Foundation; either version 2 of
14  *   the License, or (at your option) any later version.
15  *
16  *   This program is distributed in the hope that it will be useful,
17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
19  *   GNU Library General Public License for more details.
20  *
21  *   You should have received a copy of the GNU Library General Public
22  *   License along with this library; if not, write to the Free Software
23  *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24  *
25  */
26   
27 /*
28 These routines convert 16 bit linear PCM samples to 4 bit ADPCM code
29 and vice versa. The ADPCM code used is the Intel/DVI ADPCM code which
30 is being recommended by the IMA Digital Audio Technical Working Group.
31
32 The algorithm for this coder was taken from:
33 Proposal for Standardized Audio Interstreamge Formats,
34 IMA compatability project proceedings, Vol 2, Issue 2, May 1992.
35
36 - No, this is *not* a G.721 coder/decoder. The algorithm used by G.721
37   is very complicated, requiring oodles of floating-point ops per
38   sample (resulting in very poor performance). I have not done any
39   tests myself but various people have assured my that 721 quality is
40   actually lower than DVI quality.
41
42 - No, it probably isn't a RIFF ADPCM decoder either. Trying to decode
43   RIFF ADPCM with these routines seems to result in something
44   recognizable but very distorted.
45
46 - No, it is not a CDROM-XA coder either, as far as I know. I haven't
47   come across a good description of XA yet.
48  */
49
50 #include <byteswap.h>
51 #include "pcm_local.h"
52 #include "pcm_plugin.h"
53
54 typedef struct {
55         int pred_val;           /* Calculated predicted value */
56         int step_idx;           /* Previous StepSize lookup index */
57 } adpcm_state_t;
58
59 typedef void (*adpcm_f)(snd_pcm_channel_area_t *src_areas,
60                         size_t src_offset,
61                         snd_pcm_channel_area_t *dst_areas,
62                         size_t dst_offset,
63                         size_t frames, size_t channels, int getputidx,
64                         adpcm_state_t *states);
65
66 typedef struct {
67         /* This field need to be the first */
68         snd_pcm_plugin_t plug;
69         int getput_idx;
70         adpcm_f func;
71         int sformat;
72         adpcm_state_t *states;
73 } snd_pcm_adpcm_t;
74
75 /* First table lookup for Ima-ADPCM quantizer */
76 static char IndexAdjust[8] = { -1, -1, -1, -1, 2, 4, 6, 8 };
77
78 /* Second table lookup for Ima-ADPCM quantizer */
79 static short StepSize[89] = {
80         7, 8, 9, 10, 11, 12, 13, 14, 16, 17,
81         19, 21, 23, 25, 28, 31, 34, 37, 41, 45,
82         50, 55, 60, 66, 73, 80, 88, 97, 107, 118,
83         130, 143, 157, 173, 190, 209, 230, 253, 279, 307,
84         337, 371, 408, 449, 494, 544, 598, 658, 724, 796,
85         876, 963, 1060, 1166, 1282, 1411, 1552, 1707, 1878, 2066,
86         2272, 2499, 2749, 3024, 3327, 3660, 4026, 4428, 4871, 5358,
87         5894, 6484, 7132, 7845, 8630, 9493, 10442, 11487, 12635, 13899,
88         15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767
89 };
90
91 static char adpcm_encoder(int sl, adpcm_state_t * state)
92 {
93         short diff;             /* Difference between sl and predicted sample */
94         short pred_diff;        /* Predicted difference to next sample */
95
96         unsigned char sign;     /* sign of diff */
97         short step;             /* holds previous StepSize value */
98         unsigned char adjust_idx;       /* Index to IndexAdjust lookup table */
99
100         int i;
101
102         /* Compute difference to previous predicted value */
103         diff = sl - state->pred_val;
104         sign = (diff < 0) ? 0x8 : 0x0;
105         if (sign) {
106                 diff = -diff;
107         }
108
109         /*
110          * This code *approximately* computes:
111          *    adjust_idx = diff * 4 / step;
112          *    pred_diff = (adjust_idx + 0.5) * step / 4;
113          *
114          * But in shift step bits are dropped. The net result of this is
115          * that even if you have fast mul/div hardware you cannot put it to
116          * good use since the fixup would be too expensive.
117          */
118
119         step = StepSize[state->step_idx];
120
121         /* Divide and clamp */
122         pred_diff = step >> 3;
123         for (adjust_idx = 0, i = 0x4; i; i >>= 1, step >>= 1) {
124                 if (diff >= step) {
125                         adjust_idx |= i;
126                         diff -= step;
127                         pred_diff += step;
128                 }
129         }
130
131         /* Update and clamp previous predicted value */
132         state->pred_val += sign ? -pred_diff : pred_diff;
133
134         if (state->pred_val > 32767) {
135                 state->pred_val = 32767;
136         } else if (state->pred_val < -32768) {
137                 state->pred_val = -32768;
138         }
139
140         /* Update and clamp StepSize lookup table index */
141         state->step_idx += IndexAdjust[adjust_idx];
142
143         if (state->step_idx < 0) {
144                 state->step_idx = 0;
145         } else if (state->step_idx > 88) {
146                 state->step_idx = 88;
147         }
148         return (sign | adjust_idx);
149 }
150
151
152 static int adpcm_decoder(unsigned char code, adpcm_state_t * state)
153 {
154         short pred_diff;        /* Predicted difference to next sample */
155         short step;             /* holds previous StepSize value */
156         char sign;
157
158         int i;
159
160         /* Separate sign and magnitude */
161         sign = code & 0x8;
162         code &= 0x7;
163
164         /*
165          * Computes pred_diff = (code + 0.5) * step / 4,
166          * but see comment in adpcm_coder.
167          */
168
169         step = StepSize[state->step_idx];
170
171         /* Compute difference and new predicted value */
172         pred_diff = step >> 3;
173         for (i = 0x4; i; i >>= 1, step >>= 1) {
174                 if (code & i) {
175                         pred_diff += step;
176                 }
177         }
178         state->pred_val += (sign) ? -pred_diff : pred_diff;
179
180         /* Clamp output value */
181         if (state->pred_val > 32767) {
182                 state->pred_val = 32767;
183         } else if (state->pred_val < -32768) {
184                 state->pred_val = -32768;
185         }
186
187         /* Find new StepSize index value */
188         state->step_idx += IndexAdjust[code];
189
190         if (state->step_idx < 0) {
191                 state->step_idx = 0;
192         } else if (state->step_idx > 88) {
193                 state->step_idx = 88;
194         }
195         return (state->pred_val);
196 }
197
198 static void adpcm_decode(snd_pcm_channel_area_t *src_areas,
199                          size_t src_offset,
200                          snd_pcm_channel_area_t *dst_areas,
201                          size_t dst_offset,
202                          size_t frames, size_t channels, int putidx,
203                          adpcm_state_t *states)
204 {
205 #define PUT16_LABELS
206 #include "plugin_ops.h"
207 #undef PUT16_LABELS
208         void *put = put16_labels[putidx];
209         size_t channel;
210         for (channel = 0; channel < channels; ++channel, ++states) {
211                 char *src;
212                 int srcbit;
213                 char *dst;
214                 int src_step, srcbit_step, dst_step;
215                 size_t frames1;
216                 snd_pcm_channel_area_t *src_area = &src_areas[channel];
217                 snd_pcm_channel_area_t *dst_area = &dst_areas[channel];
218 #if 0
219                 if (!src_area->enabled) {
220                         if (dst_area->wanted)
221                                 snd_pcm_area_silence(dst_area, dst_offset, frames, dst_sfmt);
222                         dst_area->enabled = 0;
223                         continue;
224                 }
225                 dst_area->enabled = 1;
226 #endif
227                 srcbit = src_area->first + src_area->step * src_offset;
228                 src = src_area->addr + srcbit / 8;
229                 srcbit %= 8;
230                 src_step = src_area->step / 8;
231                 srcbit_step = src_area->step % 8;
232                 dst = snd_pcm_channel_area_addr(dst_area, dst_offset);
233                 dst_step = snd_pcm_channel_area_step(dst_area);
234                 frames1 = frames;
235                 while (frames1-- > 0) {
236                         int16_t sample;
237                         int v;
238                         if (srcbit)
239                                 v = *src & 0x0f;
240                         else
241                                 v = (*src >> 4) & 0x0f;
242                         sample = adpcm_decoder(v, states);
243                         goto *put;
244 #define PUT16_END after
245 #include "plugin_ops.h"
246 #undef PUT16_END
247                 after:
248                         src += src_step;
249                         srcbit += srcbit_step;
250                         if (srcbit == 8) {
251                                 src++;
252                                 srcbit = 0;
253                         }
254                         dst += dst_step;
255                 }
256         }
257 }
258
259 static void adpcm_encode(snd_pcm_channel_area_t *src_areas,
260                          size_t src_offset,
261                          snd_pcm_channel_area_t *dst_areas,
262                          size_t dst_offset,
263                          size_t frames, size_t channels, int getidx,
264                          adpcm_state_t *states)
265 {
266 #define GET16_LABELS
267 #include "plugin_ops.h"
268 #undef GET16_LABELS
269         void *get = get16_labels[getidx];
270         size_t channel;
271         int16_t sample = 0;
272         for (channel = 0; channel < channels; ++channel, ++states) {
273                 char *src;
274                 char *dst;
275                 int dstbit;
276                 int src_step, dst_step, dstbit_step;
277                 size_t frames1;
278                 snd_pcm_channel_area_t *src_area = &src_areas[channel];
279                 snd_pcm_channel_area_t *dst_area = &dst_areas[channel];
280 #if 0
281                 if (!src_area->enabled) {
282                         if (dst_area->wanted)
283                                 snd_pcm_area_silence(dst_area, dst_offset, frames, dst_sfmt);
284                         dst_area->enabled = 0;
285                         continue;
286                 }
287                 dst_area->enabled = 1;
288 #endif
289                 src = snd_pcm_channel_area_addr(src_area, src_offset);
290                 src_step = snd_pcm_channel_area_step(src_area);
291                 dstbit = dst_area->first + dst_area->step * dst_offset;
292                 dst = dst_area->addr + dstbit / 8;
293                 dstbit %= 8;
294                 dst_step = dst_area->step / 8;
295                 dstbit_step = dst_area->step % 8;
296                 frames1 = frames;
297                 while (frames1-- > 0) {
298                         int v;
299                         goto *get;
300 #define GET16_END after
301 #include "plugin_ops.h"
302 #undef GET16_END
303                 after:
304                         v = adpcm_encoder(sample, states);
305                         if (dstbit)
306                                 *dst = (*dst & 0xf0) | v;
307                         else
308                                 *dst = (*dst & 0x0f) | (v << 4);
309                         src += src_step;
310                         dst += dst_step;
311                         dstbit += dstbit_step;
312                         if (dstbit == 8) {
313                                 dst++;
314                                 dstbit = 0;
315                         }
316                 }
317         }
318 }
319
320 static int snd_pcm_adpcm_close(snd_pcm_t *pcm)
321 {
322         snd_pcm_adpcm_t *adpcm = pcm->private;
323         int err = 0;
324         if (adpcm->plug.close_slave)
325                 err = snd_pcm_close(adpcm->plug.slave);
326         if (adpcm->states)
327                 free(adpcm->states);
328         free(adpcm);
329         return 0;
330 }
331
332 static int snd_pcm_adpcm_hw_info(snd_pcm_t *pcm, snd_pcm_hw_info_t * info)
333 {
334         snd_pcm_adpcm_t *adpcm = pcm->private;
335         unsigned int format_mask, access_mask;
336         int err;
337         info->access_mask &= (SND_PCM_ACCBIT_MMAP_INTERLEAVED | 
338                               SND_PCM_ACCBIT_RW_INTERLEAVED |
339                               SND_PCM_ACCBIT_MMAP_NONINTERLEAVED | 
340                               SND_PCM_ACCBIT_RW_NONINTERLEAVED);
341         access_mask = info->access_mask;
342         if (access_mask == 0)
343                 return -EINVAL;
344         if (adpcm->sformat == SND_PCM_FORMAT_IMA_ADPCM)
345                 info->format_mask &= SND_PCM_FMTBIT_LINEAR;
346         else
347                 info->format_mask &= SND_PCM_FMTBIT_IMA_ADPCM;
348         format_mask = info->format_mask;
349         if (format_mask == 0)
350                 return -EINVAL;
351
352         info->format_mask = 1U << adpcm->sformat;
353         info->access_mask = SND_PCM_ACCBIT_MMAP;
354         err = snd_pcm_hw_info(adpcm->plug.slave, info);
355         info->format_mask = format_mask;
356         info->access_mask = access_mask;
357         if (err < 0)
358                 return err;
359         info->info &= ~(SND_PCM_INFO_MMAP | SND_PCM_INFO_MMAP_VALID);
360         snd_pcm_hw_info_complete(info);
361         return 0;
362 }
363
364 static int snd_pcm_adpcm_hw_params(snd_pcm_t *pcm, snd_pcm_hw_params_t * params)
365 {
366         snd_pcm_adpcm_t *adpcm = pcm->private;
367         snd_pcm_t *slave = adpcm->plug.slave;
368         snd_pcm_hw_info_t sinfo;
369         snd_pcm_hw_params_t sparams;
370         int err;
371         snd_pcm_hw_params_to_info(params, &sinfo);
372         sinfo.format_mask = 1 << adpcm->sformat;
373         sinfo.access_mask = SND_PCM_ACCBIT_MMAP;
374         err = snd_pcm_hw_params_info(slave, &sparams, &sinfo);
375         params->fail_mask = sparams.fail_mask;
376         if (err < 0)
377                 return err;
378         if (pcm->stream == SND_PCM_STREAM_PLAYBACK) {
379                 if (adpcm->sformat == SND_PCM_FORMAT_IMA_ADPCM) {
380                         adpcm->getput_idx = get_index(params->format, SND_PCM_FORMAT_S16);
381                         adpcm->func = adpcm_encode;
382                 } else {
383                         adpcm->getput_idx = put_index(SND_PCM_FORMAT_S16, adpcm->sformat);
384                         adpcm->func = adpcm_decode;
385                 }
386         } else {
387                 if (adpcm->sformat == SND_PCM_FORMAT_IMA_ADPCM) {
388                         adpcm->getput_idx = put_index(SND_PCM_FORMAT_S16, params->format);
389                         adpcm->func = adpcm_decode;
390                 } else {
391                         adpcm->getput_idx = get_index(adpcm->sformat, SND_PCM_FORMAT_S16);
392                         adpcm->func = adpcm_encode;
393                 }
394         }
395         if (adpcm->states)
396                 free(adpcm->states);
397         adpcm->states = malloc(params->channels * sizeof(*adpcm->states));
398         return 0;
399 }
400
401 static int snd_pcm_adpcm_init(snd_pcm_t *pcm)
402 {
403         snd_pcm_adpcm_t *adpcm = pcm->private;
404         unsigned int k;
405         for (k = 0; k < pcm->channels; ++k) {
406                 adpcm->states[k].pred_val = 0;
407                 adpcm->states[k].step_idx = 0;
408         }
409         return 0;
410 }
411
412 static ssize_t snd_pcm_adpcm_write_areas(snd_pcm_t *pcm,
413                                          snd_pcm_channel_area_t *areas,
414                                          size_t offset,
415                                          size_t size,
416                                          size_t *slave_sizep)
417 {
418         snd_pcm_adpcm_t *adpcm = pcm->private;
419         snd_pcm_t *slave = adpcm->plug.slave;
420         size_t xfer = 0;
421         ssize_t err = 0;
422         if (slave_sizep && *slave_sizep < size)
423                 size = *slave_sizep;
424         assert(size > 0);
425         while (xfer < size) {
426                 size_t frames = snd_pcm_mmap_playback_xfer(slave, size - xfer);
427                 adpcm->func(areas, offset, 
428                             snd_pcm_mmap_areas(slave), snd_pcm_mmap_offset(slave),
429                             frames, pcm->channels,
430                             adpcm->getput_idx, adpcm->states);
431                 err = snd_pcm_mmap_forward(slave, frames);
432                 if (err < 0)
433                         break;
434                 assert((size_t)err == frames);
435                 offset += err;
436                 xfer += err;
437                 snd_pcm_mmap_hw_forward(pcm, err);
438         }
439         if (xfer > 0) {
440                 if (slave_sizep)
441                         *slave_sizep = xfer;
442                 return xfer;
443         }
444         return err;
445 }
446
447 static ssize_t snd_pcm_adpcm_read_areas(snd_pcm_t *pcm,
448                                         snd_pcm_channel_area_t *areas,
449                                         size_t offset,
450                                         size_t size,
451                                         size_t *slave_sizep)
452 {
453         snd_pcm_adpcm_t *adpcm = pcm->private;
454         snd_pcm_t *slave = adpcm->plug.slave;
455         size_t xfer = 0;
456         ssize_t err = 0;
457         if (slave_sizep && *slave_sizep < size)
458                 size = *slave_sizep;
459         assert(size > 0);
460         while (xfer < size) {
461                 size_t frames = snd_pcm_mmap_capture_xfer(slave, size - xfer);
462                 adpcm->func(snd_pcm_mmap_areas(slave), snd_pcm_mmap_offset(slave),
463                             areas, offset, 
464                             frames, pcm->channels,
465                             adpcm->getput_idx, adpcm->states);
466                 err = snd_pcm_mmap_forward(slave, frames);
467                 if (err < 0)
468                         break;
469                 assert((size_t)err == frames);
470                 offset += err;
471                 xfer += err;
472                 snd_pcm_mmap_hw_forward(pcm, err);
473         }
474         if (xfer > 0) {
475                 if (slave_sizep)
476                         *slave_sizep = xfer;
477                 return xfer;
478         }
479         return err;
480 }
481
482 static void snd_pcm_adpcm_dump(snd_pcm_t *pcm, FILE *fp)
483 {
484         snd_pcm_adpcm_t *adpcm = pcm->private;
485         fprintf(fp, "Ima-ADPCM conversion PCM (%s)\n", 
486                 snd_pcm_format_name(adpcm->sformat));
487         if (pcm->setup) {
488                 fprintf(fp, "Its setup is:\n");
489                 snd_pcm_dump_setup(pcm, fp);
490         }
491         fprintf(fp, "Slave: ");
492         snd_pcm_dump(adpcm->plug.slave, fp);
493 }
494
495 snd_pcm_ops_t snd_pcm_adpcm_ops = {
496         close: snd_pcm_adpcm_close,
497         info: snd_pcm_plugin_info,
498         hw_info: snd_pcm_adpcm_hw_info,
499         hw_params: snd_pcm_adpcm_hw_params,
500         sw_params: snd_pcm_plugin_sw_params,
501         dig_info: snd_pcm_plugin_dig_info,
502         dig_params: snd_pcm_plugin_dig_params,
503         channel_info: snd_pcm_plugin_channel_info,
504         dump: snd_pcm_adpcm_dump,
505         nonblock: snd_pcm_plugin_nonblock,
506         async: snd_pcm_plugin_async,
507         mmap: snd_pcm_plugin_mmap,
508         munmap: snd_pcm_plugin_munmap,
509 };
510
511 int snd_pcm_adpcm_open(snd_pcm_t **pcmp, char *name, int sformat, snd_pcm_t *slave, int close_slave)
512 {
513         snd_pcm_t *pcm;
514         snd_pcm_adpcm_t *adpcm;
515         assert(pcmp && slave);
516         if (snd_pcm_format_linear(sformat) != 1 &&
517             sformat != SND_PCM_FORMAT_IMA_ADPCM)
518                 return -EINVAL;
519         adpcm = calloc(1, sizeof(snd_pcm_adpcm_t));
520         if (!adpcm) {
521                 return -ENOMEM;
522         }
523         adpcm->sformat = sformat;
524         adpcm->plug.read = snd_pcm_adpcm_read_areas;
525         adpcm->plug.write = snd_pcm_adpcm_write_areas;
526         adpcm->plug.init = snd_pcm_adpcm_init;
527         adpcm->plug.slave = slave;
528         adpcm->plug.close_slave = close_slave;
529
530         pcm = calloc(1, sizeof(snd_pcm_t));
531         if (!pcm) {
532                 free(adpcm);
533                 return -ENOMEM;
534         }
535         if (name)
536                 pcm->name = strdup(name);
537         pcm->type = SND_PCM_TYPE_ADPCM;
538         pcm->stream = slave->stream;
539         pcm->mode = slave->mode;
540         pcm->ops = &snd_pcm_adpcm_ops;
541         pcm->op_arg = pcm;
542         pcm->fast_ops = &snd_pcm_plugin_fast_ops;
543         pcm->fast_op_arg = pcm;
544         pcm->private = adpcm;
545         pcm->poll_fd = slave->poll_fd;
546         pcm->hw_ptr = &adpcm->plug.hw_ptr;
547         pcm->appl_ptr = &adpcm->plug.appl_ptr;
548         *pcmp = pcm;
549
550         return 0;
551 }
552
553 int _snd_pcm_adpcm_open(snd_pcm_t **pcmp, char *name,
554                          snd_config_t *conf, 
555                          int stream, int mode)
556 {
557         snd_config_iterator_t i;
558         char *sname = NULL;
559         int err;
560         snd_pcm_t *spcm;
561         int sformat = -1;
562         snd_config_foreach(i, conf) {
563                 snd_config_t *n = snd_config_entry(i);
564                 if (strcmp(n->id, "comment") == 0)
565                         continue;
566                 if (strcmp(n->id, "type") == 0)
567                         continue;
568                 if (strcmp(n->id, "stream") == 0)
569                         continue;
570                 if (strcmp(n->id, "sname") == 0) {
571                         err = snd_config_string_get(n, &sname);
572                         if (err < 0) {
573                                 ERR("Invalid type for %s", n->id);
574                                 return -EINVAL;
575                         }
576                         continue;
577                 }
578                 if (strcmp(n->id, "sformat") == 0) {
579                         char *f;
580                         err = snd_config_string_get(n, &f);
581                         if (err < 0) {
582                                 ERR("Invalid type for %s", n->id);
583                                 return -EINVAL;
584                         }
585                         sformat = snd_pcm_format_value(f);
586                         if (sformat < 0) {
587                                 ERR("Unknown sformat");
588                                 return -EINVAL;
589                         }
590                         if (snd_pcm_format_linear(sformat) != 1 &&
591                             sformat != SND_PCM_FORMAT_IMA_ADPCM) {
592                                 ERR("Invalid sformat");
593                                 return -EINVAL;
594                         }
595                         continue;
596                 }
597                 ERR("Unknown field %s", n->id);
598                 return -EINVAL;
599         }
600         if (!sname) {
601                 ERR("sname is not defined");
602                 return -EINVAL;
603         }
604         if (sformat < 0) {
605                 ERR("sformat is not defined");
606                 return -EINVAL;
607         }
608         /* This is needed cause snd_config_update may destroy config */
609         sname = strdup(sname);
610         if (!sname)
611                 return  -ENOMEM;
612         err = snd_pcm_open(&spcm, sname, stream, mode);
613         free(sname);
614         if (err < 0)
615                 return err;
616         err = snd_pcm_adpcm_open(pcmp, name, sformat, spcm, 1);
617         if (err < 0)
618                 snd_pcm_close(spcm);
619         return err;
620 }
621                                 
622