X-Git-Url: https://git.sur5r.net/?a=blobdiff_plain;f=arch%2Fblackfin%2Fcpu%2Fgpio.c;h=f9aff4d894ae95f2d69239e60127485cc5b1a1cc;hb=9868b1426315fc5ac4883b5267d5337b2dfcbd95;hp=f74a0b7c0e8b5e78768644b455aaca3cdbce8d6b;hpb=805fa87f6d2366e2193f3d9eb1f793ad41ae1430;p=u-boot

diff --git a/arch/blackfin/cpu/gpio.c b/arch/blackfin/cpu/gpio.c
index f74a0b7c0e..f9aff4d894 100644
--- 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