From 43bb4408af3da54940807ff87ba8a233e90c1c8d Mon Sep 17 00:00:00 2001
From: wiredopposite <jackxhorizon@gmail.com>
Date: Mon, 20 Jan 2025 11:50:17 -0700
Subject: [PATCH] fix hardware timers interrupting flash writes

---
 Firmware/RP2040/src/Board/board_api.cpp     |  6 +-
 Firmware/RP2040/src/TaskQueue/TaskQueue.cpp | 63 +++++++++++++++++++++
 Firmware/RP2040/src/TaskQueue/TaskQueue.h   | 24 ++++++++
 README.md                                   |  1 +
 4 files changed, 92 insertions(+), 2 deletions(-)

diff --git a/Firmware/RP2040/src/Board/board_api.cpp b/Firmware/RP2040/src/Board/board_api.cpp
index b711126..444c27d 100644
--- a/Firmware/RP2040/src/Board/board_api.cpp
+++ b/Firmware/RP2040/src/Board/board_api.cpp
@@ -9,6 +9,7 @@
 #include "Board/board_api.h"
 #include "Board/ogxm_log.h"
 #include "Board/board_api_private/board_api_private.h"
+#include "TaskQueue/TaskQueue.h"
 
 namespace board_api {
 
@@ -60,10 +61,11 @@ void usb::disconnect_all()
 {
     OGXM_LOG("Disconnecting USB and resetting Core1\n");
 
+    TaskQueue::suspend_delayed_tasks();
     multicore_reset_core1();
-    sleep_ms(300);
+    sleep_ms(500);
     tud_disconnect();
-    sleep_ms(300);
+    sleep_ms(500);
 }
 
 // If using PicoW, only use this method from the core running btstack and after you've called init_bluetooth
diff --git a/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp b/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
index 51c7257..ed2af89 100644
--- a/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
+++ b/Firmware/RP2040/src/TaskQueue/TaskQueue.cpp
@@ -157,6 +157,11 @@ void TaskQueue::timer_irq_handler()
 
     uint64_t now = get_time_64_us();
     uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    if (suspended_)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
 
     for (auto& task : task_queue_delayed_) 
     {
@@ -185,5 +190,63 @@ void TaskQueue::timer_irq_handler()
         timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
     }
 
+    spin_unlock(spinlock_delayed_, irq_state);
+}
+
+void TaskQueue::suspend_delayed_tasks()
+{
+    get_core0().suspend_delayed();
+#if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W)
+    get_core1().suspend_delayed();
+#endif
+}
+
+void TaskQueue::resume_delayed_tasks()
+{
+    get_core0().resume_delayed();
+#if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W)
+    get_core1().resume_delayed();
+#endif
+}
+
+void TaskQueue::suspend_delayed()
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    if (suspended_)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
+    hw_clear_bits(&timer_hw->intr, 1u << alarm_num_);
+    suspended_time_ = get_time_64_us();
+    suspended_ = true;
+    spin_unlock(spinlock_delayed_, irq_state);
+}
+
+void TaskQueue::resume_delayed()
+{
+    uint32_t irq_state = spin_lock_blocking(spinlock_delayed_);
+    if (!suspended_)
+    {
+        spin_unlock(spinlock_delayed_, irq_state);
+        return;
+    }
+
+    uint64_t now = get_time_64_us();
+    uint64_t elapsed_time = now - suspended_time_;
+
+    for (auto& task : task_queue_delayed_) 
+    {
+        if (task.function) 
+        {
+            task.target_time = std::max(task.target_time + elapsed_time, now + 10);
+        }
+    }
+    int64_t next_target_time = get_next_target_time_unsafe(task_queue_delayed_);
+    if (next_target_time >= 0) 
+    {
+        timer_hw->alarm[alarm_num_] = static_cast<uint32_t>(next_target_time);
+    }
+    suspended_ = false;
     spin_unlock(spinlock_delayed_, irq_state);
 }
\ No newline at end of file
diff --git a/Firmware/RP2040/src/TaskQueue/TaskQueue.h b/Firmware/RP2040/src/TaskQueue/TaskQueue.h
index 920111e..d094b09 100644
--- a/Firmware/RP2040/src/TaskQueue/TaskQueue.h
+++ b/Firmware/RP2040/src/TaskQueue/TaskQueue.h
@@ -36,6 +36,14 @@ public:
         {
             get_core0().process_tasks();
         }
+        static inline void suspend_delayed_tasks()
+        {
+            get_core0().suspend_delayed();
+        }
+        static inline void resume_delayed_tasks()
+        {
+            get_core0().resume_delayed();
+        }
     };
 
 #if (OGXM_BOARD != PI_PICOW) && (OGXM_BOARD != PI_PICO2W) //BTstack uses core1
@@ -61,9 +69,20 @@ public:
         {
             get_core1().process_tasks();
         }
+        static inline void suspend_delayed_tasks()
+        {
+            get_core1().suspend_delayed();
+        }
+        static inline void resume_delayed_tasks()
+        {
+            get_core1().resume_delayed();
+        }
     }; // Core1
 #endif // OGXM_BOARD != PI_PICOW
 
+    static void suspend_delayed_tasks();
+    static void resume_delayed_tasks();
+
 private:
     enum class CoreNum : uint8_t
     {
@@ -99,6 +118,9 @@ private:
     uint32_t alarm_num_;
     uint32_t new_task_id_ = 1;
 
+    bool suspended_ = false;
+    uint64_t suspended_time_ = 0;
+
     int spinlock_queue_num_ = spin_lock_claim_unused(true);
     int spinlock_delayed_num_ = spin_lock_claim_unused(true);
     spin_lock_t* spinlock_queue_ = spin_lock_instance(static_cast<uint>(spinlock_queue_num_));
@@ -124,6 +146,8 @@ private:
     bool queue_task(const std::function<void()>& function);
     void process_tasks();
 
+    void suspend_delayed();
+    void resume_delayed();
     void timer_irq_handler();
     static uint64_t get_time_64_us();
 
diff --git a/README.md b/README.md
index 79aaa49..0e9257e 100644
--- a/README.md
+++ b/README.md
@@ -95,6 +95,7 @@ Please visit [**this page**](https://bluepad32.readthedocs.io/en/latest/supporte
 - OG Xbox communicator support (in some form)
 - Generic bluetooth dongle support
 - Button macros
+- Rumble settings (intensity, enabled/disable, etc.)
 
 ## Hardware
 For Pi Pico, RP2040-Zero, 4 channel, and ESP32 configurations, please see the hardware folder for diagrams.