LPC178x: Adapt HAL pin configuration for new pin multiplexer.

[sysless.git] / arch / arm / mach-lpc178x / libs / hal / hal_gpio.c

diff --git a/arch/arm/mach-lpc178x/libs/hal/hal_gpio.c b/arch/arm/mach-lpc178x/libs/hal/hal_gpio.c
index b06ea7f425fce515d80cd3e3c80e2bc13de9aa90..50578ad4387ab5aefc51c605a57d0630d3108d54 100644 (file)
--- a/arch/arm/mach-lpc178x/libs/hal/hal_gpio.c
+++ b/arch/arm/mach-lpc178x/libs/hal/hal_gpio.c
@@ -5,52 +5,42 @@
 
 int hal_pin_conf_fnc(unsigned gpio, int fnc)
 {
-  __IO uint32_t *p = &(PINCON->PINSEL0); 
-  uint32_t mask;
+  __IO uint32_t *p = &(LPC_IOCON->P0_0);
 
   if(fnc & PORT_CONF_FNC_MASK)
     fnc = __mfld2val(PORT_CONF_FNC_MASK, fnc);
 
-  p += hal_gpio_get_port_num(gpio)*2;
-  if(gpio & 0x10)
-    p++;
-  gpio &= 0x0f;
-  gpio *= 2;
-  mask = 3 << gpio;
+  p += hal_gpio_get_port_num(gpio)*32 + (gpio & 0x1f);
 
-  *p = (*p & ~mask) | ((fnc << gpio) & mask);
+  *p = (*p & ~7) | fnc;
 
   return 0;
 }
 
 int hal_pin_conf_mode(unsigned gpio, int mode)
 {
-  __IO uint32_t *p = &(PINCON->PINMODE0); 
-  uint32_t mask;
+  __IO uint32_t *p = &(LPC_IOCON->P0_0);
 
   if(mode & PORT_CONF_MODE_MASK)
     mode = __mfld2val(PORT_CONF_MODE_MASK, mode);
 
-  p += hal_gpio_get_port_num(gpio)*2;
-  if(gpio & 0x10)
-    p++;
-  gpio &= 0x0f;
-  gpio *= 2;
-  mask = 3 << gpio;
+  p += hal_gpio_get_port_num(gpio)*32 + (gpio & 0x1f);
 
-  *p = (*p & ~mask) | ((mode << gpio) & mask);
+  *p = (*p & ~(3 << 3)) | (mode << 3);
 
   return 0;
 }
 
 int hal_pin_conf_od(unsigned gpio, int od)
 {
-  uint32_t mask = 1 << (gpio & 0x1f);
+  __IO uint32_t *p = &(LPC_IOCON->P0_0);
+
+  p += hal_gpio_get_port_num(gpio)*32 + (gpio & 0x1f);
 
   if(od)
-    (&(PINCON->PINMODE_OD0))[hal_gpio_get_port_num(gpio)] |= mask;
+    *p |= (1 << 10);
   else
-    (&(PINCON->PINMODE_OD0))[hal_gpio_get_port_num(gpio)] &= ~mask;
+    *p &= ~(1 << 10);
 
   return 0;
 }