diff --git a/cores/esp8266/core_esp8266_wiring_digital.c b/cores/esp8266/core_esp8266_wiring_digital.c
index 2df6952c6..5be065af5 100644
--- a/cores/esp8266/core_esp8266_wiring_digital.c
+++ b/cores/esp8266/core_esp8266_wiring_digital.c
@@ -77,18 +77,12 @@ extern void __pinMode(uint8_t pin, uint8_t mode) {
 }
 
 extern void ICACHE_RAM_ATTR __digitalWrite(uint8_t pin, uint8_t val) {
-  if (pin == 16) {
-    if(val == LOW) {
-	  GP16O &= ~1;
-	} else {
-	  GP16O |= 1;
-	}
-  } else if ((pin >= 0) && (pin <= 15)) {
-    if(val == LOW) {
-      GPOC = digitalPinToBitMask(pin);
-	} else {
-	  GPOS = digitalPinToBitMask(pin);
-	}
+  if(pin < 16){
+    if(val) GPOS = (1 << pin);
+    else GPOC = (1 << pin);
+  } else if(pin == 16){
+    if(val) GP16O |= 1;
+    else GP16O &= ~1;
   }
 }