From d83a7924e60c1827094ff276bbc43e0394a345ba Mon Sep 17 00:00:00 2001
From: Kajetan Rzepecki <kajetan.rzepecki+github@gmail.com>
Date: Thu, 18 Apr 2019 20:15:35 +0200
Subject: [PATCH] Fix ESP32 TOGGLE() with I2S expander (#13743)

---
 Marlin/src/HAL/HAL_ESP32/fastio_ESP32.h | 2 +-
 Marlin/src/HAL/HAL_ESP32/i2s.cpp        | 4 ++++
 Marlin/src/HAL/HAL_ESP32/i2s.h          | 2 ++
 3 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/Marlin/src/HAL/HAL_ESP32/fastio_ESP32.h b/Marlin/src/HAL/HAL_ESP32/fastio_ESP32.h
index dc2cd708d..0f1bc8024 100644
--- a/Marlin/src/HAL/HAL_ESP32/fastio_ESP32.h
+++ b/Marlin/src/HAL/HAL_ESP32/fastio_ESP32.h
@@ -37,7 +37,7 @@
 #define _PULLUP(IO, v)          pinMode(IO, v ? INPUT_PULLUP : INPUT)
 
 // Read a pin wrapper
-#define READ(IO)                digitalRead(IO)
+#define READ(IO)                (TEST(IO, 7) ? i2s_state(IO & 0x7F) : digitalRead(IO))
 
 // Write to a pin wrapper
 #define WRITE(IO, v)            (TEST(IO, 7) ? i2s_write(IO & 0x7F, v) : digitalWrite(IO, v))
diff --git a/Marlin/src/HAL/HAL_ESP32/i2s.cpp b/Marlin/src/HAL/HAL_ESP32/i2s.cpp
index 42e65eacd..7aa640ea1 100644
--- a/Marlin/src/HAL/HAL_ESP32/i2s.cpp
+++ b/Marlin/src/HAL/HAL_ESP32/i2s.cpp
@@ -315,6 +315,10 @@ void i2s_write(uint8_t pin, uint8_t val) {
   SET_BIT_TO(i2s_port_data, pin, val);
 }
 
+uint8_t i2s_state(uint8_t pin) {
+  return TEST(i2s_port_data, pin);
+}
+
 void i2s_push_sample() {
   dma.current[dma.rw_pos++] = i2s_port_data;
 }
diff --git a/Marlin/src/HAL/HAL_ESP32/i2s.h b/Marlin/src/HAL/HAL_ESP32/i2s.h
index 337d91b20..fbe7b2419 100644
--- a/Marlin/src/HAL/HAL_ESP32/i2s.h
+++ b/Marlin/src/HAL/HAL_ESP32/i2s.h
@@ -26,6 +26,8 @@ extern uint32_t i2s_port_data;
 
 int i2s_init();
 
+uint8_t i2s_state(uint8_t pin);
+
 void i2s_write(uint8_t pin, uint8_t val);
 
 void i2s_push_sample();