rt2x00: Make use of MAC80211_LED_TRIGGERS

Make use of the led triggers provided by mac80211 to control
the led status. This can be enabled through a per-driver
configuration option which will automatically enable the
generic handler in rt2x00lib.

This has been enabled for rt2500usb and rt73usb for the moment
since the led class will call set_brightness in irq context which
will not work correctly with the usb drivers who need to sleep.

Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

10 files changed, 403 insertions, 320 deletions

diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 28f3e5e94ab6..88a6ef693862 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -38,6 +38,13 @@ config RT2X00_LIB_RFKILL
 	select RFKILL
 	select INPUT_POLLDEV
 
+config RT2X00_LIB_LEDS
+	boolean
+	depends on RT2X00_LIB
+	select LEDS_CLASS
+	select LEDS_TRIGGERS
+	select MAC80211_LEDS
+
 config RT2400PCI
 	tristate "Ralink rt2400 pci/pcmcia support"
 	depends on RT2X00 && PCI
@@ -57,6 +64,13 @@ config RT2400PCI_RFKILL
 	  hardware button to control the radio state.
 	  This feature depends on the RF switch subsystem rfkill.
 
+config RT2400PCI_LEDS
+	bool "RT2400 leds support"
+	depends on RT2400PCI
+	select RT2X00_LIB_LEDS
+	---help---
+	  This adds support for led triggers provided my mac80211.
+
 config RT2500PCI
 	tristate "Ralink rt2500 pci/pcmcia support"
 	depends on RT2X00 && PCI
@@ -76,6 +90,13 @@ config RT2500PCI_RFKILL
 	  hardware button to control the radio state.
 	  This feature depends on the RF switch subsystem rfkill.
 
+config RT2500PCI_LEDS
+	bool "RT2500 leds support"
+	depends on RT2500PCI
+	select RT2X00_LIB_LEDS
+	---help---
+	  This adds support for led triggers provided my mac80211.
+
 config RT61PCI
 	tristate "Ralink rt61 pci/pcmcia support"
 	depends on RT2X00 && PCI
@@ -96,6 +117,13 @@ config RT61PCI_RFKILL
 	  hardware button to control the radio state.
 	  This feature depends on the RF switch subsystem rfkill.
 
+config RT61PCI_LEDS
+	bool "RT61 leds support"
+	depends on RT61PCI
+	select RT2X00_LIB_LEDS
+	---help---
+	  This adds support for led triggers provided my mac80211.
+
 config RT2500USB
 	tristate "Ralink rt2500 usb support"
 	depends on RT2X00 && USB
diff --git a/drivers/net/wireless/rt2x00/Makefile b/drivers/net/wireless/rt2x00/Makefile
index ab40e1f6b303..757bbc077d93 100644
--- a/drivers/net/wireless/rt2x00/Makefile
+++ b/drivers/net/wireless/rt2x00/Makefile
@@ -12,6 +12,10 @@ ifeq ($(CONFIG_RT2X00_LIB_FIRMWARE),y)
 	rt2x00lib-objs += rt2x00firmware.o
 endif
 
+ifeq ($(CONFIG_RT2X00_LIB_LEDS),y)
+	rt2x00lib-objs += rt2x00leds.o
+endif
+
 obj-$(CONFIG_RT2X00_LIB)	+= rt2x00lib.o
 obj-$(CONFIG_RT2X00_LIB_PCI)	+= rt2x00pci.o
 obj-$(CONFIG_RT2X00_LIB_USB)	+= rt2x00usb.o
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index feb8c09a33ad..69b4852d4c7f 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -243,6 +243,30 @@ static int rt2400pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 #define rt2400pci_rfkill_poll	NULL
 #endif /* CONFIG_RT2400PCI_RFKILL */
 
+#ifdef CONFIG_RT2400PCI_LEDS
+static void rt2400pci_led_brightness(struct led_classdev *led_cdev,
+				     enum led_brightness brightness)
+{
+	struct rt2x00_led *led =
+	    container_of(led_cdev, struct rt2x00_led, led_dev);
+	unsigned int enabled = brightness != LED_OFF;
+	unsigned int activity =
+	    led->rt2x00dev->led_flags & LED_SUPPORT_ACTIVITY;
+	u32 reg;
+
+	rt2x00pci_register_read(led->rt2x00dev, LEDCSR, &reg);
+
+	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC) {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, enabled);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, enabled && activity);
+	}
+
+	rt2x00pci_register_write(led->rt2x00dev, LEDCSR, reg);
+}
+#else
+#define rt2400pci_led_brightness	NULL
+#endif /* CONFIG_RT2400PCI_LEDS */
+
 /*
  * Configuration handlers.
  */
