Initial D2 sound playback support (known issues to follow on the CowonD2Info wiki page).

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

8 files changed, 383 insertions, 91 deletions

diff --git a/firmware/SOURCES b/firmware/SOURCES
index 1a01fd5..2e25594 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1022,6 +1022,7 @@ target/arm/tcc780x/timer-tcc780x.c
 target/arm/wmcodec-telechips.c
 target/arm/tcc780x/debug-tcc780x.c
 target/arm/tcc780x/pcm-tcc780x.c
+target/arm/tcc780x/cowond2/audio-cowond2.c
 #endif /* BOOTLOADER */
 #endif /* SIMULATOR */
 #endif /* COWON_D2 */
diff --git a/firmware/drivers/audio/wm8985.c b/firmware/drivers/audio/wm8985.c
index 6f8d659..411bd97 100644
--- a/firmware/drivers/audio/wm8985.c
+++ b/firmware/drivers/audio/wm8985.c
@@ -86,18 +86,6 @@
 #define OUT4MIX     0x39
 #define BIASCTL     0x3d
 
-/* Register settings for the supported samplerates: */
-#define WM8985_8000HZ      0x4d
-#define WM8985_12000HZ     0x61
-#define WM8985_16000HZ     0x55
-#define WM8985_22050HZ     0x77
-#define WM8985_24000HZ     0x79
-#define WM8985_32000HZ     0x59
-#define WM8985_44100HZ     0x63
-#define WM8985_48000HZ     0x41
-#define WM8985_88200HZ     0x7f
-#define WM8985_96000HZ     0x5d
-
 const struct sound_settings_info audiohw_settings[] = {
     [SOUND_VOLUME]        = {"dB", 0,  1, -58,   6, -25},
     [SOUND_BASS]          = {"dB", 0,  1, -12,  12,   0},
@@ -136,19 +124,6 @@ int tenthdb2master(int db)
     }
 }
 
-/* convert tenth of dB volume (-780..0) to mixer volume register value */
-int tenthdb2mixer(int db)
-{
-    if (db < -660)                 /* 1.5 dB steps */
-        return (2640 - db) / 15;
-    else if (db < -600)            /* 0.75 dB steps */
-        return (990 - db) * 2 / 15;
-    else if (db < -460)            /* 0.5 dB steps */
-        return (460 - db) / 5;
-    else                           /* 0.25 dB steps */
-        return -db * 2 / 5;
-}
-
 /* Silently enable / disable audio output */
 void audiohw_enable_output(bool enable)
 {
@@ -157,32 +132,53 @@ void audiohw_enable_output(bool enable)
         /* TODO: reset the I2S controller into known state */
         //i2s_reset();
 
-        /* TODO: Review the power-up sequence to prevent pops (see datasheet) */
+        wmcodec_write(RESET,    0x1ff);    /* Reset */
 
-        wmcodec_write(RESET, 0x1ff);    /*Reset*/
+        wmcodec_write(BIASCTL,  0x100); /* BIASCUT = 1 */
+        wmcodec_write(OUTCTRL,  0x6);   /* Thermal shutdown */
 
-        wmcodec_write(PWRMGMT1, 0x2b);
-        wmcodec_write(PWRMGMT2, 0x180);
+        wmcodec_write(PWRMGMT1, 0x8);   /* BIASEN = 1 */
+
+        /* Volume zero, mute all outputs */
+        wmcodec_write(LOUT1VOL, 0x140);
+        wmcodec_write(ROUT1VOL, 0x140);
+        wmcodec_write(LOUT2VOL, 0x140);
+        wmcodec_write(ROUT2VOL, 0x140);
+        wmcodec_write(OUT3MIX,  0x40);
+        wmcodec_write(OUT4MIX,  0x40);
+        
+        /* DAC softmute, automute, 128OSR */
+        wmcodec_write(DACCTRL,  0x4c);
+
+        wmcodec_write(OUT4ADC,  0x2);   /* POBCTRL = 1 */
+
+        /* Enable output, DAC and mixer */
         wmcodec_write(PWRMGMT3, 0x6f);
+        wmcodec_write(PWRMGMT2, 0x180);
+        wmcodec_write(PWRMGMT1, 0xd);
+        wmcodec_write(LOUTMIX,  0x1);
+        wmcodec_write(ROUTMIX,  0x1);
 
-        wmcodec_write(AINTFCE, 0x10);
-        wmcodec_write(CLKCTRL, 0x49);
+        /* Disable clock since we're acting as slave to the SoC */
+        wmcodec_write(CLKGEN,  0x0);
+        wmcodec_write(AINTFCE, 0x10);   /* 16-bit, I2S format */
 
-        wmcodec_write(OUTCTRL, 1);
+        wmcodec_write(LDACVOL, 0x1ff);  /* Full DAC digital vol */
+        wmcodec_write(RDACVOL, 0x1ff);
 
-        /* The iPod can handle multiple frequencies, but fix at 44.1KHz
-           for now */
-        audiohw_set_sample_rate(WM8985_44100HZ);
+        wmcodec_write(OUT4ADC, 0x0);    /* POBCTRL = 0 */
+
+        sleep(HZ/2);
 
-        wmcodec_write(LOUTMIX,0x1); /* Enable mixer */
-        wmcodec_write(ROUTMIX,0x1); /* Enable mixer */
         audiohw_mute(0);
-    } else {
+    }
+    else
+    {
         audiohw_mute(1);
     }
 }
 
