iio: imu: nvi v.326 DMP configuration

- Add device tree ICM DMP sensor configuration allowing which sensors enable
  the ICM DMP.
- Add this configuration ability at runtime.

Bug 200189040

Change-Id: Ifb3c4e6cd2535b4271d1bc1cb931876b62f5486b
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/1151862
(cherry picked from commit 273dfcf8424192cfcced79a6e14b0b7a3230b0fe)
Reviewed-on: http://git-master/r/1165231
GVS: Gerrit_Virtual_Submit
Tested-by: Robert Collins <rcollins@nvidia.com>
Reviewed-by: Robert Collins <rcollins@nvidia.com>

diff --git a/drivers/iio/imu/nvi_mpu/nvi.c b/drivers/iio/imu/nvi_mpu/nvi.c
index 5ec06b538edd6c63154ea3d94a4ac28574a240ea..d8f2def23f9e686cc0533be1992b00041c2df286 100644 (file)
--- a/drivers/iio/imu/nvi_mpu/nvi.c
+++ b/drivers/iio/imu/nvi_mpu/nvi.c
@@ -29,7 +29,7 @@
 
 #include "nvi.h"
 
-#define NVI_DRIVER_VERSION             (325)
+#define NVI_DRIVER_VERSION             (326)
 #define NVI_VENDOR                     "Invensense"
 #define NVI_NAME                       "mpu6xxx"
 #define NVI_NAME_MPU6050               "mpu6050"
@@ -105,7 +105,8 @@ enum NVI_INFO {
        NVI_INFO_REG_WR = 0xC6, /* use 0xD0 on cmd line */
        NVI_INFO_MEM_RD,
        NVI_INFO_MEM_WR,
-       NVI_INFO_DMP_FW
+       NVI_INFO_DMP_FW,
+       NVI_INFO_DMP_EN_MSK
 };
 
 /* regulator names in order of powering on */
