1 files changed, 12 insertions, 11 deletions

diff --git a/arch/arm/mach-imx/generic.c b/arch/arm/mach-imx/generic.c
index 41e5849ae8da..f8a742bb2d5b 100644
--- a/arch/arm/mach-imx/generic.c
+++ b/arch/arm/mach-imx/generic.c
@@ -28,14 +28,15 @@
 #include <linux/module.h>
 #include <asm/arch/imxfb.h>
 #include <asm/hardware.h>
+#include <asm/arch/imx-regs.h>
 
 #include <asm/mach/map.h>
 
 void imx_gpio_mode(int gpio_mode)
 {
         unsigned int pin = gpio_mode & GPIO_PIN_MASK;
-        unsigned int port = (gpio_mode & GPIO_PORT_MASK) >> 5;
-        unsigned int ocr = (gpio_mode & GPIO_OCR_MASK) >> 10;
+        unsigned int port = (gpio_mode & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT;
+        unsigned int ocr = (gpio_mode & GPIO_OCR_MASK) >> GPIO_OCR_SHIFT;
         unsigned int tmp;
 
         /* Pullup enable */
@@ -57,7 +58,7 @@ void imx_gpio_mode(int gpio_mode)
                 GPR(port) &= ~(1<<pin);
 
         /* use as gpio? */
-        if( ocr == 3 )
+        if(gpio_mode &  GPIO_GIUS)
                 GIUS(port) |= (1<<pin);
         else
                 GIUS(port) &= ~(1<<pin);
@@ -72,20 +73,20 @@ void imx_gpio_mode(int gpio_mode)
                 tmp |= (ocr << (pin*2));
                 OCR1(port) = tmp;
 
-                if( gpio_mode & GPIO_AOUT )
-                        ICONFA1(port) &= ~( 3<<(pin*2));
-                if( gpio_mode & GPIO_BOUT )
-                        ICONFB1(port) &= ~( 3<<(pin*2));
+                ICONFA1(port) &= ~( 3<<(pin*2));
+                ICONFA1(port) |= ((gpio_mode >> GPIO_AOUT_SHIFT) & 3) << (pin * 2);
+                ICONFB1(port) &= ~( 3<<(pin*2));
+                ICONFB1(port) |= ((gpio_mode >> GPIO_BOUT_SHIFT) & 3) << (pin * 2);
         } else {
                 tmp = OCR2(port);
                 tmp &= ~( 3<<((pin-16)*2));
                 tmp |= (ocr << ((pin-16)*2));
                 OCR2(port) = tmp;
 
-                if( gpio_mode & GPIO_AOUT )
-                        ICONFA2(port) &= ~( 3<<((pin-16)*2));
-                if( gpio_mode & GPIO_BOUT )
-                        ICONFB2(port) &= ~( 3<<((pin-16)*2));
+                ICONFA2(port) &= ~( 3<<((pin-16)*2));
+                ICONFA2(port) |= ((gpio_mode >> GPIO_AOUT_SHIFT) & 3) << ((pin-16) * 2);
+                ICONFB2(port) &= ~( 3<<((pin-16)*2));
+                ICONFB2(port) |= ((gpio_mode >> GPIO_BOUT_SHIFT) & 3) << ((pin-16) * 2);
         }
 }