From e6cabd3b0f229051a859dab2a481c50fae9ddf09 Mon Sep 17 00:00:00 2001
From: Marc Riera <marc.riera.irigoyen@gmail.com>
Date: Fri, 14 Apr 2023 00:22:20 +0200
Subject: [PATCH] Add missing MTC modes

---
 src/controller/emulated.rs                   | 20 ++++++
 src/controller/emulated/sotp031201_p4b2b7.rs | 69 ++++++++++++++++++++
 src/controller/emulated/sotp031201_p4b7.rs   |  4 +-
 src/controller/emulated/sotp031201_p5b7.rs   | 69 ++++++++++++++++++++
 4 files changed, 160 insertions(+), 2 deletions(-)
 create mode 100644 src/controller/emulated/sotp031201_p4b2b7.rs
 create mode 100644 src/controller/emulated/sotp031201_p5b7.rs

diff --git a/src/controller/emulated.rs b/src/controller/emulated.rs
index 33579f4..7f05c89 100644
--- a/src/controller/emulated.rs
+++ b/src/controller/emulated.rs
@@ -10,7 +10,9 @@ mod dgoc44u;
 mod tcpp20009;
 mod tcpp20011;
 mod sotp031201_p4b7;
+mod sotp031201_p4b2b7;
 mod sotp031201_p5b5;
+mod sotp031201_p5b7;
 mod vok00106;
 
 const FFS_MOUNT: &str = "/tmp/ffs";
@@ -24,7 +26,9 @@ pub enum ControllerModel {
     TCPP20009,
     TCPP20011,
     SOTP031201P4B7,
+    SOTP031201P4B2B7,
     SOTP031201P5B5,
+    SOTP031201P5B7,
     VOK00106,
 }
 
@@ -59,11 +63,21 @@ pub fn set_model(state: &ControllerState) -> Option<ControllerModel> {
         init_gadget(&sotp031201_p4b7::DEVICE_DESCRIPTOR, &sotp031201_p4b7::DESCRIPTORS, &sotp031201_p4b7::STRINGS);
         return Some(ControllerModel::SOTP031201P4B7);
     }
+    else if state.button_c && state.power == 1 {
+        println!("Selected controller SOTP-031201 (P4/B2-B7 mode).");
+        init_gadget(&sotp031201_p4b2b7::DEVICE_DESCRIPTOR, &sotp031201_p4b2b7::DESCRIPTORS, &sotp031201_p4b2b7::STRINGS);
+        return Some(ControllerModel::SOTP031201P4B2B7);
+    }
     else if state.button_c && state.power == 2 {
         println!("Selected controller SOTP-031201 (P5/B5 mode).");
         init_gadget(&sotp031201_p5b5::DEVICE_DESCRIPTOR, &sotp031201_p5b5::DESCRIPTORS, &sotp031201_p5b5::STRINGS);
         return Some(ControllerModel::SOTP031201P5B5);
     }
