3 files changed, 154 insertions, 116 deletions

diff --git a/cores/arduino/pins_arduino.h b/cores/arduino/pins_arduino.h
index 4bf6470..63f4257 100644
--- a/cores/arduino/pins_arduino.h
+++ b/cores/arduino/pins_arduino.h
@@ -339,8 +339,6 @@ INLINED uint8_t inlined_digitalPinToBitMask(uint8_t pin)
 	}
 }
 
-// XXX: this needs to return false (or -1) if the pin doesn't have a timer,
-// rather than throwing a compilation error.
 INLINED uint8_t inlined_digitalPinToTimer(uint8_t pin)
 {
 	switch(pin) {
@@ -455,8 +453,6 @@ INLINED uint8_t inlined_digitalPinToBitMask(uint8_t pin)
 	}
 }
 
-// XXX: this needs to return false (or -1) if the pin doesn't have a timer,
-// rather than throwing a compilation error.
 INLINED uint8_t inlined_digitalPinToTimer(uint8_t pin)
 {
 	switch(pin) {
@@ -481,4 +477,46 @@ INLINED uint8_t inlined_digitalPinToTimer(uint8_t pin)
 
 #define analogInPinToBit(P) (P)
 
+INLINED uint8_t digitalPinToPort(uint8_t pin) {
+	if (__builtin_constant_p(pin))
+		return inlined_digitalPinToPort(pin);
+	else
+		return pgm_read_byte( digital_pin_to_port_PGM + pin );
+}
+
+INLINED uint8_t digitalPinToBitMask(uint8_t pin) {
+	if (__builtin_constant_p(pin))
+		return inlined_digitalPinToBitMask(pin);
+	else
+		return pgm_read_byte( digital_pin_to_bit_mask_PGM + pin );
+}
+
+INLINED uint8_t digitalPinToTimer(uint8_t pin) {
+	if (__builtin_constant_p(pin))
+		return inlined_digitalPinToTimer(pin);
+	else
+		return pgm_read_byte( digital_pin_to_timer_PGM + pin );
+}
+
+INLINED volatile uint8_t *portOutputRegister(uint8_t index) {
+	if (__builtin_constant_p(index))
+		return inlined_portOutputRegister(index);
+	else
+		return (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + index ) );
+}
+
+INLINED volatile uint8_t* portInputRegister(uint8_t index) {
+	if (__builtin_constant_p(index))
+		return inlined_portInputRegister(index);
+	else
+		return (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + index) );
+}
+
+INLINED volatile uint8_t* portModeRegister(uint8_t index) {
+	if (__builtin_constant_p(index))
+		return inlined_portModeRegister(index);
+	else
+		return (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + index) );
+}
+
 #endif
diff --git a/cores/arduino/wiring.h b/cores/arduino/wiring.h
index 66ce2f7..433c87e 100755
--- a/cores/arduino/wiring.h
+++ b/cores/arduino/wiring.h
@@ -130,136 +130,68 @@ void detachInterrupt(uint8_t);
 void setup(void);
 void loop(void);
 
-INLINED uint8_t digitalPinToPort(uint8_t pin) {
-	if (__builtin_constant_p(pin))
-		return inlined_digitalPinToPort(pin);
-	else
-		return pgm_read_byte( digital_pin_to_port_PGM + pin );
-}
-
-INLINED uint8_t digitalPinToBitMask(uint8_t pin) {
-	if (__builtin_constant_p(pin))
-		return inlined_digitalPinToBitMask(pin);
-	else
-		return pgm_read_byte( digital_pin_to_bit_mask_PGM + pin );
-}
-
-INLINED uint8_t digitalPinToTimer(uint8_t pin) {
-	if (__builtin_constant_p(pin))
-		return inlined_digitalPinToTimer(pin);
-	else
-		return pgm_read_byte( digital_pin_to_timer_PGM + pin );
-}
-
-INLINED volatile uint8_t *portOutputRegister(uint8_t index) {
-	if (__builtin_constant_p(index))
-		return inlined_portOutputRegister(index);
-	else
-		return (volatile uint8_t *)( pgm_read_word( port_to_output_PGM + index ) );
-}
-
-INLINED volatile uint8_t* portInputRegister(uint8_t index) {
-	if (__builtin_constant_p(index))
-		return inlined_portInputRegister(index);
-	else
-		return (volatile uint8_t *)( pgm_read_word( port_to_input_PGM + index) );
-}
-
-INLINED volatile uint8_t* portModeRegister(uint8_t index) {
-	if (__builtin_constant_p(index))
-		return inlined_portModeRegister(index);
-	else
-		return (volatile uint8_t *)( pgm_read_word( port_to_mode_PGM + index) );
-}
-
 /*
  * Check if a given pin requires locking.
  * When accessing lower 32 IO ports we can use SBI/CBI instructions, which are atomic. However
  * other IO ports require load+modify+store and we need to make them atomic by disabling
  * interrupts.
  */
