[PATCH] bcm43xx: fix LED code.

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

1 files changed, 57 insertions, 31 deletions

diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_leds.c b/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
index 455a0c743f7..8f550c1a92e 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_leds.c
@@ -35,12 +35,13 @@ static void bcm43xx_led_changestate(struct bcm43xx_led *led)
 {
 	struct bcm43xx_private *bcm = led->bcm;
 	const int index = bcm43xx_led_index(led);
+	const u16 mask = (1 << index);
 	u16 ledctl;
 
 	assert(index >= 0 && index < BCM43xx_NR_LEDS);
 	assert(led->blink_interval);
 	ledctl = bcm43xx_read16(bcm, BCM43xx_MMIO_GPIO_CONTROL);
-	__change_bit(index, (unsigned long *)(&ledctl));
+	ledctl = (ledctl & mask) ? (ledctl & ~mask) : (ledctl | mask);
 	bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_CONTROL, ledctl);
 }
 
@@ -61,6 +62,8 @@ static void bcm43xx_led_blink(unsigned long d)
 static void bcm43xx_led_blink_start(struct bcm43xx_led *led,
 				    unsigned long interval)
 {
+	if (led->blink_interval)
+		return;
 	led->blink_interval = interval;
 	bcm43xx_led_changestate(led);
 	led->blink_timer.expires = jiffies + interval;
@@ -91,6 +94,39 @@ static void bcm43xx_led_blink_stop(struct bcm43xx_led *led, int sync)
 	bcm43xx_write16(bcm, BCM43xx_MMIO_GPIO_CONTROL, ledctl);
 }
 
+static void bcm43xx_led_init_hardcoded(struct bcm43xx_private *bcm,
+				       struct bcm43xx_led *led,
+				       int led_index)
+{
+	/* This function is called, if the behaviour (and activelow)
+	 * information for a LED is missing in the SPROM.
+	 * We hardcode the behaviour values for various devices here.
+	 * Note that the BCM43xx_LED_TEST_XXX behaviour values can
+	 * be used to figure out which led is mapped to which index.
+	 */
+
+	switch (led_index) {
+	case 0:
+		led->behaviour = BCM43xx_LED_ACTIVITY;
+		if (bcm->board_vendor == PCI_VENDOR_ID_COMPAQ)
+			led->behaviour = BCM43xx_LED_RADIO_ALL;
+		break;
+	case 1:
+		led->behaviour = BCM43xx_LED_RADIO_B;
+		if (bcm->board_vendor == PCI_VENDOR_ID_ASUSTEK)
+			led->behaviour = BCM43xx_LED_ASSOC;
+		break;
+	case 2:
+		led->behaviour = BCM43xx_LED_RADIO_A;
+		break;
+	case 3:
+		led->behaviour = BCM43xx_LED_OFF;
+		break;
+	default:
+		assert(0);
+	}
+}
+
 int bcm43xx_leds_init(struct bcm43xx_private *bcm)
 {
 	struct bcm43xx_led *led;
@@ -105,31 +141,12 @@ int bcm43xx_leds_init(struct bcm43xx_private *bcm)
 	for (i = 0; i < BCM43xx_NR_LEDS; i++) {
 		led = &(bcm->leds[i]);
 		led->bcm = bcm;
-		init_timer(&led->blink_timer);
-		led->blink_timer.data = (unsigned long)led;
-		led->blink_timer.function = bcm43xx_led_blink;
+		setup_timer(&led->blink_timer,
+			    bcm43xx_led_blink,
+			    (unsigned long)led);
 
 		if (sprom[i] == 0xFF) {
-			/* SPROM information not set. */
-			switch (i) {
-			case 0:
-				if (bcm->board_vendor == PCI_VENDOR_ID_COMPAQ)
-					led->behaviour = BCM43xx_LED_RADIO_ALL;
-				else
-					led->behaviour = BCM43xx_LED_ACTIVITY;
-				break;
-			case 1:
-				led->behaviour = BCM43xx_LED_RADIO_B;
-				break;
-			case 2:
-				led->behaviour = BCM43xx_LED_RADIO_A;
-				break;
-			case 3:
-				led->behaviour = BCM43xx_LED_OFF;
-				break;
-			default:
-				assert(0);
-			}
+			bcm43xx_led_init_hardcoded(bcm, led, i);
 		} else {
 			led->behaviour = sprom[i] & BCM43xx_LED_BEHAVIOUR;
 			led->activelow = !!(sprom[i] & BCM43xx_LED_ACTIVELOW);
@@ -157,19 +174,19 @@ void bcm43xx_leds_update(struct bcm43xx_private *bcm, int activity)
 	struct bcm43xx_radioinfo *radio = bcm->current_core->radio;
 	struct bcm43xx_phyinfo *phy = bcm->current_core->phy;
 	const int transferring = (jiffies - bcm->stats.last_tx) < BCM43xx_LED_XFER_THRES;
-	int i, turn_on = 0;
+	int i, turn_on;
 	unsigned long interval = 0;
 	u16 ledctl;
 
 	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;
 
+		turn_on = 0;
 		switch (led->behaviour) {
+		case BCM43xx_LED_INACTIVE:
+			continue;
 		case BCM43xx_LED_OFF:
-			turn_on = 0;
 			break;
 		case BCM43xx_LED_ON:
 			turn_on = 1;
@@ -189,7 +206,6 @@ void bcm43xx_leds_update(struct bcm43xx_private *bcm, int activity)
 				    phy->type == BCM43xx_PHYTYPE_G));
 			break;
 		case BCM43xx_LED_MODE_BG:
-			turn_on = 0;
 			if (phy->type == BCM43xx_PHYTYPE_G &&
 			    1/*FIXME: using G rates.*/)
 				turn_on = 1;
@@ -222,12 +238,22 @@ void bcm43xx_leds_update(struct bcm43xx_private *bcm, int activity)
 			continue;
 		case BCM43xx_LED_WEIRD:
 			//TODO
-			turn_on = 0;
 			break;
 		case BCM43xx_LED_ASSOC:
-			if (1/*TODO: associated*/)
+			if (bcm->softmac->associated)
 				turn_on = 1;
 			break;
+#ifdef CONFIG_BCM43XX_DEBUG
+		case BCM43xx_LED_TEST_BLINKSLOW:
+			bcm43xx_led_blink_start(led, BCM43xx_LEDBLINK_SLOW);
+			continue;
+		case BCM43xx_LED_TEST_BLINKMEDIUM:
+			bcm43xx_led_blink_start(led, BCM43xx_LEDBLINK_MEDIUM);
+			continue;
+		case BCM43xx_LED_TEST_BLINKFAST:
+			bcm43xx_led_blink_start(led, BCM43xx_LEDBLINK_FAST);
+			continue;
+#endif /* CONFIG_BCM43XX_DEBUG */
 		default:
 			assert(0);
 		};