1 files changed, 133 insertions, 0 deletions

diff --git a/drivers/media/dvb/frontends/au8522.c b/drivers/media/dvb/frontends/au8522.c
index 0b82cc2a1e16..eabf9a68e7ec 100644
--- a/drivers/media/dvb/frontends/au8522.c
+++ b/drivers/media/dvb/frontends/au8522.c
@@ -40,6 +40,8 @@ struct au8522_state {
         u32 current_frequency;
         fe_modulation_t current_modulation;
 
+        u32 fe_status;
+        unsigned int led_state;
 };
 
 static int debug;
@@ -538,11 +540,98 @@ static int au8522_init(struct dvb_frontend *fe)
         return 0;
 }
 
+static int au8522_led_gpio_enable(struct au8522_state *state, int onoff)
+{
+        struct au8522_led_config *led_config = state->config->led_cfg;
+        u8 val;
+
+        /* bail out if we cant control an LED */
+        if (!led_config || !led_config->gpio_output ||
+            !led_config->gpio_output_enable || !led_config->gpio_output_disable)
+                return 0;
+
+        val = au8522_readreg(state, 0x4000 |
+                             (led_config->gpio_output & ~0xc000));
+        if (onoff) {
+                /* enable GPIO output */
+                val &= ~((led_config->gpio_output_enable >> 8) & 0xff);
+                val |=  (led_config->gpio_output_enable & 0xff);
+        } else {
+                /* disable GPIO output */
+                val &= ~((led_config->gpio_output_disable >> 8) & 0xff);
+                val |=  (led_config->gpio_output_disable & 0xff);
+        }
+        return au8522_writereg(state, 0x8000 |
+                               (led_config->gpio_output & ~0xc000), val);
+}
+
+/* led = 0 | off
+ * led = 1 | signal ok
+ * led = 2 | signal strong
+ * led < 0 | only light led if leds are currently off
+ */
+static int au8522_led_ctrl(struct au8522_state *state, int led)
+{
+        struct au8522_led_config *led_config = state->config->led_cfg;
+        int i, ret = 0;
+
+        /* bail out if we cant control an LED */
+        if (!led_config || !led_config->gpio_leds ||
+            !led_config->num_led_states || !led_config->led_states)
+                return 0;
+
+        if (led < 0) {
+                /* if LED is already lit, then leave it as-is */
+                if (state->led_state)
+                        return 0;
+                else
+                        led *= -1;
+        }
+
+        /* toggle LED if changing state */
+        if (state->led_state != led) {
+                u8 val;
+
+                dprintk("%s: %d\n", __func__, led);
+
+                au8522_led_gpio_enable(state, 1);
+
+                val = au8522_readreg(state, 0x4000 |
+                                     (led_config->gpio_leds & ~0xc000));
+
+                /* start with all leds off */
+                for (i = 0; i < led_config->num_led_states; i++)
+                        val &= ~led_config->led_states[i];
+
+                /* set selected LED state */
+                if (led < led_config->num_led_states)
+                        val |= led_config->led_states[led];
+                else if (led_config->num_led_states)
+                        val |=
+                        led_config->led_states[led_config->num_led_states - 1];
+
+                ret = au8522_writereg(state, 0x8000 |
+                                      (led_config->gpio_leds & ~0xc000), val);
+                if (ret < 0)
+                        return ret;
+
+                state->led_state = led;
+
+                if (led == 0)
+                        au8522_led_gpio_enable(state, 0);
+        }
+
+        return 0;
+}
+
 static int au8522_sleep(struct dvb_frontend *fe)
 {
         struct au8522_state *state = fe->demodulator_priv;
         dprintk("%s()\n", __func__);
 
+        /* turn off led */
+        au8522_led_ctrl(state, 0);
+
         state->current_frequency = 0;
 
         return 0;
@@ -592,12 +681,53 @@ static int au8522_read_status(struct dvb_frontend *fe, fe_status_t *status)
                         *status |= FE_HAS_CARRIER | FE_HAS_SIGNAL;
                 break;
         }
+        state->fe_status = *status;
+
+        if (*status & FE_HAS_LOCK)
+                /* turn on LED, if it isn't on already */
+                au8522_led_ctrl(state, -1);
+        else
+                /* turn off LED */
+                au8522_led_ctrl(state, 0);
 
         dprintk("%s() status 0x%08x\n", __func__, *status);
 
         return 0;
 }
 
+static int au8522_led_status(struct au8522_state *state, const u16 *snr)
+{
+        struct au8522_led_config *led_config = state->config->led_cfg;
+        int led;
+        u16 strong;
+
+        /* bail out if we cant control an LED */
+        if (!led_config)
+                return 0;
+
+        if (0 == (state->fe_status & FE_HAS_LOCK))
+                return au8522_led_ctrl(state, 0);
+        else if (state->current_modulation == QAM_256)
+                strong = led_config->qam256_strong;
+        else if (state->current_modulation == QAM_64)
+                strong = led_config->qam64_strong;
+        else /* (state->current_modulation == VSB_8) */
+                strong = led_config->vsb8_strong;
+
+        if (*snr >= strong)
+                led = 2;
+        else
+                led = 1;
+
+        if ((state->led_state) &&
+            (((strong < *snr) ? (*snr - strong) : (strong - *snr)) <= 10))
+                /* snr didn't change enough to bother
+                 * changing the color of the led */
+                return 0;
+
+        return au8522_led_ctrl(state, led);
+}
+
 static int au8522_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
         struct au8522_state *state = fe->demodulator_priv;
@@ -621,6 +751,9 @@ static int au8522_read_snr(struct dvb_frontend *fe, u16 *snr)
                                             au8522_readreg(state, 0x4311),
                                             snr);
 
+        if (state->config->led_cfg)
+                au8522_led_status(state, snr);
+
         return ret;
 }