-void audiohw_set_master_vol(int vol_l, int vol_r)
+void audiohw_set_headphone_vol(int vol_l, int vol_r)
 {
     /* OUT1 */
     wmcodec_write(LOUT1VOL, 0x080 | vol_l);
@@ -196,12 +192,6 @@ void audiohw_set_lineout_vol(int vol_l, int vol_r)
     wmcodec_write(ROUT2VOL, 0x100 | vol_r);
 }
 
-void audiohw_set_mixer_vol(int channel1, int channel2)
-{
-    (void)channel1;
-    (void)channel2;
-}
-
 void audiohw_set_bass(int value)
 {
     eq1_reg = (eq1_reg & ~EQ_GAIN_MASK) | EQ_GAIN_VALUE(value);
@@ -231,14 +221,14 @@ void audiohw_mute(bool mute)
     if (mute)
     {
         /* Set DACMU = 1 to soft-mute the audio DACs. */
-        wmcodec_write(DACCTRL, 0x40);
+        wmcodec_write(DACCTRL, 0x4c);
     } else {
         /* Set DACMU = 0 to soft-un-mute the audio DACs. */
-        wmcodec_write(DACCTRL, 0x0);
+        wmcodec_write(DACCTRL, 0xc);
     }
 }
 
-/* Nice shutdown of WM8758 codec */
+/* Nice shutdown of WM8985 codec */
 void audiohw_close(void)
 {
     audiohw_mute(1);
@@ -250,32 +240,13 @@ void audiohw_close(void)
     wmcodec_write(PWRMGMT2, 0x40);
 }
 
-/* Change the order of the noise shaper, 5th order is recommended above 32kHz */
-void audiohw_set_nsorder(int order)
-{
-    (void)order;
-}
-
 /* Note: Disable output before calling this function */
 void audiohw_set_sample_rate(int sampling_control)
 {
-    /**** We force 44.1KHz for now. ****/
+    /* Currently the WM8985 acts as slave to the SoC I2S controller, so no
+       setup is needed here. This seems to be in contrast to every other WM
+       driver in Rockbox, so this may need to change in the future. */
     (void)sampling_control;
-
-    /* set clock div */
-    wmcodec_write(CLKCTRL, 1 | (0 << 2) | (2 << 5));
-
-    /* setup PLL for MHZ=11.2896 */
-    wmcodec_write(PLLN, (1 << 4) | 0x7);
-    wmcodec_write(PLLK1, 0x21);
-    wmcodec_write(PLLK2, 0x161);
-    wmcodec_write(PLLK3, 0x26);
-
-    /* set clock div */
-    wmcodec_write(CLKCTRL, 1 | (1 << 2) | (2 << 5) | (1 << 8));
-
-    /* set srate */
-    wmcodec_write(SRATECTRL, (0 << 1));
 }
 
 #ifdef HAVE_RECORDING
