V4L/DVB (13394): pt1: Support FE_READ_SNR

Signed-off-by: HIRANO Takahito <hiranotaka@zng.info>
Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

1 files changed, 47 insertions, 0 deletions

diff --git a/drivers/media/dvb/pt1/va1j5jf8007t.c b/drivers/media/dvb/pt1/va1j5jf8007t.c
index 71117f4ca7e6..fe897343f4f8 100644
--- a/drivers/media/dvb/pt1/va1j5jf8007t.c
+++ b/drivers/media/dvb/pt1/va1j5jf8007t.c
@@ -46,6 +46,52 @@ struct va1j5jf8007t_state {
         enum va1j5jf8007t_tune_state tune_state;
 };
 
+static int va1j5jf8007t_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+        struct va1j5jf8007t_state *state;
+        u8 addr;
+        int i;
+        u8 write_buf[1], read_buf[1];
+        struct i2c_msg msgs[2];
+        s32 word, x, y;
+
+        state = fe->demodulator_priv;
+        addr = state->config->demod_address;
+
+        word = 0;
+        for (i = 0; i < 3; i++) {
+                write_buf[0] = 0x8b + i;
+
+                msgs[0].addr = addr;
+                msgs[0].flags = 0;
+                msgs[0].len = sizeof(write_buf);
+                msgs[0].buf = write_buf;
+
+                msgs[1].addr = addr;
+                msgs[1].flags = I2C_M_RD;
+                msgs[1].len = sizeof(read_buf);
+                msgs[1].buf = read_buf;
+
+                if (i2c_transfer(state->adap, msgs, 2) != 2)
+                        return -EREMOTEIO;
+
+                word <<= 8;
+                word |= read_buf[0];
+        }
+
+        if (!word)
+                return -EIO;
+
+        x = 10 * (intlog10(0x540000 * 100 / word) - (2 << 24));
+        y = (24ll << 46) / 1000000;
+        y = ((s64)y * x >> 30) - (16ll << 40) / 10000;
+        y = ((s64)y * x >> 29) + (398ll << 35) / 10000;
+        y = ((s64)y * x >> 30) + (5491ll << 29) / 10000;
+        y = ((s64)y * x >> 30) + (30965ll << 23) / 10000;
+        *snr = y >> 15;
+        return 0;
+}
+
 static int va1j5jf8007t_get_frontend_algo(struct dvb_frontend *fe)
 {
         return DVBFE_ALGO_HW;
@@ -393,6 +439,7 @@ static struct dvb_frontend_ops va1j5jf8007t_ops = {
                         FE_CAN_GUARD_INTERVAL_AUTO | FE_CAN_HIERARCHY_AUTO,
         },
 
+        .read_snr = va1j5jf8007t_read_snr,
         .get_frontend_algo = va1j5jf8007t_get_frontend_algo,
         .read_status = va1j5jf8007t_read_status,
         .tune = va1j5jf8007t_tune,