From 92218a227dd0f8842df4cd745b0fb49a67256c22 Mon Sep 17 00:00:00 2001
From: Rekai Musuka <rekai@musuka.dev>
Date: Sun, 17 Jan 2021 18:58:57 -0600
Subject: [PATCH] feat: implement 0xff24 sound register

---
 src/bus.rs   | 10 ++++++----
 src/sound.rs | 36 +++++++++++++++++++++++++++++++++++-
 2 files changed, 41 insertions(+), 5 deletions(-)

diff --git a/src/bus.rs b/src/bus.rs
index 8a2f95b..d08a577 100644
--- a/src/bus.rs
+++ b/src/bus.rs
@@ -101,8 +101,9 @@ impl Bus {
                     0xFF0F => self.interrupt.flag.into(),
                     0xFF11 => self.sound.ch1.sound_duty.into(),
                     0xFF12 => self.sound.ch1.vol_envelope.into(),
-                    0xFF25 => self.sound.select.into(),
-                    0xFF26 => self.sound.status.into(),
+                    0xFF24 => self.sound.control.channel.into(),
+                    0xFF25 => self.sound.control.select.into(),
+                    0xFF26 => self.sound.control.status.into(),
                     _ => unimplemented!("Unable to read {:#06X} in I/O Registers", addr),
                 }
             }
@@ -162,8 +163,9 @@ impl Bus {
                     0xFF0F => self.interrupt.flag = byte.into(),
                     0xFF11 => self.sound.ch1.sound_duty = byte.into(),
                     0xFF12 => self.sound.ch1.vol_envelope = byte.into(),
-                    0xFF25 => self.sound.select = byte.into(),
-                    0xFF26 => self.sound.status = byte.into(), // FIXME: Should we control which bytes are written to here?
+                    0xFF24 => self.sound.control.channel = byte.into(),
+                    0xFF25 => self.sound.control.select = byte.into(),
+                    0xFF26 => self.sound.control.status = byte.into(), // FIXME: Should we control which bytes are written to here?
                     _ => unimplemented!("Unable to write to {:#06X} in I/O Registers", addr),
                 };
             }
diff --git a/src/sound.rs b/src/sound.rs
index 4d55a2d..fc83ca2 100644
--- a/src/sound.rs
+++ b/src/sound.rs
@@ -1,8 +1,14 @@
 #[derive(Debug, Clone, Copy, Default)]
 pub struct Sound {
-    pub status: SoundStatus,
+    pub control: SoundControl,
     pub ch1: Channel1,
+}
+
+#[derive(Debug, Clone, Copy, Default)]
+pub struct SoundControl {
+    pub channel: ChannelControl,
     pub select: SoundOutputSelect,
+    pub status: SoundStatus,
 }
 
 #[derive(Debug, Clone, Copy, Default)]
@@ -202,3 +208,31 @@ impl From<u8> for SoundOutputSelect {
         }
     }
 }
+
+#[derive(Debug, Clone, Copy, Default)]
+pub struct ChannelControl {
+    vin_to_term2: bool,
+    term2_level: u8,
+    vin_to_term1: bool,
+    term1_level: u8,
+}
+
+impl From<u8> for ChannelControl {
+    fn from(byte: u8) -> Self {
+        Self {
+            vin_to_term2: byte >> 7 == 0x01, // Get 7th bit
+            term2_level: (byte & 0x7F) >> 4, // Clear 7th then get 6 -> 4th bit
+            vin_to_term1: (byte >> 3) & 0x01 == 0x01,
+            term1_level: byte & 0x07, // Bits 2 -> 0
+        }
+    }
+}
+
+impl From<ChannelControl> for u8 {
+    fn from(control: ChannelControl) -> Self {
+        (control.vin_to_term1 as u8) << 7
+            | (control.term2_level as u8) << 4
+            | (control.vin_to_term1 as u8) << 3
+            | control.term1_level as u8
+    }
+}