diff --git a/firmware/export/wm8985.h b/firmware/export/wm8985.h
index f59bc77..56c7f8d 100644
--- a/firmware/export/wm8985.h
+++ b/firmware/export/wm8985.h
@@ -27,12 +27,8 @@
 #define AUDIOHW_CAPS (BASS_CAP | TREBLE_CAP | BASS_CUTOFF_CAP | TREBLE_CUTOFF_CAP)
 
 extern int tenthdb2master(int db);
-extern int tenthdb2mixer(int db);
 
-extern void audiohw_set_master_vol(int vol_l, int vol_r);
+extern void audiohw_set_headphone_vol(int vol_l, int vol_r);
 extern void audiohw_set_lineout_vol(int vol_l, int vol_r);
-extern void audiohw_set_mixer_vol(int channel1, int channel2);
-extern void audiohw_set_nsorder(int order);
-extern void audiohw_set_sample_rate(int sampling_control);
 
 #endif /* _WM8985_H */
diff --git a/firmware/sound.c b/firmware/sound.c
index 7c862db..6f02b27 100644
--- a/firmware/sound.c
+++ b/firmware/sound.c
@@ -301,11 +301,11 @@ static void set_prescaled_volume(void)
     audiohw_set_master_vol(tenthdb2master(l), tenthdb2master(r));
 #if defined(HAVE_WM8975) || defined(HAVE_WM8758) \
    || (defined(HAVE_WM8751) && !defined(MROBE_100)) \
-   || defined(HAVE_TSC2100)
+   || defined(HAVE_TSC2100) || defined(HAVE_WM8985)
     audiohw_set_lineout_vol(tenthdb2master(0), tenthdb2master(0));
 #endif
 
-#elif defined(HAVE_TLV320) || defined(HAVE_WM8978)
+#elif defined(HAVE_TLV320) || defined(HAVE_WM8978) || defined(HAVE_WM8985)
     audiohw_set_headphone_vol(tenthdb2master(l), tenthdb2master(r));
 #endif
 }
