diff --git a/cores/arduino/wiring_digital.c b/cores/arduino/wiring_digital.c
index 47d358d..8ccffab 100644
--- a/cores/arduino/wiring_digital.c
+++ b/cores/arduino/wiring_digital.c
@@ -102,12 +102,18 @@ void digitalWrite(uint32_t PinNumber, uint32_t Val)
     ErrorMsgHandler("digitalWrite(): pin number exceeds the total number of pins");
     return;
   }
+  
+  GPIO_TypeDef* port  = digitalPinToPort(PinNumber);
+  HAL_PinsTypeDef pin = digitalPinToBitMask(PinNumber);
 
   if (digitalPinHasPWM(PinNumber))
     // if the pin can use PWM, disable PWM
     analogWriteStop(PinNumber);
 
-  HAL_GPIO_WritePin(digitalPinToPort(PinNumber), digitalPinToBitMask(PinNumber), (Val == HIGH) ? GPIO_PIN_HIGH : GPIO_PIN_LOW);
+  if (Val == HIGH)
+    GPIO_SET_PIN(port, pin);
+  else
+    GPIO_CLEAR_PIN(port, pin);
 }
 
 // read pin
@@ -118,12 +124,14 @@ int digitalRead(uint32_t PinNumber)
     ErrorMsgHandler("digitalRead(): pin number exceeds the total number of pins");
     return -1;
   }
+  GPIO_TypeDef* port  = digitalPinToPort(PinNumber);
+  HAL_PinsTypeDef pin = digitalPinToBitMask(PinNumber);
+
   if (digitalPinHasPWM(PinNumber))
     // if the pin can use PWM, disable PWM
     analogWriteStop(PinNumber);
 
-  GPIO_PinState pinState = HAL_GPIO_ReadPin(digitalPinToPort(PinNumber), digitalPinToBitMask(PinNumber));
-  return (pinState == GPIO_PIN_LOW) ? LOW : HIGH;
+  return GPIO_READ_PIN(port, pin);
 }
 
 // toggle pin