From cf663aee73570a67e319c1fd51afc1984783440b Mon Sep 17 00:00:00 2001
From: jaby <jaby@william.zone>
Date: Fri, 2 Sep 2022 13:04:24 +0200
Subject: [PATCH] Clear voices

---
 include/PSX/System/IOPorts/IOPort.hpp   |  7 ++++++-
 include/PSX/System/IOPorts/SPU_IO.hpp   |  9 ++++++++-
 src/Library/src/BootLoader/boot_spu.cpp | 15 +++++++++++++++
 3 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/include/PSX/System/IOPorts/IOPort.hpp b/include/PSX/System/IOPorts/IOPort.hpp
index 4b88b87d..d0777453 100644
--- a/include/PSX/System/IOPorts/IOPort.hpp
+++ b/include/PSX/System/IOPorts/IOPort.hpp
@@ -138,6 +138,10 @@ public:
         const_cast<volatile IOPort<T>*>(this)->value = value;
     }
 
+    constexpr volatile T& ref() {
+        return const_cast<volatile IOPort<T>*>(this)->value;
+    }
+
     //For raw access
     constexpr operator T() const {
         return this->value;
@@ -204,7 +208,8 @@ struct __no_align ubus32_t {
 static constexpr uintptr_t IO_Base_Mask = 0xF0000000;
 static constexpr uintptr_t IO_Base_Adr  = 0x10000000;
 
-#define __declare_io_port_global(type, name, adr) static __always_inline auto& name = *reinterpret_cast<IOPort<type>*>((IO_Base_Adr + (adr & ~IO_Base_Mask)))
+#define __declare_io_port_global(type, name, adr)             static __always_inline auto& name = *reinterpret_cast<IOPort<type>*>((IO_Base_Adr + (adr & ~IO_Base_Mask)))
+#define __declare_io_port_global_array(type, name, adr, size) static __always_inline auto& name = reinterpret_cast<type(&)[size]>(*reinterpret_cast<type*>((IO_Base_Adr + (adr & ~IO_Base_Mask))));
 #define __io_port_inherit(name) \
     using IOPort::operator=; \
     constexpr name() = default; \
diff --git a/include/PSX/System/IOPorts/SPU_IO.hpp b/include/PSX/System/IOPorts/SPU_IO.hpp
index 68d23310..7486b518 100644
--- a/include/PSX/System/IOPorts/SPU_IO.hpp
+++ b/include/PSX/System/IOPorts/SPU_IO.hpp
@@ -53,6 +53,8 @@ namespace SPU {
         };
 
         struct __no_align SR : public IOPort<uint16_t> {
+            __io_port_inherit(SR);
+            
             static constexpr Bit<Mode>       SustainMode      = (31 - 16);
             static constexpr Bit<Direction>  SustainDirection = (30 - 16);
             static constexpr BitRange<Shift> SustainShift     = BitRange<Shift>::from_to((24 - 16), (28 - 16));
@@ -62,6 +64,8 @@ namespace SPU {
         };
 
         struct __no_align AD : public IOPort<uint16_t> {
+            __io_port_inherit(AD);
+
             static constexpr Bit<Mode>          AttackMode   = 15;
             static constexpr BitRange<Shift>    AttackShift  = BitRange<Shift>::from_to(10, 14);
             static constexpr BitRange<Step>     AttackStep   = BitRange<Step>::from_to(8, 9);
@@ -103,6 +107,8 @@ namespace SPU {
             static constexpr Bit<uint16_t>             CDAudioEnable       = 0;
         };
 
+        static constexpr size_t VoiceCount = 24;
+
         namespace Key {
             __declare_io_port_global(ubus32_t, on,     0x1F801D88);
             __declare_io_port_global(ubus32_t, off,    0x1F801D8C);
@@ -114,7 +120,8 @@ namespace SPU {
             __declare_io_port_global(SweepVolume, right, 0x1F801D82);
         }
 
-        __declare_io_port_global(ControlRegister, Control, 0x1F801DAA);
+              __declare_io_port_global(ControlRegister, Control, 0x1F801DAA);
+        __declare_io_port_global_array(Voice,           Voices,  0x1F801C00, VoiceCount);
     }
 }
 
diff --git a/src/Library/src/BootLoader/boot_spu.cpp b/src/Library/src/BootLoader/boot_spu.cpp
index f51ee1c5..55fb6fe7 100644
--- a/src/Library/src/BootLoader/boot_spu.cpp
+++ b/src/Library/src/BootLoader/boot_spu.cpp
@@ -19,6 +19,20 @@ namespace SPU {
         Control.write(ControlRegister());
     }
 
+    static void clear_voice() {
+        for(auto& voice : Voices) {
+            voice.volumeLeft.write(SweepVolume());
+            voice.volumeRight.write(SweepVolume());
+            voice.sampleRate.write(SampleRate());
+            voice.ad.write(AD());
+            voice.sr.write(SR());
+            voice.currentVolume.write(SweepVolume());
+            
+            voice.adr.write(0x200);
+            voice.repeatAdr.write(0x200);
+        }
+    }
+
     static void setup_control_register() {
         static constexpr auto SetupValue = ControlRegister() | ControlRegister::Enable | ControlRegister::Unmute | ControlRegister::CDAudioEnable;
 
@@ -29,6 +43,7 @@ namespace SPU {
         clear_key();
         clear_main_volume();
         clear_control_register();
+        clear_voice();
 
         setup_control_register();
     }