From b6e765daaa87798fe98614ab6a512b6aa038d982 Mon Sep 17 00:00:00 2001
From: Jesse Hills <3060199+jesserockz@users.noreply.github.com>
Date: Tue, 21 Mar 2023 14:13:25 +1300
Subject: [PATCH] InterruptLock per core for rp2040

---
 esphome/core/helpers.cpp | 19 +++++++++++++++++--
 esphome/core/helpers.h   |  6 +++++-
 2 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/esphome/core/helpers.cpp b/esphome/core/helpers.cpp
index 4ac9303b2..e1ff95f6a 100644
--- a/esphome/core/helpers.cpp
+++ b/esphome/core/helpers.cpp
@@ -415,8 +415,23 @@ IRAM_ATTR InterruptLock::~InterruptLock() { xt_wsr_ps(state_); }
 IRAM_ATTR InterruptLock::InterruptLock() { portDISABLE_INTERRUPTS(); }
 IRAM_ATTR InterruptLock::~InterruptLock() { portENABLE_INTERRUPTS(); }
 #elif defined(USE_RP2040)
-IRAM_ATTR InterruptLock::InterruptLock() { state_ = save_and_disable_interrupts(); }
-IRAM_ATTR InterruptLock::~InterruptLock() { restore_interrupts(state_); }
+IRAM_ATTR InterruptLock::InterruptLock() {
+  auto core = get_core_num();
+  if (this->stack_top_[core] == STACK_SIZE) {
+    panic("InterruptLock stack overflow");
+  }
+  uint8_t stack_top = this->stack_top_[core]++;
+  state_stack_[core][stack_top] = save_and_disable_interrupts();
+}
+IRAM_ATTR InterruptLock::~InterruptLock() {
+  auto core = get_core_num();
+  if (!this->stack_top_[core]) {
+    return;  // No state to restore
+  }
+  uint8_t stack_top = --this->stack_top_[core];
+  uint32_t state = this->state_stack_[core][stack_top];
+  restore_interrupts(state);
+}
 #endif
 
 uint8_t HighFrequencyLoopRequester::num_requests = 0;  // NOLINT(cppcoreguidelines-avoid-non-const-global-variables)
diff --git a/esphome/core/helpers.h b/esphome/core/helpers.h
index 0d2a7e298..3f5d960c1 100644
--- a/esphome/core/helpers.h
+++ b/esphome/core/helpers.h
@@ -583,8 +583,12 @@ class InterruptLock {
   ~InterruptLock();
 
  protected:
-#if defined(USE_ESP8266) || defined(USE_RP2040)
+#if defined(USE_ESP8266)
   uint32_t state_;
+#elif defined(USE_RP2040)
+  static const uint8_t STACK_SIZE = 15;
+  uint8_t stack_top_[2] = {0, 0};
+  uint32_t state_stack_[2][STACK_SIZE];
 #endif
 };