@@ -511,34 +535,6 @@ static void rt2400pci_config_cw(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * LED functions.
- */
-static void rt2400pci_enable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u32 reg;
-
-	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
-
-	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, 70);
-	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, 30);
-	rt2x00_set_field32(&reg, LEDCSR_LINK,
-			   (rt2x00dev->led_mode != LED_MODE_ASUS));
-	rt2x00_set_field32(&reg, LEDCSR_ACTIVITY,
-			   (rt2x00dev->led_mode != LED_MODE_TXRX_ACTIVITY));
-	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
-}
-
-static void rt2400pci_disable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u32 reg;
-
-	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
-	rt2x00_set_field32(&reg, LEDCSR_LINK, 0);
-	rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 0);
-	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
-}
-
-/*
  * Link tuning
  */
 static void rt2400pci_link_stats(struct rt2x00_dev *rt2x00dev,
@@ -703,6 +699,11 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 			   (rt2x00dev->rx->data_size / 128));
 	rt2x00pci_register_write(rt2x00dev, CSR9, reg);
 
+	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
+	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, 30);
+	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
+
 	rt2x00pci_register_write(rt2x00dev, CNT3, 0x3f080000);
 
 	rt2x00pci_register_read(rt2x00dev, ARCSR0, &reg);
@@ -872,11 +873,6 @@ static int rt2400pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2400pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
 
-	/*
-	 * Enable LED
-	 */
-	rt2400pci_enable_led(rt2x00dev);
-
 	return 0;
 }
 
@@ -884,11 +880,6 @@ static void rt2400pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	/*
-	 * Disable LED
-	 */
-	rt2400pci_disable_led(rt2x00dev);
-
 	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0);
 
 	/*
@@ -1273,8 +1264,24 @@ static int rt2400pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Store led mode, for correct led behaviour.
 	 */
-	rt2x00dev->led_mode =
-	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+#ifdef CONFIG_RT2400PCI_LEDS
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+
+	switch (value) {
+	case LED_MODE_ASUS:
+	case LED_MODE_ALPHA:
+	case LED_MODE_DEFAULT:
+		rt2x00dev->led_flags = LED_SUPPORT_RADIO;
+		break;
+	case LED_MODE_TXRX_ACTIVITY:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ACTIVITY;
+		break;
+	case LED_MODE_SIGNAL_STRENGTH:
+		rt2x00dev->led_flags = LED_SUPPORT_RADIO;
+		break;
+	}
+#endif /* CONFIG_RT2400PCI_LEDS */
 
 	/*
 	 * Detect if this device has an hardware controlled radio.
@@ -1594,6 +1601,7 @@ static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
 	.link_stats		= rt2400pci_link_stats,
 	.reset_tuner		= rt2400pci_reset_tuner,
 	.link_tuner		= rt2400pci_link_tuner,
+	.led_brightness		= rt2400pci_led_brightness,
 	.write_tx_desc		= rt2400pci_write_tx_desc,
 	.write_tx_data		= rt2x00pci_write_tx_data,
 	.kick_tx_queue		= rt2400pci_kick_tx_queue,
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index 36c64f751b1d..bb359a14b4ae 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -243,6 +243,30 @@ static int rt2500pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 #define rt2500pci_rfkill_poll	NULL
 #endif /* CONFIG_RT2500PCI_RFKILL */
 
+#ifdef CONFIG_RT2500PCI_LEDS
+static void rt2500pci_led_brightness(struct led_classdev *led_cdev,
+				     enum led_brightness brightness)
+{
+	struct rt2x00_led *led =
+	    container_of(led_cdev, struct rt2x00_led, led_dev);
+	unsigned int enabled = brightness != LED_OFF;
+	unsigned int activity =
+	    led->rt2x00dev->led_flags & LED_SUPPORT_ACTIVITY;
+	u32 reg;
+
+	rt2x00pci_register_read(led->rt2x00dev, LEDCSR, &reg);
+
+	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC) {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, enabled);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, enabled && activity);
+	}
+
+	rt2x00pci_register_write(led->rt2x00dev, LEDCSR, reg);
+}
+#else
+#define rt2500pci_led_brightness	NULL
+#endif /* CONFIG_RT2500PCI_LEDS */
+
 /*
  * Configuration handlers.
  */
@@ -549,34 +573,6 @@ static void rt2500pci_config(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * LED functions.
- */
-static void rt2500pci_enable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u32 reg;
-
-	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
-
-	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, 70);
-	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, 30);
-	rt2x00_set_field32(&reg, LEDCSR_LINK,
-			   (rt2x00dev->led_mode != LED_MODE_ASUS));
-	rt2x00_set_field32(&reg, LEDCSR_ACTIVITY,
-			   (rt2x00dev->led_mode != LED_MODE_TXRX_ACTIVITY));
-	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
-}
-
-static void rt2500pci_disable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u32 reg;
-
-	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
-	rt2x00_set_field32(&reg, LEDCSR_LINK, 0);
-	rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 0);
-	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
-}
-
-/*
  * Link tuning
  */
 static void rt2500pci_link_stats(struct rt2x00_dev *rt2x00dev,
@@ -795,6 +791,11 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, CSR11_CW_SELECT, 0);
 	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
 
+	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
+	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, 30);
+	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
+
 	rt2x00pci_register_write(rt2x00dev, CNT3, 0);
 
 	rt2x00pci_register_read(rt2x00dev, TXCSR8, &reg);
@@ -1026,11 +1027,6 @@ static int rt2500pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2500pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
 
