Support Controller

examples/PoolBox/application/src/main.cpp

@@ -24,7 +24,6 @@ static void setup() {
 
 static void update() {
     Periphery::query_controller();
-    
     FontWriter::FontWriter cursor;
 
     const auto end_pos = cursor.write(FontWriter::Position::create(0, 32), "Cody is cute\n&\na \x1b[8;0;0mBAAAAABY!!!");

include/PSX/Periphery/raw_controller.hpp

@@ -106,7 +106,9 @@ namespace JabyEngine {
                     return (ButtonState::was_down(button) && !ButtonState::is_down(button));
                 }
 
-                friend class RawController;
+                friend class  RawController;
+                friend struct ControllerHelper;
+                friend void   query_controller();
             };
 
             Header      header;

include/PSX/System/IOPorts/periphery_io.hpp

@@ -3,6 +3,15 @@
 
 namespace JabyEngine {
     namespace Periphery_IO {
+        __declare_io_value(JOY_TX_DATA, uint32_t) {
+            static constexpr JOY_TX_DATA create(uint8_t byte) {
+                return JOY_TX_DATA{byte};
+            }
+        };
+
+        __declare_io_value(JOY_RX_DATA, uint8_t) {
+        };
+
         __declare_io_value(JOY_STAT, uint32_t) {
             static constexpr auto TXReadyStart    = Bit(0);
             static constexpr auto RXFifoNonEmpty  = Bit(1);
@@ -48,8 +57,8 @@ namespace JabyEngine {
             }
         };
 
-        __declare_io_port_w_type(,      uint8_t, JOY_TX_DATA, 0x1F801040);
+        __declare_io_port(,      JOY_TX_DATA, 0x1F801040);
-        __declare_io_port_w_type(const, uint8_t, JOY_RX_DATA, 0x1F801040);
+        __declare_io_port(const, JOY_RX_DATA, 0x1F801040);
         __declare_io_port(const, JOY_STAT, 0x1F801044);
         __declare_io_port(,      JOY_MODE, 0x1F801048);
         __declare_io_port(,      JOY_CTRL, 0x1F80104A);

src/Library/internal-include/periphery_internal.hpp

@@ -1,26 +1,34 @@
 #pragma once
-#include <PSX/System/IOPorts/periphery_io.hpp>
 #include <PSX/System/IOPorts/interrupt_io.hpp>
+#include <PSX/System/IOPorts/periphery_io.hpp>
+#include <PSX/Periphery/periphery.hpp>
+#include <stdio.h>
+
+extern "C" void busy_loop(int count);
 
 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);
+            busy_loop(500);
         }
 
         static void close_connection() {
             JOY_CTRL.write(JOY_CTRL::close());
         }
 
+        static void send_byte(uint8_t byte) {
+            while(!JOY_STAT.read().is_ready_transfer());
+            JOY_TX_DATA.write(JOY_TX_DATA::create(byte));
+        }
+
+        static uint8_t read_byte() {
+            while(!JOY_STAT.read().has_response());
+            return JOY_RX_DATA.read().raw;
+        }
+
         static void acknowledge() {
             while(JOY_STAT.read().is_set(JOY_STAT::ACKIrqLow));
             JOY_CTRL.write(JOY_CTRL.read().set(JOY_CTRL::ACK));

src/Library/src/BootLoader/periphery_boot.cpp

@@ -1,5 +1,6 @@
 #include "../../internal-include/periphery_internal.hpp"
 #include <PSX/System/syscalls.hpp>
+#include <stdio.h>
 
 namespace JabyEngine {
     namespace boot {
@@ -9,8 +10,7 @@ namespace JabyEngine {
                 Periphery_IO::JOY_BAUD.write(Periphery_IO::JOY_BAUD::create());
 
                 SysCall::EnterCriticalSection();
-                    Interrupt::enable_irq(Interrupt::Periphery);
+                    Interrupt::disable_irq(Interrupt::Periphery);
-                    Interrupt::ack_irq(Interrupt::Periphery);
                 SysCall::ExitCriticalSection();
             }
         }

src/Library/src/Periphery/periphery.cpp

@@ -1,3 +1,4 @@
+#include "../../internal-include/periphery_internal.hpp"
 #include <PSX/Periphery/periphery.hpp>
 #include <stdio.h>
 
@@ -5,11 +6,128 @@ namespace JabyEngine {
     namespace Periphery {
         RawController controller[PortCount][DeviceCount];
 
+        struct ControllerHelper {
+            static bool is_config(const RawController &cont) {
+                return (static_cast<uint8_t>(cont.header.state) & 0xF);
+            }
+
+            static void advance_config(RawController &cont) {
+                cont.header.state = static_cast<RawController::State>((static_cast<uint32_t>(cont.header.state) << 1));
+            }
+
+            static uint8_t* raw_device_data(RawController &cont) {
+                return reinterpret_cast<uint8_t*>(&cont.button.currentState);
+            }
+        };
+
+        static void set_config_command(RawController::State state, uint8_t (&header)[3], uint8_t (&data)[6]) {
+            static constexpr uint32_t CMDIDX = 1;
+
+            switch(state) {
+                case RawController::State::EnterConfigMode:
+                    static constexpr uint8_t EnterCFGModeCMD = 0x1;
+
+                    header[CMDIDX] = 0x43;
+                    data[0]        = EnterCFGModeCMD;
+                break;
+
+                case RawController::State::LockAnalog:
+                    static constexpr uint8_t valID = 0;
+                    static constexpr uint8_t selID = 1;
+
+                    header[CMDIDX] = 0x44;
+                    data[valID]    = static_cast<uint8_t>(LED::State::On);
+                    data[selID]    = static_cast<uint8_t>(LED::Lock::On);
+                break;
+
+                case RawController::State::UnlockRumble:
+                    header[CMDIDX] = 0x4D;
+
+                    data[1] = 0x1;
+                    for(int32_t n = 2; n < 6; n++) {
+                        data[n] = 0xFF;
+                    }
+                break;
+
+                case RawController::State::ExitConfigMode:
+                    header[CMDIDX] = 0x43;
+                break;
+            }
+        }
+
+        static size_t send_data(const uint8_t* header, uint8_t* headerResponse, const uint8_t* data, uint8_t* response) {
+            const uint8_t *src  = header;
+            uint8_t       *dst  = headerResponse;
+            size_t         size = 3;
+
+            for(size_t n = 0; n < size; n++) {
+                Periphery::send_byte(*src);
+                Periphery::acknowledge();
+                *dst = Periphery::read_byte();
+
+                if(n == 2) {
+                    const uint8_t id = headerResponse[1];
+                    size += (id == 0xFF) ? 0 : ((id & 0xF) << 1);
+
+                    src = data;
+                    dst = response;
+                }
+
+                else {
+                    src++;
+                    dst++;
+                }
+
+                busy_loop(15);
+            }
+            return size;
+        }
+
         void query_controller() {
-            printf("Needs implementation\n");
+            static constexpr auto TypeIDX = 1;
+
+            for(uint32_t port = 0; port < Periphery::PortCount; port++) {
+                Periphery::connect_to(port);
+                for(uint32_t id = 0; id < Periphery::DeviceCount; id++) {
+                    auto &cur_controller = controller[port][id];
+
+                    uint8_t header[] = {static_cast<uint8_t>((id + 1)), 0x42, 0x0};
+                    uint8_t data[]   = {cur_controller.header.rumble0, cur_controller.header.rumble1, 0x0, 0x0, 0x0, 0x0};
+
+                    // Can this move to the if??
+                    set_config_command(cur_controller.header.state, header, data);
+                    if(ControllerHelper::is_config(cur_controller)) {    
+                        send_data(header, header, data, data);
+                        ControllerHelper::advance_config(cur_controller);
+                    }
+
+                    else {
+                        cur_controller.button.exchange_state();
+
+                        send_data(header, header, data, ControllerHelper::raw_device_data(cur_controller));
+                        const bool isValidController = (header[TypeIDX] != 0xFF);
+
+                        if(header[TypeIDX] != cur_controller.header.type) {
+                            cur_controller.header.type  = header[TypeIDX];
+                            cur_controller.header.state = isValidController ? RawController::State::EnterConfigMode : RawController::State::Disconnected;
+
+                            /*if(!isValidController)
+                            {
+                                printf("Disconnected!\n");
+                            }
+
+                            else
+                            {
+                                printf("ID: 0x%02X 0x%04X\n", header[TypeIDX], contData.button);
+                            }*/
+                        }
+                    }
+                }
+                Periphery::close_connection();
+            }
         }
 
         static_assert(!Configuration::Periphery::UsePortB,    "Port B not supported yet");
-        static_assert(!Configuration::Periphery::UseMultiTap, "MultiTap not supported yet");
+        static_assert(!Configuration::Periphery::UseMultiTap, "MultiTap not supported yet");        
     }
 }