1 files changed, 24 insertions, 279 deletions

diff --git a/apps/codecs/mpa.c b/apps/codecs/mpa.c
index 736eef1ffe..f052b9df88 100644
--- a/apps/codecs/mpa.c
+++ b/apps/codecs/mpa.c
@@ -22,6 +22,7 @@
 #include <codecs/libmad/mad.h>
 
 #include "playback.h"
+#include "dsp.h"
 #include "mp3data.h"
 #include "lib/codeclib.h"
 
@@ -29,7 +30,6 @@ struct mad_stream Stream IDATA_ATTR;
 struct mad_frame Frame IDATA_ATTR;
 struct mad_synth Synth IDATA_ATTR;
 mad_timer_t Timer;
-struct dither d0, d1;
 
 /* The following function is used inside libmad - let's hope it's never
    called. 
@@ -38,122 +38,6 @@ struct dither d0, d1;
 void abort(void) {
 }
 
-/* The "dither" code to convert the 24-bit samples produced by libmad was
-   taken from the coolplayer project - coolplayer.sourceforge.net */
-
-struct dither {
-    mad_fixed_t error[3];
-    mad_fixed_t random;
-};
-
-# define SAMPLE_DEPTH   16
-# define scale(x, y)    dither((x), (y))
-
-/*
- * NAME:                prng()
- * DESCRIPTION: 32-bit pseudo-random number generator
- */
-static __inline
-unsigned long prng(unsigned long state)
-{
-    return (state * 0x0019660dL + 0x3c6ef35fL) & 0xffffffffL;
-}
-
-/*
- * NAME:        dither()
- * DESCRIPTION: dither and scale sample
- */
-inline int dither(mad_fixed_t sample, struct dither *dither)
-{
-    unsigned int scalebits;
-    mad_fixed_t output, mask, random;
-
-    enum {
-        MIN = -MAD_F_ONE,
-        MAX =  MAD_F_ONE - 1
-     };
-
-    /* noise shape */
-    sample += dither->error[0] - dither->error[1] + dither->error[2];
-
-    dither->error[2] = dither->error[1];
-    dither->error[1] = dither->error[0]/2;
-
-    /* bias */
-    output = sample + (1L << (MAD_F_FRACBITS + 1 - SAMPLE_DEPTH - 1));
-
-    scalebits = MAD_F_FRACBITS + 1 - SAMPLE_DEPTH;
-    mask = (1L << scalebits) - 1;
-
-    /* dither */
-    random  = prng(dither->random);
-    output += (random & mask) - (dither->random & mask);
-
-    //dither->random = random;
-
-    /* clip */
-    if (output > MAX) {
-        output = MAX;
- 
-        if (sample > MAX)
-            sample = MAX;
-    } else if (output < MIN) {
-        output = MIN;
-
-        if (sample < MIN)
-            sample = MIN;
-    }
-
-    /* quantize */
-    output &= ~mask;
-
-    /* error feedback */
-    dither->error[0] = sample - output;
-
-    /* scale */
-    return output >> scalebits;
-}
-
-inline int detect_silence(mad_fixed_t sample)
-{
-    unsigned int scalebits;
-    mad_fixed_t output, mask;
-
-    enum {
-        MIN = -MAD_F_ONE,
-        MAX =  MAD_F_ONE - 1
-    };
-
-    /* bias */
-    output = sample + (1L << (MAD_F_FRACBITS + 1 - SAMPLE_DEPTH - 1));
-
-    scalebits = MAD_F_FRACBITS + 1 - SAMPLE_DEPTH;
-    mask = (1L << scalebits) - 1;
-
-    /* clip */
-    if (output > MAX) {
-        output = MAX;
-
-        if (sample > MAX)
-            sample = MAX;
-    } else if (output < MIN) {
-        output = MIN;
-
-        if (sample < MIN)
-            sample = MIN;
-    }
-
-    /* quantize */
-    output &= ~mask;
-
-    /* scale */
-    output >>= scalebits + 4;
-  
-    if (output == 0x00 || output == 0xff)
-        return 1;
-    
-    return 0;
-}
 
 #define INPUT_CHUNK_SIZE   8192
 #define OUTPUT_BUFFER_SIZE 65536 /* Must be an integer multiple of 4. */
