From 747048ba893f15285e6d855186959d3a4272e8e3 Mon Sep 17 00:00:00 2001
From: Jaby <Jaby>
Date: Tue, 2 Jan 2024 19:47:23 -0600
Subject: [PATCH] Extend Periphery code

---
 include/PSX/Auxiliary/bits.hpp                |  4 ++
 include/PSX/System/IOPorts/periphery_io.hpp   | 41 ++++++++++++++++++-
 .../internal-include/periphery_internal.hpp   | 31 ++++++++++++++
 src/Library/src/BootLoader/periphery_boot.cpp |  8 +++-
 4 files changed, 81 insertions(+), 3 deletions(-)
 create mode 100644 src/Library/internal-include/periphery_internal.hpp

diff --git a/include/PSX/Auxiliary/bits.hpp b/include/PSX/Auxiliary/bits.hpp
index d5ce81ca..370764ca 100644
--- a/include/PSX/Auxiliary/bits.hpp
+++ b/include/PSX/Auxiliary/bits.hpp
@@ -150,6 +150,10 @@ namespace JabyEngine {
             return *reinterpret_cast<S*>(&value);
         }
     }
+
+    static constexpr uint32_t operator<<(uint32_t value, const Bit& bit) {
+        return value << bit.pos;
+    }
 }
 
 #define __start_end_bit2_start_length(start_bit, end_bit) start_bit, (end_bit - start_bit + 1)
\ No newline at end of file
diff --git a/include/PSX/System/IOPorts/periphery_io.hpp b/include/PSX/System/IOPorts/periphery_io.hpp
index dc049264..08e52e28 100644
--- a/include/PSX/System/IOPorts/periphery_io.hpp
+++ b/include/PSX/System/IOPorts/periphery_io.hpp
@@ -3,6 +3,39 @@
 
 namespace JabyEngine {
     namespace Periphery_IO {
+        __declare_io_value(JOY_STAT, uint32_t) {
+            static constexpr auto TXReadyStart    = Bit(0);
+            static constexpr auto RXFifoNonEmpty  = Bit(1);
+            static constexpr auto TXReadyFinished = Bit(2);
+            static constexpr auto RXParityError   = Bit(3);
+            static constexpr auto ACKIrqLow       = Bit(7);
+
+            inline bool has_response() const {
+                return this->is_set(RXFifoNonEmpty);
+            }
+
+            inline bool is_ready_transfer() const {
+                return this->is_set(TXReadyFinished);
+            }
+        };
+
+        __declare_io_value(JOY_CTRL, uint16_t) {
+            static constexpr auto TXEnable      = Bit(0);
+            static constexpr auto SelectJoy     = Bit(1);
+            static constexpr auto ACK           = Bit(4);
+            static constexpr auto ACKIrqEnable  = Bit(12);
+            static constexpr auto PortBSelected = Bit(13);
+            static constexpr auto PortASelected = !PortBSelected;
+
+            static constexpr JOY_CTRL create_for(uint16_t port) {
+                return JOY_CTRL{static_cast<uint16_t>(port << PortBSelected)}.set(TXEnable, SelectJoy, ACKIrqEnable);
+            }
+
+            static constexpr JOY_CTRL close() {
+                return JOY_CTRL{0};
+            }
+        };
+
         __declare_io_value(JOY_MODE, uint16_t) {
             static constexpr JOY_MODE create() {
                 return JOY_MODE{0x000D};
@@ -15,7 +48,11 @@ namespace JabyEngine {
             }
         };
 
-        __declare_io_port(, JOY_MODE, 0x1F801048);
-        __declare_io_port(, JOY_BAUD, 0x1F80104E);
+        __declare_io_port_w_type(,      uint8_t, JOY_TX_DATA, 0x1F801040);
+        __declare_io_port_w_type(const, uint8_t, JOY_RX_DATA, 0x1F801040);
+        __declare_io_port(const, JOY_STAT, 0x1F801044);
+        __declare_io_port(,      JOY_MODE, 0x1F801048);
+        __declare_io_port(,      JOY_CTRL, 0x1F80104A);
+        __declare_io_port(,      JOY_BAUD, 0x1F80104E);
     }
 }
\ No newline at end of file
diff --git a/src/Library/internal-include/periphery_internal.hpp b/src/Library/internal-include/periphery_internal.hpp
new file mode 100644
index 00000000..098acb6f
--- /dev/null
+++ b/src/Library/internal-include/periphery_internal.hpp
@@ -0,0 +1,31 @@
+#pragma once
+#include <PSX/System/IOPorts/periphery_io.hpp>
+#include <PSX/System/IOPorts/interrupt_io.hpp>
+
+namespace JabyEngine {
+    namespace Periphery {
+        using namespace Periphery_IO;
+
+        static void busy_cycle(uint32_t cycles) {
+            while(cycles--) {
+                asm("");
+            }
+        }
+
+        static void connect_to(uint16_t port) {
+            JOY_CTRL.write(JOY_CTRL::create_for(port));
+            busy_cycle(500);
+        }
+
+        static void close_connection() {
+            JOY_CTRL.write(JOY_CTRL::close());
+        }
+
+        static void acknowledge() {
+            while(JOY_STAT.read().is_set(JOY_STAT::ACKIrqLow));
+            JOY_CTRL.write(JOY_CTRL.read().set(JOY_CTRL::ACK));
+            
+            Interrupt::ack_irq(Interrupt::Periphery);
+        }
+    }
+}
\ No newline at end of file
diff --git a/src/Library/src/BootLoader/periphery_boot.cpp b/src/Library/src/BootLoader/periphery_boot.cpp
index 63843cf7..8e0e3f90 100644
--- a/src/Library/src/BootLoader/periphery_boot.cpp
+++ b/src/Library/src/BootLoader/periphery_boot.cpp
@@ -1,4 +1,5 @@
-#include <PSX/System/IOPorts/periphery_io.hpp>
+#include "../../internal-include/periphery_internal.hpp"
+#include <PSX/System/syscalls.hpp>
 
 namespace JabyEngine {
     namespace boot {
@@ -6,6 +7,11 @@ namespace JabyEngine {
             void setup() {
                 Periphery_IO::JOY_MODE.write(Periphery_IO::JOY_MODE::create());
                 Periphery_IO::JOY_BAUD.write(Periphery_IO::JOY_BAUD::create());
+
+                SysCall::EnterCriticalSection();
+                    Interrupt::enable_irq(Interrupt::Periphery);
+                    Interrupt::ack_irq(Interrupt::Periphery);
+                SysCall::ExitCriticalSection();
             }
         }
     }