From a9971968c0ea3c25989bf590ca6a9eaaa017aa09 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Fri, 10 Feb 2023 22:57:15 +0000
Subject: [PATCH] Update I2CManager_NonBlocking.h

Reduce platform-specific part of ATOMIC_BLOCK definition to one inline function _getInterruptState()
---
 I2CManager_NonBlocking.h | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/I2CManager_NonBlocking.h b/I2CManager_NonBlocking.h
index 2511a8c..991b45c 100644
--- a/I2CManager_NonBlocking.h
+++ b/I2CManager_NonBlocking.h
@@ -40,22 +40,23 @@ static inline uint8_t _deferInterrupts(void) {
   noInterrupts();
   return 1;
 }
-static inline void _enableInterruptsIf(uint8_t wasEnabled) {
-  if (wasEnabled) interrupts();
+static inline void _conditionalEnableInterrupts(bool *wasEnabled) {
+  if (*wasEnabled) interrupts();
 }
+#define ATOMIC_BLOCK(x) \
+for (bool _int_saved __attribute__((__cleanup__(_conditionalEnableInterrupts))) \
+            =_getInterruptState(),_ToDo=_deferInterrupts(); _ToDo; _ToDo=0)
+
 #if defined(__AVR__) // Nano, Uno, Mega2580, NanoEvery, etc.
-  #define ATOMIC_BLOCK(x) \
-  for (bool _int_saved=(SREG & (1<<SREG_I)),_ToDo=_deferInterrupts(); \
-      _ToDo; _ToDo=0, _enableInterruptsIf(_int_saved))
+  static inline bool _getInterruptState(void) {
+    return bitRead(SREG, SREG_I);  // true if enabled, false if disabled
+  }
 #elif defined(__arm__)  // STM32, SAMD, Teensy
-  static __inline__ bool _getInterruptState( void ) {
+  static inline bool _getInterruptState( void ) {
     uint32_t reg;
     __asm__ __volatile__ ("MRS %0, primask" : "=r" (reg) );
     return !(reg & 1);  // true if interrupts enabled, false otherwise
   }
-  #define ATOMIC_BLOCK(x) \
-  for (bool _int_saved=_getInterruptState(),_ToDo=_deferInterrupts(); \
-      _ToDo; _ToDo=0, _enableInterruptsIf(_int_saved))
 #else
   #warning "ATOMIC_BLOCK() not defined for this target type, I2C interrupts disabled"
   #define ATOMIC_BLOCK(x) // expand to nothing.