Blackfin arch: fix PORT_J BUG for BF537/6 EMAC driver reported by Kalle Pokki <kalle.pokki@iki.fi>

Cc: Kalle Pokki <kalle.pokki@iki.fi>
Signed-off-by: Michael Hennerich <michael.hennerich@analog.com>
Signed-off-by: Bryan Wu <bryan.wu@analog.com>

1 files changed, 11 insertions, 8 deletions

diff --git a/arch/blackfin/kernel/bfin_gpio.c b/arch/blackfin/kernel/bfin_gpio.c
index 9f309480024..5d488ef965c 100644
--- a/arch/blackfin/kernel/bfin_gpio.c
+++ b/arch/blackfin/kernel/bfin_gpio.c
@@ -180,11 +180,13 @@ static int cmp_label(unsigned short ident, const char *label)
 #ifdef BF537_FAMILY
 static void port_setup(unsigned short gpio, unsigned short usage)
 {
-        if (usage == GPIO_USAGE) {
-                *port_fer[gpio_bank(gpio)] &= ~gpio_bit(gpio);
-        } else
-                *port_fer[gpio_bank(gpio)] |= gpio_bit(gpio);
-        SSYNC();
+        if (!check_gpio(gpio)) {
+                if (usage == GPIO_USAGE) {
+                        *port_fer[gpio_bank(gpio)] &= ~gpio_bit(gpio);
+                } else
+                        *port_fer[gpio_bank(gpio)] |= gpio_bit(gpio);
+                SSYNC();
+        }
 }
 #else
 # define port_setup(...)  do { } while (0)
@@ -644,11 +646,10 @@ int peripheral_request(unsigned short per, const char *label)
         if (!(per & P_DEFINED))
                 return -ENODEV;
 
-        if (check_gpio(ident) < 0)
-                return -EINVAL;
-
         local_irq_save(flags);
 
+        if (!check_gpio(ident)) {
+
         if (unlikely(reserved_gpio_map[gpio_bank(ident)] & gpio_bit(ident))) {
                 printk(KERN_ERR
                        "%s: Peripheral %d is already reserved as GPIO by %s !\n",
@@ -658,6 +659,8 @@ int peripheral_request(unsigned short per, const char *label)
                 return -EBUSY;
         }
 
+        }
+
         if (unlikely(reserved_peri_map[gpio_bank(ident)] & gpio_bit(ident))) {
 
         /*