From e12666cf50ce552c96ea6e31079ca26a63fad5f6 Mon Sep 17 00:00:00 2001
From: Naxdy <naxdy@naxdy.org>
Date: Mon, 14 Oct 2024 20:45:32 +0200
Subject: [PATCH] feat(procon): implement spi functionality

---
 src/hid/procon.rs | 187 ++++++++++++++++++++++++++++++++++++++++------
 src/input.rs      |   2 +-
 2 files changed, 164 insertions(+), 25 deletions(-)

diff --git a/src/hid/procon.rs b/src/hid/procon.rs
index a7579e9..474cfdf 100644
--- a/src/hid/procon.rs
+++ b/src/hid/procon.rs
@@ -228,18 +228,23 @@ impl ProconByteReport {
         self[26] = 0x02;
     }
 
-    fn sw_spi_readfromaddress(&mut self, offset_address: u8, address: u8, length: u8) {
+    fn sw_spi_readfromaddress(
+        &mut self,
+        offset_address: u8,
+        address: u8,
+        length: u8,
+        switch_host_address: &[u8],
+    ) {
         let read_info = [address, offset_address, 0x00, 0x00, length];
         self[15..(15 + read_info.len())].copy_from_slice(&read_info);
 
         let mut output_spi_data = [0u8; 30];
 
-        output_spi_data
-            .iter_mut()
-            .enumerate()
-            .for_each(|(i, e)| *e = sw_spi_getaddressdata(offset_address, address + i as u8));
+        output_spi_data.iter_mut().enumerate().for_each(|(i, e)| {
+            *e = sw_spi_getaddressdata(offset_address, address + i as u8, switch_host_address)
+        });
 
-        self[19..(19 + length as usize)].copy_from_slice(&output_spi_data[..(length as usize)]);
+        self[20..(20 + length as usize)].copy_from_slice(&output_spi_data[..(length as usize)]);
     }
 
     fn set_trigerret(&mut self, time_10_ms: u16) {
@@ -252,9 +257,133 @@ impl ProconByteReport {
     }
 }
 
-fn sw_spi_getaddressdata(_offset_address: u8, _address: u8) -> u8 {
-    // TODO
-    0
+fn sw_spi_getaddressdata(offset_address: u8, address: u8, switch_host_address: &[u8]) -> u8 {
+    match offset_address {
+        0x00 => 0x00,
+        0x20..=0x40 => match address {
+            0x26 | 0x00 => 0x95,
+            // Size of pairing data
+            0x27 | 0x01 => 0x22,
+            // Checksum
+            0x28 | 0x29 | 0x02 | 0x03 => 0x00,
+            // Host BT address (Big-endian)
+            0x2A..=0x2F => switch_host_address[(address - 0x2a) as usize],
+            0x04..=0x09 => switch_host_address[(address - 4) as usize],
+            // Bluetooth LTK (Little-endian) NOT IMPLEMENTED YET
+            0x30..=0x3F => 0x00,
+            0x0A..=0x19 => 0x00,
+            // Host capability 0x68 is Nintendo Switch. 0x08 is PC
+            0x4A | 0x24 => 0x68,
+            0x4B | 0x25 => 0,
+            _ => 0x00,
+        },
+        0x50 => 0x00,
+        0x60 => match address {
+            0x00..0x0f => 0xff,
+            0x12 => 0x03,
+            0x13 => 0x02,
+            0x1b => 0x01,
+            0x20 => 35,
+            0x21 => 0,
+            0x22 => 185,
+            0x23 => 255,
+            0x24 => 26,
+            0x25 => 1,
+            0x26 => 0,
+            0x27 => 64,
+            0x28 => 0,
+            0x29 => 64,
+            0x2A => 0,
+            0x2B => 64,
+            0x2C => 1,
+            0x2D => 0,
+            0x2E => 1,
+            0x2F => 0,
+            0x30 => 1,
+            0x31 => 0,
+            0x32 => 0x3B,
+            0x33 => 0x34,
+            0x34 => 0x3B,
+            0x35 => 0x34,
+            0x36 => 0x3B,
+            0x37 => 0x34,
+            0x3d..=0x45 => mk_switch_analog_calibration_data()[(address - 0x3d) as usize],
+            0x46..=0x4e => mk_switch_analog_calibration_data()[(address - 0x3d) as usize],
+            0x4F => 0xFF,
+            0x50 => 26,
+            0x51 => 26,
+            0x52 => 26,
+            0x53..=0x55 => 94,
+            0x56 => 255,
+            0x57 => 255,
+            0x58 => 255,
+            0x59..=0x5B => 255,
+            0x5C => 0x01,
+            0x80 => 80,
+            0x81 => 253,
+            0x82 => 0,
+            0x83 => 0,
+            0x84 => 198,
+            0x85 => 15,
+            0x98 | 0x86 => 15,
+            0x99 | 0x87 => 48,
+            0x9A | 0x88 => 97,
+            0x9B | 0x89 => 174,
+            0x9C | 0x8A => 144,
+            0x9D | 0x8B => 217,
+            0x9E | 0x8C => 212,
+            0x9F | 0x8D => 20,
+            0xA0 | 0x8E => 84,
+            0xA1 | 0x8F => 65,
+            0xA2 | 0x90 => 21,
+            0xA3 | 0x91 => 84,
+            0xA4 | 0x92 => 199,
+            0xA5 | 0x93 => 121,
+            0xA6 | 0x94 => 156,
+            0xA7 | 0x95 => 51,
+            0xA8 | 0x96 => 54,
+            0xA9 | 0x97 => 99,
+            _ => 0,
+        },
+        0x80 => match address {
+            0x10..=0x1a => 0xff,
+            0x1b..=0x25 => 0xff,
+            0x26..=0x3f => 0xff,
+            _ => 0xff,
+        },
+        _ => 0xff,
+    }
+}
+
+fn mk_switch_analog_calibration_data() -> [u8; 18] {
+    fn switch_analog_encode(in_lower: u16, in_upper: u16) -> [u8; 3] {
+        let mut out = [0u8; 3];
+
+        [out[0], out[1]] = in_lower.to_le_bytes();
+
+        out[1] |= ((in_upper & 0xf) << 4) as u8;
+        out[2] = ((in_upper & 0xff0) >> 4) as u8;
+
+        out
+    }
+
+    const MIN: u16 = 128 << 4;
+    const MAXX: u16 = 128 << 4;
+    const CENTER: u16 = 128 << 4;
+
+    let mut out = [0u8; 18];
+
+    out[0..3].copy_from_slice(&switch_analog_encode(MAXX, MAXX));
+    out[3..6].copy_from_slice(&switch_analog_encode(CENTER, CENTER));
+    out[6..9].copy_from_slice(&switch_analog_encode(MIN, MIN));
+
+    out[9..12].copy_from_slice(&switch_analog_encode(CENTER, CENTER));
+    out[12..15].copy_from_slice(&switch_analog_encode(MIN, MIN));
+    out[15..18].copy_from_slice(&switch_analog_encode(MAXX, MAXX));
+
+    info!("Returning switch data: {:x}", out);
+
+    out
 }
 
 #[derive(Clone, Copy, Debug, PartialEq, Eq, Default, PackedStruct, Format)]
@@ -321,7 +450,7 @@ pub struct ProconButtonsLeft {
 }
 
 #[derive(Clone, Copy, Debug, PartialEq, Eq, Default, PackedStruct, Format)]
-#[packed_struct(bit_numbering = "msb0", endian = "msb", size_bytes = "9")]
+#[packed_struct(bit_numbering = "msb0", endian = "lsb", size_bytes = "9")]
 pub struct ProconState {
     #[packed_field(bits = "0..=7")]
     pub buttons_right: ProconButtonsRight,
@@ -333,7 +462,7 @@ pub struct ProconState {
     pub lstick_x: u16,
     #[packed_field(bits = "40..=47")]
     pub lstick_y: u8,
-    #[packed_field(bits = "48..=60")]
+    #[packed_field(bits = "48..=63")]
     pub rstick_x: u16,
     #[packed_field(bits = "64..=71")]
     pub rstick_y: u8,
@@ -374,8 +503,8 @@ impl From<&GcState> for ProconState {
                 button_b: value.buttons_1.button_b,
                 button_x: value.buttons_1.button_x,
                 button_y: value.buttons_1.button_y,
-                trigger_r: value.buttons_2.button_r,
-                trigger_zr: value.buttons_2.button_z,
+                trigger_r: value.buttons_2.button_z,
+                trigger_zr: value.buttons_2.button_r,
                 ..Default::default()
             },
             buttons_shared: ProconButtonsShared {
@@ -383,9 +512,9 @@ impl From<&GcState> for ProconState {
                 button_home: value.buttons_2.button_start && value.buttons_2.button_z,
                 ..Default::default()
             },
-            lstick_x: value.stick_x as u16 * 257,
+            lstick_x: value.stick_x as u16 * 16,
             lstick_y: value.stick_y,
-            rstick_x: value.cstick_x as u16 * 257,
+            rstick_x: value.cstick_x as u16 * 16,
             rstick_y: value.cstick_y,
         }
     }
@@ -394,6 +523,7 @@ impl From<&GcState> for ProconState {
 pub struct ProconReportBuilder {
     switch_reporting_mode: u8,
     switch_mac_address: [u8; 6],
+    switch_host_address: [u8; 6],
     switch_ltk: [u8; 16],
 }
 
@@ -401,12 +531,26 @@ impl Default for ProconReportBuilder {
     fn default() -> Self {
         Self {
             switch_reporting_mode: 0,
-            switch_mac_address: [0u8; 6],
+            switch_mac_address: gen_switch_mac_address(),
+            switch_host_address: [0u8; 6],
             switch_ltk: gen_switch_ltk(),
         }
     }
 }
 
+fn gen_switch_mac_address() -> [u8; 6] {
+    let mut mac_addr = [0u8; 6];
+
+    mac_addr.iter_mut().for_each(|e| {
+        *e = RoscRng.next_u64() as u8;
+    });
+
+    mac_addr[0] &= 0xfe;
+    mac_addr[5] = 0x9b;
+
+    mac_addr
+}
+
 fn gen_switch_ltk() -> [u8; 16] {
     let mut switch_ltk = [0u8; 16];
 
@@ -495,6 +639,7 @@ impl ProconReportBuilder {
                     current_report_info.raw_data[12],
                     current_report_info.raw_data[11],
                     current_report_info.raw_data[15],
+                    &self.switch_host_address,
                 );
             }
             SW_CMD_SET_SHIPMODE => {
@@ -526,18 +671,12 @@ impl ProconReportBuilder {
 
         match pairing_phase {
             1 => {
-                self.switch_mac_address
+                self.switch_host_address
                     .iter_mut()
                     .enumerate()
                     .for_each(|(i, e)| *e = host_address[5 - i]);
 
-                self.switch_mac_address
-                    .iter()
-                    .rev()
-                    .enumerate()
-                    .for_each(|(i, e)| {
-                        report[16 + i] = *e;
-                    });
+                report[16..=21].copy_from_slice(&self.switch_mac_address);
 
                 report[22..(22 + PRO_CONTROLLER_STRING.len())]
                     .copy_from_slice(&PRO_CONTROLLER_STRING);
diff --git a/src/input.rs b/src/input.rs
index bba8efa..d30693d 100644
--- a/src/input.rs
+++ b/src/input.rs
@@ -551,7 +551,7 @@ pub async fn update_stick_states_task(
     spi_acs: Output<'static>,
     spi_ccs: Output<'static>,
 ) {
-    Timer::after_secs(1).await;
+    Timer::after_secs(3).await;
     *SPI_SHARED.lock().await = Some(spi);
     *SPI_ACS_SHARED.lock().await = Some(spi_acs);
     *SPI_CCS_SHARED.lock().await = Some(spi_ccs);