Initial DSP implementation. DSP supports resampling audio stream from

codecs (currently works corrently only with mp3's, somebody should fix
that).


git-svn-id: svn://svn.rockbox.org/rockbox/trunk@6877 a1c6a512-1295-4272-9138-f99709370657

15 files changed, 805 insertions, 328 deletions

diff --git a/apps/SOURCES b/apps/SOURCES
index 738533105b..271c2ba48c 100644
--- a/apps/SOURCES
+++ b/apps/SOURCES
@@ -52,6 +52,7 @@ recorder/recording.c
 playback.c
 metadata.c
 codecs.c
+dsp.c
 #ifndef SIMULATOR
 pcm_recording.c
 #endif
diff --git a/apps/codecs.c b/apps/codecs.c
index 053a622841..da4b1f8bff 100644
--- a/apps/codecs.c
+++ b/apps/codecs.c
@@ -88,6 +88,7 @@ struct codec_api ci = {
     NULL,
     NULL,
     NULL,
+    NULL,
     
     splash,
 
diff --git a/apps/codecs.h b/apps/codecs.h
index e322d758e1..c373bdb8ba 100644
--- a/apps/codecs.h
+++ b/apps/codecs.h
@@ -143,6 +143,7 @@ struct codec_api {
     /* Insert PCM data into audio buffer for playback. Playback will start
        automatically. */
     bool (*audiobuffer_insert)(char *data, size_t length);
+    bool (*audiobuffer_insert_split)(void *ch1, void *ch2, size_t length);
     /* Set song position in WPS (value in ms). */
     void (*set_elapsed)(unsigned int value);
     
diff --git a/apps/codecs/a52.c b/apps/codecs/a52.c
index bc711965ec..663e7941ec 100644
--- a/apps/codecs/a52.c
+++ b/apps/codecs/a52.c
@@ -24,6 +24,7 @@
 #include <codecs/liba52/a52.h>
 
 #include "playback.h"
+#include "dsp.h"
 #include "lib/codeclib.h"
 
 #define BUFFER_SIZE 4096
@@ -173,12 +174,26 @@ 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*128));
 
+  ci->configure(DSP_DITHER, (bool *)false);
+  ci->configure(DSP_SET_STEREO_MODE, (int *)STEREO_INTERLEAVED);
+  ci->configure(DSP_SET_SAMPLE_DEPTH, (int *)(16));
+  
   next_track:
 
   if (codec_init(api)) {
       return CODEC_ERROR;
   }
 
+  while (!rb->taginfo_ready)
+      rb->yield();
+    
+  if (rb->id3->frequency != NATIVE_FREQUENCY) {
+      rb->configure(DSP_SET_FREQUENCY, (long *)(rb->id3->frequency));
+      rb->configure(CODEC_DSP_ENABLE, (bool *)true);
+  } else {
+      rb->configure(CODEC_DSP_ENABLE, (bool *)false);
+  }
+    
   /* Intialise the A52 decoder and check for success */
   state = a52_init (0); // Parameter is "accel"
 
diff --git a/apps/codecs/flac.c b/apps/codecs/flac.c
index 07e5b8f566..d7ae037d26 100644
--- a/apps/codecs/flac.c
+++ b/apps/codecs/flac.c
@@ -22,6 +22,7 @@
 #include <codecs/libFLAC/include/FLAC/seekable_stream_decoder.h>
 #include "playback.h"
 #include "lib/codeclib.h"
+#include "dsp.h"
 
 #define FLAC_MAX_SUPPORTED_BLOCKSIZE 4608
 #define FLAC_MAX_SUPPORTED_CHANNELS  2
@@ -180,12 +181,26 @@ enum codec_status codec_start(struct codec_api* api)
   ci->configure(CODEC_SET_FILEBUF_WATERMARK, (int *)(1024*512));
   ci->configure(CODEC_SET_FILEBUF_CHUNKSIZE, (int *)(1024*128));
 
+  ci->configure(DSP_DITHER, (bool *)false);
+  ci->configure(DSP_SET_STEREO_MODE, (int *)STEREO_INTERLEAVED);
+  ci->configure(DSP_SET_SAMPLE_DEPTH, (int *)(16));
+  
   next_track:
 
   if (codec_init(api)) {
     return CODEC_ERROR;
   }
 
+  while (!rb->taginfo_ready)
+      rb->yield();
+    
+  if (rb->id3->frequency != NATIVE_FREQUENCY) {
+      rb->configure(DSP_SET_FREQUENCY, (long *)(rb->id3->frequency));
+      rb->configure(CODEC_DSP_ENABLE, (bool *)true);
+  } else {
+      rb->configure(CODEC_DSP_ENABLE, (bool *)false);
+  }
+  
   /* Create a decoder instance */
 
   flacDecoder=FLAC__seekable_stream_decoder_new();
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())
diff --git a/apps/codecs/vorbis.c b/apps/codecs/vorbis.c
index f2939aa68d..9afeb053e1 100644
--- a/apps/codecs/vorbis.c
+++ b/apps/codecs/vorbis.c
@@ -21,6 +21,7 @@
 
 #include "Tremor/ivorbisfile.h"
 #include "playback.h"
