i.MX31: fix mxc_iomux_set_pad()

mxc_iomux_set_pad() is buggy on i.MX31 - it calculates the register and
the offset therein wrongly. Fix it.

Signed-off-by: Guennadi Liakhovetski <lg@denx.de>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 5 insertions, 4 deletions

diff --git a/arch/arm/mach-mx3/iomux.c b/arch/arm/mach-mx3/iomux.c
index 6e664be8cc13..7a5088b519a8 100644
--- a/arch/arm/mach-mx3/iomux.c
+++ b/arch/arm/mach-mx3/iomux.c
@@ -74,17 +74,18 @@ void mxc_iomux_set_pad(enum iomux_pins pin, u32 config)
         u32 field, l;
         void __iomem *reg;
 
-        reg = IOMUXSW_PAD_CTL + (pin + 2) / 3;
+        pin &= IOMUX_PADNUM_MASK;
+        reg = IOMUXSW_PAD_CTL + (pin + 2) / 3 * 4;
         field = (pin + 2) % 3;
 
-        pr_debug("%s: reg offset = 0x%x field = %d\n",
+        pr_debug("%s: reg offset = 0x%x, field = %d\n",
                         __func__, (pin + 2) / 3, field);
 
         spin_lock(&gpio_mux_lock);
 
         l = __raw_readl(reg);
-        l &= ~(0x1ff << (field * 9));
-        l |= config << (field * 9);
+        l &= ~(0x1ff << (field * 10));
+        l |= config << (field * 10);
         __raw_writel(l, reg);
 
         spin_unlock(&gpio_mux_lock);