[PATCH] zd1211rw: Add LED support

This patch includes a big cleanup of the existing unused LED code,
and adds support for controlling the LED.

The link LED will blink if the device is not associated. The LED
switches between 2 seconds on and 1 second off. If the device is
associated the LED is switched on.

The link LED also indicates packet TX. I do a little bit more led
resetting than the vendor driver, but the device works now as
expected for single LED and double LED devices.

Signed-off-by: Ulrich Kunitz <kune@deine-taler.de>
Signed-off-by: Daniel Drake <dsd@gentoo.org>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

4 files changed, 117 insertions, 78 deletions

diff --git a/drivers/net/wireless/zd1211rw/zd_chip.c b/drivers/net/wireless/zd1211rw/zd_chip.c
index c213a2e56e5..aa661b2b76c 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.c
+++ b/drivers/net/wireless/zd1211rw/zd_chip.c
@@ -325,13 +325,22 @@ static int read_pod(struct zd_chip *chip, u8 *rf_type)
 	chip->patch_cr157 = (value >> 13) & 0x1;
 	chip->patch_6m_band_edge = (value >> 21) & 0x1;
 	chip->new_phy_layout = (value >> 31) & 0x1;
+	chip->link_led = ((value >> 4) & 1) ? LED1 : LED2;
+	chip->supports_tx_led = 1;
+	if (value & (1 << 24)) { /* LED scenario */
+		if (value & (1 << 29))
+			chip->supports_tx_led = 0;
+	}
 
 	dev_dbg_f(zd_chip_dev(chip),
 		"RF %s %#01x PA type %#01x patch CCK %d patch CR157 %d "
-		"patch 6M %d new PHY %d\n",
+		"patch 6M %d new PHY %d link LED%d tx led %d\n",
 		zd_rf_name(*rf_type), *rf_type,
 		chip->pa_type, chip->patch_cck_gain,
-		chip->patch_cr157, chip->patch_6m_band_edge, chip->new_phy_layout);
+		chip->patch_cr157, chip->patch_6m_band_edge,
+		chip->new_phy_layout,
+		chip->link_led == LED1 ? 1 : 2,
+		chip->supports_tx_led);
 	return 0;
 error:
 	*rf_type = 0;
@@ -1289,89 +1298,60 @@ u8 zd_chip_get_channel(struct zd_chip *chip)
 	return channel;
 }
 
-static u16 led_mask(int led)
-{
-	switch (led) {
-	case 1:
-		return LED1;
-	case 2:
-		return LED2;
-	default:
-		return 0;
-	}
-}
-
-static int read_led_reg(struct zd_chip *chip, u16 *status)
-{
-	ZD_ASSERT(mutex_is_locked(&chip->mutex));
-	return zd_ioread16_locked(chip, status, CR_LED);
-}
-
-static int write_led_reg(struct zd_chip *chip, u16 status)
+int zd_chip_control_leds(struct zd_chip *chip, enum led_status status)
 {
-	ZD_ASSERT(mutex_is_locked(&chip->mutex));
-	return zd_iowrite16_locked(chip, status, CR_LED);
-}
+	static const zd_addr_t a[] = {
+		FW_LINK_STATUS,
+		CR_LED,
+	};
 
-int zd_chip_led_status(struct zd_chip *chip, int led, enum led_status status)
-{
-	int r, ret;
-	u16 mask = led_mask(led);
-	u16 reg;
+	int r;
+	u16 v[ARRAY_SIZE(a)];
+	struct zd_ioreq16 ioreqs[ARRAY_SIZE(a)] = {
+		[0] = { FW_LINK_STATUS },
+		[1] = { CR_LED },
+	};
+	u16 other_led;
 
-	if (!mask)
-		return -EINVAL;
 	mutex_lock(&chip->mutex);
-	r = read_led_reg(chip, &reg);
+	r = zd_ioread16v_locked(chip, v, (const zd_addr_t *)a, ARRAY_SIZE(a));
 	if (r)
-		return r;
+		goto out;
+
+	other_led = chip->link_led == LED1 ? LED2 : LED1;
+
 	switch (status) {
-	case LED_STATUS:
-		return (reg & mask) ? LED_ON : LED_OFF;
 	case LED_OFF:
-		reg &= ~mask;
-		ret = LED_OFF;
+		ioreqs[0].value = FW_LINK_OFF;
+		ioreqs[1].value = v[1] & ~(LED1|LED2);
 		break;
-	case LED_FLIP:
-		reg ^= mask;
-		ret = (reg&mask) ? LED_ON : LED_OFF;
+	case LED_SCANNING:
+		ioreqs[0].value = FW_LINK_OFF;
+		ioreqs[1].value = v[1] & ~other_led;
+		if (get_seconds() % 3 == 0) {
+			ioreqs[1].value &= ~chip->link_led;
+		} else {
+			ioreqs[1].value |= chip->link_led;
+		}
 		break;
-	case LED_ON:
-		reg |= mask;
-		ret = LED_ON;
+	case LED_ASSOCIATED:
+		ioreqs[0].value = FW_LINK_TX;
+		ioreqs[1].value = v[1] & ~other_led;
+		ioreqs[1].value |= chip->link_led;
 		break;
 	default:
-		return -EINVAL;
-	}
-	r = write_led_reg(chip, reg);
-	if (r) {
-		ret = r;
+		r = -EINVAL;
 		goto out;
 	}
-out:
-	mutex_unlock(&chip->mutex);
-	return r;
-}
 