+#include "dsp.h"
 #include "lib/codeclib.h"
 
 static struct codec_api* rb;
@@ -92,10 +93,6 @@ enum codec_status codec_start(struct codec_api* api)
     long n;
     int current_section;
     int eof;
-#if BYTE_ORDER == BIG_ENDIAN
-    int i;
-    char x;
-#endif
 
     TEST_CODEC_API(api);
 
@@ -110,15 +107,27 @@ enum codec_status codec_start(struct codec_api* api)
     rb->configure(CODEC_SET_FILEBUF_LIMIT, (int *)(1024*1024*2));
     rb->configure(CODEC_SET_FILEBUF_CHUNKSIZE, (int *)(1024*64));
     
-    /* We need to flush reserver memory every track load. */
+    rb->configure(DSP_DITHER, (bool *)false);
+    rb->configure(DSP_SET_STEREO_MODE, (int *)STEREO_INTERLEAVED);
+    rb->configure(DSP_SET_SAMPLE_DEPTH, (int *)(16));
+    
+/* We need to flush reserver memory every track load. */
   next_track:
     if (codec_init(rb)) {
         return CODEC_ERROR;
     }
 
-
+    while (!rb->taginfo_ready)
+        rb->yield();
+    
+    if (rb->id3->frequency != NATIVE_FREQUENCY) {
+        rb->configure(DSP_SET_FREQUENCY, (long *)(rb->id3->frequency));
+        rb->configure(CODEC_DSP_ENABLE, (bool *)true);
+    } else {
+        rb->configure(CODEC_DSP_ENABLE, (bool *)false);
+    }
+    
     /* Create a decoder instance */
-
     callbacks.read_func=read_handler;
     callbacks.seek_func=seek_handler;
     callbacks.tell_func=tell_handler;
@@ -148,17 +157,10 @@ enum codec_status codec_start(struct codec_api* api)
             if (rb->stop_codec || rb->reload_codec)
                 break ;
         
-            rb->yield();
             while (!rb->audiobuffer_insert(pcmbuf, n))
                 rb->yield();
 
             rb->set_elapsed(ov_time_tell(&vf));
-
-#if BYTE_ORDER == BIG_ENDIAN
-            for (i=0;i<n;i+=2) { 
-                x=pcmbuf[i]; pcmbuf[i]=pcmbuf[i+1]; pcmbuf[i+1]=x;
-            }
-#endif
         }
     }
   
diff --git a/apps/codecs/wav.c b/apps/codecs/wav.c
index dfed97d64c..49bd12da1f 100644
--- a/apps/codecs/wav.c
+++ b/apps/codecs/wav.c
@@ -20,6 +20,7 @@
 #include "codec.h"
 #include "playback.h"
 #include "lib/codeclib.h"
+#include "dsp.h"
 
 #define BYTESWAP(x) (((x>>8) & 0xff) | ((x<<8) & 0xff00))
 
@@ -60,12 +61,26 @@ enum codec_status codec_start(struct codec_api* api)
   ci->configure(CODEC_SET_FILEBUF_WATERMARK, (int *)(1024*512));
   ci->configure(CODEC_SET_FILEBUF_CHUNKSIZE, (int *)(1024*256));
 
+  ci->configure(DSP_DITHER, (bool *)false);
+  ci->configure(DSP_SET_STEREO_MODE, (int *)STEREO_INTERLEAVED);
+  ci->configure(DSP_SET_SAMPLE_DEPTH, (int *)(16));
+  
   next_track:
 
   if (codec_init(api)) {
     return CODEC_ERROR;
   }
 
+  while (!rb->taginfo_ready)
+      rb->yield();
+    
+  if (rb->id3->frequency != NATIVE_FREQUENCY) {
+      rb->configure(DSP_SET_FREQUENCY, (long *)(rb->id3->frequency));
+      rb->configure(CODEC_DSP_ENABLE, (bool *)true);
+  } else {
+      rb->configure(CODEC_DSP_ENABLE, (bool *)false);
+  }
+    
   /* FIX: Correctly parse WAV header - we assume canonical 44-byte header */
 
   header=ci->request_buffer(&n,44);
@@ -116,7 +131,7 @@ enum codec_status codec_start(struct codec_api* api)
 
     /* Byte-swap data */
     for (i=0;i<n/2;i++) {
-      wavbuf[i]=BYTESWAP(wavbuf[i]);
+      wavbuf[i]=SWAB16(wavbuf[i]);
     }
 
     samplesdone+=nsamples;
diff --git a/apps/codecs/wavpack.c b/apps/codecs/wavpack.c
index 2ea8f052df..275f5f11e4 100644
--- a/apps/codecs/wavpack.c
+++ b/apps/codecs/wavpack.c
@@ -22,6 +22,7 @@
 #include <codecs/libwavpack/wavpack.h>
 #include "playback.h"
 #include "lib/codeclib.h"
