step-frame added to debug

[picodrive.git] / Pico / sound / mix.s

diff --git a/Pico/sound/mix.s b/Pico/sound/mix.s
index ab0853e..d44e228 100644 (file)
--- a/Pico/sound/mix.s
+++ b/Pico/sound/mix.s
@@ -1,63 +1,11 @@
 @ vim:filetype=armasm
 
-.global memcpy32 @ int *dest, int *src, int count
-
-memcpy32:
-    stmfd   sp!, {r4,lr}
-
-    subs    r2, r2, #4
-    bmi     mcp32_fin
-
-mcp32_loop:
-    ldmia   r1!, {r3,r4,r12,lr}
-    subs    r2, r2, #4
-    stmia   r0!, {r3,r4,r12,lr}
-    bpl     mcp32_loop
-
-mcp32_fin:
-    tst     r2, #3
-    ldmeqfd sp!, {r4,pc}
-    tst     r2, #1
-    ldrne   r3, [r1], #4
-    strne   r3, [r0], #4
-
-mcp32_no_unal1:
-    tst     r2, #2
-    ldmneia r1!, {r3,r12}
-    ldmfd   sp!, {r4,lr}
-    stmneia r0!, {r3,r12}
-    bx      lr
-
-
-
-.global memset32 @ int *dest, int c, int count
-
-memset32:
-    stmfd   sp!, {lr}
-
-    mov     r3, r1
-    subs    r2, r2, #4
-    bmi     mst32_fin
-
-    mov     r12,r1
-    mov     lr, r1
-
-mst32_loop:
-    subs    r2, r2, #4
-    stmia   r0!, {r1,r3,r12,lr}
-    bpl     mst32_loop
-
-mst32_fin:
-    tst     r2, #1
-    strne   r1, [r0], #4
-
-    tst     r2, #2
-    stmneia r0!, {r1,r3}
-
-    ldmfd   sp!, {lr}
-    bx      lr
+@ Generic routines for mixing audio samples
+@ (c) Copyright 2007, Grazvydas "notaz" Ignotas
 
 
+.text
+.align 4
 
 @ this assumes src is word aligned
 .global mix_16h_to_32 @ int *dest, short *src, int count
@@ -206,10 +154,10 @@ m16_32_s2_no_unal2:
 @ limit
 @ reg=int_sample, lr=1, r3=tmp, kills flags
 .macro Limit reg
-    add     r3, lr, \reg, asr #16
+    add     r3, lr, \reg, asr #15
     bics    r3, r3, #1                 @ in non-overflow conditions r3 is 0 or 1
     movne   \reg, #0x8000
-    submi   \reg, \reg, #1
+    subpl   \reg, \reg, #1
 .endm
 
 
@@ -223,11 +171,11 @@ m16_32_s2_no_unal2:
 @    mov     r3, #0x8000
 @    subpl   r3, r3, #1
 
-    add     r3, lr, \reg, asr #16
+    add     r3, lr, \reg, asr #15
     bics    r3, r3, #1                 @ in non-overflow conditions r3 is 0 or 1
     moveq   \reg, \reg, lsl #16
     movne   \reg, #0x80000000
-    submi   \reg, \reg, #0x00010000
+    subpl   \reg, \reg, #0x00010000
 .endm
 
 
@@ -292,7 +240,7 @@ mix_32_to_16_mono:
     @ check if dest is word aligned
     tst     r0, #2
     beq     m32_16_mo_no_unalw
-    ldrsh   r5, [r0], #2
+    ldrsh   r5, [r0]
     ldr     r4, [r1], #4
     sub     r2, r2, #1
     add     r4, r4, r5
@@ -339,7 +287,7 @@ m32_16_mo_end:
 m32_16_mo_no_unal2:
     tst     r2, #1
     ldmeqfd sp!, {r4-r8,pc}
-    ldrsh   r5, [r0], #2
+    ldrsh   r5, [r0]
     ldr     r4, [r1], #4
     add     r4, r4, r5
     Limit   r4
@@ -348,3 +296,72 @@ m32_16_mo_no_unal2:
     ldmfd   sp!, {r4-r8,lr}
     bx      lr
 
+
+
+.data
+.align 4
+
+.global mix_32_to_16l_level
+mix_32_to_16l_level:
+    .word   0
+
+.text
+.align 4
+
+@ same as mix_32_to_16l_stereo, but with additional shift
+.global mix_32_to_16l_stereo_lvl @ short *dest, int *src, int count
+
+mix_32_to_16l_stereo_lvl:
+    stmfd   sp!, {r4-r9,lr}
+
+    ldr     r9, =mix_32_to_16l_level
+    mov     lr, #1
+    ldr     r9, [r9]
+
+    mov     r2, r2, lsl #1
+    subs    r2, r2, #4
+    bmi     m32_16l_st_l_end
+
+m32_16l_st_l_loop:
+    ldmia   r0,  {r8,r12}
+    ldmia   r1!, {r4-r7}
+    mov     r8, r8, lsl #16
+    mov     r12,r12,lsl #16
+    add     r4, r4, r8, asr #16
+    add     r5, r5, r8, asr #16
+    add     r6, r6, r12,asr #16
+    add     r7, r7, r12,asr #16
+    mov     r4, r4, asr r9
+    mov     r5, r5, asr r9
+    mov     r6, r6, asr r9
+    mov     r7, r7, asr r9
+    Limitsh r4
+    Limitsh r5
+    Limitsh r6
+    Limitsh r7
+    subs    r2, r2, #4
+    orr     r4, r5, r4, lsr #16
+    orr     r5, r7, r6, lsr #16
+    stmia   r0!, {r4,r5}
+    bpl     m32_16l_st_l_loop
+
+m32_16l_st_l_end:
+    @ check for remaining bytes to convert
+    tst     r2, #2
+    beq     m32_16l_st_l_no_unal2
+    ldrsh   r6, [r0]
+    ldmia   r1!,{r4,r5}
+    add     r4, r4, r6
+    add     r5, r5, r6
+    mov     r4, r4, asr r9
+    mov     r5, r5, asr r9
+    Limitsh r4
+    Limitsh r5
+    orr     r4, r5, r4, lsr #16
+    str     r4, [r0], #4
+
+m32_16l_st_l_no_unal2:
+    ldmfd   sp!, {r4-r9,lr}
+    bx      lr
+
+