@@ -162,7 +46,6 @@ unsigned char OutputBuffer[OUTPUT_BUFFER_SIZE];
 unsigned char *OutputPtr;
 unsigned char *GuardPtr = NULL;
 const unsigned char *OutputBufferEnd = OutputBuffer + OUTPUT_BUFFER_SIZE;
-long resampled_data[2][5000]; /* enough to cope with 11khz upsampling */
 
 mad_fixed_t mad_frame_overlap[2][32][18] IDATA_ATTR;
 unsigned char mad_main_data[MAD_BUFFER_MDLEN] IDATA_ATTR;
@@ -174,73 +57,7 @@ extern char iramstart[];
 extern char iramend[];
 #endif
 
-#undef DEBUG_GAPLESS
-
-struct resampler {
-    long last_sample, phase, delta;
-};
-
-#if CONFIG_CPU==MCF5249 && !defined(SIMULATOR)
-
-#define INIT() asm volatile ("move.l #0xb0, %macsr") /* frac, round, clip */
-#define FRACMUL(x, y) \
-({ \
-    long t; \
-    asm volatile ("mac.l %[a], %[b], %%acc0\n\t" \
-                  "movclr.l %%acc0, %[t]\n\t" \
-                  : [t] "=r" (t) : [a] "r" (x), [b] "r" (y)); \
-    t; \
-})
-
-#else
-
-#define INIT()
-#define FRACMUL(x, y) (long)(((long long)(x)*(long long)(y)) << 1)
-#endif
-
-/* linear resampling, introduces one sample delay, because of our inability to
-   look into the future at the end of a frame */
-long downsample(long *in, long *out, int num, struct resampler *s)
-{
-    long i = 1, pos;
-    long last = s->last_sample;
-
-    INIT();
-    pos = s->phase >> 16;
-    /* check if we need last sample of previous frame for interpolation */
-    if (pos > 0)
-        last = in[pos - 1];
-    out[0] = last + FRACMUL((s->phase & 0xffff) << 15, in[pos] - last);
-    s->phase += s->delta;
-    while ((pos = s->phase >> 16) < num) {
-        out[i++] = in[pos - 1] + FRACMUL((s->phase & 0xffff) << 15, in[pos] - in[pos - 1]);
-        s->phase += s->delta;
-    }
-    /* wrap phase accumulator back to start of next frame */
-    s->phase -= num << 16;
-    s->last_sample = in[num - 1];
-    return i;
-}
-
-long upsample(long *in, long *out, int num, struct resampler *s)
-{
-    long i = 0, pos;
-
-    INIT();
-    while ((pos = s->phase >> 16) == 0) {
-        out[i++] = s->last_sample + FRACMUL((s->phase & 0xffff) << 15, in[pos] - s->last_sample);
-        s->phase += s->delta;
-    }
-    while ((pos = s->phase >> 16) < num) {
-        out[i++] = in[pos - 1] + FRACMUL((s->phase & 0xffff) << 15, in[pos] - in[pos - 1]);
-        s->phase += s->delta;
-    }
-    /* wrap phase accumulator back to start of next frame */
-    s->phase -= num << 16;
-    s->last_sample = in[num - 1];
-    return i;
-}
-
+/*
 long resample(long *in, long *out, int num, struct resampler *s)
 {
     if (s->delta >= (1 << 16))
@@ -248,7 +65,7 @@ long resample(long *in, long *out, int num, struct resampler *s)
     else
         return upsample(in, out, num, s);
 }
-
+*/
 /* this is the codec entry point */
 enum codec_status codec_start(struct codec_api* api)
 {
@@ -257,20 +74,12 @@ enum codec_status codec_start(struct codec_api* api)
     int Status = 0;
     size_t size;
     int file_end;
-    unsigned short Sample;
     char *InputBuffer;
     unsigned int samplecount;
     unsigned int samplesdone;
     bool first_frame;
-#ifdef DEBUG_GAPLESS
-    bool first = true;
-    int fd;
-#endif
-    int i;
-    int yieldcounter = 0;
     int stop_skip, start_skip;
-    struct resampler lr = { 0, 0, 0 }, rr = { 0, 0, 0 };
-    long length;  
+    // struct resampler lr = { 0, 0, 0 }, rr = { 0, 0, 0 };
     /* Generic codec inititialisation */
 
     TEST_CODEC_API(api);
@@ -289,7 +98,13 @@ enum codec_status codec_start(struct codec_api* api)
 
     ci->configure(CODEC_SET_FILEBUF_LIMIT, (int *)(1024*1024*2));
     ci->configure(CODEC_SET_FILEBUF_CHUNKSIZE, (int *)(1024*16));
-  
+    ci->configure(DSP_SET_CLIP_MIN, (int *)-MAD_F_ONE);
+    ci->configure(DSP_SET_CLIP_MAX, (int *)(MAD_F_ONE - 1));
+    ci->configure(DSP_SET_SAMPLE_DEPTH, (int *)(MAD_F_FRACBITS));
+    ci->configure(DSP_DITHER, (bool *)true);
+    ci->configure(DSP_SET_STEREO_MODE, (int *)STEREO_NONINTERLEAVED);
+    ci->configure(CODEC_DSP_ENABLE, (bool *)true);
+    
     ci->memset(&Stream, 0, sizeof(struct mad_stream));
     ci->memset(&Frame, 0, sizeof(struct mad_frame));
     ci->memset(&Synth, 0, sizeof(struct mad_synth));
@@ -309,14 +124,6 @@ enum codec_status codec_start(struct codec_api* api)
        for gapless playback */
   next_track:
   
-#ifdef DEBUG_GAPLESS
-    if (first)
-        fd = ci->open("/first.pcm", O_WRONLY | O_CREAT);
-    else
-        fd = ci->open("/second.pcm", O_WRONLY | O_CREAT);
-    first = false;
-#endif
-  
     info = ci->mp3data;
     first_frame = false;
     file_end = 0;
@@ -325,6 +132,8 @@ enum codec_status codec_start(struct codec_api* api)
     while (!*ci->taginfo_ready)
         ci->yield();
   
+    ci->configure(DSP_SET_FREQUENCY, (int *)ci->id3->frequency);
+    
     ci->request_buffer(&size, ci->id3->first_frame_offset);
     ci->advance_buffer(size);
  
@@ -350,13 +159,7 @@ enum codec_status codec_start(struct codec_api* api)
         samplecount = ci->id3->length * (ci->id3->frequency / 100) / 10;
         samplesdone = ci->id3->elapsed * (ci->id3->frequency / 100) / 10;
     }
