Blackfin: bfin_gpio: Use proper mask for comparing pfunc
For BF537_FAMILY, when offset != 1, the mask is 1.
Thus add proper mask for comparing pfunc with function.
Also has small refactor for better readability.
In portmux_setup(), it looks odd having "pmux &= ~(3 << 1);"
while in current code we do pmux |= (function << offset);.
Signed-off-by: Axel Lin <axel.lin@ingics.com>
Signed-off-by: Sonic Zhang <sonic.zhang@analog.com>
diff --git a/arch/blackfin/kernel/bfin_gpio.c b/arch/blackfin/kernel/bfin_gpio.c
index ed978f1..780d27d 100644
--- a/arch/blackfin/kernel/bfin_gpio.c
+++ b/arch/blackfin/kernel/bfin_gpio.c
@@ -255,7 +255,7 @@
u16 ident = P_IDENT(per);
u16 function = P_FUNCT2MUX(per);
s8 offset = port_mux[ident];
- u16 m, pmux, pfunc;
+ u16 m, pmux, pfunc, mask;
if (offset < 0)
return 0;
@@ -270,10 +270,12 @@
continue;
if (offset == 1)
- pfunc = (pmux >> offset) & 3;
+ mask = 3;
else
- pfunc = (pmux >> offset) & 1;
- if (pfunc != function) {
+ mask = 1;
+
+ pfunc = (pmux >> offset) & mask;
+ if (pfunc != (function & mask)) {
pr_err("pin group conflict! request pin %d func %d conflict with pin %d func %d\n",
ident, function, m, pfunc);
return -EINVAL;
@@ -288,17 +290,20 @@
u16 ident = P_IDENT(per);
u16 function = P_FUNCT2MUX(per);
s8 offset = port_mux[ident];
- u16 pmux;
+ u16 pmux, mask;
if (offset == -1)
return;
pmux = bfin_read_PORT_MUX();
- if (offset != 1)
- pmux &= ~(1 << offset);
+ if (offset == 1)
+ mask = 3;
else
- pmux &= ~(3 << 1);
- pmux |= (function << offset);
+ mask = 1;
+
+ pmux &= ~(mask << offset);
+ pmux |= ((function & mask) << offset);
+
bfin_write_PORT_MUX(pmux);
}
#elif defined(CONFIG_BF54x) || defined(CONFIG_BF60x)