diff --git a/cores/arduino/wiring_digital.c b/cores/arduino/wiring_digital.c
index e4f6633..4547e95 100644
--- a/cores/arduino/wiring_digital.c
+++ b/cores/arduino/wiring_digital.c
@@ -37,7 +37,7 @@ void pinMode(uint32_t PinNumber, uint32_t PinMode)
     return;
   }
 
-  if (digitalPinPwmIsOn(PinNumber))
+  if (digitalPinPwmIsOn(PinNumber) > 0 )
     // if the pin use PWM, disable PWM
     analogWriteStop(PinNumber);
 
@@ -102,15 +102,22 @@ __attribute__((noinline, section(".ram_text"))) void digitalWrite(uint32_t PinNu
     ErrorMsgHandler("digitalWrite(): pin number exceeds the total number of pins");
     return;
   }
-  
-  if (digitalPinPwmIsOn(PinNumber))
+
+  GPIO_TypeDef* port = digitalPinToPort(PinNumber);
+  HAL_PinsTypeDef mask = digitalPinToBitMask(PinNumber);
+
+  if (digitalPinPwmIsOn(PinNumber) > 0)
+  {
     // if the pin use PWM, disable PWM
     analogWriteStop(PinNumber);
-
+    // and configure pin as output
+    HAL_GPIO_PinConfig(port, mask, HAL_GPIO_MODE_GPIO_OUTPUT, HAL_GPIO_PULL_NONE, HAL_GPIO_DS_2MA);
+  }
+    
   if (Val == HIGH)
-    GPIO_SET_PIN(digitalPinToPort(PinNumber), digitalPinToBitMask(PinNumber));
+    GPIO_SET_PIN(port, mask);
   else
-    GPIO_CLEAR_PIN(digitalPinToPort(PinNumber), digitalPinToBitMask(PinNumber));
+    GPIO_CLEAR_PIN(port, mask);
 }
 
 // read pin
@@ -122,9 +129,9 @@ __attribute__((noinline, section(".ram_text"))) int digitalRead(uint32_t PinNumb
     return -1;
   }
 
-  if (digitalPinPwmIsOn(PinNumber))
+  if (digitalPinPwmIsOn(PinNumber) > 0)
     // if the pin use PWM, disable PWM
-    analogWriteStop(PinNumber);
+    analogWriteStop(PinNumber); // pin is configured as input here
 
   return GPIO_READ_PIN(digitalPinToPort(PinNumber), digitalPinToBitMask(PinNumber));
 }
@@ -138,11 +145,17 @@ __attribute__((noinline, section(".ram_text"))) void digitalToggle(uint32_t PinN
     return;
   }
 
-  if (digitalPinPwmIsOn(PinNumber))
+  GPIO_TypeDef* port = digitalPinToPort(PinNumber);
+  HAL_PinsTypeDef mask = digitalPinToBitMask(PinNumber);
+
+  if (digitalPinPwmIsOn(PinNumber) > 0)
+  {
     // if the pin use PWM, disable PWM
     analogWriteStop(PinNumber);
-
-  GPIO_TOGGLE_PIN(digitalPinToPort(PinNumber), digitalPinToBitMask(PinNumber));
+    // and configure pin as output
+    HAL_GPIO_PinConfig(port, mask, HAL_GPIO_MODE_GPIO_OUTPUT, HAL_GPIO_PULL_NONE, HAL_GPIO_DS_2MA);
+  }
+  GPIO_TOGGLE_PIN(port, mask);
 }
 
 #ifdef __cplusplus