-	/*
-	 * Enable LED
-	 */
-	rt2500pci_enable_led(rt2x00dev);
-
 	return 0;
 }
 
@@ -1038,11 +1034,6 @@ static void rt2500pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	/*
-	 * Disable LED
-	 */
-	rt2500pci_disable_led(rt2x00dev);
-
 	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0);
 
 	/*
@@ -1445,8 +1436,24 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Store led mode, for correct led behaviour.
 	 */
-	rt2x00dev->led_mode =
-	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+#ifdef CONFIG_RT2500PCI_LEDS
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+
+	switch (value) {
+	case LED_MODE_ASUS:
+	case LED_MODE_ALPHA:
+	case LED_MODE_DEFAULT:
+		rt2x00dev->led_flags = LED_SUPPORT_RADIO;
+		break;
+	case LED_MODE_TXRX_ACTIVITY:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ACTIVITY;
+		break;
+	case LED_MODE_SIGNAL_STRENGTH:
+		rt2x00dev->led_flags = LED_SUPPORT_RADIO;
+		break;
+	}
+#endif /* CONFIG_RT2500PCI_LEDS */
 
 	/*
 	 * Detect if this device has an hardware controlled radio.
@@ -1906,6 +1913,7 @@ static const struct rt2x00lib_ops rt2500pci_rt2x00_ops = {
 	.link_stats		= rt2500pci_link_stats,
 	.reset_tuner		= rt2500pci_reset_tuner,
 	.link_tuner		= rt2500pci_link_tuner,
+	.led_brightness		= rt2500pci_led_brightness,
 	.write_tx_desc		= rt2500pci_write_tx_desc,
 	.write_tx_data		= rt2x00pci_write_tx_data,
 	.kick_tx_queue		= rt2500pci_kick_tx_queue,
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index df176cd2ef42..6352ebe8cb43 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -282,6 +282,31 @@ static const struct rt2x00debug rt2500usb_rt2x00debug = {
 };
 #endif /* CONFIG_RT2X00_LIB_DEBUGFS */
 
+#ifdef CONFIG_RT2500USB_LEDS
+static void rt2500usb_led_brightness(struct led_classdev *led_cdev,
+				     enum led_brightness brightness)
+{
+	struct rt2x00_led *led =
+	    container_of(led_cdev, struct rt2x00_led, led_dev);
+	unsigned int enabled = brightness != LED_OFF;
+	unsigned int activity =
+	    led->rt2x00dev->led_flags & LED_SUPPORT_ACTIVITY;
+	u16 reg;
+
+	rt2500usb_register_read(led->rt2x00dev, MAC_CSR20, &reg);
+
+	if (led->type == LED_TYPE_RADIO || led->type == LED_TYPE_ASSOC) {
+		rt2x00_set_field16(&reg, MAC_CSR20_LINK, enabled);
+		rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY,
+				   enabled && activity);
+	}
+
+	rt2500usb_register_write(led->rt2x00dev, MAC_CSR20, reg);
+}
+#else
+#define rt2500usb_led_brightness	NULL
+#endif /* CONFIG_RT2500USB_LEDS */
+
 /*
  * Configuration handlers.
  */
@@ -526,36 +551,6 @@ static void rt2500usb_config(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * LED functions.
- */
-static void rt2500usb_enable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u16 reg;
-
-	rt2500usb_register_read(rt2x00dev, MAC_CSR21, &reg);
-	rt2x00_set_field16(&reg, MAC_CSR21_ON_PERIOD, 70);
-	rt2x00_set_field16(&reg, MAC_CSR21_OFF_PERIOD, 30);
-	rt2500usb_register_write(rt2x00dev, MAC_CSR21, reg);
-
-	rt2500usb_register_read(rt2x00dev, MAC_CSR20, &reg);
-	rt2x00_set_field16(&reg, MAC_CSR20_LINK,
-			   (rt2x00dev->led_mode != LED_MODE_ASUS));
-	rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY,
-			   (rt2x00dev->led_mode != LED_MODE_TXRX_ACTIVITY));
-	rt2500usb_register_write(rt2x00dev, MAC_CSR20, reg);
-}
-
-static void rt2500usb_disable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u16 reg;
-
-	rt2500usb_register_read(rt2x00dev, MAC_CSR20, &reg);
-	rt2x00_set_field16(&reg, MAC_CSR20_LINK, 0);
-	rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY, 0);
-	rt2500usb_register_write(rt2x00dev, MAC_CSR20, reg);
-}
-
-/*
  * Link tuning
  */
 static void rt2500usb_link_stats(struct rt2x00_dev *rt2x00dev,
@@ -751,6 +746,11 @@ static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
 
+	rt2500usb_register_read(rt2x00dev, MAC_CSR21, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR21_ON_PERIOD, 70);
+	rt2x00_set_field16(&reg, MAC_CSR21_OFF_PERIOD, 30);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR21, reg);
+
 	rt2500usb_register_read(rt2x00dev, TXRX_CSR5, &reg);
 	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0, 13);
 	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0_VALID, 1);