+#include "dsp.h"
 
 static struct codec_api *rb;
 static struct codec_api *ci;
@@ -61,14 +62,27 @@ enum codec_status codec_start(struct codec_api* api)
     ci->configure(CODEC_SET_FILEBUF_LIMIT, (int *)(1024*1024*10));
     ci->configure(CODEC_SET_FILEBUF_WATERMARK, (int *)(1024*512));
     ci->configure(CODEC_SET_FILEBUF_CHUNKSIZE, (int *)(1024*128));
+  
+    ci->configure(DSP_DITHER, (bool *)false);
+    ci->configure(DSP_SET_STEREO_MODE, (int *)STEREO_INTERLEAVED);
+    ci->configure(DSP_SET_SAMPLE_DEPTH, (int *)(16));
 
     next_track:
 
     if (codec_init(api))
         return CODEC_ERROR;
 
+    while (!rb->taginfo_ready)
+        ci->yield();
+        
+    if (ci->id3->frequency != NATIVE_FREQUENCY) {
+        ci->configure(DSP_SET_FREQUENCY, (long *)(ci->id3->frequency));
+        ci->configure(CODEC_DSP_ENABLE, (bool *)true);
+    } else {
+        ci->configure(CODEC_DSP_ENABLE, (bool *)false);
+    }
+    
     /* Create a decoder instance */
-
     wpc = WavpackOpenFileInput (read_callback, error);
 
     if (!wpc)