@@ -1439,7 +1440,7 @@ static int nvi_en(struct nvi_state *st)
                } else {
                        /* batch disabled - test if a DMP sensor is enabled */
                        for (i = 0; i < DEV_N_AUX; i++) {
-                               if (st->hal->dmp->en_msk & (1 << i)) {
+                               if (st->dmp_en_msk & (1 << i)) {
                                        if (st->snsr[i].enable) {
                                                dmp_en = true;
                                                break;
@@ -2890,7 +2891,7 @@ static int nvi_rd(struct nvi_state *st)
                        if (val & (1 << st->hal->bit->dmp_int_stp))
                                nvi_push_event(st, DEV_STP);
                }
-               if (st->en_msk & st->hal->dmp->en_msk)
+               if (st->en_msk & st->dmp_en_msk)
                        /* nvi_dmp_rd */
                        return nvi_fifo_rd(st, -1, 0, st->hal->dmp->fn_rd);
 
@@ -3026,7 +3027,7 @@ static int nvi_enable(void *client, int snsr_id, int enable)
 
        if (st->en_msk & (1 << DEV_DMP)) {
                /* DMP is currently on */
-               if (!(st->en_msk & st->hal->dmp->en_msk))
+               if (!(st->en_msk & st->dmp_en_msk))
                        /* DMP may get turned off (may stay on due to batch) so
                         * we update timings that may have changed while DMP
                         * was on.
@@ -3266,6 +3267,7 @@ static int nvi_nvs_write(void *client, int snsr_id, unsigned int nvs)
        case NVI_INFO_MEM_RD:
        case NVI_INFO_MEM_WR:
        case NVI_INFO_DMP_FW:
+       case NVI_INFO_DMP_EN_MSK:
                break;
 
        case NVI_INFO_DBG_SPEW:
@@ -3312,22 +3314,35 @@ static int nvi_nvs_read(void *client, int snsr_id, char *buf)
        switch (info & 0xFF) {
        case NVI_INFO_VER:
                t = sprintf(buf, "NVI driver v. %u\n", NVI_DRIVER_VERSION);
+               if (st->en_msk & (1 << FW_LOADED)) {
+                       t += sprintf(buf + t, "DMP FW v. %u\n",
+                                    st->hal->dmp->fw_ver);
+                       t += sprintf(buf + t, "DMP enabled=%u\n",
+                                    !!(st->en_msk & (1 << DEV_DMP)));
+               }
                t += sprintf(buf + t, "standby_en=%x\n",
                             !!(st->en_msk & (1 << EN_STDBY)));
                t += sprintf(buf + t, "bypass_timeout_ms=%u\n",
                             st->bypass_timeout_ms);
-               for (i = 0; i < DEV_MPU_N; i++) {
+               for (i = 0; i < DEV_N_AUX; i++) {
                        if (st->snsr[i].push_delay_ns)
                                t += sprintf(buf + t,
                                             "%s_push_delay_ns=%lld\n",
                                             st->snsr[i].cfg.name,
                                             st->snsr[i].push_delay_ns);
                }
-               if (st->en_msk & (1 << FW_LOADED))
-                       t += sprintf(buf + t, "DMP FW v. %u\n",
-                                    st->hal->dmp->fw_ver);
-                       t += sprintf(buf + t, "DMP enabled=%u\n",
-                                    !!(st->en_msk & (1 << DEV_DMP)));
+
+               for (i = 0; i < DEV_N_AUX; i++) {
+                       if ((st->dmp_dev_msk | MSK_DEV_MPU_AUX) & (1 << i)) {
+                               if (st->dmp_en_msk & (1 << i))
+                                       t += sprintf(buf + t, "%s_dmp_en=1\n",
+                                                    st->snsr[i].cfg.name);
+                               else
+                                       t += sprintf(buf + t, "%s_dmp_en=0\n",
+                                                    st->snsr[i].cfg.name);
+                       }
+               }
+
                return t;
 
        case NVI_INFO_DBG:
@@ -3448,6 +3463,10 @@ static int nvi_nvs_read(void *client, int snsr_id, char *buf)
                ret = nvi_dmp_fw(st);
                return sprintf(buf, "DMP FW: ERR=%d\n", ret);
 
+       case NVI_INFO_DMP_EN_MSK:
+               st->dmp_en_msk = (info >> 8) & MSK_DEV_ALL;
+               return sprintf(buf, "st->dmp_en_msk=%x\n", st->dmp_en_msk);
+
        default:
                i = info - NVI_INFO_SNSR_SPEW;
                if (i < DEV_N)
@@ -3834,7 +3853,8 @@ static struct sensor_cfg nvi_cfg_dflt[] = {
        },
 };
 
-static int nvi_of_dt(struct nvi_state *st, struct device_node *dn)
+/* device tree parameters before HAL initialized */
+static int nvi_of_dt_pre(struct nvi_state *st, struct device_node *dn)
 {
        u32 tmp;
        char str[64];
@@ -3857,7 +3877,7 @@ static int nvi_of_dt(struct nvi_state *st, struct device_node *dn)
                        st->en_msk &= ~(1 << EN_STDBY);
        }
        of_property_read_u32(dn, "bypass_timeout_ms", &st->bypass_timeout_ms);
-       for (i = 0; i < DEV_MPU_N; i++) {
+       for (i = 0; i < DEV_N_AUX; i++) {
                sprintf(str, "%s_push_delay_ns", st->snsr[i].cfg.name);
                of_property_read_u32(dn, str,
                                     (u32 *)&st->snsr[i].push_delay_ns);
@@ -3866,6 +3886,45 @@ static int nvi_of_dt(struct nvi_state *st, struct device_node *dn)
        return 0;
 }
 
+/* device tree parameters after HAL initialized */
+static void nvi_of_dt_post(struct nvi_state *st, struct device_node *dn)
+{
+       u32 tmp;
+       char str[64];
+       unsigned int msk;
+       unsigned int i;
+
+       /* sensor specific parameters */
+       for (i = 0; i < DEV_N; i++)
+               nvs_of_dt(dn, &st->snsr[i].cfg, NULL);
+
+       /* sensor overrides that enable the DMP.
+        * If the sensor is specific to the DMP and this override is
+        * disable, then the virtual sensor is removed.
+        */
+       if (st->hal->dmp) {
+               st->dmp_dev_msk = st->hal->dmp->dev_msk;
+               st->dmp_en_msk = st->hal->dmp->en_msk;
+               for (i = 0; i < DEV_N_AUX; i++) {
+                       sprintf(str, "%s_dmp_en",
+                               st->snsr[i].cfg.name);
+                       if (!of_property_read_u32(dn, str, &tmp)) {
+                               if (tmp) {
+                                       msk = 1 << i;
+                                       if (MSK_DEV_DMP & msk)
+                                               st->dmp_dev_msk |= msk;
+                                       st->dmp_en_msk |= msk;
+                               } else {
+                                       msk = ~(1 << i);
+                                       if (MSK_DEV_DMP & (1 << i))
+                                               st->dmp_dev_msk &= msk;
+                                       st->dmp_en_msk &= msk;
+                               }
+                       }
+               }
+       }
+}
+
 static int nvi_init(struct nvi_state *st,
                    const struct i2c_device_id *i2c_dev_id)
 {
@@ -3875,17 +3934,14 @@ static int nvi_init(struct nvi_state *st,
        unsigned int n;
        int ret;
 
-       nvi_of_dt(st, st->i2c->dev.of_node);
+       nvi_of_dt_pre(st, st->i2c->dev.of_node);
        nvi_pm_init(st);
        ret = nvi_id_dev(st, i2c_dev_id);
        if (ret)
                return ret;
 
        if (st->i2c->dev.of_node) {
-               /* sensor specific parameters */
-               for (i = 0; i < DEV_N; i++)
-                       nvs_of_dt(st->i2c->dev.of_node,
-                                 &st->snsr[i].cfg, NULL);
+               nvi_of_dt_post(st, st->i2c->dev.of_node);
        } else {
                pdata = dev_get_platdata(&st->i2c->dev);
                if (pdata) {
@@ -3917,7 +3973,7 @@ static int nvi_init(struct nvi_state *st,
        } else {
                dev_info(&st->i2c->dev, "%s DMP FW loaded\n", __func__);
                /* remove DMP dependent sensors not supported by this DMP */
-               n = MSK_DEV_DMP ^ st->hal->dmp->dev_msk;
+               n = MSK_DEV_DMP ^ st->dmp_dev_msk;
        }
        if (n) {
                for (i = 0; i < DEV_N; i++) {

diff --git a/drivers/iio/imu/nvi_mpu/nvi.h b/drivers/iio/imu/nvi_mpu/nvi.h
index 0e7cb2cde82604aa7752d621582e54133e42cd64..a907f8d8ee435467e2afd27b6c0d928c8a8004ef 100644 (file)
--- a/drivers/iio/imu/nvi_mpu/nvi.h
+++ b/drivers/iio/imu/nvi_mpu/nvi.h
@@ -65,6 +65,7 @@
 #define MSK_DEV_MPU                    ((1 << DEV_ACC) | \
                                         (1 << DEV_GYR) | \
                                         (1 << DEV_TMP))
+#define MSK_DEV_MPU_AUX                        (MSK_DEV_MPU | (1 << DEV_AUX))
 #define MSK_DEV_DMP                    ((1 << DEV_SM) | \
                                         (1 << DEV_STP) | \
                                         (1 << DEV_QTN) | \
@@ -446,6 +447,8 @@ struct nvi_state {
        unsigned int errs;
        unsigned int info;
        unsigned int en_msk;
+       unsigned int dmp_en_msk;
+       unsigned int dmp_dev_msk;
        unsigned int bm_timeout_us;
        struct nvi_snsr snsr[DEV_N_AUX];
        struct nvi_src src[SRC_N];

diff --git a/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c b/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
index ae3aa143eacf8f9bcabd070b67e45035afa27f6e..be3a7e01351ce487c943da1b13622ff0f3c48136 100644 (file)
--- a/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
+++ b/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
@@ -14,16 +14,10 @@
 #include "nvi.h"
 #include "nvi_dmp_icm.h"
 
-#ifdef MPL520
-#define ICM_DMP_DEV_MSK                        ((1 << DEV_SM) | \
-                                        (1 << DEV_QTN))
-#else /* MPL520 */
 #define ICM_DMP_DEV_MSK                        ((1 << DEV_SM) | \
                                         (1 << DEV_QTN) | \
                                         (1 << DEV_GMR) | \
                                         (1 << DEV_GYU))
-#endif /* MPL520 */
-
 #define DEFAULT_ACCEL_GAIN             (0x02000000)
 #define PED_ACCEL_GAIN                 (0x04000000)
 #define DMP_ACC_PERIOD_US_PED          (19608)
@@ -318,11 +312,7 @@ static struct nvi_dmp_dev nvi_dmp_devs[] = {
        {
                .dev                    = DEV_GYR,
                .depend_msk             = (1 << DEV_GYU),
-#ifdef MPL520
-               .buf_n                  = 6,
-#else /* MPL520 */
                .buf_n                  = 12,
-#endif /* MPL520 */
                .matrix                 = true,
                .out_ctl                = 0x00402000,
                .int_ctl                = GYRO_CALIBR_SET,
@@ -434,19 +424,11 @@ static struct nvi_dmp_hdr nvi_dmp_hdr1s[] = {
                .data_n                 = 12,
                .hdr_msk                = 0x8000,
        },
-#ifdef MPL520
-       {
-               .dev                    = DEV_GYR,
-               .data_n                 = 6,
-               .hdr_msk                = 0x4000,
-       },
-#else /* MPL520 */
        {
                .dev                    = DEV_GYU,
                .data_n                 = 6,
                .hdr_msk                = 0x4000,
        },
-#endif /* MPL520 */
        {
                .dev                    = DEV_AUX,
                .aux_port               = 0,
@@ -480,19 +462,11 @@ static struct nvi_dmp_hdr nvi_dmp_hdr1s[] = {
                .data_n                 = 6,
                .hdr_msk                = 0x0080,
        },
-#ifdef MPL520
-       {
-               .dev                    = -1, /* disable for 520 */
-               .data_n                 = 12,
-               .hdr_msk                = 0x0040,
-       },
-#else /* MPL520 */
        {
                .dev                    = DEV_GYR,
                .data_n                 = 12,
                .hdr_msk                = 0x0040,
        },
-#endif /* MPL520 */
        {
                .dev                    = -1, /* CPASS_CALIBR */
                .data_n                 = 12,
@@ -1008,10 +982,6 @@ static int nvi_dd_able(struct nvi_state *st, unsigned int en_msk)
        int ret;
 
        st->en_msk &= ~MSK_DEV_SNSR;
-#ifdef MPL520
-       /* hack for MPL520 */
-       st->snsr[DEV_GYU].period_us = st->snsr[DEV_GYR].period_us;
-#endif /* MPL520 */
        ret = nvi_dmp_period(st, en_msk);
        if (ret)
                return ret;