@@ -924,21 +924,11 @@ static int rt2500usb_enable_radio(struct rt2x00_dev *rt2x00dev)
 		return -EIO;
 	}
 
-	/*
-	 * Enable LED
-	 */
-	rt2500usb_enable_led(rt2x00dev);
-
 	return 0;
 }
 
 static void rt2500usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
-	/*
-	 * Disable LED
-	 */
-	rt2500usb_disable_led(rt2x00dev);
-
 	rt2500usb_register_write(rt2x00dev, MAC_CSR13, 0x2121);
 	rt2500usb_register_write(rt2x00dev, MAC_CSR14, 0x2121);
 
@@ -1370,8 +1360,24 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Store led mode, for correct led behaviour.
 	 */
-	rt2x00dev->led_mode =
-	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+#ifdef CONFIG_RT2500USB_LEDS
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+
+	switch (value) {
+	case LED_MODE_ASUS:
+	case LED_MODE_ALPHA:
+	case LED_MODE_DEFAULT:
+		rt2x00dev->led_flags = LED_SUPPORT_RADIO;
+		break;
+	case LED_MODE_TXRX_ACTIVITY:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ACTIVITY;
+		break;
+	case LED_MODE_SIGNAL_STRENGTH:
+		rt2x00dev->led_flags = LED_SUPPORT_RADIO;
+		break;
+	}
+#endif /* CONFIG_RT2500USB_LEDS */
 
 	/*
 	 * Check if the BBP tuning should be disabled.
@@ -1817,6 +1823,7 @@ static const struct rt2x00lib_ops rt2500usb_rt2x00_ops = {
 	.link_stats		= rt2500usb_link_stats,
 	.reset_tuner		= rt2500usb_reset_tuner,
 	.link_tuner		= rt2500usb_link_tuner,
+	.led_brightness		= rt2500usb_led_brightness,
 	.write_tx_desc		= rt2500usb_write_tx_desc,
 	.write_tx_data		= rt2x00usb_write_tx_data,
 	.get_tx_data_len	= rt2500usb_get_tx_data_len,
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 93dbe2b286d5..76270e9c31ff 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -30,12 +30,14 @@
 #include <linux/skbuff.h>
 #include <linux/workqueue.h>
 #include <linux/firmware.h>
+#include <linux/leds.h>
 #include <linux/mutex.h>
 #include <linux/etherdevice.h>
 
 #include <net/mac80211.h>
 
 #include "rt2x00debug.h"
+#include "rt2x00leds.h"
 #include "rt2x00reg.h"
 #include "rt2x00queue.h"
 
@@ -512,6 +514,8 @@ struct rt2x00lib_ops {
 			    struct link_qual *qual);
 	void (*reset_tuner) (struct rt2x00_dev *rt2x00dev);
 	void (*link_tuner) (struct rt2x00_dev *rt2x00dev);
+	void (*led_brightness) (struct led_classdev *led_cdev,
+				enum led_brightness brightness);
 
 	/*
 	 * TX control handlers
@@ -665,6 +669,19 @@ struct rt2x00_dev {
 #endif /* CONFIG_RT2X00_LIB_DEBUGFS */
 
 	/*
+	 * LED structure for changing the LED status
+	 * by mac8011 or the kernel.
+	 */
+#ifdef CONFIG_RT2X00_LIB_LEDS
+	unsigned int led_flags;
+	struct rt2x00_trigger trigger_qual;
+	struct rt2x00_led led_radio;
+	struct rt2x00_led led_assoc;
+	struct rt2x00_led led_qual;
+	u16 led_mcu_reg;
+#endif /* CONFIG_RT2X00_LIB_LEDS */
+
+	/*
 	 * Device flags.
 	 * In these flags the current status and some
 	 * of the device capabilities are stored.
@@ -756,16 +773,6 @@ struct rt2x00_dev {
 	u16 tx_power;
 
 	/*
-	 * LED register (for rt61pci & rt73usb).
-	 */
-	u16 led_reg;
-
-	/*
-	 * Led mode (LED_MODE_*)
-	 */
-	u8 led_mode;
-
-	/*
 	 * Rssi <-> Dbm offset
 	 */
 	u8 rssi_offset;
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 2b99f22ef036..23dc566b26fd 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -379,6 +379,11 @@ static void rt2x00lib_link_tuner(struct work_struct *work)
 	rt2x00lib_precalculate_link_signal(&rt2x00dev->link.qual);
 
 	/*
+	 * Send a signal to the led to update the led signal strength.
+	 */
+	rt2x00leds_led_quality(rt2x00dev, rt2x00dev->link.qual.avg_rssi);
+
+	/*
 	 * Evaluate antenna setup, make this the last step since this could
 	 * possibly reset some statistics.
 	 */
@@ -1140,6 +1145,11 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 	}
 
 	/*
+	 * Register LED.
+	 */
+	rt2x00leds_register(rt2x00dev);
+
+	/*
 	 * Allocatie rfkill.
 	 */
 	retval = rt2x00rfkill_allocate(rt2x00dev);