diff --git a/apps/dsp.c b/apps/dsp.c
new file mode 100644
index 0000000000..963e98da2e
--- /dev/null
+++ b/apps/dsp.c
@@ -0,0 +1,397 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 Miika Pekkarinen
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#include "kernel.h"
+#include "logf.h"
+
+#include "dsp.h"
+#include "playback.h"
+#include "system.h"
+
+/* The "dither" code to convert the 24-bit samples produced by libmad was
+   taken from the coolplayer project - coolplayer.sourceforge.net */
+struct s_dither {
+    int error[3];
+    int random;
+};
+
+static struct s_dither dither[2];
+struct dsp_configuration dsp_config;
+static int channel;
+static int fracbits;
+
+#define SAMPLE_DEPTH     16
+
+/*
+ * NAME:                prng()
+ * DESCRIPTION: 32-bit pseudo-random number generator
+ */
+static __inline
+unsigned long prng(unsigned long state)
+{
+    return (state * 0x0019660dL + 0x3c6ef35fL) & 0xffffffffL;
+}
+
+inline long dsp_noiseshape(long sample)
+{
+    sample += dither[channel].error[0] - dither[channel].error[1]
+            + dither[channel].error[2];
+    dither[channel].error[2] = dither[channel].error[1];
+    dither[channel].error[1] = dither[channel].error[0]/2;
+    
+    return sample;
+}
+
+inline long dsp_bias(long sample)
+{
+    sample = sample + (1L << (fracbits - SAMPLE_DEPTH));
+    
+    return sample;
+}
+
+inline long dsp_dither(long *mask)
+{
+    long random, output;
+    
+    random  = prng(dither[channel].random);
+    output = (random & *mask) - (dither[channel].random & *mask);
+    dither[channel].random = random;
+    
+    return output;
+}
+
+inline void dsp_clip(long *sample, long *output)
+{
+    if (*output > dsp_config.clip_max) {
+        *output = dsp_config.clip_max;
+ 
+        if (*sample > dsp_config.clip_max)
+            *sample = dsp_config.clip_max;
+    } else if (*output < dsp_config.clip_min) {
+        *output = dsp_config.clip_min;
+
+        if (*sample < dsp_config.clip_min)
+            *sample = dsp_config.clip_min;
+    }
+}
+
+/*
+ * NAME:        dither()
+ * DESCRIPTION: dither and scale sample
+ */
+inline int scale_dither_clip(long sample)
+{
+    unsigned int scalebits;
+    long output, mask;
+
+    /* noise shape */
+    sample = dsp_noiseshape(sample);
+
+    /* bias */
+    output = dsp_bias(sample);
+    
+    scalebits = fracbits + 1 - SAMPLE_DEPTH;
+    mask = (1L << scalebits) - 1;
+    
+    /* dither */
+    output += dsp_dither(&mask);
+    
+    /* clip */
+    dsp_clip(&sample, &output);
+    
+    /* quantize */
+    output &= ~mask;
+
+    /* error feedback */
+    dither->error[0] = sample - output;
+
+    /* scale */
+    return output >> scalebits;
+}
+
+inline int scale_clip(long sample)
+{
+    unsigned int scalebits;
+    long output, mask;
+    
+    output = sample;
+    scalebits = fracbits + 1 - SAMPLE_DEPTH;
+    mask = (1L << scalebits) - 1;
+    
+    dsp_clip(&sample, &output);
+    output &= ~mask;
+
+    return output >> scalebits;
+}
+
+void dsp_scale_dither_clip(short *dest, long *src, int samplecount)
+{
+    dest += channel;
+    while (samplecount-- > 0) {
+        *dest = scale_dither_clip(*src);
+        src++;
+        dest += 2;
+    }
+}
+
+void dsp_scale_clip(short *dest, long *src, int samplecount)
+{
+    dest += channel;
+    while (samplecount-- > 0) {
+        *dest = scale_clip(*src);
+        src++;
+        dest += 2;
+    }
+}
+
+struct resampler {
+    long last_sample, phase, delta;
+};
+
+static struct resampler resample[2];
+
+#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 *out, long *in, 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 *out, long *in, 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;
+}
+
+#define MAX_CHUNK_SIZE 1024
+static char samplebuf[MAX_CHUNK_SIZE*4];
+/* enough to cope with 11khz upsampling */
+long resampled[MAX_CHUNK_SIZE * 4];
+
+int process(short *dest, long *src, int samplecount)
+{
+    long *p;
+    int length = samplecount;
+
+    p = resampled;
+    
+    /* Resample as necessary */
+    if (dsp_config.frequency > NATIVE_FREQUENCY)
+        length = upsample(resampled, src, samplecount, &resample[channel]);
+    else if (dsp_config.frequency < NATIVE_FREQUENCY)
+        length = downsample(resampled, src, samplecount, &resample[channel]);
+    else
+        p = src;
+    
+    /* Scale & dither */
+    if (dsp_config.dither_enabled) {
+        dsp_scale_dither_clip(dest, p, length);
+    } else {
+        dsp_scale_clip(dest, p, length);
+    }
+    
+    return length;
+}
+
+void convert_stereo_mode(long *dest, long *src, int samplecount)
+{
+    int i;
+    
+    samplecount /= 2;
+    
+    for (i = 0; i < samplecount; i++) {
+        dest[i] = src[i*2 + 0];
+        dest[i+samplecount] = src[i*2 + 1];
+    }
+}
+
+/* Not yet functional. */
+void scale_up(long *dest, short *src, int samplecount)
+{
+    int i;
+    
+    for (i = 0; i < samplecount; i++)
+        dest[i] = (long)(src[i] << 8);
+}
+
+void scale_up_convert_stereo_mode(long *dest, short *src, int samplecount)
+{
+    int i;
+    
+    samplecount /= 2;
+    
+    for (i = 0; i < samplecount; i++) {
+        dest[i] = (long)(src[i*2+0] << SAMPLE_DEPTH);
+        dest[i+samplecount] = (long)(src[i*2+1] << SAMPLE_DEPTH);
+        //dest[i] = (long)(((src[i*2 + 0] << 8)&0x7fff) | ((1L << 31) & src[i*2+0]<<15));
+        //dest[i+samplecount] = (long)(((src[i*2 + 1] << 8)&0x7fff) | ((1L << 31) & src[i*2+1]<<15));
+    }
+}
+
+int dsp_process(char *dest, char *src, int samplecount)
+{
+    int copy_n, rc;
+    char *p;
+    int processed_bytes = 0;
+    
+    fracbits = dsp_config.sample_depth;
+    
+    while (samplecount > 0) {
+        yield();
+        copy_n = MIN(MAX_CHUNK_SIZE / 4, samplecount);
+        
+        p = src;
+        /* Scale up to 32-bit samples. */
+        if (dsp_config.sample_depth <= SAMPLE_DEPTH) {
+            if (dsp_config.stereo_mode == STEREO_INTERLEAVED)
+                scale_up_convert_stereo_mode((long *)samplebuf, 
+                                             (short *)p, copy_n);
+            else
+                scale_up((long *)samplebuf, (short *)p, copy_n);
+            p = samplebuf;
+            fracbits = 31;
+        }
+        
+        /* Convert to non-interleaved stereo. */
+        else if (dsp_config.stereo_mode == STEREO_INTERLEAVED) {
+            convert_stereo_mode((long *)samplebuf, (long *)p, copy_n);
+            p = samplebuf;
+        }
+            
+        /* Apply DSP functions. */
+        if (dsp_config.stereo_mode == STEREO_INTERLEAVED) {
+            channel = 0;
+            rc = process((short *)dest, (long *)p, copy_n / 2) * 4;
+            p += copy_n * 2;
+            channel = 1;
+            process((short *)dest, (long *)p, copy_n / 2);
+            dest += rc;
+        } else {
+            rc = process((short *)dest, (long *)p, copy_n) * 2;
+            dest += rc * 2;
+        }
+        
+        samplecount -= copy_n;
+        if (dsp_config.sample_depth <= SAMPLE_DEPTH)
+            src += copy_n * 2;
+        else
+            src += copy_n * 4;
+            
+        processed_bytes += rc;
+    }
+    
+    /* Set stereo channel */
+    channel = channel ? 0 : 1;
+    
+    return processed_bytes;
+}
+
+bool dsp_configure(int setting, void *value)
+{
+    switch (setting) {
+    case DSP_SET_FREQUENCY:
+        dsp_config.frequency = (int)value;
+        resample[0].delta = resample[1].delta = 
+            (unsigned long)value*65536/NATIVE_FREQUENCY;
+        break ;
+        
+    case DSP_SET_CLIP_MIN:
+        dsp_config.clip_min = (long)value;
+        break ;
+        
+    case DSP_SET_CLIP_MAX:
+        dsp_config.clip_max = (long)value;
+        break ;
+        
+    case DSP_SET_SAMPLE_DEPTH:
+        dsp_config.sample_depth = (long)value;
+        break ;
+    
+    case DSP_SET_STEREO_MODE:
+        dsp_config.stereo_mode = (long)value;
+        channel = 0;
+        break ;
+    
+    case DSP_RESET:
+        dsp_config.dither_enabled = false;
+        dsp_config.clip_max = 0x7fffffff;
+        dsp_config.clip_min = 0x80000000;
+        dsp_config.frequency = NATIVE_FREQUENCY;
+        channel = 0;
+        break ;
+        
+    case DSP_DITHER:
+        dsp_config.dither_enabled = (bool)value;
+        break ;
+    
+    default:
+        return 0;
+    }
+    
+    return 1;
+}
+
+
diff --git a/apps/dsp.h b/apps/dsp.h
new file mode 100644
index 0000000000..65cd837c1e
--- /dev/null
+++ b/apps/dsp.h
@@ -0,0 +1,48 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2005 Miika Pekkarinen
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#ifndef _DSP_H
+#define _DSP_H
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <stdbool.h>
+
+#define NATIVE_FREQUENCY       44100
+#define STEREO_INTERLEAVED     0
+#define STEREO_NONINTERLEAVED  1
+/* Not supported yet. */
+#define STEREO_MONO            2
+
+struct dsp_configuration {
+    long frequency;
+    long clip_min, clip_max;
+    int sample_depth;
+    bool dither_enabled;
+    int stereo_mode;
+};
+
+extern struct dsp_configuration dsp_config;
+
+int dsp_process(char *dest, char *src, int samplecount);
+bool dsp_configure(int setting, void *value);
+
+#endif
+
+
diff --git a/apps/playback.c b/apps/playback.c
index 500f44e45b..996e825e13 100644
--- a/apps/playback.c
+++ b/apps/playback.c
@@ -49,6 +49,7 @@
 #include "playback.h"
 #include "pcm_playback.h"
 #include "buffer.h"