-    /* rb->snprintf(buf2, sizeof(buf2), "sc: %d", samplecount);
-       rb->splash(0, true, buf2);
-       rb->snprintf(buf2, sizeof(buf2), "length: %d", ci->id3->length);
-       rb->splash(HZ*5, true, buf2);
-       rb->snprintf(buf2, sizeof(buf2), "frequency: %d", ci->id3->frequency);
-       rb->splash(HZ*5, true, buf2); */
-    lr.delta = rr.delta = ci->id3->frequency*65536/44100; 
+    
     /* This is the decoding loop. */
     while (1) {
         ci->yield();
@@ -387,9 +190,6 @@ enum codec_status codec_start(struct codec_api* api)
             mad_stream_buffer(&Stream, InputBuffer, size);
         }
     
-        //if ((int)ci->curpos >= ci->id3->first_frame_offset)
-        //first_frame = true;
-        
         if(mad_frame_decode(&Frame,&Stream))
         {
             if (Stream.error == MAD_FLAG_INCOMPLETE || Stream.error == MAD_ERROR_BUFLEN) {
@@ -428,78 +228,23 @@ enum codec_status codec_start(struct codec_api* api)
 
         mad_synth_frame(&Synth,&Frame);
     
-        //if (!first_frame) {
-        //samplecount -= Synth.pcm.length;
-        //continue ;
-        //}
-    
         /* Convert MAD's numbers to an array of 16-bit LE signed integers */
         /* We skip start_skip number of samples here, this should only happen for
            very first frame in the stream. */
         /* TODO: possible for start_skip to exceed one frames worth of samples? */
-        length = resample((long *)&Synth.pcm.samples[0][start_skip], resampled_data[0], Synth.pcm.length, &lr);
-        if (MAD_NCHANNELS(&Frame.header) == 2)
-            resample((long *)&Synth.pcm.samples[1][start_skip], resampled_data[1], Synth.pcm.length, &rr);
-        for (i = 0; i < length; i++)
-        {
-            start_skip = 0; /* not very elegant, and might want to keep this value */
-            samplesdone++;
-            //if (ci->mp3data->padding > 0) {
-            //  ci->mp3data->padding--;
-            //  continue ;
-            //}
-            /*if (!first_frame) {
-              if (detect_silence(Synth.pcm.samples[0][i]))
-              continue ;
-              first_frame = true;
-              }*/
-      
-            /* Left channel */
-            Sample = scale(resampled_data[0][i], &d0);
-            *(OutputPtr++) = Sample >> 8;
-            *(OutputPtr++) = Sample & 0xff;
-
-            /* Right channel. If the decoded stream is monophonic then
-             * the right output channel is the same as the left one.
-             */
-            if (MAD_NCHANNELS(&Frame.header) == 2)
-                Sample = scale(resampled_data[1][i], &d1);
-            *(OutputPtr++) = Sample >> 8;
-            *(OutputPtr++) = Sample & 0xff;
-      
-            samplecount--;
-            if (samplecount == 0) {
-#ifdef DEBUG_GAPLESS
-                ci->write(fd, OutputBuffer, (int)OutputPtr - (int)OutputBuffer);
-#endif
-                while (!ci->audiobuffer_insert(OutputBuffer, (int)OutputPtr - (int)OutputBuffer))
-                    ci->yield();
-                goto song_end;
-            }
-  
-            if (yieldcounter++ == 200) {
-                ci->yield();
-                yieldcounter = 0;
-            }
-      
-            /* Flush the buffer if it is full. */
-            if (OutputPtr == OutputBufferEnd)
-            {
-#ifdef DEBUG_GAPLESS
-                ci->write(fd, OutputBuffer, OUTPUT_BUFFER_SIZE);
-#endif
-                while (!ci->audiobuffer_insert(OutputBuffer, OUTPUT_BUFFER_SIZE))
-                    ci->yield();
-                OutputPtr = OutputBuffer;
-            }
-        }
+        //length = resample((long *)&Synth.pcm.samples[0][start_skip], resampled_data[0], Synth.pcm.length, &lr);
+        //if (MAD_NCHANNELS(&Frame.header) == 2)
+        //    resample((long *)&Synth.pcm.samples[1][start_skip], resampled_data[1], Synth.pcm.length, &rr);
+        ci->audiobuffer_insert_split(&Synth.pcm.samples[0][start_skip], 
+                                     &Synth.pcm.samples[1][start_skip],
+                                     (Synth.pcm.length - start_skip) * 4);
+        start_skip = 0; /* not very elegant, and might want to keep this value */
+        
+        samplesdone += Synth.pcm.length;
+        samplecount -= Synth.pcm.length;
         ci->set_elapsed(samplesdone / (ci->id3->frequency/1000));
     }
   
-  song_end:
-#ifdef DEBUG_GAPLESS
-    ci->close(fd);
-#endif
     Stream.error = 0;
   
     if (ci->request_next_track())