@@ -1187,6 +1197,11 @@ void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 	rt2x00rfkill_free(rt2x00dev);
 
 	/*
+	 * Free LED.
+	 */
+	rt2x00leds_unregister(rt2x00dev);
+
+	/*
 	 * Free ieee80211_hw memory.
 	 */
 	rt2x00lib_remove_hw(rt2x00dev);
@@ -1227,6 +1242,7 @@ int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state)
 	 */
 	rt2x00lib_stop(rt2x00dev);
 	rt2x00lib_uninitialize(rt2x00dev);
+	rt2x00leds_suspend(rt2x00dev);
 	rt2x00debug_deregister(rt2x00dev);
 
 exit:
@@ -1270,9 +1286,10 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
 	NOTICE(rt2x00dev, "Waking up.\n");
 
 	/*
-	 * Open the debugfs entry.
+	 * Open the debugfs entry and restore led handling.
 	 */
 	rt2x00debug_register(rt2x00dev);
+	rt2x00leds_resume(rt2x00dev);
 
 	/*
 	 * Only continue if mac80211 had open interfaces.
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index ca9630c75520..2a611e4366b5 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -183,4 +183,37 @@ static inline void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev)
 }
 #endif /* CONFIG_RT2X00_LIB_RFKILL */
 
+/*
+ * LED handlers
+ */
+#ifdef CONFIG_RT2X00_LIB_LEDS
+void rt2x00leds_led_quality(struct rt2x00_dev *rt2x00dev, int rssi);
+int rt2x00leds_register(struct rt2x00_dev *rt2x00dev);
+void rt2x00leds_unregister(struct rt2x00_dev *rt2x00dev);
+void rt2x00leds_suspend(struct rt2x00_dev *rt2x00dev);
+void rt2x00leds_resume(struct rt2x00_dev *rt2x00dev);
+#else
+static inline void rt2x00leds_led_quality(struct rt2x00_dev *rt2x00dev,
+					  int rssi)
+{
+}
+
+static inline int rt2x00leds_register(struct rt2x00_dev *rt2x00dev)
+{
+	return 0;
+}
+
+static inline void rt2x00leds_unregister(struct rt2x00_dev *rt2x00dev)
+{
+}
+
+static inline void rt2x00leds_suspend(struct rt2x00_dev *rt2x00dev)
+{
+}
+
+static inline void rt2x00leds_resume(struct rt2x00_dev *rt2x00dev)
+{
+}
+#endif /* CONFIG_RT2X00_LIB_LEDS */
+
 #endif /* RT2X00LIB_H */
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 1dd30510ed1e..a049ff62e791 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -155,6 +155,12 @@ rf_write:
 	rt2x00_rf_write(rt2x00dev, word, value);
 }
 
+#ifdef CONFIG_RT61PCI_LEDS
+/*
+ * This function is only called from rt61pci_led_brightness()
+ * make gcc happy by placing this function inside the
+ * same ifdef statement as the caller.
+ */
 static void rt61pci_mcu_request(struct rt2x00_dev *rt2x00dev,
 				const u8 command, const u8 token,
 				const u8 arg0, const u8 arg1)
@@ -181,6 +187,7 @@ static void rt61pci_mcu_request(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&reg, HOST_CMD_CSR_INTERRUPT_MCU, 1);
 	rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, reg);
 }
