diff --git a/esphome/core/helpers.cpp b/esphome/core/helpers.cpp index 4ac9303b20..e1ff95f6a1 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 0d2a7e298a..3f5d960c14 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 };