Implement noise shaping dither

src/music/main_mixer.s

@@ -870,123 +870,103 @@ C_end_channel_state_loop:
         B       C_channel_state_loop
 
 C_main_mixer_return:
-        ADR     R0, C_downsampler
-        BX      R0
+    ADR     R5, V_noise_shape
+    LDRB    R4, [R5, #0]            @ left noise shape
+    LSL     R4, R4, #16
+    LDRB    R5, [R5, #1]            @ right noise shape
+    LSL     R5, R5, #16
+    ADR     R0, C_downsampler
+    BX      R0
+
+
+V_noise_shape:
+    .byte 0, 0
 
     .arm
     .align  2
 
 C_downsampler:
-    LDR     R4, =0xFFF80035
-    LDR     R5, =0x0034FFF8
     LDR     R8, [SP, #ARG_FRAME_LENGTH]
     LDR     R9, [SP, #ARG_BUFFER_POS]
     LDR     R10, hq_buffer
     MOV     R11, #0xFF000000
+    MOV     LR, #0x007F0000
 
 C_downsampler_loop:
         LDMIA   R10, {R0, R1, R2, R3}
 
-        MOV     R12, R0             @ left sample #1
-        CMP     R12, #0x40000000
+        ADD     R12, R4, R0         @ left sample #1
+        CMP     R12, #0x3FC00000
         MOVGE   R12, #0x3FC00000
         CMP     R12, #-0x40000000
         MOVLT   R12, #-0x40000000
+        AND     R4, LR, R12
         AND     R6, R11, R12, LSL#1
 
-        MOV     R0, R0, LSL#16      @ right
-        CMP     R0, #0x40000000
+        ADD     R0, R5, R0, LSL#16  @ right
+        CMP     R0, #0x3FC00000
         MOVGE   R0, #0x3FC00000
         CMP     R0, #-0x40000000
         MOVLT   R0, #-0x40000000
+        AND     R5, LR, R0
         AND     R7, R11, R0, LSL#1
 
-        MOV     R12, R1             @ left sample #2
-        CMP     R12, #0x40000000
+        ADD     R12, R4, R1         @ left sample #2
+        CMP     R12, #0x3FC00000
         MOVGE   R12, #0x3FC00000
         CMP     R12, #-0x40000000
         MOVLT   R12, #-0x40000000
+        AND     R4, LR, R12
         AND     R12, R11, R12, LSL#1
         ORR     R6, R12, R6, LSR#8
 
-        MOV     R1, R1, LSL#16      @ right
-        CMP     R1, #0x40000000
+        ADD     R1, R5, R1, LSL#16  @ right
+        CMP     R1, #0x3FC00000
         MOVGE   R1, #0x3FC00000
         CMP     R1, #-0x40000000
         MOVLT   R1, #-0x40000000
+        AND     R5, LR, R1
         AND     R1, R11, R1, LSL#1
         ORR     R7, R1, R7, LSR#8
 
-        MOV     R12, R2             @ left sample #3
-        CMP     R12, #0x40000000
+        ADD     R12, R4, R2         @ left sample #3
+        CMP     R12, #0x3FC00000
         MOVGE   R12, #0x3FC00000
         CMP     R12, #-0x40000000
         MOVLT   R12, #-0x40000000
+        AND     R4, LR, R12
         AND     R12, R11, R12, LSL#1
         ORR     R6, R12, R6, LSR#8
 
-        MOV     R2, R2, LSL#16      @ right
-        CMP     R2, #0x40000000
+        ADD     R2, R5, R2, LSL#16  @ right
+        CMP     R2, #0x3FC00000
         MOVGE   R2, #0x3FC00000
         CMP     R2, #-0x40000000
         MOVLT   R2, #-0x40000000
+        AND     R5, LR, R2
         AND     R2, R11, R2, LSL#1
         ORR     R7, R2, R7, LSR#8
 
-        MOV     R12, R3             @ left sample #4
-        CMP     R12, #0x40000000
+        ADD     R12, R4, R3         @ left sample #4
+        CMP     R12, #0x3FC00000
         MOVGE   R12, #0x3FC00000
         CMP     R12, #-0x40000000
         MOVLT   R12, #-0x40000000
+        AND     R4, LR, R12
         AND     R12, R11, R12, LSL#1
         ORR     R6, R12, R6, LSR#8
 
-        MOV     R3, R3, LSL#16      @ right
-        CMP     R3, #0x40000000
+        ADD     R3, R5, R3, LSL#16  @ right
+        CMP     R3, #0x3FC00000
         MOVGE   R3, #0x3FC00000
         CMP     R3, #-0x40000000
         MOVLT   R3, #-0x40000000
+        AND     R5, LR, R3
         AND     R3, R11, R3, LSL#1
         ORR     R7, R3, R7, LSR#8
 
-        @LDR     R12, [R9, #0x630]    @ left
         STR     R6, [R9, #0x630]
-        @LDR     R6, [R9]            @ right
         STR     R7, [R9], #4
-
-        @MOVS    R7, R12, ASR#24
-        @ADDMI   R7, R7, #1
-        @MUL     R3, R4, R7
-        @MOVS    R7, R6, ASR#24
-        @ADDMI   R7, R7, #1
-        @MLA     R3, R5, R7, R3
-        @MOV     R12, R12, LSL#8
-        @MOV     R6, R6, LSL#8
-
-        @MOVS    R7, R12, ASR#24
-        @ADDMI   R7, R7, #1
-        @MUL     R2, R4, R7
-        @MOVS    R7, R6, ASR#24
-        @ADDMI   R7, R7, #1
-        @MLA     R2, R5, R7, R2
-        @MOV     R12, R12, LSL#8
-        @MOV     R6, R6, LSL#8
-
-        @MOVS    R7, R12, ASR#24
-        @ADDMI   R7, R7, #1
-        @MUL     R1, R4, R7
-        @MOVS    R7, R6, ASR#24
-        @ADDMI   R7, R7, #1
-        @MLA     R1, R5, R7, R1
-        @MOV     R12, R12, LSL#8
-        @MOV     R6, R6, LSL#8
-
-        @MOVS    R7, R12, ASR#24
-        @ADDMI   R7, R7, #1
-        @MUL     R0, R4, R7
-        @MOVS    R7, R6, ASR#24
-        @ADDMI   R7, R7, #1
-        @MLA     R0, R5, R7, R0
         MOV     R0, #0
         MOV     R1, #0
         MOV     R2, #0
@@ -997,6 +977,7 @@ C_downsampler_loop:
         SUBS    R8, #4
         BGT     C_downsampler_loop
 
+    ADR     R1, V_noise_shape
     ADR     R0, (C_downsampler_return+1)
     BX      R0
 
@@ -1006,6 +987,10 @@ C_downsampler_loop:
     .thumb
 
 C_downsampler_return:
+    LSR     R4, #16
+    STRB    R4, [R1, #0]
+    LSR     R5, #16
+    STRB    R5, [R1, #1]
     LDR     R0, [SP, #ARG_PCM_STRUCT]
     LDR     R3, mixer_finished_status           @ this is used to indicate the interrupt handler the rendering was finished properly
     STR     R3, [R0]