diff --git a/firmware/target/arm/tcc780x/cowond2/audio-cowond2.c b/firmware/target/arm/tcc780x/cowond2/audio-cowond2.c
new file mode 100644
index 0000000..f735282
--- /dev/null
+++ b/firmware/target/arm/tcc780x/cowond2/audio-cowond2.c
@@ -0,0 +1,92 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2007 by Michael Sevakis
+ *
+ * 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 "system.h"
+#include "cpu.h"
+#include "audio.h"
+#include "sound.h"
+
+int audio_channels = 2;
+int audio_output_source = AUDIO_SRC_PLAYBACK;
+
+void audio_set_output_source(int source)
+{
+    int oldmode = set_fiq_status(FIQ_DISABLED);
+
+    if ((unsigned)source >= AUDIO_NUM_SOURCES)
+        source = AUDIO_SRC_PLAYBACK;
+
+    audio_output_source = source;
+
+    /*if (source != AUDIO_SRC_PLAYBACK)
+        IISCONFIG |= (1 << 29);*/
+
+    set_fiq_status(oldmode);
+}
+
+void audio_input_mux(int source, unsigned flags)
+{
+    static int last_source = AUDIO_SRC_PLAYBACK;
+    static bool last_recording = false;
+    bool recording = flags & SRCF_RECORDING;
+
+    switch (source)
+    {
+        default:                        /* playback - no recording */
+            source = AUDIO_SRC_PLAYBACK;
+        case AUDIO_SRC_PLAYBACK:
+            audio_channels = 2;
+            if (source != last_source)
+            {
+                /*audiohw_set_monitor(false);
+                audiohw_disable_recording();*/
+            }
+            break;
+
+        case AUDIO_SRC_MIC:             /* recording only */
+            audio_channels = 1;
+            if (source != last_source)
+            {
+                /*audiohw_set_monitor(false);
+                audiohw_enable_recording(true);  /. source mic */
+            }
+            break;
+
+        case AUDIO_SRC_FMRADIO:         /* recording and playback */
+            audio_channels = 2;
+
+            if (source == last_source && recording == last_recording)
+                break;
+
+            last_recording = recording;
+
+            if (recording)
+            {
+                /*audiohw_set_monitor(false);
+                audiohw_enable_recording(false);*/
+            }
+            else
+            {
+                /*audiohw_disable_recording();
+                audiohw_set_monitor(true); /. line 1 analog audio path */
+            }
+            break;
+    } /* end switch */
+
+    last_source = source;
+} /* audio_input_mux */
diff --git a/firmware/target/arm/tcc780x/cowond2/power-cowond2.c b/firmware/target/arm/tcc780x/cowond2/power-cowond2.c
index c744125..504f4e6 100644
--- a/firmware/target/arm/tcc780x/cowond2/power-cowond2.c
+++ b/firmware/target/arm/tcc780x/cowond2/power-cowond2.c
@@ -22,6 +22,7 @@
 #include "power.h"
 #include "pcf50606.h"
 #include "button-target.h"
+#include "tuner.h"
 
 #ifndef SIMULATOR
 
@@ -69,7 +70,6 @@ void EXT3(void)
     unsigned char data[3]; /* 0 = INT1, 1 = INT2, 2 = INT3 */
 
     /* Clear pending interrupts from pcf50606 */
-    int fiq_status = disable_fiq_save();
     pcf50606_read_multiple(0x02, data, 3);
 
     if (data[0] & 0x04)
@@ -87,7 +87,6 @@ void EXT3(void)
         /* Touchscreen event, do something about it */
         button_set_touch_available();
     }
-    restore_fiq(fiq_status);
 }
 #endif
 
diff --git a/firmware/target/arm/tcc780x/crt0.S b/firmware/target/arm/tcc780x/crt0.S
index cef27f1..aaa9517 100644
--- a/firmware/target/arm/tcc780x/crt0.S
+++ b/firmware/target/arm/tcc780x/crt0.S
@@ -100,10 +100,18 @@ copied_start:
     mov    r0,#0xd2
     msr    cpsr, r0
     ldr    sp, =irq_stack
+    
     /* Set up stack for FIQ mode */ 
     mov    r0,#0xd1
     msr    cpsr, r0
     ldr    sp, =fiq_stack
+    
+    /* Load the banked FIQ mode registers with useful values here.
+       These values will be used in the FIQ handler in pcm-tcc780x.c */
+    .equ   DADO_BASE,  0xF0059020
+    
+    ldr    r10, =DADO_BASE
+    ldr    r11, =dma_play_data
 
     /* Let abort and undefined modes use IRQ stack */
     mov    r0,#0xd7
diff --git a/firmware/target/arm/tcc780x/pcm-tcc780x.c b/firmware/target/arm/tcc780x/pcm-tcc780x.c
index a931274..7db775d 100644
--- a/firmware/target/arm/tcc780x/pcm-tcc780x.c
+++ b/firmware/target/arm/tcc780x/pcm-tcc780x.c
@@ -7,7 +7,8 @@
  *                     \/            \/     \/    \/            \/
  * $Id$
  *
- * Copyright (C) 2007 by Karl Kurbjun
+ * Copyright (C) 2006 by Michael Sevakis
+ * Copyright (C) 2008 by Rob Purchase
  *
  * 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.
