Optimized digitalWrite(), etc. from Alvaro Lopez.

1 files changed, 24 insertions, 9 deletions

diff --git a/cores/arduino/wiring_digital.c b/cores/arduino/wiring_digital.c
index 0949da4..95666b1 100755
--- a/cores/arduino/wiring_digital.c
+++ b/cores/arduino/wiring_digital.c
@@ -27,7 +27,7 @@
 #include "wiring_private.h"
 #include "pins_arduino.h"
 
-void pinMode(uint8_t pin, uint8_t mode)
+void pinMode_lookup(uint8_t pin, uint8_t mode)
 {
 	uint8_t bit = digitalPinToBitMask(pin);
 	uint8_t port = digitalPinToPort(pin);
@@ -121,7 +121,23 @@ static void turnOffPWM(uint8_t timer)
 	}
 }
 
-void digitalWrite(uint8_t pin, uint8_t val)
+void __digitalWriteOR_locked(volatile uint8_t*out, uint8_t bit)
+{
+	uint8_t oldSREG = SREG;
+	cli();
+	*out |= bit;
+	SREG=oldSREG;
+}
+
+void __digitalWriteAND_locked(volatile uint8_t*out, uint8_t bit)
+{
+	uint8_t oldSREG = SREG;
+	cli();
+	*out &= bit; // NOTE - no inversion here, invert before calling!!!
+	SREG=oldSREG;
+}
+
+void digitalWrite_lookup(uint8_t pin, uint8_t val)
 {
 	uint8_t timer = digitalPinToTimer(pin);
 	uint8_t bit = digitalPinToBitMask(pin);
@@ -136,20 +152,19 @@ void digitalWrite(uint8_t pin, uint8_t val)
 
 	out = portOutputRegister(port);
 
+	uint8_t oldSREG = SREG;
+	cli();
+
 	if (val == LOW) {
-		uint8_t oldSREG = SREG;
-                cli();
 		*out &= ~bit;
-		SREG = oldSREG;
 	} else {
-		uint8_t oldSREG = SREG;
-                cli();
 		*out |= bit;
-		SREG = oldSREG;
 	}
+
+	SREG = oldSREG;
 }
 
-int digitalRead(uint8_t pin)
+int digitalRead_lookup(uint8_t pin)
 {
 	uint8_t timer = digitalPinToTimer(pin);
 	uint8_t bit = digitalPinToBitMask(pin);