[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>

diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_leds.c b/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
index 72a243a..c8f5ad7 100644 (file)
--- 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 6f18e2f..d3716cf 100644 (file)
--- 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 6a877df..eb8fca8 100644 (file)
--- 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)