iio: imu: nvi v.324 Fix wake up IRQ

- The DMP IRQ mode is selected by which sensors are enabled which is tracked
  by a bit mask.  The bug was that this bit mask was not clearing the bit
  pertaining to a disabled sensor and hence the DMP was programming the
  incorrect IRQ mode when suspending.

Bug 200199302

Change-Id: Id4fa3f1bc96041a3c5eb81689af0a5be04075697
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/1150633
(cherry picked from commit 73a907d169612ce79ca6363c7bbd7661e6a8c335)
Reviewed-on: http://git-master/r/1165229
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 faa8e5c6a3f40cd73f328c41f7fd27f87ff67d7a..61221e066a8971609ae412859ab02e326ed6f0a4 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             (323)
+#define NVI_DRIVER_VERSION             (324)
 #define NVI_VENDOR                     "Invensense"
 #define NVI_NAME                       "mpu6xxx"
 #define NVI_NAME_MPU6050               "mpu6050"
@@ -1450,6 +1450,7 @@ static int nvi_en(struct nvi_state *st)
 
                if (dmp_en) {
                        ret_t |= st->hal->dmp->fn_en(st); /* nvi_dmp_en */
+                       st->en_msk |= (1 << DEV_DMP);
                        if (ret_t) {
                                /* reprogram for non-DMP mode below */
                                dmp_en = false;
@@ -1467,8 +1468,8 @@ static int nvi_en(struct nvi_state *st)
                }
        }
        if (!dmp_en) {
-               if (st->hal->dmp) {
-                       st->en_msk &= ~(st->hal->dmp->en_msk | (1 << DEV_DMP));
+               if (st->en_msk & (1 << DEV_DMP)) {
+                       st->en_msk &= ~(MSK_DEV_SNSR | (1 << DEV_DMP));
                        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
                                dev_info(&st->i2c->dev,
                                         "%s DMP disabled\n", __func__);

diff --git a/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c b/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
index 32d674548bb7d4a4832da73a2f02732752f8ec21..5ceeb9622372649667e777ac4366c05d4dd81f6c 100644 (file)
--- a/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
+++ b/drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
@@ -819,11 +819,11 @@ static int nvi_dmp_period(struct nvi_state *st, unsigned int en_msk)
        return ret_t;
 }
 
-static int nvi_dmp_irq(struct nvi_state *st)
+static int nvi_dmp_irq(struct nvi_state *st, unsigned int en_msk)
 {
        u32 able;
 
-       if ((st->en_msk & MSK_DEV_ALL) & ~((1 << DEV_SM) | (1 << DEV_STP)))
+       if ((en_msk & MSK_DEV_ALL) & ~((1 << DEV_SM) | (1 << DEV_STP)))
                /* DMP requires FIFO IRQ */
                able = 0;
        else
@@ -910,8 +910,7 @@ static int nvi_dmp_init_gmf(struct nvi_state *st)
        }
 
        for (i = 0; i < 9; i++) {
-               adj[i] = nmp->matrix[i] * (nmp->q30[i % AXIS_N] >>
-                                          DMP_MULTI_SHIFT);
+               adj[i] = nmp->matrix[i] * nmp->q30[i % AXIS_N];
                mtx[i] = 0;
        }
 
@@ -983,6 +982,7 @@ static int nvi_dd_able(struct nvi_state *st, unsigned int en_msk)
        unsigned int i;
        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;
@@ -1010,23 +1010,23 @@ static int nvi_dd_able(struct nvi_state *st, unsigned int en_msk)
                                if (ret < 0)
                                        return ret;
 
-                               if (ret > 0) {
+                               if (ret > 0)
                                        /* disable without error */
-                                       if (dd->dev == DEV_AUX)
-                                               en_msk &= ~(1 <<
-                                                           (dd->aux_port +
-                                                            DEV_N_AUX));
-                                       else if (dd->dev < DEV_AUX)
-                                               en_msk &= ~(1 << dd->dev);
-                                       continue;
-                               }
+                                       en = false;
                        }
+               }
 
+               if (en) {
                        if (dd->out_ctl)
                                out_ctl |= dd->out_ctl;
                        st->snsr[dd->dev].matrix = dd->matrix;
                        st->snsr[dd->dev].buf_n = dd->buf_n;
                        st->snsr[dd->dev].buf_shft = dd->buf_shft;
+               } else {
+                       if (dd->dev == DEV_AUX)
+                               en_msk &= ~(1 << (dd->aux_port + DEV_N_AUX));
+                       else if (dd->dev < DEV_AUX)
+                               en_msk &= ~(1 << dd->dev);
                }
        }
 
@@ -1042,12 +1042,9 @@ static int nvi_dd_able(struct nvi_state *st, unsigned int en_msk)
                out_ctl |= DMP_DATA_OUT_CTL_HDR2_BIT;
        ret = nvi_mem_wr_be_mc(st, DATA_OUT_CTL1, 4, out_ctl,
                               &st->mc.icm.data_out_ctl);
-       if (ret) {
-               st->en_msk &= ~(en_msk & ((1 << DEV_N_AUX) - 1));
+       if (ret)
                return ret;
-       }
 
-       st->en_msk |= (en_msk & ((1 << DEV_N_AUX) - 1));
        /* inv_enable_accel_cal_V3 */
        /* inv_enable_gyro_cal_V3 */
        if (en_msk & (1 << DEV_ACC))
@@ -1068,7 +1065,9 @@ static int nvi_dd_able(struct nvi_state *st, unsigned int en_msk)
        /* SMD_EN is self-clearing so we don't want it in the cache */
        st->mc.icm.motion_event_ctl &= ~SMD_EN;
        /* inv_set_wom */
-       ret |= nvi_dmp_irq(st);
+       ret |= nvi_dmp_irq(st, en_msk);
+       if (!ret)
+               st->en_msk |= (en_msk & ((1 << DEV_N_AUX) - 1));
        return ret;
 }