+#endif /* CONFIG_RT61PCI_LEDS */
 
 static void rt61pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
 {
@@ -268,6 +275,48 @@ static int rt61pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
 #define rt61pci_rfkill_poll	NULL
 #endif /* CONFIG_RT61PCI_RFKILL */
 
+#ifdef CONFIG_RT61PCI_LEDS
+static void rt61pci_led_brightness(struct led_classdev *led_cdev,
+				   enum led_brightness brightness)
+{
+	struct rt2x00_led *led =
+	    container_of(led_cdev, struct rt2x00_led, led_dev);
+	unsigned int enabled = brightness != LED_OFF;
+	unsigned int a_mode =
+	    (enabled && led->rt2x00dev->curr_band == IEEE80211_BAND_5GHZ);
+	unsigned int bg_mode =
+	    (enabled && led->rt2x00dev->curr_band == IEEE80211_BAND_2GHZ);
+
+	if (led->type == LED_TYPE_RADIO) {
+		rt2x00_set_field16(&led->rt2x00dev->led_mcu_reg,
+				   MCU_LEDCS_RADIO_STATUS, enabled);
+
+		rt61pci_mcu_request(led->rt2x00dev, MCU_LED, 0xff,
+				    (led->rt2x00dev->led_mcu_reg & 0xff),
+				    ((led->rt2x00dev->led_mcu_reg >> 8)));
+	} else if (led->type == LED_TYPE_ASSOC) {
+		rt2x00_set_field16(&led->rt2x00dev->led_mcu_reg,
+				   MCU_LEDCS_LINK_BG_STATUS, bg_mode);
+		rt2x00_set_field16(&led->rt2x00dev->led_mcu_reg,
+				   MCU_LEDCS_LINK_A_STATUS, a_mode);
+
+		rt61pci_mcu_request(led->rt2x00dev, MCU_LED, 0xff,
+				    (led->rt2x00dev->led_mcu_reg & 0xff),
+				    ((led->rt2x00dev->led_mcu_reg >> 8)));
+	} else if (led->type == LED_TYPE_QUALITY) {
+		/*
+		 * The brightness is divided into 6 levels (0 - 5),
+		 * this means we need to convert the brightness
+		 * argument into the matching level within that range.
+		 */
+		rt61pci_mcu_request(led->rt2x00dev, MCU_LED_STRENGTH, 0xff,
+				    brightness / (LED_FULL / 6), 0);
+	}
+}
+#else
+#define rt61pci_led_brightness	NULL
+#endif /* CONFIG_RT61PCI_LEDS */
+
 /*
  * Configuration handlers.
  */
@@ -682,78 +731,6 @@ static void rt61pci_config(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * LED functions.
- */
-static void rt61pci_enable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u32 reg;
-	u8 arg0;
-	u8 arg1;
-
-	rt2x00pci_register_read(rt2x00dev, MAC_CSR14, &reg);
-	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, 70);
-	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, 30);
-	rt2x00pci_register_write(rt2x00dev, MAC_CSR14, reg);
-
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_RADIO_STATUS, 1);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_A_STATUS,
-			   rt2x00dev->rx_status.band == IEEE80211_BAND_5GHZ);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_BG_STATUS,
-			   rt2x00dev->rx_status.band != IEEE80211_BAND_5GHZ);
-
-	arg0 = rt2x00dev->led_reg & 0xff;
-	arg1 = (rt2x00dev->led_reg >> 8) & 0xff;
-
-	rt61pci_mcu_request(rt2x00dev, MCU_LED, 0xff, arg0, arg1);
-}
-
-static void rt61pci_disable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u16 led_reg;
-	u8 arg0;
-	u8 arg1;
-
-	led_reg = rt2x00dev->led_reg;
-	rt2x00_set_field16(&led_reg, MCU_LEDCS_RADIO_STATUS, 0);
-	rt2x00_set_field16(&led_reg, MCU_LEDCS_LINK_BG_STATUS, 0);
-	rt2x00_set_field16(&led_reg, MCU_LEDCS_LINK_A_STATUS, 0);
-
-	arg0 = led_reg & 0xff;
-	arg1 = (led_reg >> 8) & 0xff;
-
-	rt61pci_mcu_request(rt2x00dev, MCU_LED, 0xff, arg0, arg1);
-}
-
-static void rt61pci_activity_led(struct rt2x00_dev *rt2x00dev, int rssi)
-{
-	u8 led;
-
-	if (rt2x00dev->led_mode != LED_MODE_SIGNAL_STRENGTH)
-		return;
-
-	/*
-	 * Led handling requires a positive value for the rssi,
-	 * to do that correctly we need to add the correction.
-	 */
-	rssi += rt2x00dev->rssi_offset;
-
-	if (rssi <= 30)
-		led = 0;
-	else if (rssi <= 39)
-		led = 1;
-	else if (rssi <= 49)
-		led = 2;
-	else if (rssi <= 53)
-		led = 3;
-	else if (rssi <= 63)
-		led = 4;
-	else
-		led = 5;
-
-	rt61pci_mcu_request(rt2x00dev, MCU_LED_STRENGTH, 0xff, led, 0);
-}
-
-/*
  * Link tuning
  */
 static void rt61pci_link_stats(struct rt2x00_dev *rt2x00dev,
@@ -787,11 +764,6 @@ static void rt61pci_link_tuner(struct rt2x00_dev *rt2x00dev)
 	u8 up_bound;
 	u8 low_bound;
 
-	/*
-	 * Update Led strength
-	 */
-	rt61pci_activity_led(rt2x00dev, rssi);
-
 	rt61pci_bbp_read(rt2x00dev, 17, &r17);
 
 	/*
@@ -1192,6 +1164,11 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00pci_register_write(rt2x00dev, MAC_CSR13, 0x0000e000);
 
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR14, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, 30);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR14, reg);
+
 	/*
 	 * Invalidate all Shared Keys (SEC_CSR0),
 	 * and clear the Shared key Cipher algorithms (SEC_CSR1 & SEC_CSR5)
@@ -1403,11 +1380,6 @@ static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, RX_CNTL_CSR_ENABLE_RX_DMA, 1);
 	rt2x00pci_register_write(rt2x00dev, RX_CNTL_CSR, reg);
 
-	/*
-	 * Enable LED
-	 */
-	rt61pci_enable_led(rt2x00dev);
-
 	return 0;
 }
 
@@ -1415,11 +1387,6 @@ static void rt61pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
 	u32 reg;
 
-	/*
-	 * Disable LED
-	 */
-	rt61pci_disable_led(rt2x00dev);
-
 	rt2x00pci_register_write(rt2x00dev, MAC_CSR10, 0x00001818);
 
 	/*
@@ -2048,35 +2015,51 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * If the eeprom value is invalid,
 	 * switch to default led mode.
 	 */
+#ifdef CONFIG_RT61PCI_LEDS
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
 