@@ -16,52 +17,176 @@
  * KIND, either express or implied.
  *
  ****************************************************************************/
+#include <stdlib.h>
 #include "system.h"
 #include "kernel.h"
 #include "logf.h"
 #include "audio.h"
 #include "sound.h"
-#include "file.h"
+#include "pcm.h"
+
+struct dma_data
+{
+/* NOTE: The order of size and p is important if you use assembler
+   optimised fiq handler, so don't change it. */
+    uint16_t *p;
+    size_t size;
+#if NUM_CORES > 1
+    unsigned core;
+#endif
+    int locked;
+    int state;
+};
+
+/****************************************************************************
+ ** Playback DMA transfer
+ **/
+struct dma_data dma_play_data SHAREDBSS_ATTR =
+{
+    /* Initialize to a locked, stopped state */
+    .p = NULL,
+    .size = 0,
+#if NUM_CORES > 1
+    .core = 0x00,
+#endif
+    .locked = 0,
+    .state = 0
+};
+
+static unsigned long pcm_freq SHAREDDATA_ATTR = HW_SAMPR_DEFAULT; /* 44.1 is default */
 
 void pcm_postinit(void)
 {
+    /*audiohw_postinit();*/
+    pcm_apply_settings();
 }
 
 const void * pcm_play_dma_get_peak_buffer(int *count)
 {
-    (void) count;
-    return 0;
+    unsigned long addr = (unsigned long)dma_play_data.p;
+    size_t cnt = dma_play_data.size;
+    *count = cnt >> 2;
+    return (void *)((addr + 2) & ~3);
 }
 
 void pcm_play_dma_init(void)
 {
+    /* Set DAI clock divided from PLL0 (192MHz).
+       The best approximation of 256*44.1kHz is 11.291MHz. */
+    BCLKCTR &= ~DEV_DAI;
+    PCLK_DAI = (1<<28) | 61682;  /* DCO mode */
+    BCLKCTR |= DEV_DAI;
+    
+    /* Enable DAI block in Master mode, 256fs->32fs, 16bit LSB */
+    DAMR = 0x3c8e80;
+    DAVC = 0x0;         /* Digital Volume = max */
+    
+    /* Set DAI interrupts as FIQs */
+    IRQSEL = ~(DAI_RX_IRQ_MASK | DAI_TX_IRQ_MASK);
+    
+    pcm_set_frequency(SAMPR_44);
+
+    /* Initialize default register values. */
+    audiohw_init();
+
+    /* Power on */
+    audiohw_enable_output(true);
+    
+    /* Unmute the master channel (DAC should be at zero point now). */
+    audiohw_mute(false);
+
+    dma_play_data.size = 0;
+#if NUM_CORES > 1
+    dma_play_data.core = 0; /* no core in control */
+#endif
 }
 
 void pcm_apply_settings(void)
 {
+    pcm_curr_sampr = pcm_freq;
 }
 
 void pcm_set_frequency(unsigned int frequency)
 {
     (void) frequency;
+    pcm_freq = HW_SAMPR_DEFAULT;
+}
+
+static void play_start_pcm(void)
+{
+    pcm_apply_settings();
+
+    DAMR &= ~(1<<14);   /* disable tx */
+    dma_play_data.state = 1;
+
+    if (dma_play_data.size >= 16)
+    {
+        DADO_L(0) = *dma_play_data.p++;
+        DADO_R(0) = *dma_play_data.p++;
+        DADO_L(1) = *dma_play_data.p++;
+        DADO_R(1) = *dma_play_data.p++;
+        DADO_L(2) = *dma_play_data.p++;
+        DADO_R(2) = *dma_play_data.p++;
+        DADO_L(3) = *dma_play_data.p++;
+        DADO_R(3) = *dma_play_data.p++;
+        dma_play_data.size -= 16;
+    }
+
+    DAMR |= (1<<14);   /* enable tx */
+}
+
+static void play_stop_pcm(void)
+{
+    DAMR &= ~(1<<14);   /* disable tx */
+    dma_play_data.state = 0;
 }
 
 void pcm_play_dma_start(const void *addr, size_t size)
 {
-    (void) addr;
-    (void) size;
+    dma_play_data.p    = (void *)(((uintptr_t)addr + 2) & ~3);
+    dma_play_data.size = (size & ~3);
+
+#if NUM_CORES > 1
+    /* This will become more important later - and different ! */
+    dma_play_data.core = processor_id(); /* save initiating core */
+#endif
+
+    IEN |= DAI_TX_IRQ_MASK;
+
+    play_start_pcm();
 }
 
 void pcm_play_dma_stop(void)
 {
+    play_stop_pcm();
+    dma_play_data.size = 0;
+#if NUM_CORES > 1
+    dma_play_data.core = 0; /* no core in control */
+#endif
 }
 
 void pcm_play_lock(void)
 {
+    int status = disable_fiq_save();
+
+    if (++dma_play_data.locked == 1)
+    {
+        IEN &= ~DAI_TX_IRQ_MASK;
+    }
+
+    restore_fiq(status);
 }
 
 void pcm_play_unlock(void)
 {
+   int status = disable_fiq_save();
+
+    if (--dma_play_data.locked == 0 && dma_play_data.state != 0)
+    {
+        IEN |= DAI_TX_IRQ_MASK;
+    }
+
+   restore_fiq(status);
 }
 
 void pcm_play_dma_pause(bool pause)