-INLINED int registerWriteNeedsLocking(volatile uint8_t *reg)
+INLINED int portWriteNeedsLocking(uint8_t pin)
 {
 	/* SBI/CBI instructions only work on lower 32 IO ports */
-	if (reg > (volatile uint8_t*)&_SFR_IO8(0x1F)) {
+	if (inlined_portOutputRegister(inlined_digitalPinToPort(pin)) > (volatile uint8_t*)&_SFR_IO8(0x1F)) {
 		return 1;
 	}
 	return 0;
 }
 
-#define digitalWrite_implementation(pin, value)\
-do {\
-	uint8_t oldSREG;\
-	uint8_t bit = digitalPinToBitMask(pin);\
-	uint8_t port = digitalPinToPort(pin);\
-	volatile uint8_t *reg = portOutputRegister(port);\
-\
-	if (!__builtin_constant_p(pin) || registerWriteNeedsLocking(reg)) {\
-		oldSREG = SREG;\
-		cli();\
-	}\
-\
-	if (value == LOW) {\
-		*reg &= ~bit;\
-	} else {\
-		*reg |= bit;\
-	}\
-\
-	if (!__builtin_constant_p(pin) || registerWriteNeedsLocking(reg)) {\
-		SREG = oldSREG;\
-	}\
-} while(0)
-	
+/*
+ * These functions will perform OR/AND on a given register, and are atomic.
+ */
+extern void __digitalWriteOR_locked(volatile uint8_t*out, uint8_t bit);
+extern void __digitalWriteAND_locked(volatile uint8_t*out, uint8_t bit);
+
 INLINED void digitalWrite(uint8_t pin, uint8_t value)
 {
-	if (__builtin_constant_p(pin)) digitalWrite_implementation(pin, value);
-	else digitalWrite_lookup(pin, value);
+	if (__builtin_constant_p(pin)) {
+		if (portWriteNeedsLocking(pin)) {
+			if (value==LOW) {
+				__digitalWriteAND_locked(inlined_portOutputRegister(inlined_digitalPinToPort(pin)),~inlined_digitalPinToBitMask(pin));
+			} else {
+				__digitalWriteOR_locked(inlined_portOutputRegister(inlined_digitalPinToPort(pin)),inlined_digitalPinToBitMask(pin));
+			}
+		} else {
+			if (value==LOW) {
+				*inlined_portOutputRegister(inlined_digitalPinToPort(pin)) &= ~(inlined_digitalPinToBitMask(pin));
+			} else {
+				*inlined_portOutputRegister(inlined_digitalPinToPort(pin)) |= inlined_digitalPinToBitMask(pin);
+			}
+		}
+	} else {
+		digitalWrite_lookup(pin,value);
+	}
 }
 
-#define pinMode_implementation(pin, value)\
-do {\
-	uint8_t bit = digitalPinToBitMask(pin);\
-	uint8_t oldSREG;\
-	uint8_t port = digitalPinToPort(pin);\
-	volatile uint8_t *reg = portModeRegister(port);\
-\
-	if (!__builtin_constant_p(pin) || registerWriteNeedsLocking(reg)) {\
-		oldSREG = SREG;\
-		cli();\
-	}\
-\
-	if (value == INPUT) { \
-		*reg &= ~bit;\
-	} else {\
-		*reg |= bit;\
-	}\
-\
-	if (!__builtin_constant_p(pin) || registerWriteNeedsLocking(reg)) {\
-		SREG = oldSREG;\
-	}\
-} while(0)
-
-INLINED void pinMode(uint8_t pin, uint8_t value)
+INLINED void pinMode(uint8_t pin, uint8_t mode)
 {
-	if (__builtin_constant_p(pin)) pinMode_implementation(pin, value);
-	else pinMode_lookup(pin, value);
+	if (__builtin_constant_p(pin)) {
+		if (mode==INPUT) {
+			*inlined_portModeRegister(inlined_digitalPinToPort(pin)) &= ~(inlined_digitalPinToBitMask(pin));
+		} else {
+			*inlined_portModeRegister(inlined_digitalPinToPort(pin)) |= inlined_digitalPinToBitMask(pin);
+		}
+	} else {
+		pinMode_lookup(pin,mode);
+	}
 }
 
-#define digitalRead_implementation(pin)\
-do {\
-	uint8_t bit = digitalPinToBitMask(pin);\
-	uint8_t port = digitalPinToPort(pin);\
-\
-	if (port == NOT_A_PIN) return LOW;\
-\
-	if (*portInputRegister(port) & bit) return HIGH;\
-	return LOW;\
-} while(0)
-
 INLINED int digitalRead(uint8_t pin)
 {
-	if (__builtin_constant_p(pin)) digitalRead_implementation(pin);
-	else return digitalRead_lookup(pin);
+	if (__builtin_constant_p(pin)) {
+		return !! *inlined_portInputRegister(inlined_digitalPinToPort(pin));
+	} else {
+		return digitalRead_lookup(pin);
+	}
 }
 
 
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;
 }