+#include "dsp.h"
 #ifdef HAVE_LCD_BITMAP
 #include "icons.h"
 #include "peakmeter.h"
@@ -171,7 +172,7 @@ int mp3_get_file_pos(void);
 
 /* Simulator stubs. */
 #ifdef SIMULATOR
-bool audiobuffer_insert(char *buf, size_t length)
+bool pcm_insert_buffer(char *buf, size_t length)
 {
     (void)buf;
     (void)length;
@@ -179,6 +180,20 @@ bool audiobuffer_insert(char *buf, size_t length)
     return true;
 }
 
+void pcm_flush_buffer(size_t length)
+{
+    (void)length;
+}
+
+
+void* pcm_request_buffer(size_t length, size_t *realsize)
+{
+    (void)length;
+    (void)realsize;
+    
+    return NULL;
+}
+
 void audiobuffer_add_event(void (*event_handler)(void))
 {
     (void)event_handler;
@@ -229,6 +244,92 @@ int ata_sleep(void)
 }
 #endif
 
+bool codec_audiobuffer_insert_callback(char *buf, size_t length)
+{
+    char *dest;
+    size_t realsize;
+    int factor;
+    int next_channel = 0;
+    int processed_length;
+    
+    /* If non-interleaved stereo mode. */
+    if (dsp_config.stereo_mode == STEREO_NONINTERLEAVED) {
+        next_channel = length / 2;
+    }
+    
+    if (dsp_config.sample_depth > 16) {
+        length /= 2;
+        factor = 1;
+    } else {
+        factor = 0;
+    }
+            
+    while (length > 0) {
+        /* Request a few extra bytes for resampling. */
+        /* FIXME: Required extra bytes SHOULD be calculated. */
+        while ((dest = pcm_request_buffer(length+16384, &realsize)) == NULL)
+            yield();
+        
+        if (realsize < 16384) {
+            pcm_flush_buffer(0);
+            continue ;
+        }
+        
+        realsize -= 16384;
+        
+        if (next_channel) {
+            processed_length = dsp_process(dest, buf, realsize / 4) * 2;
+            dsp_process(dest, buf + next_channel, realsize / 4);
+        } else {
+            processed_length = dsp_process(dest, buf, realsize / 2);
+        }
+        pcm_flush_buffer(processed_length);
+        length -= realsize;
+        buf += realsize << factor;
+    }
+    
+    return true;
+}
+
+bool codec_audiobuffer_insert_split_callback(void *ch1, void *ch2, 
+                                             size_t length)
+{
+    char *dest;
+    size_t realsize;
+    int factor;
+    int processed_length;
+    
+    /* non-interleaved stereo mode. */
+    if (dsp_config.sample_depth > 16) {
+        factor = 0;
+    } else {
+        length /= 2;
+        factor = 1;
+    }
+    
+    while (length > 0) {
+        /* Request a few extra bytes for resampling. */
+        while ((dest = pcm_request_buffer(length+4096, &realsize)) == NULL)
+            yield();
+        
+        if (realsize < 4096) {
+            pcm_flush_buffer(0);
+            continue ;
+        }
+        
+        realsize -= 4096;
+        
+        processed_length = dsp_process(dest, ch1, realsize / 4) * 2;
+        dsp_process(dest, ch2, realsize / 4);
+        pcm_flush_buffer(processed_length);
+        length -= realsize;
+        ch1 += realsize >> factor;
+        ch2 += realsize >> factor;
+    }
+    
+    return true;
+}
+
 void* get_codec_memory_callback(size_t *size)
 {
     *size = MALLOC_BUFSIZE;
@@ -260,7 +361,7 @@ size_t codec_filebuf_callback(void *ptr, size_t size)
     
     if (ci.stop_codec || !playing)
         return 0;
-        
+    
     copy_n = MIN((off_t)size, (off_t)cur_ti->available + cur_ti->filerem);
     
     while (copy_n > cur_ti->available) {
@@ -283,7 +384,7 @@ size_t codec_filebuf_callback(void *ptr, size_t size)
         buf_ridx -= codecbuflen;
     ci.curpos += copy_n;
     cur_ti->available -= copy_n;
-    codecbufused -= copy_n;    
+    codecbufused -= copy_n;
     
     return copy_n;
 }
@@ -427,8 +528,18 @@ void codec_configure_callback(int setting, void *value)
         conf_bufferlimit = (unsigned int)value;
         break;
         
+    case CODEC_DSP_ENABLE:
+        if ((bool)value)
+            ci.audiobuffer_insert = codec_audiobuffer_insert_callback;
+        else
+            ci.audiobuffer_insert = pcm_insert_buffer;
+        break ;
+
+#ifndef SIMULATOR        
     default:
-        logf("Illegal key: %d", setting);
+        if (!dsp_configure(setting, value))
+            logf("Illegal key: %d", setting);
+#endif
     }
 }
 