@@ -71,16 +196,116 @@ void pcm_play_dma_pause(bool pause)
 
 size_t pcm_get_bytes_waiting(void)
 {
-    return 0;
+    return dma_play_data.size & ~3;
 }
 
+#if 1
+void fiq_handler(void) ICODE_ATTR __attribute__((naked));
 void fiq_handler(void)
 {
-    /* Clear FIQ status */
-    CREQ = DAI_TX_IRQ_MASK | DAI_RX_IRQ_MASK;
-
-    /* Return from FIQ */
+    /* r10 contains DADO_L0 base address (set in crt0.S to minimise code in the
+     * FIQ handler. r11 contains address of p (also set in crt0.S). Most other
+     * addresses we need are generated by using offsets with these two.
+     * r8 and r9 contains local copies of p and size respectively.
+     * r0-r3 and r12 is a working register.
+     */
     asm volatile (
-        "subs   pc, lr, #4   \r\n"
+        "stmfd   sp!, { r0-r3, lr }  \n" /* stack scratch regs and lr */
+        
+        "ldmia   r11, { r8-r9 }      \n" /* r8 = p, r9 = size */
+        "cmp     r9, #0x10           \n" /* is size <16? */
+        "blt     .more_data          \n" /* if so, ask pcmbuf for more data */
+
+    ".fill_fifo:                     \n"
+        "ldr     r12, [r8], #4       \n" /* load two samples */
+        "str     r12, [r10, #0x0]    \n" /* write top sample to DADO_L0 */
+        "mov     r12, r12, lsr #16   \n" /* put right sample at the bottom */
+        "str     r12, [r10, #0x4]    \n" /* write low sample to DADO_R0*/
+        "ldr     r12, [r8], #4       \n" /* load two samples */
+        "str     r12, [r10, #0x8]    \n" /* write top sample to DADO_L1 */
+        "mov     r12, r12, lsr #16   \n" /* put right sample at the bottom */
+        "str     r12, [r10, #0xc]    \n" /* write low sample to DADO_R1*/
+        "ldr     r12, [r8], #4       \n" /* load two samples */
+        "str     r12, [r10, #0x10]   \n" /* write top sample to DADO_L2 */
+        "mov     r12, r12, lsr #16   \n" /* put right sample at the bottom */
+        "str     r12, [r10, #0x14]   \n" /* write low sample to DADO_R2*/
+        "ldr     r12, [r8], #4       \n" /* load two samples */
+        "str     r12, [r10, #0x18]   \n" /* write top sample to DADO_L3 */
+        "mov     r12, r12, lsr #16   \n" /* put right sample at the bottom */
+        "str     r12, [r10, #0x1c]   \n" /* write low sample to DADO_R3*/
+        "sub     r9, r9, #0x10       \n" /* 4 words written */
+        "stmia   r11, { r8-r9 }      \n" /* save p and size */
+
+    ".exit:                          \n"
+        "mov     r8, #0xc000         \n" /* DAI_TX_IRQ_MASK | DAI_RX_IRQ_MASK */
+        "ldr     r9, =0xf3001004     \n" /* CREQ */
+        "str     r8, [r9]            \n" /* clear DAI IRQs */
+        "ldmfd   sp!, { r0-r3, lr }  \n"
+        "subs    pc, lr, #4          \n" /* FIQ specific return sequence */
+
+    ".more_data:                     \n"
+        "ldr     r2, =pcm_callback_for_more \n"
+        "ldr     r2, [r2]            \n" /* get callback address */
+        "cmp     r2, #0              \n" /* check for null pointer */
+        "movne   r0, r11             \n" /* r0 = &p */
+        "addne   r1, r11, #4         \n" /* r1 = &size */
+        "blxne   r2                  \n" /* call pcm_callback_for_more */
+        "ldmia   r11, { r8-r9 }      \n" /* reload p and size */
+        "cmp     r9, #0x10           \n" /* did we actually get more data? */
+        "bge     .fill_fifo          \n" /* yes: fill the fifo */
+        "ldr     r12, =pcm_play_dma_stop \n"
+        "blx     r12                 \n" /* no: stop playback */
+        "ldr     r12, =pcm_play_dma_stopped_callback \n"
+        "blx     r12                 \n"
+        "b       .exit               \n"
+        ".ltorg                      \n"
     );
 }