-int zd_chip_led_flip(struct zd_chip *chip, int led,
-	const unsigned int *phases_msecs, unsigned int count)
-{
-	int i, r;
-	enum led_status status;
-
-	r = zd_chip_led_status(chip, led, LED_STATUS);
-	if (r)
-		return r;
-	status = r;
-	for (i = 0; i < count; i++) {
-		r = zd_chip_led_status(chip, led, LED_FLIP);
-		if (r < 0)
+	if (v[0] != ioreqs[0].value || v[1] != ioreqs[1].value) {
+		r = zd_iowrite16a_locked(chip, ioreqs, ARRAY_SIZE(ioreqs));
+		if (r)
 			goto out;
-		msleep(phases_msecs[i]);
 	}
-
+	r = 0;
 out:
-	zd_chip_led_status(chip, led, status);
+	mutex_unlock(&chip->mutex);
 	return r;
 }
 
@@ -1673,4 +1653,3 @@ int zd_rfwritev_cr_locked(struct zd_chip *chip,
 
 	return 0;
 }
-
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.h b/drivers/net/wireless/zd1211rw/zd_chip.h
index 4b125085989..ae59597ce4e 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.h
+++ b/drivers/net/wireless/zd1211rw/zd_chip.h
@@ -428,6 +428,7 @@
 /* masks for controlling LEDs */
 #define LED1				0x0100
 #define LED2				0x0200
+#define LED_SW				0x0400
 
 /* Seems to indicate that the configuration is over.
  */
@@ -629,6 +630,10 @@
 #define FW_SOFT_RESET           FW_REG(4)
 #define FW_FLASH_CHK            FW_REG(5)
 
+#define FW_LINK_OFF		0x0
+#define FW_LINK_TX		0x1
+/* 0x2 - link led on? */
+
 enum {
 	CR_BASE_OFFSET			= 0x9000,
 	FW_START_OFFSET			= 0xee00,
@@ -663,8 +668,11 @@ struct zd_chip {
 	u8 pwr_int_values[E2P_CHANNEL_COUNT];
 	/* SetPointOFDM in the vendor driver */
 	u8 ofdm_cal_values[3][E2P_CHANNEL_COUNT];
-	u8 pa_type:4, patch_cck_gain:1, patch_cr157:1, patch_6m_band_edge:1,
-	   new_phy_layout:1, is_zd1211b:1;
+	u16 link_led;
+	unsigned int pa_type:4,
+		patch_cck_gain:1, patch_cr157:1, patch_6m_band_edge:1,
+		new_phy_layout:1,
+		is_zd1211b:1, supports_tx_led:1;
 };
 
 static inline struct zd_chip *zd_usb_to_chip(struct zd_usb *usb)
@@ -812,15 +820,12 @@ int zd_chip_lock_phy_regs(struct zd_chip *chip);
 int zd_chip_unlock_phy_regs(struct zd_chip *chip);
 
 enum led_status {
-	LED_OFF	   = 0,
-	LED_ON     = 1,
-	LED_FLIP   = 2,
-	LED_STATUS = 3,
+	LED_OFF = 0,
+	LED_SCANNING = 1,
+	LED_ASSOCIATED = 2,
 };
 
-int zd_chip_led_status(struct zd_chip *chip, int led, enum led_status status);
-int zd_chip_led_flip(struct zd_chip *chip, int led,
-	             const unsigned int *phases_msecs, unsigned int count);
+int zd_chip_control_leds(struct zd_chip *chip, enum led_status status);
 
 int zd_set_beacon_interval(struct zd_chip *chip, u32 interval);
 
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index 1989f1c05fb..2d12837052b 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -33,6 +33,10 @@
 static void ieee_init(struct ieee80211_device *ieee);
 static void softmac_init(struct ieee80211softmac_device *sm);
 
+static void housekeeping_init(struct zd_mac *mac);
+static void housekeeping_enable(struct zd_mac *mac);
+static void housekeeping_disable(struct zd_mac *mac);
+
 int zd_mac_init(struct zd_mac *mac,
 	        struct net_device *netdev,
 	        struct usb_interface *intf)
@@ -46,6 +50,7 @@ int zd_mac_init(struct zd_mac *mac,
 	ieee_init(ieee);
 	softmac_init(ieee80211_priv(netdev));
 	zd_chip_init(&mac->chip, netdev, intf);
+	housekeeping_init(mac);
 	return 0;
 }
 
@@ -178,6 +183,7 @@ int zd_mac_open(struct net_device *netdev)
 	if (r < 0)
 		goto disable_rx;
 
+	housekeeping_enable(mac);
 	ieee80211softmac_start(netdev);
 	return 0;
 disable_rx:
@@ -204,6 +210,7 @@ int zd_mac_stop(struct net_device *netdev)
 	 */
 
 	zd_chip_disable_rx(chip);
+	housekeeping_disable(mac);
 	ieee80211softmac_stop(netdev);
 
 	zd_chip_disable_hwint(chip);
@@ -1080,3 +1087,46 @@ void zd_dump_rx_status(const struct rx_status *status)
 	}
 }
 #endif /* DEBUG */