@@ -647,6 +758,8 @@ bool audio_load_track(int offset, bool start_play, int peek_offset)
         conf_bufferlimit = 0;
         conf_watermark = AUDIO_DEFAULT_WATERMARK;
         conf_filechunk = AUDIO_DEFAULT_FILECHUNK;
+        dsp_configure(DSP_RESET, 0);
+        ci.configure(CODEC_DSP_ENABLE, false);
     }
     
     tracks[track_widx].codecbuf = &codecbuf[buf_widx];
@@ -697,7 +810,7 @@ bool audio_load_track(int offset, bool start_play, int peek_offset)
         copy_n = MIN(size - i, copy_n);
         copy_n = MIN((int)fill_bytesleft, copy_n);
         rc = read(fd, &codecbuf[buf_widx], copy_n);
-        if (rc < 0) {
+        if (rc <= 0) {
             logf("File error!");
             close(fd);
             return false;
@@ -1152,7 +1265,7 @@ struct mp3entry* audio_next_track(void)
 
 bool audio_has_changed_track(void)
 {
-    if (track_changed && track_count > 0) {
+    if (track_changed && track_count > 0 && playing) {
         if (!cur_ti->taginfo_ready)
             return false;
         track_changed = false;
@@ -1384,6 +1497,7 @@ int mp3_get_file_pos(void)
 void audio_set_buffer_margin(int seconds)
 {
     (void)seconds;
+    logf("bufmargin: %d", seconds);
 }
 #endif
 
@@ -1395,7 +1509,7 @@ void mpeg_id3_options(bool _v1first)
 void audio_init(void)
 {
     logf("audio api init");
-    codecbuflen = audiobufend - audiobuf - PCMBUF_SIZE 
+    codecbuflen = audiobufend - audiobuf - PCMBUF_SIZE - PCMBUF_GUARD
                   - MALLOC_BUFSIZE - GUARD_BUFSIZE;
     //codecbuflen = 2*512*1024;
     codecbufused = 0;
@@ -1412,7 +1526,8 @@ void audio_init(void)
     
     /* Initialize codec api. */    
     ci.read_filebuf = codec_filebuf_callback;
-    ci.audiobuffer_insert = audiobuffer_insert;
+    ci.audiobuffer_insert = pcm_insert_buffer;
+    ci.audiobuffer_insert_split = codec_audiobuffer_insert_split_callback;
     ci.get_codec_memory = get_codec_memory_callback;
     ci.request_buffer = codec_request_buffer_callback;
     ci.advance_buffer = codec_advance_buffer_callback;
diff --git a/apps/playback.h b/apps/playback.h
index 24fc8570e8..672ddae440 100644
--- a/apps/playback.h
+++ b/apps/playback.h
@@ -27,10 +27,19 @@
 #include "id3.h"
 #include "mp3data.h"
 
-/* File buffer configuration keys. */
-#define CODEC_SET_FILEBUF_WATERMARK     1
-#define CODEC_SET_FILEBUF_CHUNKSIZE     2
-#define CODEC_SET_FILEBUF_LIMIT         3
+enum {
+    CODEC_SET_FILEBUF_WATERMARK = 1,
+    CODEC_SET_FILEBUF_CHUNKSIZE,
+    CODEC_SET_FILEBUF_LIMIT,
+    CODEC_DSP_ENABLE,
+    DSP_SET_FREQUENCY,
+    DSP_SET_CLIP_MIN,
+    DSP_SET_CLIP_MAX,
+    DSP_SET_SAMPLE_DEPTH,
+    DSP_SET_STEREO_MODE,
+    DSP_RESET,
+    DSP_DITHER
+};
 
 /* Not yet implemented. */
 #define CODEC_SET_AUDIOBUF_WATERMARK    4
diff --git a/firmware/export/pcm_playback.h b/firmware/export/pcm_playback.h
index aa29601f70..3fe60670b3 100644
--- a/firmware/export/pcm_playback.h
+++ b/firmware/export/pcm_playback.h
@@ -19,6 +19,10 @@
 #ifndef PCM_PLAYBACK_H
 #define PCM_PLAYBACK_H
 
+/* Guard buffer for crossfader when dsp is enabled. */
+#define PCMBUF_GUARD  32768
+
+/* PCM audio buffer. */
 #define PCMBUF_SIZE   (1*1024*1024)
 
 void pcm_init(void);
@@ -44,7 +48,9 @@ bool pcm_is_lowdata(void);
 bool pcm_crossfade_init(void);
 void audiobuffer_add_event(void (*event_handler)(void));
 unsigned int audiobuffer_get_latency(void);
-bool audiobuffer_insert(char *buf, size_t length);
+bool pcm_insert_buffer(char *buf, size_t length);
+void pcm_flush_buffer(size_t length);
+void* pcm_request_buffer(size_t length, size_t *realsize);
 bool pcm_is_crossfade_enabled(void);
 void pcm_crossfade_enable(bool on_off);
 
diff --git a/firmware/pcm_playback.c b/firmware/pcm_playback.c
index 24fc5d4530..12ecfd14e2 100644
--- a/firmware/pcm_playback.c
+++ b/firmware/pcm_playback.c
@@ -67,6 +67,7 @@ static int crossfade_pos;
 static int crossfade_amount;
 static int crossfade_rem;
 
+static char *guardbuf;
 static void (*pcm_event_handler)(void);
 
 static unsigned char *next_start;
@@ -258,7 +259,6 @@ void pcm_play_pause(bool play)
         IIS2CONFIG = 0x800;
     }
     pcm_paused = !play;
-    pcm_boost(false);
 }
 
 bool pcm_is_playing(void)
@@ -401,15 +401,8 @@ bool pcm_crossfade_init(void)
     
 }
 
-static void crossfade_start(void)
+void pcm_flush_fillpos(void)
 {
-    if (!crossfade_init)
-        return ;
-        
-    crossfade_init = 0;
-    if (PCMBUF_SIZE - audiobuffer_free < CHUNK_SIZE * 6)
-        return ;
-        
     if (audiobuffer_fillpos) {
         while (!pcm_play_add_chunk(&audiobuffer[audiobuffer_pos], 
                                    audiobuffer_fillpos, pcm_event_handler)) {
@@ -419,13 +412,26 @@ static void crossfade_start(void)
         audiobuffer_pos += audiobuffer_fillpos;
         if (audiobuffer_pos >= PCMBUF_SIZE)
             audiobuffer_pos -= PCMBUF_SIZE;
+        audiobuffer_free -= audiobuffer_fillpos;
+        audiobuffer_fillpos = 0;
     }
+}
+
+static void crossfade_start(void)
+{
+    if (!crossfade_init)
+        return ;
+        
+    crossfade_init = 0;
+    if (PCMBUF_SIZE - audiobuffer_free < CHUNK_SIZE * 6)
+        return ;
+    
+    pcm_flush_fillpos();
     pcm_boost(true);
     crossfade_active = true;
     crossfade_pos = audiobuffer_pos;
     crossfade_amount = (PCMBUF_SIZE - audiobuffer_free - (CHUNK_SIZE * 2))/2;
     crossfade_rem = crossfade_amount;
-    audiobuffer_fillpos = 0;
     
     crossfade_pos -= crossfade_amount*2;
     if (crossfade_pos < 0)
@@ -451,12 +457,11 @@ int crossfade(short *buf, const short *buf2, int length)
     return size;
 }
 
-bool audiobuffer_insert(char *buf, size_t length)
+inline static bool prepare_insert(size_t length)
 {
-    size_t copy_n = 0;
-    
     crossfade_start();
-    if (audiobuffer_free < length + CHUNK_SIZE && !crossfade_active) {
+    if (audiobuffer_free < length + audiobuffer_fillpos
+           + CHUNK_SIZE && !crossfade_active) {
         pcm_boost(false);
         return false;
     }
@@ -467,7 +472,94 @@ bool audiobuffer_insert(char *buf, size_t length)
         if (audiobuffer_free < PCMBUF_SIZE - CHUNK_SIZE*4)
             pcm_play_start();
     }
+    
+    return true;
+}
+
+void* pcm_request_buffer(size_t length, size_t *realsize)
+{
+    void *ptr = NULL;
+    
+    if (!prepare_insert(length)) {
+        *realsize = 0;
+        return NULL;
+    }
+    
+    if (crossfade_active) {
+        *realsize = MIN(length, PCMBUF_GUARD);
+        ptr = &guardbuf[0];
+    } else {
+        *realsize = MIN(length, PCMBUF_SIZE - audiobuffer_pos
+                            - audiobuffer_fillpos);
+        if (*realsize < length) {
+            *realsize += MIN((long)(length - *realsize), PCMBUF_GUARD);
+            //logf("gbr:%d/%d", *realsize, length);
+        }
+        ptr = &audiobuffer[audiobuffer_pos + audiobuffer_fillpos];
+    }
+    
+    return ptr;
+}
+
+void pcm_flush_buffer(size_t length)
+{
+    int copy_n;
+    char *buf;
+    
+    if (crossfade_active) {
+        buf = &guardbuf[0];
+        length = MIN(length, PCMBUF_GUARD);
+        while (length > 0 && crossfade_active) {
+            copy_n = MIN(length, PCMBUF_SIZE - (unsigned int)crossfade_pos);
+            copy_n = 2 * crossfade((short *)&audiobuffer[crossfade_pos], 
+                    (const short *)buf, copy_n/2);
+            buf += copy_n;
+            length -= copy_n;
+            crossfade_pos += copy_n;
+            if (crossfade_pos >= PCMBUF_SIZE)
+                crossfade_pos -= PCMBUF_SIZE;
+        }
+        
+        if (length > 0) {
+            memcpy(&audiobuffer[audiobuffer_pos], buf, length);
+            audiobuffer_fillpos = length;
+            goto try_flush;
+        }
+    } else {
+       /* if (length == 0) {
+            pcm_flush_fillpos();
+            audiobuffer_pos = 0;
+            return ;
+        } */
+    
+        audiobuffer_fillpos += length;
+        
+        try_flush:
+        if (audiobuffer_fillpos < CHUNK_SIZE && PCMBUF_SIZE
+            - audiobuffer_pos - audiobuffer_fillpos > 0)
+            return ;
+        
+        copy_n = MIN((long)(audiobuffer_fillpos - (PCMBUF_SIZE 
+                    - audiobuffer_pos)), PCMBUF_GUARD);
+        if (copy_n > 0) {
+            //logf("guard buf used:%d", copy_n);
+            audiobuffer_fillpos -= copy_n;
+            pcm_flush_fillpos();
+            memcpy(&audiobuffer[0], &guardbuf[0], copy_n);
+            audiobuffer_fillpos = copy_n;
+            goto try_flush;
+        }
+        pcm_flush_fillpos();
+    }
+}
 
+bool pcm_insert_buffer(char *buf, size_t length)
+{
+    size_t copy_n = 0;
+    
+    if (!prepare_insert(length))
+        return false;
+        
     while (length > 0) {
         if (crossfade_active) {
             copy_n = MIN(length, PCMBUF_SIZE - (unsigned int)crossfade_pos);
@@ -521,7 +613,8 @@ bool audiobuffer_insert(char *buf, size_t length)
 void pcm_play_init(void)
 {
     audiobuffer = &audiobuf[(audiobufend - audiobuf) - 
-                            PCMBUF_SIZE];
+                            PCMBUF_SIZE - PCMBUF_GUARD];
+    guardbuf = &audiobuffer[PCMBUF_SIZE];
     audiobuffer_free = PCMBUF_SIZE;
     audiobuffer_pos = 0;
     audiobuffer_fillpos = 0;
@@ -532,11 +625,6 @@ void pcm_play_init(void)
     crossfade_active = false;
     crossfade_init = false;
     pcm_event_handler = NULL;
-    if (crossfade_enabled) {
-        pcm_play_set_watermark(PCM_CF_WATERMARK, pcm_watermark_callback);
-    } else {
-        pcm_play_set_watermark(PCM_WATERMARK, pcm_watermark_callback);
-    }
 }
 
 void pcm_crossfade_enable(bool on_off)
@@ -555,6 +643,11 @@ void pcm_play_start(void)
     int size;
     char *start;
     
+    if (crossfade_enabled) {
+        pcm_play_set_watermark(PCM_CF_WATERMARK, pcm_watermark_callback);
+    } else {
+        pcm_play_set_watermark(PCM_WATERMARK, pcm_watermark_callback);
+    }
     crossfade_active = false;
     if(!pcm_is_playing())
     {