blackfin: gpio: Use proper mask for comparing function

[u-boot] / arch / blackfin / cpu / gpio.c

diff --git a/arch/blackfin/cpu/gpio.c b/arch/blackfin/cpu/gpio.c
index f74a0b7c0e8b5e78768644b455aaca3cdbce8d6b..f9aff4d894ae95f2d69239e60127485cc5b1a1cc 100644 (file)
--- a/arch/blackfin/cpu/gpio.c
+++ b/arch/blackfin/cpu/gpio.c
@@ -247,7 +247,7 @@ static struct {
 
 static void portmux_setup(unsigned short per)
 {
-       u16 y, offset, muxreg;
+       u16 y, offset, muxreg, mask;
        u16 function = P_FUNCT2MUX(per);
 
        for (y = 0; y < ARRAY_SIZE(port_mux_lut); y++) {
@@ -258,12 +258,13 @@ static void portmux_setup(unsigned short per)
                        offset = port_mux_lut[y].offset;
                        muxreg = bfin_read_PORT_MUX();
 
-                       if (offset != 1)
-                               muxreg &= ~(1 << offset);
+                       if (offset == 1)
+                               mask = 3;
                        else
-                               muxreg &= ~(3 << 1);
+                               mask = 1;
 
-                       muxreg |= (function << offset);
+                       muxreg &= ~(mask << offset);
+                       muxreg |= ((function & mask) << offset);
                        bfin_write_PORT_MUX(muxreg);
                }
        }
@@ -662,8 +663,8 @@ void special_gpio_free(unsigned gpio)
                return;
        }
 
-       reserve(special_gpio, gpio);
-       reserve(peri, gpio);
+       unreserve(special_gpio, gpio);
+       unreserve(peri, gpio);
        set_label(gpio, "free");
 }
 #endif