1 files changed, 34 insertions, 28 deletions

diff --git a/firmware/target/arm/pcm-telechips.c b/firmware/target/arm/pcm-telechips.c
index ae4aa5ef38..3d62fcd1a9 100644
--- a/firmware/target/arm/pcm-telechips.c
+++ b/firmware/target/arm/pcm-telechips.c
@@ -33,7 +33,12 @@ 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;
+    union
+    {
+        uint16_t *p;
+        const void *p_r;
+        void *p_w;
+    };
     size_t size;
 #if NUM_CORES > 1
     unsigned core;
@@ -143,7 +148,7 @@ static void play_stop_pcm(void)
 
 void pcm_play_dma_start(const void *addr, size_t size)
 {
-    dma_play_data.p    = (uint16_t*)addr;
+    dma_play_data.p_r  = addr;
     dma_play_data.size = size;
 
 #if NUM_CORES > 1
@@ -248,8 +253,9 @@ void fiq_handler(void)
      * r0-r3 and r12 is a working register.
      */
     asm volatile (
-        "stmfd   sp!, { r0-r4, lr }  \n" /* stack scratch regs and lr */
-        "mov     r4, #0              \n" /* Was the callback called? */
+        "sub     lr, lr, #4          \n"
+        "stmfd   sp!, { r0-r3, lr }  \n" /* stack scratch regs and lr */
+        "mov     r14, #0             \n" /* Was the callback called? */
 #if defined(CPU_TCC780X)
         "mov     r8, #0xc000         \n" /* DAI_TX_IRQ_MASK | DAI_RX_IRQ_MASK */
         "ldr     r9, =0xf3001004     \n" /* CREQ */
@@ -260,7 +266,7 @@ void fiq_handler(void)
         "str     r8, [r9]            \n" /* clear DAI IRQs */
         "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 */
+        "blo     .more_data          \n" /* if so, ask pcmbuf for more data */
 
     ".fill_fifo:                     \n"
         "ldr     r12, [r8], #4       \n" /* load two samples */
@@ -282,29 +288,30 @@ void fiq_handler(void)
         "sub     r9, r9, #0x10       \n" /* 4 words written */
         "stmia   r11, { r8-r9 }      \n" /* save p and size */
 
-        "cmp     r4, #0              \n" /* Callback called? */
-        "beq     .exit               \n"
-        /* "mov     r4, #0              \n" If get_more could be called multiple times! */
-        "ldr     r2, =pcm_play_dma_started\n"
-        "ldr     r2, [r2]            \n"
-        "cmp     r2, #0              \n"
-        "blxne   r2                  \n"
+        "cmp     r14, #0             \n" /* Callback called? */
+        "ldmeqfd sp!, { r0-r3, pc }^ \n" /* no? -> exit */
 
-    ".exit:                          \n"
-        "ldmfd   sp!, { r0-r4, lr }  \n"
-        "subs    pc, lr, #4          \n" /* FIQ specific return sequence */
+        "ldr     r1, =pcm_play_status_callback \n"
+        "ldr     r1, [r1]            \n"
+        "cmp     r1, #0              \n"
+        "movne   r0, %1              \n"
+        "blxne   r1                  \n"
+        "ldmfd   sp!, { r0-r3, pc }^ \n" /* exit */
 
     ".more_data:                     \n"
-        "mov     r4, #1              \n" /* Remember we got more data in this FIQ */
-        "ldr     r2, =pcm_play_get_more_callback \n"
-        "mov     r0, r11             \n" /* r0 = &p */
-        "add     r1, r11, #4         \n" /* r1 = &size */
-        "blx     r2                  \n" /* call pcm_play_get_more_callback */
-        "ldmia   r11, { r8-r9 }      \n" /* load new p and size */
-        "cmp     r9, #0x10           \n" /* did we actually get enough data? */
-        "bpl     .fill_fifo          \n" /* not stop and enough? refill */
-        "b       .exit               \n"
+        "mov     r14, #1             \n" /* Remember we got more data in this FIQ */
+        "mov     r0, %0              \n" /* r0 = status */
+        "mov     r1, r11             \n" /* r1 = &dma_play_data.p_r */
+        "add     r2, r11, #4         \n" /* r2 = &dma_play_data.size */
+        "mov     lr, pc              \n"
+        "ldr     pc, =pcm_play_dma_complete_callback \n"
+        "cmp     r0, #0              \n" /* any more to play? */
+        "ldmneia r11, { r8-r9 }      \n" /* load new p and size */
+        "cmpne   r9, #0x0f           \n" /* did we actually get enough data? */
+        "bhi     .fill_fifo          \n" /* not stop and enough? refill */
+        "ldmfd   sp!, { r0-r3, pc }^ \n" /* exit */
         ".ltorg                      \n"
+        : : "i"(PCM_DMAST_OK), "i"(PCM_DMAST_STARTED)
     );
 }
 #else /* C version for reference */
@@ -316,9 +323,8 @@ void fiq_handler(void)
     if (dma_play_data.size < 16)
     {
         /* p is empty, get some more data */
-        new_buffer = true;
-        pcm_play_get_more_callback((void**)&dma_play_data.p,
-                                   &dma_play_data.size);
+        new_buffer = pcm_play_dma_complete_callback(&dma_play_data.p_r,
+                                                    &dma_play_data.size);
     }
 
     if (dma_play_data.size >= 16)
@@ -339,7 +345,7 @@ void fiq_handler(void)
     CREQ = DAI_TX_IRQ_MASK | DAI_RX_IRQ_MASK;
 
     if (new_buffer)
-        pcm_play_dma_started_callback();
+        pcm_play_dma_status_callback(PCM_DMAST_STARTED);
 }
 #endif