+
+#define LINK_LED_WORK_DELAY HZ
+
+static void link_led_handler(void *p)
+{
+	struct zd_mac *mac = p;
+	struct zd_chip *chip = &mac->chip;
+	struct ieee80211softmac_device *sm = ieee80211_priv(mac->netdev);
+	int is_associated;
+	int r;
+
+	spin_lock_irq(&mac->lock);
+	is_associated = sm->associated != 0;
+	spin_unlock_irq(&mac->lock);
+
+	r = zd_chip_control_leds(chip,
+		                 is_associated ? LED_ASSOCIATED : LED_SCANNING);
+	if (r)
+		dev_err(zd_mac_dev(mac), "zd_chip_control_leds error %d\n", r);
+
+	queue_delayed_work(zd_workqueue, &mac->housekeeping.link_led_work,
+		           LINK_LED_WORK_DELAY);
+}
+
+static void housekeeping_init(struct zd_mac *mac)
+{
+	INIT_WORK(&mac->housekeeping.link_led_work, link_led_handler, mac);
+}
+
+static void housekeeping_enable(struct zd_mac *mac)
+{
+	dev_dbg_f(zd_mac_dev(mac), "\n");
+	queue_delayed_work(zd_workqueue, &mac->housekeeping.link_led_work,
+			   0);
+}
+
+static void housekeeping_disable(struct zd_mac *mac)
+{
+	dev_dbg_f(zd_mac_dev(mac), "\n");
+	cancel_rearming_delayed_workqueue(zd_workqueue,
+		&mac->housekeeping.link_led_work);
+	zd_chip_control_leds(&mac->chip, LED_OFF);
+}
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zd1211rw/zd_mac.h
index 29b51fd7d4e..b8ea3de7924 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zd1211rw/zd_mac.h
@@ -120,6 +120,10 @@ enum mac_flags {
 	MAC_FIXED_CHANNEL = 0x01,
 };
 
+struct housekeeping {
+	struct work_struct link_led_work;
+};
+
 #define ZD_MAC_STATS_BUFFER_SIZE 16
 
 struct zd_mac {
@@ -128,6 +132,7 @@ struct zd_mac {
 	struct net_device *netdev;
 	/* Unlocked reading possible */
 	struct iw_statistics iw_stats;
+	struct housekeeping housekeeping;
 	unsigned int stats_count;
 	u8 qual_buffer[ZD_MAC_STATS_BUFFER_SIZE];
 	u8 rssi_buffer[ZD_MAC_STATS_BUFFER_SIZE];