From be6b1bd8debe818432e960231f7f6d424985bcee Mon Sep 17 00:00:00 2001
From: Jacques Gagnon <darthcloud@gmail.com>
Date: Sun, 15 Oct 2023 02:19:32 -0400
Subject: [PATCH] [PS] Restore turbo and fix pressure mode

---
 main/adapter/wired/ps.c |  9 +++++----
 main/wired/ps_spi.c     | 21 ++++++++++++++++++++-
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/main/adapter/wired/ps.c b/main/adapter/wired/ps.c
index 5b4c7894..3172e4ee 100644
--- a/main/adapter/wired/ps.c
+++ b/main/adapter/wired/ps.c
@@ -92,7 +92,7 @@ static DRAM_ATTR const uint32_t ps_btns_mask[32] = {
     BIT(PS_L2), BIT(PS_L1), 0, BIT(PS_L3),
     BIT(PS_R2), BIT(PS_R1), 0, BIT(PS_R3),
 };
-static const uint8_t ps_btns_idx[32] = {
+static DRAM_ATTR const uint8_t ps_btns_idx[32] = {
     0, 0, 0, 0,
     0, 0, 0, 0,
     5, 4, 7, 6,
@@ -180,9 +180,10 @@ void IRAM_ATTR ps_init_buffer(int32_t dev_mode, struct wired_data *wired_data) {
             memset((void *)map, 0, sizeof(*map));
             map->buttons = 0xFFFF;
             map->analog_btn = 0x00;
-            for (uint32_t i = 0; i < ADAPTER_MAX_AXES; i++) {
+            for (uint32_t i = 0; i < 4; i++) {
                 map->axes[ps_axes_idx[i]] = ps_axes_meta[i].neutral;
             }
+            memset(map->pressure, 0x00, sizeof(map->pressure));
 
             map_mask->buttons = 0x0000;
             map_mask->sticks = 0x00000000;
@@ -252,7 +253,7 @@ static void ps_ctrl_from_generic(struct wired_ctrl *ctrl_data, struct wired_data
         }
     }
 
-    for (uint32_t i = 0; i < ADAPTER_MAX_AXES; i++) {
+    for (uint32_t i = 0; i < 4; i++) {
         if (ctrl_data->map_mask[0] & (axis_to_btn_mask(i) & ps_desc[0])) {
             if (ctrl_data->axes[i].value > ctrl_data->axes[i].meta->size_max) {
                 map_tmp.axes[ps_axes_idx[i]] = 255;
@@ -393,5 +394,5 @@ void IRAM_ATTR ps_gen_turbo_mask(struct wired_data *wired_data) {
         }
     }
 
-    wired_gen_turbo_mask_axes8(wired_data, map_mask->axes, ADAPTER_MAX_AXES, ps_axes_idx, ps_axes_meta);
+    wired_gen_turbo_mask_axes8(wired_data, map_mask->axes, 4, ps_axes_idx, ps_axes_meta);
 }
diff --git a/main/wired/ps_spi.c b/main/wired/ps_spi.c
index 5b1d071a..4344eb16 100644
--- a/main/wired/ps_spi.c
+++ b/main/wired/ps_spi.c
@@ -470,7 +470,26 @@ static void ps_cmd_rsp_hdlr(struct ps_ctrl_port *port, uint8_t id, uint8_t cmd,
                     load_mouse_axes(id + port->mt_first_port, &rsp[2]);
                     break;
                 default:
-                    memcpy(rsp, wired_adapter.data[id + port->mt_first_port].output, size);
+                    *(uint16_t *)&rsp[0] = wired_adapter.data[id + port->mt_first_port].output16[0]
+                        | wired_adapter.data[id + port->mt_first_port].output_mask16[0];
+                    if (size >  2) {
+                        for (uint32_t i = 2; i < 6; ++i) {
+                            rsp[i] = (wired_adapter.data[id + port->mt_first_port].output_mask[i]) ?
+                                wired_adapter.data[id + port->mt_first_port].output_mask[i]
+                                : wired_adapter.data[id + port->mt_first_port].output[i];
+                        }
+                    }
+                    if (size > 6) {
+                        *(uint16_t *)&rsp[6] = wired_adapter.data[id + port->mt_first_port].output16[3]
+                            & wired_adapter.data[id + port->mt_first_port].output_mask16[3];
+                        *(uint32_t *)&rsp[8] = wired_adapter.data[id + port->mt_first_port].output32[2]
+                            & wired_adapter.data[id + port->mt_first_port].output_mask32[2];
+                        *(uint32_t *)&rsp[12] = wired_adapter.data[id + port->mt_first_port].output32[3]
+                            & wired_adapter.data[id + port->mt_first_port].output_mask32[3];
+                        *(uint16_t *)&rsp[16] = wired_adapter.data[id + port->mt_first_port].output16[8]
+                            & wired_adapter.data[id + port->mt_first_port].output_mask16[8];
+                    }
+                    ++wired_adapter.data[id + port->mt_first_port].frame_cnt;
                     break;
             }
             if (cmd != 0x42 && cmd != 0x43) {