-	rt2x00dev->led_mode = rt2x00_get_field16(eeprom, EEPROM_LED_LED_MODE);
+	value = rt2x00_get_field16(eeprom, EEPROM_LED_LED_MODE);
+
+	switch (value) {
+	case LED_MODE_TXRX_ACTIVITY:
+	case LED_MODE_ASUS:
+	case LED_MODE_ALPHA:
+	case LED_MODE_DEFAULT:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ASSOC;
+		break;
+	case LED_MODE_SIGNAL_STRENGTH:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ASSOC |
+		    LED_SUPPORT_QUALITY;
+		break;
+	}
 
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LED_MODE,
-			   rt2x00dev->led_mode);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_0,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_LED_MODE, value);
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_0,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_0));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_1,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_1,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_1));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_2,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_2,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_2));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_3,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_3,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_3));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_4,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_4,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_4));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_ACT,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_ACT,
 			   rt2x00_get_field16(eeprom, EEPROM_LED_POLARITY_ACT));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_BG,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_READY_BG,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_RDY_G));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_A,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_READY_A,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_RDY_A));
+#endif /* CONFIG_RT61PCI_LEDS */
 
 	return 0;
 }
@@ -2484,6 +2467,7 @@ static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
 	.link_stats		= rt61pci_link_stats,
 	.reset_tuner		= rt61pci_reset_tuner,
 	.link_tuner		= rt61pci_link_tuner,
+	.led_brightness		= rt61pci_led_brightness,
 	.write_tx_desc		= rt61pci_write_tx_desc,
 	.write_tx_data		= rt2x00pci_write_tx_data,
 	.kick_tx_queue		= rt61pci_kick_tx_queue,
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 9cbc879da037..ca5a9855adbd 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -278,6 +278,50 @@ static const struct rt2x00debug rt73usb_rt2x00debug = {
 };
 #endif /* CONFIG_RT2X00_LIB_DEBUGFS */
 
+#ifdef CONFIG_RT73USB_LEDS
+static void rt73usb_led_brightness(struct led_classdev *led_cdev,
+				   enum led_brightness brightness)
+{
+	struct rt2x00_led *led =
+	   container_of(led_cdev, struct rt2x00_led, led_dev);
+	unsigned int enabled = brightness != LED_OFF;
+	unsigned int a_mode =
+	    (enabled && led->rt2x00dev->curr_band == IEEE80211_BAND_5GHZ);
+	unsigned int bg_mode =
+	    (enabled && led->rt2x00dev->curr_band == IEEE80211_BAND_2GHZ);
+
+	if (led->type == LED_TYPE_RADIO) {
+		rt2x00_set_field16(&led->rt2x00dev->led_mcu_reg,
+				   MCU_LEDCS_RADIO_STATUS, enabled);
+
+		rt2x00usb_vendor_request_sw(led->rt2x00dev, USB_LED_CONTROL, 0,
+					    led->rt2x00dev->led_mcu_reg,
+					    REGISTER_TIMEOUT);
+	} else if (led->type == LED_TYPE_ASSOC) {
+		rt2x00_set_field16(&led->rt2x00dev->led_mcu_reg,
+				   MCU_LEDCS_LINK_BG_STATUS, bg_mode);
+		rt2x00_set_field16(&led->rt2x00dev->led_mcu_reg,
+				   MCU_LEDCS_LINK_A_STATUS, a_mode);
+
+		rt2x00usb_vendor_request_sw(led->rt2x00dev, USB_LED_CONTROL, 0,
+					    led->rt2x00dev->led_mcu_reg,
+					    REGISTER_TIMEOUT);
+	} else if (led->type == LED_TYPE_QUALITY) {
+		/*
+		 * The brightness is divided into 6 levels (0 - 5),
+		 * this means we need to convert the brightness
+		 * argument into the matching level within that range.
+		 */
+		rt2x00usb_vendor_request_sw(led->rt2x00dev, USB_LED_CONTROL,
+					    brightness / (LED_FULL / 6),
+					    led->rt2x00dev->led_mcu_reg,
+					    REGISTER_TIMEOUT);
+	}
+}
+#else
+#define rt73usb_led_brightness	NULL
+#endif /* CONFIG_RT73USB_LEDS */
+
 /*
  * Configuration handlers.
  */