+#else /* C version for reference */
+void fiq_handler(void) ICODE_ATTR __attribute__((naked));
+void fiq_handler(void)
+{
+    asm volatile(   "stmfd sp!, {r0-r7, ip, lr} \n"   /* Store context */
+                    "sub   sp, sp, #8           \n"); /* Reserve stack */
+                    
+    register pcm_more_callback_type get_more;
+
+    if (dma_play_data.size < 16)
+    {
+        /* p is empty, get some more data */
+        get_more = pcm_callback_for_more;
+        if (get_more)
+        {
+            get_more((unsigned char**)&dma_play_data.p,
+                     &dma_play_data.size);
+        }
+    }
+
+    if (dma_play_data.size >= 16)
+    {
+        DADO_L(0) = *dma_play_data.p++;
+        DADO_R(0) = *dma_play_data.p++;
+        DADO_L(1) = *dma_play_data.p++;
+        DADO_R(1) = *dma_play_data.p++;
+        DADO_L(2) = *dma_play_data.p++;
+        DADO_R(2) = *dma_play_data.p++;
+        DADO_L(3) = *dma_play_data.p++;
+        DADO_R(3) = *dma_play_data.p++;
+
+        dma_play_data.size -= 16;
+    }
+    else
+    {
+        /* No more data, so disable the FIFO/interrupt */
+        pcm_play_dma_stop();
+        pcm_play_dma_stopped_callback();
+    }
+    
+    /* Clear FIQ status */
+    CREQ = DAI_TX_IRQ_MASK | DAI_RX_IRQ_MASK;
+    
+    asm volatile(   "add   sp, sp, #8           \n"   /* Cleanup stack   */
+                    "ldmfd sp!, {r0-r7, ip, lr} \n"   /* Restore context */
+                    "subs  pc, lr, #4           \n"); /* Return from FIQ */
+}
+#endif