1 files changed, 135 insertions, 3 deletions

diff --git a/drivers/net/wireless/ath9k/main.c b/drivers/net/wireless/ath9k/main.c
index dca00c3a985a..996bb9c6540d 100644
--- a/drivers/net/wireless/ath9k/main.c
+++ b/drivers/net/wireless/ath9k/main.c
@@ -1172,12 +1172,130 @@ enum ath9k_ht_macmode ath_cwm_macmode(struct ath_softc *sc)
         return sc->sc_ht_info.tx_chan_width;
 }
 
+/********************************/
+/*       LED functions          */
+/********************************/
+
+static void ath_led_brightness(struct led_classdev *led_cdev,
+                               enum led_brightness brightness)
+{
+        struct ath_led *led = container_of(led_cdev, struct ath_led, led_cdev);
+        struct ath_softc *sc = led->sc;
+
+        switch (brightness) {
+        case LED_OFF:
+                if (led->led_type == ATH_LED_ASSOC ||
+                    led->led_type == ATH_LED_RADIO)
+                        sc->sc_flags &= ~SC_OP_LED_ASSOCIATED;
+                ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN,
+                                (led->led_type == ATH_LED_RADIO) ? 1 :
+                                !!(sc->sc_flags & SC_OP_LED_ASSOCIATED));
+                break;
+        case LED_FULL:
+                if (led->led_type == ATH_LED_ASSOC)
+                        sc->sc_flags |= SC_OP_LED_ASSOCIATED;
+                ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 0);
+                break;
+        default:
+                break;
+        }
+}
+
+static int ath_register_led(struct ath_softc *sc, struct ath_led *led,
+                            char *trigger)
+{
+        int ret;
+
+        led->sc = sc;
+        led->led_cdev.name = led->name;
+        led->led_cdev.default_trigger = trigger;
+        led->led_cdev.brightness_set = ath_led_brightness;
+
+        ret = led_classdev_register(wiphy_dev(sc->hw->wiphy), &led->led_cdev);
+        if (ret)
+                DPRINTF(sc, ATH_DBG_FATAL,
+                        "Failed to register led:%s", led->name);
+        else
+                led->registered = 1;
+        return ret;
+}
+
+static void ath_unregister_led(struct ath_led *led)
+{
+        if (led->registered) {
+                led_classdev_unregister(&led->led_cdev);
+                led->registered = 0;
+        }
+}
+
+static void ath_deinit_leds(struct ath_softc *sc)
+{
+        ath_unregister_led(&sc->assoc_led);
+        sc->sc_flags &= ~SC_OP_LED_ASSOCIATED;
+        ath_unregister_led(&sc->tx_led);
+        ath_unregister_led(&sc->rx_led);
+        ath_unregister_led(&sc->radio_led);
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
+}
+
+static void ath_init_leds(struct ath_softc *sc)
+{
+        char *trigger;
+        int ret;
+
+        /* Configure gpio 1 for output */
+        ath9k_hw_cfg_output(sc->sc_ah, ATH_LED_PIN,
+                            AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
+        /* LED off, active low */
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
+
+        trigger = ieee80211_get_radio_led_name(sc->hw);
+        snprintf(sc->radio_led.name, sizeof(sc->radio_led.name),
+                "ath9k-%s:radio", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->radio_led, trigger);
+        sc->radio_led.led_type = ATH_LED_RADIO;
+        if (ret)
+                goto fail;
+
+        trigger = ieee80211_get_assoc_led_name(sc->hw);
+        snprintf(sc->assoc_led.name, sizeof(sc->assoc_led.name),
+                "ath9k-%s:assoc", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->assoc_led, trigger);
+        sc->assoc_led.led_type = ATH_LED_ASSOC;
+        if (ret)
+                goto fail;
+
+        trigger = ieee80211_get_tx_led_name(sc->hw);
+        snprintf(sc->tx_led.name, sizeof(sc->tx_led.name),
+                "ath9k-%s:tx", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->tx_led, trigger);
+        sc->tx_led.led_type = ATH_LED_TX;
+        if (ret)
+                goto fail;
+
+        trigger = ieee80211_get_rx_led_name(sc->hw);
+        snprintf(sc->rx_led.name, sizeof(sc->rx_led.name),
+                "ath9k-%s:rx", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->rx_led, trigger);
+        sc->rx_led.led_type = ATH_LED_RX;
+        if (ret)
+                goto fail;
+
+        return;
+
+fail:
+        ath_deinit_leds(sc);
+}
+
 static int ath_detach(struct ath_softc *sc)
 {
         struct ieee80211_hw *hw = sc->hw;
 
         DPRINTF(sc, ATH_DBG_CONFIG, "%s: Detach ATH hw\n", __func__);
 
+        /* Deinit LED control */
+        ath_deinit_leds(sc);
+
         /* Unregister hw */
 
         ieee80211_unregister_hw(hw);
@@ -1271,18 +1389,21 @@ static int ath_attach(u16 devid,
                 goto bad;
         }
 
+        /* Initialize LED control */
+        ath_init_leds(sc);
+
         /* initialize tx/rx engine */
 
         error = ath_tx_init(sc, ATH_TXBUF);
         if (error != 0)
-                goto bad1;
+                goto detach;
 
         error = ath_rx_init(sc, ATH_RXBUF);
         if (error != 0)
-                goto bad1;
+                goto detach;
 
         return 0;
-bad1:
+detach:
         ath_detach(sc);
 bad:
         return error;
@@ -1427,6 +1548,10 @@ static void ath_pci_remove(struct pci_dev *pdev)
 
 static int ath_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 {
+        struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+        struct ath_softc *sc = hw->priv;
+
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
         pci_save_state(pdev);
         pci_disable_device(pdev);
         pci_set_power_state(pdev, 3);
@@ -1436,6 +1561,8 @@ static int ath_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 
 static int ath_pci_resume(struct pci_dev *pdev)
 {
+        struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+        struct ath_softc *sc = hw->priv;
         u32 val;
         int err;
 
@@ -1452,6 +1579,11 @@ static int ath_pci_resume(struct pci_dev *pdev)
         if ((val & 0x0000ff00) != 0)
                 pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
 
+        /* Enable LED */
+        ath9k_hw_cfg_output(sc->sc_ah, ATH_LED_PIN,
+                            AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
+
         return 0;
 }