@@ -630,68 +674,6 @@ static void rt73usb_config(struct rt2x00_dev *rt2x00dev,
 }
 
 /*
- * LED functions.
- */
-static void rt73usb_enable_led(struct rt2x00_dev *rt2x00dev)
-{
-	u32 reg;
-
-	rt73usb_register_read(rt2x00dev, MAC_CSR14, &reg);
-	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, 70);
-	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, 30);
-	rt73usb_register_write(rt2x00dev, MAC_CSR14, reg);
-
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_RADIO_STATUS, 1);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_A_STATUS,
-			   (rt2x00dev->rx_status.band == IEEE80211_BAND_5GHZ));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_BG_STATUS,
-			   (rt2x00dev->rx_status.band != IEEE80211_BAND_5GHZ));
-
-	rt2x00usb_vendor_request_sw(rt2x00dev, USB_LED_CONTROL, 0x0000,
-				    rt2x00dev->led_reg, REGISTER_TIMEOUT);
-}
-
-static void rt73usb_disable_led(struct rt2x00_dev *rt2x00dev)
-{
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_RADIO_STATUS, 0);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_BG_STATUS, 0);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_A_STATUS, 0);
-
-	rt2x00usb_vendor_request_sw(rt2x00dev, USB_LED_CONTROL, 0x0000,
-				    rt2x00dev->led_reg, REGISTER_TIMEOUT);
-}
-
-static void rt73usb_activity_led(struct rt2x00_dev *rt2x00dev, int rssi)
-{
-	u32 led;
-
-	if (rt2x00dev->led_mode != LED_MODE_SIGNAL_STRENGTH)
-		return;
-
-	/*
-	 * Led handling requires a positive value for the rssi,
-	 * to do that correctly we need to add the correction.
-	 */
-	rssi += rt2x00dev->rssi_offset;
-
-	if (rssi <= 30)
-		led = 0;
-	else if (rssi <= 39)
-		led = 1;
-	else if (rssi <= 49)
-		led = 2;
-	else if (rssi <= 53)
-		led = 3;
-	else if (rssi <= 63)
-		led = 4;
-	else
-		led = 5;
-
-	rt2x00usb_vendor_request_sw(rt2x00dev, USB_LED_CONTROL, led,
-				    rt2x00dev->led_reg, REGISTER_TIMEOUT);
-}
-
-/*
  * Link tuning
  */
 static void rt73usb_link_stats(struct rt2x00_dev *rt2x00dev,
@@ -725,11 +707,6 @@ static void rt73usb_link_tuner(struct rt2x00_dev *rt2x00dev)
 	u8 up_bound;
 	u8 low_bound;
 
-	/*
-	 * Update Led strength
-	 */
-	rt73usb_activity_led(rt2x00dev, rssi);
-
 	rt73usb_bbp_read(rt2x00dev, 17, &r17);
 
 	/*
@@ -914,8 +891,6 @@ static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
 		return status;
 	}
 
-	rt73usb_disable_led(rt2x00dev);
-
 	return 0;
 }
 
@@ -993,6 +968,11 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 
 	rt73usb_register_write(rt2x00dev, MAC_CSR13, 0x00007f00);
 
+	rt73usb_register_read(rt2x00dev, MAC_CSR14, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, 30);
+	rt73usb_register_write(rt2x00dev, MAC_CSR14, reg);
+
 	/*
 	 * Invalidate all Shared Keys (SEC_CSR0),
 	 * and clear the Shared key Cipher algorithms (SEC_CSR1 & SEC_CSR5)
@@ -1152,21 +1132,11 @@ static int rt73usb_enable_radio(struct rt2x00_dev *rt2x00dev)
 		return -EIO;
 	}
 
-	/*
-	 * Enable LED
-	 */
-	rt73usb_enable_led(rt2x00dev);
-
 	return 0;
 }
 
 static void rt73usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
-	/*
-	 * Disable LED
-	 */
-	rt73usb_disable_led(rt2x00dev);
-
 	rt73usb_register_write(rt2x00dev, MAC_CSR10, 0x00001818);
 
 	/*
@@ -1615,33 +1585,49 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Store led settings, for correct led behaviour.
 	 */
+#ifdef CONFIG_RT73USB_LEDS
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
 
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LED_MODE,
-			   rt2x00dev->led_mode);
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_0,
+	switch (value) {
+	case LED_MODE_TXRX_ACTIVITY:
+	case LED_MODE_ASUS:
+	case LED_MODE_ALPHA:
+	case LED_MODE_DEFAULT:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ASSOC;
+		break;
+	case LED_MODE_SIGNAL_STRENGTH:
+		rt2x00dev->led_flags =
+		    LED_SUPPORT_RADIO | LED_SUPPORT_ASSOC |
+		    LED_SUPPORT_QUALITY;
+		break;
+	}
+
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_LED_MODE, value);
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_0,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_0));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_1,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_1,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_1));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_2,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_2,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_2));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_3,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_3,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_3));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_4,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_4,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_GPIO_4));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_ACT,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_ACT,
 			   rt2x00_get_field16(eeprom, EEPROM_LED_POLARITY_ACT));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_BG,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_READY_BG,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_RDY_G));
-	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_A,
+	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_READY_A,
 			   rt2x00_get_field16(eeprom,
 					      EEPROM_LED_POLARITY_RDY_A));
+#endif /* CONFIG_RT73USB_LEDS */
 
 	return 0;
 }
@@ -2084,6 +2070,7 @@ static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
 	.link_stats		= rt73usb_link_stats,
 	.reset_tuner		= rt73usb_reset_tuner,
 	.link_tuner		= rt73usb_link_tuner,
+	.led_brightness		= rt73usb_led_brightness,
 	.write_tx_desc		= rt73usb_write_tx_desc,
 	.write_tx_data		= rt2x00usb_write_tx_data,
 	.get_tx_data_len	= rt73usb_get_tx_data_len,