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

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);
 }

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_ */

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)