From 588c232fcf9febbc61994d4892fa4b27eddec049 Mon Sep 17 00:00:00 2001
From: Thibaut VIARD <aethaniel@sam-geek.org>
Date: Mon, 21 Sep 2015 18:02:39 +0200
Subject: [PATCH] [samd] fixing issue #28

---
 cores/arduino/wiring_digital.c | 59 +++++++++++++++++++++++-----------
 1 file changed, 40 insertions(+), 19 deletions(-)

diff --git a/cores/arduino/wiring_digital.c b/cores/arduino/wiring_digital.c
index e123c7f85..3bee23d2c 100644
--- a/cores/arduino/wiring_digital.c
+++ b/cores/arduino/wiring_digital.c
@@ -69,26 +69,47 @@ void pinMode( uint32_t ulPin, uint32_t ulMode )
   }
 }
 
-void digitalWrite( uint32_t ulPin, uint32_t ulVal )
+void digitalWrite(uint32_t ulPin, uint32_t ulVal)
 {
-  // Handle the case the pin isn't usable as PIO
-  if ( g_APinDescription[ulPin].ulPinType == PIO_NOT_A_PIN )
+  uint32_t ulGPIOPin=g_APinDescription[ulPin].ulPin;
+  PortGroup* port=&(PORT->Group[g_APinDescription[ulPin].ulPort]);
+
+  // Handle the case the pin is invalid
+  if (ulPin >= PINS_COUNT)
   {
     return ;
   }
 
-  // Enable pull-up resistor
-  PORT->Group[g_APinDescription[ulPin].ulPort].PINCFG[g_APinDescription[ulPin].ulPin].reg=(uint8_t)(PORT_PINCFG_PULLEN) ;
-
-  switch ( ulVal )
+  // Test if pin is set to INPUT mode, then activate pull-up according to ulVal
+  if (port->DIR.reg & (1ul<<ulGPIOPin) != 0)
   {
-    case LOW:
-      PORT->Group[g_APinDescription[ulPin].ulPort].OUTCLR.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
-    break ;
-
-    default:
-      PORT->Group[g_APinDescription[ulPin].ulPort].OUTSET.reg = (1ul << g_APinDescription[ulPin].ulPin) ;
-    break ;
+    switch (ulVal)
+    {
+      case LOW:
+        // Disable pull-up resistor
+        port->PINCFG[ulGPIOPin].bit.PULLEN=0;
+      break;
+
+      case HIGH:
+      default:
+        // Enable pull-up resistor
+        port->PINCFG[ulGPIOPin].bit.PULLEN=1;
+      break;
+    }
+  }
+  else // pin is set to OUTPUT mode, we output the requested voltage level
+  {
+    switch (ulVal)
+    {
+      case LOW:
+        port->OUTCLR.reg=(1ul<<ulGPIOPin);
+      break;
+
+      case HIGH:
+      default:
+        port->OUTSET.reg=(1ul<<ulGPIOPin);
+      break;
+    }
   }
 
   return ;
@@ -96,18 +117,18 @@ void digitalWrite( uint32_t ulPin, uint32_t ulVal )
 
 int digitalRead( uint32_t ulPin )
 {
-  // Handle the case the pin isn't usable as PIO
-  if ( g_APinDescription[ulPin].ulPinType == PIO_NOT_A_PIN )
+  // Handle the case the pin is invalid
+  if (ulPin >= PINS_COUNT)
   {
-    return LOW ;
+    return LOW;
   }
 
   if ( (PORT->Group[g_APinDescription[ulPin].ulPort].IN.reg & (1ul << g_APinDescription[ulPin].ulPin)) != 0 )
   {
-    return HIGH ;
+    return HIGH;
   }
 
-  return LOW ;
+  return LOW;
 }
 
 #ifdef __cplusplus