1 files changed, 72 insertions, 4 deletions

diff --git a/cores/arduino/wiring_digital.c b/cores/arduino/wiring_digital.c
index 9671045..95666b1 100755
--- a/cores/arduino/wiring_digital.c
+++ b/cores/arduino/wiring_digital.c
@@ -27,9 +27,28 @@
 #include "wiring_private.h"
 #include "pins_arduino.h"
 
-void pinMode_lookup(uint8_t pin, uint8_t val)
+void pinMode_lookup(uint8_t pin, uint8_t mode)
 {
-	pinMode_implementation(pin, val);
+	uint8_t bit = digitalPinToBitMask(pin);
+	uint8_t port = digitalPinToPort(pin);
+	volatile uint8_t *reg;
+
+	if (port == NOT_A_PIN) return;
+
+	// JWS: can I let the optimizer do this?
+	reg = portModeRegister(port);
+
+	if (mode == INPUT) { 
+		uint8_t oldSREG = SREG;
+                cli();
+		*reg &= ~bit;
+		SREG = oldSREG;
+	} else {
+		uint8_t oldSREG = SREG;
+                cli();
+		*reg |= bit;
+		SREG = oldSREG;
+	}
 }
 
 // Forcing this inline keeps the callers from having to push their own stuff
@@ -102,12 +121,61 @@ static void turnOffPWM(uint8_t timer)
 	}
 }
 
+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)
 {
-	digitalWrite_implementation(pin, val);
+	uint8_t timer = digitalPinToTimer(pin);
+	uint8_t bit = digitalPinToBitMask(pin);
+	uint8_t port = digitalPinToPort(pin);
+	volatile uint8_t *out;
+
+	if (port == NOT_A_PIN) return;
+
+	// If the pin that support PWM output, we need to turn it off
+	// before doing a digital write.
+	if (timer != NOT_ON_TIMER) turnOffPWM(timer);
+
+	out = portOutputRegister(port);
+
+	uint8_t oldSREG = SREG;
+	cli();
+
+	if (val == LOW) {
+		*out &= ~bit;
+	} else {
+		*out |= bit;
+	}
+
+	SREG = oldSREG;
 }
 
 int digitalRead_lookup(uint8_t pin)
 {
-	digitalRead_implementation(pin);
+	uint8_t timer = digitalPinToTimer(pin);
+	uint8_t bit = digitalPinToBitMask(pin);
+	uint8_t port = digitalPinToPort(pin);
+
+	if (port == NOT_A_PIN) return LOW;
+
+	// If the pin that support PWM output, we need to turn it off
+	// before getting a digital reading.
+	if (timer != NOT_ON_TIMER) turnOffPWM(timer);
+
+	if (*portInputRegister(port) & bit) return HIGH;
+	return LOW;
 }