From 5bd656d8cb1308414151e8a7759468593cd18b50 Mon Sep 17 00:00:00 2001
From: Gwilym Kuiper <gw@ilym.me>
Date: Tue, 26 Oct 2021 22:14:34 +0100
Subject: [PATCH] Add support for stereo sound

---
 agb/src/sound/mixer/mixer.s     | 40 +++++++++++++++++++++++++++++++++
 agb/src/sound/mixer/sw_mixer.rs | 40 +++++++++++++++++++++++----------
 2 files changed, 68 insertions(+), 12 deletions(-)

diff --git a/agb/src/sound/mixer/mixer.s b/agb/src/sound/mixer/mixer.s
index 151c883e..440753b6 100644
--- a/agb/src/sound/mixer/mixer.s
+++ b/agb/src/sound/mixer/mixer.s
@@ -93,6 +93,46 @@ same_modification:
 
 agb_arm_end agb_rs__mixer_add
 
+agb_arm_func agb_rs__mixer_add_stereo
+    @ Arguments
+    @ r0 - pointer to the data to be copied (u8 array)
+    @ r1 - pointer to the sound buffer (i16 array which will alternate left and right channels, 32-bit aligned)
+    @
+    @ The sound buffer must be SOUND_BUFFER_SIZE * 2 in size = 176 * 2
+    push {r4-r8}
+
+    ldr r5, =0x0000FFFF
+
+.macro mixer_add_loop_simple_stereo
+    ldrsh r6, [r0], #2        @ load the current sound sample to r6
+
+    ldr r4, [r1]             @ read the current value
+
+    mov r7, r6, asr #8
+    and r7, r7, r5
+    lsl r6, r6, #24
+    orr r6, r7, r6, asr #8
+
+    add r4, r4, r6, lsl #4  @ r4 += r6 << 4 (calculating both the left and right samples together)
+
+    str r4, [r1], #4         @ store the new value, and increment the pointer
+.endm
+
+    mov r8, #SOUND_BUFFER_SIZE
+1:
+    mixer_add_loop_simple_stereo
+    mixer_add_loop_simple_stereo
+    mixer_add_loop_simple_stereo
+    mixer_add_loop_simple_stereo
+
+    subs r8, r8, #4          @ loop counter
+    bne 1b                   @ jump back if we're done with the loop
+
+    pop {r4-r8}
+    bx lr
+
+agb_arm_end agb_rs__mixer_add_stereo
+
 .macro clamp_s8 reg:req
     cmn \reg, #127
     mvnlt \reg, #127
diff --git a/agb/src/sound/mixer/sw_mixer.rs b/agb/src/sound/mixer/sw_mixer.rs
index 408eb230..00a1f56b 100644
--- a/agb/src/sound/mixer/sw_mixer.rs
+++ b/agb/src/sound/mixer/sw_mixer.rs
@@ -13,6 +13,8 @@ extern "C" {
         right_amount: Num<i16, 4>,
     );
 
+    fn agb_rs__mixer_add_stereo(sound_data: *const u8, sound_buffer: *mut Num<i16, 4>);
+
     fn agb_rs__mixer_collapse(sound_buffer: *mut i8, input_buffer: *const Num<i16, 4>);
 }
 
@@ -133,12 +135,16 @@ impl MixerBuffer {
                 continue;
             }
 
+            let playback_speed = if channel.is_stereo {
+                2.into()
+            } else {
+                channel.playback_speed
+            };
+
             let right_amount = ((channel.panning + 1) / 2) * channel.volume;
             let left_amount = ((-channel.panning + 1) / 2) * channel.volume;
 
-            if (channel.pos + channel.playback_speed * SOUND_BUFFER_SIZE).floor()
-                >= channel.data.len()
-            {
+            if (channel.pos + playback_speed * SOUND_BUFFER_SIZE).floor() >= channel.data.len() {
                 // TODO: This should probably play what's left rather than skip the last bit
                 if channel.should_loop {
                     channel.pos -= channel.data.len();
@@ -148,16 +154,26 @@ impl MixerBuffer {
                 }
             }
 
-            unsafe {
-                agb_rs__mixer_add(
-                    channel.data.as_ptr().add(channel.pos.floor()),
-                    buffer.as_mut_ptr(),
-                    channel.playback_speed,
-                    left_amount,
-                    right_amount,
-                );
+            if channel.is_stereo {
+                unsafe {
+                    agb_rs__mixer_add_stereo(
+                        channel.data.as_ptr().add(channel.pos.floor()),
+                        buffer.as_mut_ptr(),
+                    );
+                }
+            } else {
+                unsafe {
+                    agb_rs__mixer_add(
+                        channel.data.as_ptr().add(channel.pos.floor()),
+                        buffer.as_mut_ptr(),
+                        playback_speed,
+                        left_amount,
+                        right_amount,
+                    );
+                }
             }
-            channel.pos += channel.playback_speed * SOUND_BUFFER_SIZE;
+
+            channel.pos += playback_speed * SOUND_BUFFER_SIZE;
         }
 
         let write_buffer = self.get_write_buffer();