+    else if state.button_c && state.power == 3 {
+        println!("Selected controller SOTP-031201 (P5/B7 mode).");
+        init_gadget(&sotp031201_p5b7::DEVICE_DESCRIPTOR, &sotp031201_p5b7::DESCRIPTORS, &sotp031201_p5b7::STRINGS);
+        return Some(ControllerModel::SOTP031201P5B7);
+    }
 /*     else if state.button_a {
         println!("Selected controller VOK-00106.");
         return Some(ControllerModel::VOK00106);
@@ -88,9 +102,15 @@ pub fn set_state(state: &mut ControllerState, model: &ControllerModel) {
         ControllerModel::SOTP031201P4B7 => {
             sotp031201_p4b7::update_gadget(state);
         }
+        ControllerModel::SOTP031201P4B2B7 => {
+            sotp031201_p4b2b7::update_gadget(state);
+        }
         ControllerModel::SOTP031201P5B5 => {
             sotp031201_p5b5::update_gadget(state);
         }
+        ControllerModel::SOTP031201P5B7 => {
+            sotp031201_p5b7::update_gadget(state);
+        }
         ControllerModel::VOK00106 => {
             vok00106::update_gadget(state);
         }
diff --git a/src/controller/emulated/sotp031201_p4b2b7.rs b/src/controller/emulated/sotp031201_p4b2b7.rs
new file mode 100644
index 0000000..25ce65a
--- /dev/null
+++ b/src/controller/emulated/sotp031201_p4b2b7.rs
@@ -0,0 +1,69 @@
+use std::fs::File;
+use std::io::Write;
+
+use bitflags::bitflags;
+use crate::controller::physical::ControllerState;
+use crate::controller::emulated::{DeviceDescriptor, ENDPOINT1};
+
+pub const DESCRIPTORS: [u8; 48] = [0x01, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00,
+0x09, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+0x07, 0x05, 0x81, 0x03, 0x08, 0x00, 0x14,
+0x09, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+0x07, 0x05, 0x81, 0x03, 0x08, 0x00, 0x14];
+pub const STRINGS: [u8; 16] = [0x02, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00];
+
+pub const DEVICE_DESCRIPTOR: DeviceDescriptor = DeviceDescriptor{b_device_class: 0x00, b_device_sub_class: 0x0, id_vendor: 0x0AE4, id_product: 0x0101, i_manufacturer: "TAITO", i_product: "Densha de Go! Plug & Play (MTC P4/B2-B7 mode)", i_serial_number: "SOTP-031201"};
+
+const POWER_NOTCHES: [u8; 6] = [0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0C];
+const BRAKE_NOTCHES: [u8; 10] = [0x08, 0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x02, 0x01];
+
+bitflags! {
+        struct Buttons1: u8 {
+            const NONE = 0;
+            const S = 1;
+            const D = 2;
+            const A = 4;
+            const A2 = 8;
+            const B = 16;
+            const C = 32;
+        }
+        struct Buttons2: u8 {
+            const NONE = 0;
+            const START = 1;
+            const SELECT = 2;
+            const UP = 4;
+            const DOWN = 8;
+            const LEFT = 16;
+            const RIGHT = 32;
+        }
+    }
+
+pub fn update_gadget(state: &mut ControllerState) {
+    // Calculate data for handles
+    let mut handle = POWER_NOTCHES[state.power as usize];
+    if state.brake > 0 {
+        handle = BRAKE_NOTCHES[state.brake as usize];
+    }
+
+    // Calculate data for buttons 1
+    let mut buttons1 = Buttons1::NONE;
+    if state.button_a { buttons1.insert(Buttons1::A) }
+    if state.button_b { buttons1.insert(Buttons1::B) }
+    if state.button_c { buttons1.insert(Buttons1::C) }
+    if state.button_d { buttons1.insert(Buttons1::D) }
+
+    // Calculate data for buttons 2
+    let mut buttons2 = Buttons2::NONE;
+    if state.button_up { buttons2.insert(Buttons2::UP) }
+    if state.button_down { buttons2.insert(Buttons2::DOWN) }
+    if state.button_left { buttons2.insert(Buttons2::LEFT) }
+    if state.button_right { buttons2.insert(Buttons2::RIGHT) }
+    if state.button_start { buttons2.insert(Buttons2::START) }
+    if state.button_select { buttons2.insert(Buttons2::SELECT) }
+
+    // Assemble data and send it to endpoint
+    let data = [0x1, handle, buttons1.bits, buttons2.bits];
+    if let Ok(mut file) = File::create(ENDPOINT1) {
+        file.write(&data).ok();
+    }
+}
\ No newline at end of file
diff --git a/src/controller/emulated/sotp031201_p4b7.rs b/src/controller/emulated/sotp031201_p4b7.rs
index e1f0c47..2838817 100644
--- a/src/controller/emulated/sotp031201_p4b7.rs
+++ b/src/controller/emulated/sotp031201_p4b7.rs
@@ -12,9 +12,9 @@ pub const DESCRIPTORS: [u8; 48] = [0x01, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x0
 0x07, 0x05, 0x81, 0x03, 0x08, 0x00, 0x14];
 pub const STRINGS: [u8; 16] = [0x02, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00];
 
-pub const DEVICE_DESCRIPTOR: DeviceDescriptor = DeviceDescriptor{b_device_class: 0x00, b_device_sub_class: 0x0, id_vendor: 0x0AE4, id_product: 0x0101, i_manufacturer: "TAITO", i_product: "Densha de Go! Plug & Play (MTC P5/B7 mode)", i_serial_number: "SOTP-031201"};
+pub const DEVICE_DESCRIPTOR: DeviceDescriptor = DeviceDescriptor{b_device_class: 0x00, b_device_sub_class: 0x0, id_vendor: 0x0AE4, id_product: 0x0101, i_manufacturer: "TAITO", i_product: "Densha de Go! Plug & Play (MTC P4/B7 mode)", i_serial_number: "SOTP-031201"};
 
-const POWER_NOTCHES: [u8; 6] = [0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E];
+const POWER_NOTCHES: [u8; 6] = [0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0D];
 const BRAKE_NOTCHES: [u8; 10] = [0x09, 0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x02, 0x01];
 
 bitflags! {
diff --git a/src/controller/emulated/sotp031201_p5b7.rs b/src/controller/emulated/sotp031201_p5b7.rs
new file mode 100644
index 0000000..e1f0c47
--- /dev/null
+++ b/src/controller/emulated/sotp031201_p5b7.rs
@@ -0,0 +1,69 @@
+use std::fs::File;
+use std::io::Write;
+
+use bitflags::bitflags;
+use crate::controller::physical::ControllerState;
+use crate::controller::emulated::{DeviceDescriptor, ENDPOINT1};
+
+pub const DESCRIPTORS: [u8; 48] = [0x01, 0x00, 0x00, 0x00, 0x30, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00,
+0x09, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+0x07, 0x05, 0x81, 0x03, 0x08, 0x00, 0x14,
+0x09, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00,
+0x07, 0x05, 0x81, 0x03, 0x08, 0x00, 0x14];
+pub const STRINGS: [u8; 16] = [0x02, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00];
+
+pub const DEVICE_DESCRIPTOR: DeviceDescriptor = DeviceDescriptor{b_device_class: 0x00, b_device_sub_class: 0x0, id_vendor: 0x0AE4, id_product: 0x0101, i_manufacturer: "TAITO", i_product: "Densha de Go! Plug & Play (MTC P5/B7 mode)", i_serial_number: "SOTP-031201"};
+
+const POWER_NOTCHES: [u8; 6] = [0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E];
+const BRAKE_NOTCHES: [u8; 10] = [0x09, 0x08, 0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x02, 0x01];
+
+bitflags! {
+        struct Buttons1: u8 {
+            const NONE = 0;
+            const S = 1;
+            const D = 2;
+            const A = 4;
+            const A2 = 8;
+            const B = 16;
+            const C = 32;
+        }
+        struct Buttons2: u8 {
+            const NONE = 0;
+            const START = 1;
+            const SELECT = 2;
+            const UP = 4;
+            const DOWN = 8;
+            const LEFT = 16;
+            const RIGHT = 32;
+        }
+    }
+
+pub fn update_gadget(state: &mut ControllerState) {
+    // Calculate data for handles
+    let mut handle = POWER_NOTCHES[state.power as usize];
+    if state.brake > 0 {
+        handle = BRAKE_NOTCHES[state.brake as usize];
+    }
+
+    // Calculate data for buttons 1
+    let mut buttons1 = Buttons1::NONE;
+    if state.button_a { buttons1.insert(Buttons1::A) }
+    if state.button_b { buttons1.insert(Buttons1::B) }
+    if state.button_c { buttons1.insert(Buttons1::C) }
+    if state.button_d { buttons1.insert(Buttons1::D) }
+
+    // Calculate data for buttons 2
+    let mut buttons2 = Buttons2::NONE;
+    if state.button_up { buttons2.insert(Buttons2::UP) }
+    if state.button_down { buttons2.insert(Buttons2::DOWN) }
+    if state.button_left { buttons2.insert(Buttons2::LEFT) }
+    if state.button_right { buttons2.insert(Buttons2::RIGHT) }
+    if state.button_start { buttons2.insert(Buttons2::START) }
+    if state.button_select { buttons2.insert(Buttons2::SELECT) }
+
+    // Assemble data and send it to endpoint
+    let data = [0x1, handle, buttons1.bits, buttons2.bits];
+    if let Ok(mut file) = File::create(ENDPOINT1) {
+        file.write(&data).ok();
+    }
+}
\ No newline at end of file