apps/dsp.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2005 Miika Pekkarinen
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19 #include <string.h>
  20 #include "dsp.h"
  21 #include "kernel.h"
  22 #include "playback.h"
  23 #include "system.h"
  24
  25 /* The "dither" code to convert the 24-bit samples produced by libmad was
  26  * taken from the coolplayer project - coolplayer.sourceforge.net
  27  */
  28
  29 /* 16-bit samples are scaled based on these constants. The shift should be
  30  * no more than 15.
  31  */
  32 #define WORD_SHIFT          12
  33 #define WORD_FRACBITS       27
  34
  35 #define NATIVE_DEPTH        16
  36 #define SAMPLE_BUF_SIZE     256
  37 #define RESAMPLE_BUF_SIZE   (256 * 4)   /* Enough for 11,025 Hz -> 44,100 Hz*/
  38
  39 #if CONFIG_CPU == MCF5249 && !defined(SIMULATOR)
  40
  41 #define INIT() asm volatile ("move.l #0xb0, %macsr") /* frac, round, clip */
  42 /* Multiply 2 32-bit integers and return the 32 most significant bits of the
  43  * result.
  44  */
  45 #define FRACMUL(x, y) \
  46 ({ \
  47     long t; \
  48     asm volatile ("mac.l %[a], %[b], %%acc0\n\t" \
  49                   "movclr.l %%acc0, %[t]\n\t" \
  50                   : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \
  51     t; \
  52 })
  53
  54 #else
  55
  56 #define INIT()
  57 #define FRACMUL(x, y) (long) (((((long long) (x)) * ((long long) (y))) >> 32))
  58
  59 #endif
  60
  61 struct dsp_config
  62 {
  63     long frequency;
  64     long clip_min;
  65     long clip_max;
  66     int sample_depth;
  67     int sample_bytes;
  68     int stereo_mode;
  69     int frac_bits;
  70     bool dither_enabled;
  71 };
  72
  73 struct resample_data
  74 {
  75     long last_sample;
  76     long phase;
  77     long delta;
  78 };
  79
  80 struct dither_data
  81 {
  82     long error[3];
  83     long random;
  84 };
  85
  86 static struct dsp_config dsp;
  87 static struct dither_data dither_data[2] IDATA_ATTR;
  88 static struct resample_data resample_data[2] IDATA_ATTR;
  89
  90 /* The internal format is 32-bit samples, non-interleaved, stereo. This
  91  * format is similar to the raw output from several codecs, so the amount
  92  * of copying needed is minimized for that case.
  93  */
  94
  95 static long sample_buf[SAMPLE_BUF_SIZE] IDATA_ATTR;
  96 static long resample_buf[RESAMPLE_BUF_SIZE] IDATA_ATTR;
  97
  98
  99 /* Convert at most count samples to the internal format, if needed. Returns
 100  * number of samples ready for further processing. Updates src to point
 101  * past the samples "consumed" and dst is set to point to the samples to
 102  * consume. Note that for mono, dst[0] equals dst[1], as there is no point
 103  * in processing the same data twice.
 104  */
 105 static int convert_to_internal(char* src[], int count, long* dst[])
 106 {
 107     count = MIN(SAMPLE_BUF_SIZE / 2, count);
 108
 109     if ((dsp.sample_depth <= NATIVE_DEPTH)
 110         || (dsp.stereo_mode == STEREO_INTERLEAVED))
 111     {
 112         dst[0] = &sample_buf[0];
 113         dst[1] = (dsp.stereo_mode == STEREO_MONO)
 114             ? dst[0] : &sample_buf[SAMPLE_BUF_SIZE / 2];
 115     }
 116     else
 117     {
 118         dst[0] = (long*) src[0];
 119         dst[1] = (long*) ((dsp.stereo_mode == STEREO_MONO) ? src[0] : src[1]);
 120     }
 121
 122     if (dsp.sample_depth <= NATIVE_DEPTH)
 123     {
 124         short* s0 = (short*) src[0];
 125         long* d0 = dst[0];
 126         long* d1 = dst[1];
 127         int scale = WORD_SHIFT;
 128         int i;
 129
 130         if (dsp.stereo_mode == STEREO_INTERLEAVED)
 131         {
 132             for (i = 0; i < count; i++)
 133             {
 134                 *d0++ = *s0++ << scale;
 135                 *d1++ = *s0++ << scale;
 136             }
 137         }
 138         else if (dsp.stereo_mode == STEREO_NONINTERLEAVED)
 139         {
 140             short* s1 = (short*) src[1];
 141
 142             for (i = 0; i < count; i++)
 143             {
 144                 *d0++ = *s0++ << scale;
 145                 *d1++ = *s1++ << scale;
 146             }
 147         }
 148         else
 149         {
 150             for (i = 0; i < count; i++)
 151             {
 152                 *d0++ = *s0++ << scale;
 153             }
 154         }
 155     }
 156     else if (dsp.stereo_mode == STEREO_INTERLEAVED)
 157     {
 158         long* s0 = (long*) src[0];
 159         long* d0 = dst[0];
 160         long* d1 = dst[1];
 161         int i;
 162
 163         for (i = 0; i < count; i++)
 164         {
 165             *d0++ = *s0++;
 166             *d1++ = *s0++;
 167         }
 168     }
 169
 170     if (dsp.stereo_mode == STEREO_NONINTERLEAVED)
 171     {
 172         src[0] += count * dsp.sample_bytes;
 173         src[1] += count * dsp.sample_bytes;
 174     }
 175     else if (dsp.stereo_mode == STEREO_INTERLEAVED)
 176     {
 177         src[0] += count * dsp.sample_bytes * 2;
 178     }
 179     else
 180     {
 181         src[0] += count * dsp.sample_bytes;
 182     }
 183
 184     return count;
 185 }
 186
 187 /* Linear resampling that introduces a one sample delay, because of our
 188  * inability to look into the future at the end of a frame.
 189  */
 190
 191 static long downsample(long *dst, long *src, int count,
 192     struct resample_data *r)
 193 {
 194     long phase = r->phase;
 195     long delta = r->delta;
 196     long last_sample = r->last_sample;
 197     int pos = phase >> 16;
 198     int i = 1;
 199
 200     INIT();
 201
 202     /* Do we need last sample of previous frame for interpolation? */
 203     if (pos > 0)
 204     {
 205         last_sample = src[pos - 1];
 206     }
 207
 208     *dst++ = last_sample + FRACMUL((phase & 0xffff) << 15,
 209         src[pos] - last_sample);
 210     phase += delta;
 211
 212     while ((pos = phase >> 16) < count)
 213     {
 214         *dst++ = src[pos - 1] + FRACMUL((phase & 0xffff) << 15,
 215             src[pos] - src[pos - 1]);
 216         phase += delta;
 217         i++;
 218     }
 219
 220     /* Wrap phase accumulator back to start of next frame. */
 221     r->phase = phase - (count << 16);
 222     r->delta = delta;
 223     r->last_sample = src[count - 1];
 224     return i;
 225 }
 226
 227 static long upsample(long *dst, long *src, int count, struct resample_data *r)
 228 {
 229     long phase = r->phase;
 230     long delta = r->delta;
 231     long last_sample = r->last_sample;
 232     int i = 0;
 233     int pos;
 234
 235     INIT();
 236
 237     while ((pos = phase >> 16) == 0)
 238     {
 239         *dst++ = last_sample + FRACMUL((phase & 0xffff) << 15,
 240             src[pos] - last_sample);
 241         phase += delta;
 242         i++;
 243     }
 244
 245     while ((pos = phase >> 16) < count)
 246     {
 247         *dst++ = src[pos - 1] + FRACMUL((phase & 0xffff) << 15,
 248             src[pos] - src[pos - 1]);
 249         phase += delta;
 250         i++;
 251     }
 252
 253     /* Wrap phase accumulator back to start of next frame. */
 254     r->phase = phase - (count << 16);
 255     r->delta = delta;
 256     r->last_sample = src[count - 1];
 257     return i;
 258 }
 259
 260 /* Resample count stereo samples. Updates the src array, if resampling is
 261  * done, to refer to the resampled data. Returns number of stereo samples
 262  * for further processing.
 263  */
 264 static inline int resample(long* src[], int count)
 265 {
 266     long new_count;
 267
 268     if (dsp.frequency != NATIVE_FREQUENCY)
 269     {
 270         long* d0 = &resample_buf[0];
 271         /* Only process the second channel if needed. */
 272         long* d1 = (src[0] == src[1]) ? d0
 273             : &resample_buf[RESAMPLE_BUF_SIZE / 2];
 274
 275         if (dsp.frequency < NATIVE_FREQUENCY)
 276         {
 277             new_count = upsample(d0, src[0], count, &resample_data[0]);
 278
 279             if (d0 != d1)
 280             {
 281                 upsample(d1, src[1], count, &resample_data[1]);
 282             }
 283         }
 284         else
 285         {
 286             new_count = downsample(d0, src[0], count, &resample_data[0]);
 287
 288             if (d0 != d1)
 289             {
 290                 downsample(d1, src[1], count, &resample_data[1]);
 291             }
 292         }
 293
 294         src[0] = d0;
 295         src[1] = d1;
 296     }
 297     else
 298     {
 299         new_count = count;
 300     }
 301
 302     return new_count;
 303 }
 304
 305 static inline long clip_sample(long sample)
 306 {
 307     if (sample > dsp.clip_max)
 308     {
 309         sample = dsp.clip_max;
 310     }
 311     else if (sample < dsp.clip_min)
 312     {
 313         sample = dsp.clip_min;
 314     }
 315
 316     return sample;
 317 }
 318
 319 /* The "dither" code to convert the 24-bit samples produced by libmad was
 320  * taken from the coolplayer project - coolplayer.sourceforge.net
 321  */
 322
 323 static long dither_sample(long sample, long bias, long mask,
 324     struct dither_data* dither)
 325 {
 326     long output;
 327     long random;
 328
 329     /* Noise shape and bias */
 330
 331     sample += dither->error[0] - dither->error[1] + dither->error[2];
 332     dither->error[2] = dither->error[1];
 333     dither->error[1] = dither->error[0] / 2;
 334
 335     output = sample + bias;
 336
 337     /* Dither */
 338
 339     random = dither->random * 0x0019660dL + 0x3c6ef35fL;
 340     sample += (random & mask) - (dither->random & mask);
 341     dither->random = random;
 342
 343     /* Clip and quantize */
 344
 345     sample = clip_sample(sample);
 346     output = clip_sample(output) & ~mask;
 347
 348     /* Error feedback */
 349
 350     dither->error[0] = sample - output;
 351
 352     return output;
 353 }
 354
 355 static void write_samples(short* dst, long* src[], int count)
 356 {
 357     long* s0 = src[0];
 358     long* s1 = src[1];
 359     int scale = dsp.frac_bits + 1 - NATIVE_DEPTH;
 360
 361     if (dsp.dither_enabled)
 362     {
 363         long bias = (1L << (dsp.frac_bits - NATIVE_DEPTH));
 364         long mask = (1L << scale) - 1;
 365
 366         while (count-- > 0)
 367         {
 368             *dst++ = (short) (dither_sample(*s0++, bias, mask, &dither_data[0])
 369                 >> scale);
 370             *dst++ = (short) (dither_sample(*s1++, bias, mask, &dither_data[1])
 371                 >> scale);
 372         }
 373     }
 374     else
 375     {
 376         while (count-- > 0)
 377         {
 378             *dst++ = (short) (clip_sample(*s0++) >> scale);
 379             *dst++ = (short) (clip_sample(*s1++) >> scale);
 380         }
 381     }
 382 }
 383
 384 /* Process and convert src audio to dst based on the DSP configuration,
 385  * reading size bytes of audio data. dst is assumed to be large enough; use
 386  * dst_get_dest_size() to get the required size. src is an array of
 387  * pointers; for mono and interleaved stereo, it contains one pointer to the
 388  * start of the audio data; for non-interleaved stereo, it contains two
 389  * pointers, one for each audio channel. Returns number of bytes written to
 390  * dest.
 391  */
 392 long dsp_process(char* dst, char* src[], long size)
 393 {
 394     long* tmp[2];
 395     long written = 0;
 396     long factor = (dsp.stereo_mode != STEREO_MONO) ? 2 : 1;
 397     int samples;
 398
 399     size /= dsp.sample_bytes * factor;
 400
 401     while (size > 0)
 402     {
 403         samples = convert_to_internal(src, size, tmp);
 404         size -= samples;
 405         samples = resample(tmp, samples);
 406         write_samples((short*) dst, tmp, samples);
 407         written += samples;
 408         dst += samples * sizeof(short) * 2;
 409         yield();
 410     }
 411
 412     return written * sizeof(short) * 2;
 413 }
 414
 415 /* Given size bytes of input data, calculate the maximum number of bytes of
 416  * output data that would be generated (the calculation is not entirely
 417  * exact and rounds upwards to be on the safe side; during resampling,
 418  * the number of samples generated depends on the current state of the
 419  * resampler).
 420  */
 421 long dsp_output_size(long size)
 422 {
 423     if (dsp.stereo_mode == STEREO_MONO)
 424     {
 425         size *= 2;
 426     }
 427
 428     if (dsp.sample_depth > NATIVE_DEPTH)
 429     {
 430         size /= 2;
 431     }
 432
 433     if (dsp.frequency != NATIVE_FREQUENCY)
 434     {
 435         size = (long) ((((unsigned long) size * NATIVE_FREQUENCY)
 436             + (dsp.frequency - 1)) / dsp.frequency);
 437     }
 438
 439     return (size + 3) & ~3;
 440 }
 441
 442 /* Given size bytes of output buffer, calculate number of bytes of input
 443  * data that would be consumed in order to fill the output buffer.
 444  */
 445 long dsp_input_size(long size)
 446 {
 447     if (dsp.stereo_mode == STEREO_MONO)
 448     {
 449         size /= 2;
 450     }
 451
 452     if (dsp.sample_depth > NATIVE_DEPTH)
 453     {
 454         size *= 2;
 455     }
 456
 457     if (dsp.frequency != NATIVE_FREQUENCY)
 458     {
 459         size = (long) ((((unsigned long) size * dsp.frequency)
 460             + (NATIVE_FREQUENCY - 1)) / NATIVE_FREQUENCY);
 461     }
 462
 463     return size;
 464 }
 465
 466 int dsp_stereo_mode(void)
 467 {
 468     return dsp.stereo_mode;
 469 }
 470
 471 bool dsp_configure(int setting, void *value)
 472 {
 473     switch (setting)
 474     {
 475     case DSP_SET_FREQUENCY:
 476         dsp.frequency = ((int) value == 0) ? NATIVE_FREQUENCY : (int) value;
 477         memset(resample_data, 0, sizeof(resample_data));
 478         resample_data[0].delta = resample_data[1].delta =
 479             (unsigned long) dsp.frequency * 65536 / NATIVE_FREQUENCY;
 480         break;
 481
 482     case DSP_SET_CLIP_MIN:
 483         dsp.clip_min = (long) value;
 484         break;
 485
 486     case DSP_SET_CLIP_MAX:
 487         dsp.clip_max = (long) value;
 488         break;
 489
 490     case DSP_SET_SAMPLE_DEPTH:
 491         dsp.sample_depth = (long) value;
 492
 493         if (dsp.sample_depth <= NATIVE_DEPTH)
 494         {
 495             dsp.frac_bits = WORD_FRACBITS;
 496             dsp.sample_bytes = sizeof(short);
 497             dsp.clip_max =  ((1 << WORD_FRACBITS) - 1);
 498             dsp.clip_min = -((1 << WORD_FRACBITS));
 499         }
 500         else
 501         {
 502             dsp.frac_bits = (long) value;
 503             dsp.sample_bytes = sizeof(long);
 504         }
 505
 506         break;
 507
 508     case DSP_SET_STEREO_MODE:
 509         dsp.stereo_mode = (long) value;
 510         break;
 511
 512     case DSP_RESET:
 513         dsp.dither_enabled = false;
 514         dsp.stereo_mode = STEREO_NONINTERLEAVED;
 515         dsp.clip_max =  ((1 << WORD_FRACBITS) - 1);
 516         dsp.clip_min = -((1 << WORD_FRACBITS));
 517         dsp.frequency = NATIVE_FREQUENCY;
 518         dsp.sample_depth = NATIVE_DEPTH;
 519         dsp.frac_bits = WORD_FRACBITS;
 520         break;
 521
 522     case DSP_DITHER:
 523         memset(dither_data, 0, sizeof(dither_data));
 524         dsp.dither_enabled = (bool) value;
 525         break;
 526
 527     default:
 528         return 0;
 529     }
 530
 531     return 1;
 532 }