[PATCH] bcm43xx: fix some gpio register trashing (hopefully :D)

Signed-off-by: Michael Buesch <mbuesch@freenet.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

3 files changed, 46 insertions, 43 deletions

diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_leds.c b/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
index 72a243aac6cb..c8f5ad75d2a6 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
@@ -165,7 +165,7 @@ void bcm43xx_leds_exit(struct bcm43xx_private *bcm)
                 led = &(bcm->leds[i]);
                 bcm43xx_led_blink_stop(led, 1);
         }
-        bcm43xx_leds_turn_off(bcm);
+        bcm43xx_leds_switch_all(bcm, 0);
 }
 
 void bcm43xx_leds_update(struct bcm43xx_private *bcm, int activity)
@@ -268,18 +268,26 @@ void bcm43xx_leds_update(struct bcm43xx_private *bcm, int activity)
         bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_CONTROL, ledctl);
 }
 
-void bcm43xx_leds_turn_off(struct bcm43xx_private *bcm)
+void bcm43xx_leds_switch_all(struct bcm43xx_private *bcm, int on)
 {
         struct bcm43xx_led *led;
-        u16 ledctl = 0;
+        u16 ledctl;
         int i;
+        int bit_on;
 
+        ledctl = bcm43xx_read16(bcm, BCM43xx_MMIO_GPIO_CONTROL);
         for (i = 0; i < BCM43xx_NR_LEDS; i++) {
                 led = &(bcm->leds[i]);
                 if (led->behaviour == BCM43xx_LED_INACTIVE)
                         continue;
-                if (led->activelow)
+                if (on)
+                        bit_on = led->activelow ? 0 : 1;
+                else
+                        bit_on = led->activelow ? 1 : 0;
+                if (bit_on)
                         ledctl |= (1 << i);
+                else
+                        ledctl &= ~(1 << i);
         }
         bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_CONTROL, ledctl);
 }
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_leds.h b/drivers/net/wireless/bcm43xx/bcm43xx_leds.h
index 6f18e2f95db4..d3716cf3aebc 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_leds.h
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_leds.h
@@ -51,6 +51,6 @@ enum { /* LED behaviour values */
 int bcm43xx_leds_init(struct bcm43xx_private *bcm);
 void bcm43xx_leds_exit(struct bcm43xx_private *bcm);
 void bcm43xx_leds_update(struct bcm43xx_private *bcm, int activity);
-void bcm43xx_leds_turn_off(struct bcm43xx_private *bcm);
+void bcm43xx_leds_switch_all(struct bcm43xx_private *bcm, int on);
 
 #endif /* BCM43xx_LEDS_H_ */
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_main.c b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
index 6a877df2ecf0..eb8fca8279f4 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_main.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
@@ -2182,13 +2182,10 @@ static int switch_to_gpio_core(struct bcm43xx_private *bcm)
                 if (unlikely(err == -ENODEV)) {
                         printk(KERN_ERR PFX "gpio error: "
                                "Neither ChipCommon nor PCI core available!\n");
-                        return -ENODEV;
-                } else if (unlikely(err != 0))
-                        return -ENODEV;
-        } else if (unlikely(err != 0))
-                return -ENODEV;
+                }
+        }
 
-        return 0;
+        return err;
 }
 
 /* Initialize the GPIOs
@@ -2198,45 +2195,48 @@ static int bcm43xx_gpio_init(struct bcm43xx_private *bcm)
 {
         struct bcm43xx_coreinfo *old_core;
         int err;
-        u32 mask, value;
+        u32 mask, set;
 
-        value = bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD);
-        value &= ~0xc000;
-        bcm43xx_write32(bcm, BCM43xx_MMIO_STATUS_BITFIELD, value);
+        bcm43xx_write32(bcm, BCM43xx_MMIO_STATUS_BITFIELD,
+                        bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD)
+                        & 0xFFFF3FFF);
 
-        mask = 0x0000001F;
-        value = 0x0000000F;
-        bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_CONTROL,
-                        bcm43xx_read16(bcm, BCM43xx_MMIO_GPIO_CONTROL) & 0xFFF0);
+        bcm43xx_leds_switch_all(bcm, 0);
         bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_MASK,
                         bcm43xx_read16(bcm, BCM43xx_MMIO_GPIO_MASK) | 0x000F);
 
-        old_core = bcm->current_core;
-        
-        err = switch_to_gpio_core(bcm);
-        if (err)
-                return err;
-
-        if (bcm->current_core->rev >= 2){
-                mask  |= 0x10;
-                value |= 0x10;
-        }
+        mask = 0x0000001F;
+        set = 0x0000000F;
         if (bcm->chip_id == 0x4301) {
-                mask  |= 0x60;
-                value |= 0x60;
+                mask |= 0x0060;
+                set |= 0x0060;
+        }
+        if (0 /* FIXME: conditional unknown */) {
+                bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_MASK,
+                                bcm43xx_read16(bcm, BCM43xx_MMIO_GPIO_MASK)
+                                | 0x0100);
+                mask |= 0x0180;
+                set |= 0x0180;
         }
         if (bcm->sprom.boardflags & BCM43xx_BFL_PACTRL) {
-                mask  |= 0x200;
-                value |= 0x200;
+                bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_MASK,
+                                bcm43xx_read16(bcm, BCM43xx_MMIO_GPIO_MASK)
+                                | 0x0200);
+                mask |= 0x0200;
+                set |= 0x0200;
         }
+        if (bcm->current_core->rev >= 2)
+                mask  |= 0x0010; /* FIXME: This is redundant. */
 
+        old_core = bcm->current_core;
+        err = switch_to_gpio_core(bcm);
+        if (err)
+                goto out;
         bcm43xx_write32(bcm, BCM43xx_GPIO_CONTROL,
-                        (bcm43xx_read32(bcm, BCM43xx_GPIO_CONTROL) & mask) | value);
-
+                        (bcm43xx_read32(bcm, BCM43xx_GPIO_CONTROL) & mask) | set);
         err = bcm43xx_switch_core(bcm, old_core);
-        assert(err == 0);
-
-        return 0;
+out:
+        return err;
 }
 
 /* Turn off all GPIO stuff. Call this on module unload, for example. */
@@ -2384,11 +2384,6 @@ static int bcm43xx_chip_init(struct bcm43xx_private *bcm)
                 goto err_gpio_cleanup;
         bcm43xx_radio_turn_on(bcm);
 
-        if (modparam_noleds)
-                bcm43xx_leds_turn_off(bcm);
-        else
-                bcm43xx_leds_update(bcm, 0);
-
         bcm43xx_write16(bcm, 0x03E6, 0x0000);
         err = bcm43xx_phy_init(bcm);
         if (err)