From b01a819644b5c5483d7d0be230b765a784e4a363 Mon Sep 17 00:00:00 2001
From: Yuri D'Elia <wavexx@thregr.org>
Date: Sun, 11 Jul 2021 20:25:52 +0200
Subject: [PATCH] Make Read/Write/Toggle pin functional for testing

Make ReadPin return the last value set by WritePin for proper testing.

Add a slow-path to TogglePin that goes through a read-write cycle. This
is useful both for testing and for platforms that don't have an
efficient toggle like AVR.
---
 src/hal/gpio.h | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/hal/gpio.h b/src/hal/gpio.h
index 3d45e88..df8da66 100644
--- a/src/hal/gpio.h
+++ b/src/hal/gpio.h
@@ -57,11 +57,21 @@ __attribute__((always_inline)) inline void WritePin(const GPIO_pin portPin, Leve
 }
 
 __attribute__((always_inline)) inline Level ReadPin(const GPIO_pin portPin) {
+#ifdef __AVR__
     return (Level)((portPin.port->PINx & (1 << portPin.pin)) != 0);
+#else
+    // Return the value modified by WritePin
+    return (Level)((portPin.port->PORTx & (1 << portPin.pin)) != 0);
+#endif
 }
 
 __attribute__((always_inline)) inline void TogglePin(const GPIO_pin portPin) {
+#ifdef __AVR__
+    // Optimized path for AVR, resulting in a pin toggle
     portPin.port->PINx |= (1 << portPin.pin);
+#else
+    WritePin(portPin, (Level)(ReadPin(portPin) != Level::high));
+#endif
 }
 
 __attribute__((always_inline)) inline void Init(const GPIO_pin portPin, GPIO_InitTypeDef GPIO_Init) {