1 files changed, 30 insertions, 0 deletions

diff --git a/firmware/drivers/tuner/stfm1000.c b/firmware/drivers/tuner/stfm1000.c
index ae96993285..8626d4e2a5 100644
--- a/firmware/drivers/tuner/stfm1000.c
+++ b/firmware/drivers/tuner/stfm1000.c
@@ -31,6 +31,36 @@
 #include "fmradio_i2c.h" /* physical interface driver */
 #include "stfm1000.h"
 
+#define STFM100_I2C_ADDR    0xc0
+
+#define CHIPID  0x80
+
+static int stfm1000_read_reg(uint8_t reg, uint32_t *val)
+{
+    uint8_t buf[4];
+    buf[0] = reg;
+    int ret = fmradio_i2c_write(STFM100_I2C_ADDR, buf, 1);
+    if(ret < 0) return ret;
+    ret = fmradio_i2c_read(STFM100_I2C_ADDR, buf, 4);
+    *val = buf[0] | buf[1] << 8 | buf[2] << 16 | buf[3] << 24;
+    return ret;
+}
+
+static int stfm1000_write_reg(uint8_t reg, uint32_t val)
+{
+    uint8_t buf[5];
+    buf[0] = reg;
+    buf[1] = val & 0xff; buf[2] = (val >> 8) & 0xff;
+    buf[3] = (val >> 16) & 0xff; buf[4] = (val >> 24) & 0xff;
+    return fmradio_i2c_write(STFM100_I2C_ADDR, buf, 5);
+}
+
+void stfm1000_dbg_info(struct stfm1000_dbg_info *nfo)
+{
+    memset(nfo, 0, sizeof(struct stfm1000_dbg_info));
+    stfm1000_read_reg(CHIPID, &nfo->chipid);
+}
+
 void stfm1000_init(void)
 {
 }