iio: imu: nvi: v.320 ICM DMP support
Erik Lilliebjerg [Mon, 14 Mar 2016 02:48:22 +0000 (19:48 -0700)]
- Add ICM DMP support to MPU/ICM driver.
- Add ICM DMP support to AKM compass driver.
- Add ICM Significant Motion sensor support.
- Fix AKM09911 support.
- Fix BMPx80 possible NULL ptr crash on auto-detect.
- Fix coverity 20453.

Coverity ID: 20453

Bug 1416640
Bug 1722103
Bug 100130656
Bug 200162691

Change-Id: I19ae42f2fdeb67eb45bc54f440366ecaf0583b44
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/1031756
(cherry picked from commit 057038054403bfd75a8d3adf46c9d62e567dc7c9)
Reviewed-on: http://git-master/r/1030810
Reviewed-by: Automatic_Commit_Validation_User
GVS: Gerrit_Virtual_Submit
Reviewed-by: Robert Collins <rcollins@nvidia.com>
Tested-by: Robert Collins <rcollins@nvidia.com>

drivers/iio/common/nvs/nvs_iio.c
drivers/iio/imu/nvi_mpu/nvi.c
drivers/iio/imu/nvi_mpu/nvi.h
drivers/iio/imu/nvi_mpu/nvi_dmp_icm.c
drivers/iio/imu/nvi_mpu/nvi_dmp_icm.h
drivers/iio/imu/nvi_mpu/nvi_dmp_mpu.c
drivers/iio/imu/nvi_mpu/nvi_icm.c
drivers/iio/imu/nvi_mpu/nvi_mpu.c
drivers/iio/magnetometer/nvi_ak89xx.c
drivers/iio/pressure/nvi_bmpX80.c
include/linux/mpu_iio.h

index fe4e625..01b2a54 100644 (file)
@@ -76,7 +76,7 @@
 #include <linux/iio/trigger.h>
 #include <linux/nvs.h>
 
-#define NVS_IIO_DRIVER_VERSION         (212)
+#define NVS_IIO_DRIVER_VERSION         (213)
 
 enum NVS_ATTR {
        NVS_ATTR_ENABLE,
@@ -615,7 +615,7 @@ static int nvs_enable(struct iio_dev *indio_dev, bool en)
        if (!ret)
                st->enabled = enable;
        if (*st->fn_dev->sts & NVS_STS_SPEW_MSG)
-               dev_info(st->dev, "%s %s enable=%d ret=%d",
+               dev_info(st->dev, "%s %s enable=%x ret=%d",
                         __func__, st->cfg->name, enable, ret);
        return ret;
 }
index 77e2b45..45849d0 100644 (file)
@@ -29,7 +29,7 @@
 
 #include "nvi.h"
 
-#define NVI_DRIVER_VERSION             (305)
+#define NVI_DRIVER_VERSION             (320)
 #define NVI_VENDOR                     "Invensense"
 #define NVI_NAME                       "mpu6xxx"
 #define NVI_NAME_MPU6050               "mpu6050"
@@ -152,7 +152,7 @@ static int (* const nvi_nb_vreg_pf[])(struct notifier_block *nb,
        nvi_nb_vreg_vlogic,
 };
 
-static void nvi_err(struct nvi_state *st)
+void nvi_err(struct nvi_state *st)
 {
        st->errs++;
        if (!st->errs)
@@ -399,7 +399,7 @@ int nvi_i2c_rd(struct nvi_state *st, const struct nvi_br *br, u8 *buf)
        return nvi_i2c_r(st, br->bank, br->reg, len, buf);
 }
 
-int nvi_mem_wr(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data,
+int nvi_mem_wr(struct nvi_state *st, u16 addr, u16 len, u8 *data,
               bool validate)
 {
        struct i2c_msg msg[6];
@@ -416,9 +416,9 @@ int nvi_mem_wr(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data,
                return ret;
 
        buf_bank[0] = st->hal->reg->mem_bank.reg;
-       buf_bank[1] = mem_addr >> 8;
+       buf_bank[1] = addr >> 8;
        buf_addr[0] = st->hal->reg->mem_addr.reg;
-       buf_addr[1] = mem_addr & 0xFF;
+       buf_addr[1] = addr & 0xFF;
        buf_data[0] = st->hal->reg->mem_rw.reg;
        msg[0].addr = st->i2c->addr;
        msg[0].flags = 0;
@@ -443,7 +443,7 @@ int nvi_mem_wr(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data,
        msg[5].flags = I2C_M_RD;
        msg[5].buf = &buf_data[1];
        data_i = 0;
-       bank_len = (mem_addr + len - 1) >> 8;
+       bank_len = (addr + len - 1) >> 8;
        for (; buf_bank[1] <= bank_len; buf_bank[1]++) {
                if (buf_bank[1] == bank_len)
                        data_len = len - data_i;
@@ -475,17 +475,34 @@ int nvi_mem_wr(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data,
        return 0;
 }
 
-int nvi_mem_wr_be(struct nvi_state *st, u16 mem_addr, u16 len, u32 val)
+int nvi_mem_wr_be(struct nvi_state *st, u16 addr, u16 len, u32 val)
 {
        u8 buf[4];
        unsigned int i;
+       int ret;
 
        for (i = 0; i < len; i++)
                buf[i] = (u8)(val >> (8 * (len - (i + 1))));
-       return nvi_mem_wr(st, mem_addr, len, buf, false);
+       ret = nvi_mem_wr(st, addr, len, buf, false);
+       if (st->sts & NVI_DBG_SPEW_MSG)
+               dev_info(&st->i2c->dev, "%s 0x%08x=>0x%04hx err=%d\n",
+                        __func__, val, addr, ret);
+       return ret;
 }
 
-int nvi_mem_rd(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data)
+int nvi_mem_wr_be_mc(struct nvi_state *st, u16 addr, u16 len, u32 val, u32 *mc)
+{
+       int ret = 0;
+
+       if (val != *mc || st->mc_dis) {
+               ret = nvi_mem_wr_be(st, addr, len, val);
+               if (!ret)
+                       *mc = val;
+       }
+       return ret;
+}
+
+int nvi_mem_rd(struct nvi_state *st, u16 addr, u16 len, u8 *data)
 {
        struct i2c_msg msg[4];
        u8 buf_bank[2];
@@ -500,9 +517,9 @@ int nvi_mem_rd(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data)
                return ret;
 
        buf_bank[0] = st->hal->reg->mem_bank.reg;
-       buf_bank[1] = mem_addr >> 8;
+       buf_bank[1] = addr >> 8;
        buf_addr[0] = st->hal->reg->mem_addr.reg;
-       buf_addr[1] = mem_addr & 0xFF;
+       buf_addr[1] = addr & 0xFF;
        msg[0].addr = st->i2c->addr;
        msg[0].flags = 0;
        msg[0].len = sizeof(buf_bank);
@@ -518,7 +535,7 @@ int nvi_mem_rd(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data)
        msg[3].addr = st->i2c->addr;
        msg[3].flags = I2C_M_RD;
        data_i = 0;
-       bank_len = (mem_addr + len - 1) >> 8;
+       bank_len = (addr + len - 1) >> 8;
        for (; buf_bank[1] <= bank_len; buf_bank[1]++) {
                if (buf_bank[1] == bank_len)
                        data_len = len - data_i;
@@ -538,6 +555,27 @@ int nvi_mem_rd(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data)
        return 0;
 }
 
+int nvi_mem_rd_le(struct nvi_state *st, u16 addr, u16 len, u32 *val)
+{
+       u32 buf_le = 0;
+       u8 buf_rd[4];
+       unsigned int i;
+       int ret;
+
+       ret = nvi_mem_rd(st, addr, len, buf_rd);
+       if (!ret) {
+               /* convert to little endian */
+               for (i = 0; i < len; i++) {
+                       buf_le <<= 8;
+                       buf_le |= buf_rd[i];
+               }
+
+               *val = buf_le;
+       }
+
+       return ret;
+}
+
 static int nvi_rd_accel_offset(struct nvi_state *st)
 {
        u8 buf[2];
@@ -578,15 +616,6 @@ int nvi_wr_gyro_offset(struct nvi_state *st, unsigned int axis, u16 offset)
                              __func__, (u8 *)&st->rc.gyro_offset[axis], true);
 }
 
-static int nvi_wr_fifo_sz(struct nvi_state *st, u8 val)
-{
-       if (!st->hal->reg->fifo_sz.reg)
-               return 0;
-
-       return nvi_i2c_wr_rc(st, &st->hal->reg->fifo_sz, val,
-                            __func__, &st->rc.fifo_sz);
-}
-
 int nvi_wr_fifo_cfg(struct nvi_state *st, int fifo)
 {
        u8 fifo_cfg;
@@ -665,6 +694,7 @@ int nvi_int_able(struct nvi_state *st, const char *fn, bool en)
        if (en) {
                if (st->en_msk & (1 << DEV_DMP)) {
                        int_en |= 1 << st->hal->bit->int_dmp;
+                       int_en |= 1 << st->hal->bit->int_fifo_ovrflw_0;
                } else if (st->en_msk & MSK_DEV_ALL) {
                        int_msk = 1 << st->hal->bit->int_data_rdy_0;
                        if (st->rc.fifo_cfg & 0x01) {
@@ -724,8 +754,10 @@ static void nvi_flush_push(struct nvi_state *st)
 static int nvi_user_ctrl_rst(struct nvi_state *st, u8 user_ctrl)
 {
        u8 fifo_rst;
+       unsigned int msk;
+       unsigned int n;
        int i;
-       int ret;
+       int ret = 0;
        int ret_t = 0;
 
        if (user_ctrl & BIT_SIG_COND_RST)
@@ -736,12 +768,31 @@ static int nvi_user_ctrl_rst(struct nvi_state *st, u8 user_ctrl)
                st->buf_i = 0;
                if (st->hal->reg->fifo_rst.reg) {
                        /* ICM part */
+                       if (st->en_msk & (1 << DEV_DMP)) {
+                               ret = nvi_wr_fifo_cfg(st, 0);
+                       } else {
+                               n = 0;
+                               for (i = 0; i < DEV_AXIS_N; i++) {
+                                       if (st->hal->dev[i]->fifo_en_msk &&
+                                                           st->snsr[i].enable)
+                                               n++;
+                               }
+
+                               msk = st->snsr[DEV_AUX].enable;
+                               msk |= st->aux.dmp_en_msk;
+                               if (st->hal->dev[DEV_AUX]->fifo_en_msk && msk)
+                                       n++;
+                               if (n > 1)
+                                       ret = nvi_wr_fifo_cfg(st, 0);
+                               else
+                                       ret = nvi_wr_fifo_cfg(st, -1);
+                       }
                        if (st->en_msk & (1 << DEV_DMP))
                                fifo_rst = 0x1E;
                        else
                                fifo_rst = 0;
-                       ret = nvi_i2c_wr(st, &st->hal->reg->fifo_rst,
-                                        0x1F, __func__);
+                       ret |= nvi_i2c_wr(st, &st->hal->reg->fifo_rst,
+                                         0x1F, __func__);
                        ret |= nvi_i2c_wr(st, &st->hal->reg->fifo_rst,
                                          fifo_rst, __func__);
                        if (ret)
@@ -773,6 +824,10 @@ static int nvi_user_ctrl_rst(struct nvi_state *st, u8 user_ctrl)
                }
                ret_t |= ret;
                st->rc.user_ctrl = user_ctrl;
+               if (user_ctrl & BIT_DMP_RST && st->hal->dmp) {
+                       if (st->hal->dmp->dmp_reset_delay_ms)
+                               msleep(st->hal->dmp->dmp_reset_delay_ms);
+               }
        }
 
        return ret_t;
@@ -782,21 +837,15 @@ int nvi_user_ctrl_en(struct nvi_state *st, const char *fn,
                     bool en_dmp, bool en_fifo, bool en_i2c, bool en_irq)
 {
        struct aux_port *ap;
-       unsigned int n;
        int i;
        int ret = 0;
-       u16 val = 0;
+       u32 val = 0;
 
        if (en_dmp) {
-               if (st->en_msk & (1 << DEV_DMP)) {
-                       ret |= nvi_wr_fifo_cfg(st, 0);
-                       ret |= nvi_wr_fifo_sz(st, 0x03);
-               } else {
+               if (!(st->en_msk & (1 << DEV_DMP)))
                        en_dmp = false;
-               }
        }
        if (en_fifo && !en_dmp) {
-               n = 0;
                for (i = 0; i < st->hal->src_n; i++)
                        st->src[i].fifo_data_n = 0;
 
@@ -807,15 +856,14 @@ int nvi_user_ctrl_en(struct nvi_state *st, const char *fn,
                                st->src[st->hal->dev[i]->src].fifo_data_n +=
                                                  st->hal->dev[i]->fifo_data_n;
                                st->fifo_src = st->hal->dev[i]->src;
-                               n++;
                        }
                }
 
-               if (st->hal->dev[DEV_AUX]->fifo_en_msk) {
+               if (st->hal->dev[DEV_AUX]->fifo_en_msk &&
+                                                   st->snsr[DEV_AUX].enable) {
                        st->src[st->hal->dev[DEV_AUX]->src].fifo_data_n +=
                                                            st->aux.ext_data_n;
-                       if (st->snsr[DEV_AUX].enable)
-                               st->fifo_src = st->hal->dev[DEV_AUX]->src;
+                       st->fifo_src = st->hal->dev[DEV_AUX]->src;
                        for (i = 0; i < AUX_PORT_IO; i++) {
                                ap = &st->aux.port[i];
                                if (st->snsr[DEV_AUX].enable & (1 << i) &&
@@ -823,19 +871,12 @@ int nvi_user_ctrl_en(struct nvi_state *st, const char *fn,
                                                             ap->nmp.handler) {
                                        val |= (1 <<
                                                st->hal->bit->slv_fifo_en[i]);
-                                       n++;
                                }
                        }
                }
-               if (n > 1) {
-                       ret |= nvi_wr_fifo_cfg(st, 0);
-                       ret |= nvi_wr_fifo_sz(st, 0x0F);
-               } else if (n == 1) {
-                       ret |= nvi_wr_fifo_cfg(st, -1);
-                       ret |= nvi_wr_fifo_sz(st, 0x01);
-               } else {
+
+               if (!val)
                        en_fifo = false;
-               }
        }
        ret |= nvi_i2c_write_rc(st, &st->hal->reg->fifo_en, val,
                                __func__, (u8 *)&st->rc.fifo_en, false);
@@ -886,7 +927,7 @@ int nvi_wr_pm1(struct nvi_state *st, const char *fn, u8 pm1)
        st->pm = NVI_PM_ERR;
        if (pm1 & BIT_H_RESET && !ret) {
                st->en_msk &= MSK_RST;
-               memset(&st->rc, 0, sizeof(struct nvi_rc));
+               memset(&st->rc, 0, sizeof(st->rc));
                if (st->hal->fn->por2rc)
                        st->hal->fn->por2rc(st);
                for (i = 0; i < st->hal->src_n; i++)
@@ -900,6 +941,7 @@ int nvi_wr_pm1(struct nvi_state *st, const char *fn, u8 pm1)
                                break;
                }
 
+               msleep(POR_MS);
                st->rc.pm1 = pm1_rd;
                nvi_rd_accel_offset(st);
                nvi_rd_gyro_offset(st);
@@ -1048,8 +1090,17 @@ static int nvi_pm(struct nvi_state *st, const char *fn, int pm_req)
 
        case NVI_PM_ON:
                pm1 = INV_CLK_INTERNAL;
-               if (st->en_msk & MSK_PM_ACC_EN)
-                       pm2 &= ~BIT_PWR_ACCEL_STBY;
+               if (pm2 & BIT_PWR_ACCEL_STBY) {
+                       for (i = 0; i < DEV_N_AUX; i++) {
+                               if (MSK_PM_ACC_EN & (1 << i)) {
+                                       if (st->snsr[i].enable) {
+                                               pm2 &= ~BIT_PWR_ACCEL_STBY;
+                                               break;
+                                       }
+                               }
+                       }
+               }
+
                break;
 
        case NVI_PM_ON_FULL:
@@ -1180,6 +1231,7 @@ void nvi_push_delay(struct nvi_state *st)
 int nvi_aux_delay(struct nvi_state *st, const char *fn)
 {
        u8 val;
+       unsigned int msk_en;
        unsigned int src_us;
        unsigned int delay;
        unsigned int i;
@@ -1187,23 +1239,26 @@ int nvi_aux_delay(struct nvi_state *st, const char *fn)
 
        /* determine valid delays by ports enabled */
        delay = 0;
-       for (i = 0; i < AUX_PORT_MAX; i++) {
-               if (st->snsr[DEV_AUX].enable & (1 << i)) {
+       msk_en = st->snsr[DEV_AUX].enable | st->aux.dmp_en_msk;
+       for (i = 0; msk_en; i++) {
+               if (msk_en & (1 << i)) {
+                       msk_en &= ~(1 << i);
                        if (delay < st->aux.port[i].nmp.delay_ms)
                                delay = st->aux.port[i].nmp.delay_ms;
                }
        }
-       delay *= 1000; /* ms => us */
-       if (st->en_msk & (1 << DEV_DMP))
-               src_us = st->hal->dmp->dmp_period_us;
-       else
-               src_us = st->src[st->hal->dev[DEV_AUX]->src].period_us_src;
-       if (delay % src_us) {
-               delay /= src_us;
+       src_us = st->src[st->hal->dev[DEV_AUX]->src].period_us_src;
+       if (src_us) {
+               delay *= 1000; /* ms => us */
+               if (delay % src_us) {
+                       delay /= src_us;
+               } else {
+                       delay /= src_us;
+                       if (delay)
+                               delay--;
+               }
        } else {
-               delay /= src_us;
-               if (delay)
-                       delay--;
+               delay = 0;
        }
        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
                dev_info(&st->i2c->dev, "%s-%s aux.delay_hw=%u=>%u\n",
@@ -1222,70 +1277,100 @@ int nvi_aux_delay(struct nvi_state *st, const char *fn)
        return ret;
 }
 
+static int nvi_timeout(struct nvi_state *st)
+{
+       bool disabled = true;
+       unsigned int timeout_us = -1;
+       unsigned int i;
+
+       /* find the fastest batch timeout of all the enabled devices */
+       for (i = 0; i < DEV_N_AUX; i++) {
+               if (st->snsr[i].enable) {
+                       if (st->snsr[i].timeout_us < timeout_us)
+                               timeout_us = st->snsr[i].timeout_us;
+                       disabled = false;
+               }
+       }
+
+       disabled = true; /* batch mode is currently disabled */
+       if (disabled)
+               timeout_us = 0; /* batch mode disabled */
+       if (timeout_us != st->bm_timeout_us) {
+               st->bm_timeout_us = timeout_us;
+               return 1;
+       }
+
+       return 0;
+}
+
 static int nvi_period_src(struct nvi_state *st, int src)
 {
-       unsigned int dev_msk = st->en_msk;
+       bool enabled = false;
        unsigned int period_us = -1;
-       unsigned int timeout_us = -1;
+       unsigned int dev_msk;
        unsigned int i;
-       int ret = 0;
 
        if (src < 0)
                return 0;
 
-       /* find the fastest polling of all the enabled devices */
-       dev_msk &= st->hal->src[src].dev_msk;
-       for (i = 0; i < DEV_N_AUX; i++) {
+       /* find the fastest period of all the enabled devices */
+       dev_msk = st->hal->src[src].dev_msk;
+       for (i = 0; dev_msk; i++) {
                if (dev_msk & (1 << i)) {
-                       if (st->snsr[i].period_us) {
+                       dev_msk &= ~(1 << i);
+                       if (st->snsr[i].enable && st->snsr[i].period_us) {
                                if (st->snsr[i].period_us < period_us)
                                        period_us = st->snsr[i].period_us;
+                               enabled = true;
                        }
-                       if (st->snsr[i].timeout_us < timeout_us)
-                               timeout_us = st->snsr[i].timeout_us;
                }
        }
-       if (period_us == -1)
-               period_us = st->src[src].period_us_req;
-       if (period_us != st->src[src].period_us_req) {
-               st->src[src].period_us_req = period_us;
-               ret |= 1;
-       }
-       if (timeout_us == -1)
-               timeout_us = st->src_timeout_us[src];
-       if (timeout_us != st->src_timeout_us[src]) {
-               st->src_timeout_us[src] = timeout_us;
-               ret |= 1;
+
+       if (enabled) {
+               if (period_us < st->hal->src[src].period_us_min)
+                       period_us = st->hal->src[src].period_us_min;
+               if (period_us > st->hal->src[src].period_us_max)
+                       period_us = st->hal->src[src].period_us_max;
+               if (period_us != st->src[src].period_us_req) {
+                       st->src[src].period_us_req = period_us;
+                       return 1;
+               }
        }
-       return ret;
+
+       return 0;
 }
 
-static int nvi_period_aux(struct nvi_state *st)
+int nvi_period_aux(struct nvi_state *st)
 {
+       bool enabled = false;
        unsigned int period_us = -1;
        unsigned int timeout_us = -1;
+       unsigned int msk_en;
        unsigned int i;
+       int ret;
 
-       for (i = 0; i < AUX_PORT_MAX; i++) {
-               if (st->snsr[DEV_AUX].enable & (1 << i)) {
+       msk_en = st->snsr[DEV_AUX].enable | st->aux.dmp_en_msk;
+       for (i = 0; msk_en; i++) {
+               if (msk_en & (1 << i)) {
+                       msk_en &= ~(1 << i);
                        if (st->aux.port[i].period_us) {
                                if (st->aux.port[i].period_us < period_us)
                                        period_us = st->aux.port[i].period_us;
+                               if (st->aux.port[i].timeout_us < timeout_us)
+                                       timeout_us =
+                                                   st->aux.port[i].timeout_us;
+                               enabled = true;
                        }
-                       if (st->aux.port[i].timeout_us < timeout_us)
-                               timeout_us = st->aux.port[i].timeout_us;
                }
        }
 
-       if (period_us == -1)
-               period_us = st->snsr[DEV_AUX].period_us;
-       if (st->snsr[DEV_AUX].period_us != period_us)
+       if (enabled) {
                st->snsr[DEV_AUX].period_us = period_us;
-       if (timeout_us == -1)
-               timeout_us = st->snsr[DEV_AUX].timeout_us;
-       if (st->snsr[DEV_AUX].timeout_us != timeout_us)
                st->snsr[DEV_AUX].timeout_us = timeout_us;
-       return nvi_period_src(st, st->hal->dev[DEV_AUX]->src);
+       }
+       ret = nvi_period_src(st, st->hal->dev[DEV_AUX]->src);
+       ret |= nvi_timeout(st);
+       return ret;
 }
 
 static int nvi_period_all(struct nvi_state *st)
@@ -1295,27 +1380,39 @@ static int nvi_period_all(struct nvi_state *st)
 
        for (src = 0; src < st->hal->src_n; src++) {
                if (st->hal->src[src].dev_msk & (1 << DEV_AUX))
-                       ret |= nvi_period_aux(st);
+                       continue; /* run nvi_period_aux last for timeout */
                else
                        ret |= nvi_period_src(st, src);
        }
 
+       ret |= nvi_period_aux(st);
        return ret;
 }
 
 static int nvi_en(struct nvi_state *st)
 {
-       bool batch = false;
+       bool dmp_en = false;
        unsigned int i;
        int ret;
        int ret_t = 0;
 
-       if (st->snsr[DEV_GYR].enable)
-               ret_t = nvi_pm(st, __func__, NVI_PM_ON_FULL);
-       else if (st->en_msk & MSK_DEV_ALL)
-               ret_t = nvi_pm(st, __func__, NVI_PM_ON);
-       else
+       while (1) {
+               if (st->snsr[DEV_GYR].enable) {
+                       ret_t = nvi_pm(st, __func__, NVI_PM_ON_FULL);
+                       break;
+               }
+
+               for (i = 0; i < DEV_N_AUX; i++) {
+                       if (st->snsr[i].enable) {
+                               ret_t = nvi_pm(st, __func__, NVI_PM_ON);
+                               break;
+                       }
+               }
+               if (i < DEV_N_AUX)
+                       break;
+
                return nvi_pm(st, __func__, NVI_PM_AUTO);
+       }
 
        ret_t |= nvi_int_able(st, __func__, false);
        ret_t |= nvi_user_ctrl_en(st, __func__, false, false, false, false);
@@ -1323,25 +1420,63 @@ static int nvi_en(struct nvi_state *st)
                if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
                        dev_err(&st->i2c->dev, "%s en_msk=%x ERR=%d\n",
                                __func__, st->en_msk, ret_t);
-
                return ret_t;
        }
 
-       for (i = 0; i < st->hal->src_n; i++) {
-               if (!st->src_timeout_us[i]) {
-                       batch = false;
-                       break;
+       if (st->en_msk & (1 << FW_LOADED)) {
+               /* test if batch is needed or more specifically that an
+                * enabled sensor doesn't support batch.  The DMP can't
+                * do batch and non-batch at the same time.
+                */
+               if (st->bm_timeout_us) {
+                       dmp_en = true;
+               } 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->snsr[i].enable) {
+                                               dmp_en = true;
+                                               break;
+                                       }
+                               }
+                       }
                }
-       }
 
-       if (st->en_msk & (1 << FW_LOADED) &&
-                               (batch || st->en_msk & st->hal->dmp->en_msk)) {
-               st->en_msk |= (1 << DEV_DMP);
-               ret_t |= st->hal->dmp->fn_en(st); /* nvi_dmp_en */
-               if (ret_t)
+               if (dmp_en) {
+                       ret_t |= st->hal->dmp->fn_en(st); /* nvi_dmp_en */
+                       if (ret_t) {
+                               /* reprogram for non-DMP mode below */
+                               dmp_en = false;
+                               if (st->sts & (NVS_STS_SPEW_MSG |
+                                              NVI_DBG_SPEW_MSG))
+                                       dev_err(&st->i2c->dev,
+                                               "%s DMP ERR=%d\n",
+                                               __func__, ret_t);
+                       } else {
+                               if (st->sts & (NVS_STS_SPEW_MSG |
+                                              NVI_DBG_SPEW_MSG))
+                                       dev_info(&st->i2c->dev,
+                                                "%s DMP enabled\n", __func__);
+                       }
+               }
+       }
+       if (!dmp_en) {
+               if (st->hal->dmp) {
                        st->en_msk &= ~(st->hal->dmp->en_msk | (1 << DEV_DMP));
-       } else {
-               st->en_msk &= ~(1 << DEV_DMP);
+                       if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
+                               dev_info(&st->i2c->dev,
+                                        "%s DMP disabled\n", __func__);
+                       if (st->aux.dmp_en_msk) {
+                               st->aux.dmp_en_msk = 0;
+                               nvi_aux_enable(st, __func__, true, true);
+                       }
+                       for (i = 0; i < DEV_N_AUX; i++)
+                               st->snsr[i].odr = 0;
+
+                       for (i = 0; i < AUX_PORT_MAX; i++)
+                               st->aux.port[i].odr = 0;
+               }
+
                for (i = 0; i < st->hal->src_n; i++)
                        ret_t |= st->hal->src[i].fn_period(st);
 
@@ -1350,6 +1485,8 @@ static int nvi_en(struct nvi_state *st)
                        if (ret) {
                                ret_t |= ret;
                                st->en_msk &= ~(1 << DEV_ACC);
+                       } else {
+                               st->en_msk |= (1 << DEV_ACC);
                        }
                }
                if (st->snsr[DEV_GYR].enable) {
@@ -1357,10 +1494,13 @@ static int nvi_en(struct nvi_state *st)
                        if (ret) {
                                ret_t |= ret;
                                st->en_msk &= ~(1 << DEV_GYR);
+                       } else {
+                               st->en_msk |= (1 << DEV_GYR);
                        }
                }
                nvi_push_delay(st);
                /* NVI_PM_AUTO to go to NVI_PM_ON_CYCLE if need be */
+               /* this also restores correct PM mode if error */
                ret_t |= nvi_pm(st, __func__, NVI_PM_AUTO);
                if (st->pm > NVI_PM_ON_CYCLE)
                        ret_t |= nvi_reset(st, __func__, true, false, true);
@@ -1377,20 +1517,30 @@ static void nvi_aux_dbg(struct nvi_state *st, char *tag, int val)
        struct aux_port *p;
        struct aux_ports *a;
        u8 data[4];
-       int i;
+       unsigned int i;
+       int ret;
 
        if (!(st->sts & NVI_DBG_SPEW_AUX))
                return;
 
        dev_info(&st->i2c->dev, "%s %s %d\n", __func__, tag, val);
+       a = &st->aux;
        for (i = 0; i < AUX_PORT_IO; i++) {
-               nvi_i2c_rd(st, &st->hal->reg->i2c_slv_addr[i], &data[0]);
-               nvi_i2c_rd(st, &st->hal->reg->i2c_slv_reg[i], &data[1]);
-               nvi_i2c_rd(st, &st->hal->reg->i2c_slv_ctrl[i], &data[2]);
-               nvi_i2c_rd(st, &st->hal->reg->i2c_slv_do[i], &data[3]);
+               ret = nvi_i2c_rd(st, &st->hal->reg->i2c_slv_addr[i], &data[0]);
+               ret |= nvi_i2c_rd(st, &st->hal->reg->i2c_slv_reg[i], &data[1]);
+               ret |= nvi_i2c_rd(st, &st->hal->reg->i2c_slv_ctrl[i],
+                                 &data[2]);
+               ret |= nvi_i2c_rd(st, &st->hal->reg->i2c_slv_do[i], &data[3]);
                /* HW = hardware */
-               pr_info("HW: P%d AD=%x RG=%x CL=%x DO=%x\n",
-                       i, data[0], data[1], data[2], data[3]);
+               if (ret)
+                       pr_info("HW: ERR=%d\n", ret);
+               else
+                       pr_info("HW: P%d AD=%x RG=%x CL=%x DO=%x\n",
+                               i, data[0], data[1], data[2], data[3]);
+               /* RC = hardware register cache */
+               pr_info("HC: P%d AD=%x RG=%x CL=%x DO=%x\n",
+                       i, st->rc.i2c_slv_addr[i], st->rc.i2c_slv_reg[i],
+                       st->rc.i2c_slv_ctrl[i], st->rc.i2c_slv_do[i]);
                n = &st->aux.port[i].nmp;
                /* NS = nmp structure */
                pr_info("NS: P%d AD=%x RG=%x CL=%x DO=%x MS=%u US=%u SB=%x\n",
@@ -1398,14 +1548,14 @@ static void nvi_aux_dbg(struct nvi_state *st, char *tag, int val)
                        st->aux.port[i].period_us, n->shutdown_bypass);
                p = &st->aux.port[i];
                /* PS = port structure */
-               pr_info("PS: P%d OFFSET=%u EN=%x HWDOUT=%x\n",
-                       i, p->ext_data_offset,
+               pr_info("PS: P%d OFFSET=%u DMP_CTRL=%x EN=%x HWDOUT=%x\n",
+                       i, p->ext_data_offset, !!(a->dmp_ctrl_msk & (1 << i)),
                        !!(st->snsr[DEV_AUX].enable & (1 << i)), p->hw_do);
        }
-       a = &st->aux;
-       pr_info("AUX: EN=%x MEN=%x MDLY=%x GDLY=%u DATN=%u BPEN=%x BPLK=%d\n",
+
+       pr_info("AUX: EN=%x MEN=%x DEN=%x DLY=%x SRC=%u DN=%u BEN=%x BLK=%d\n",
                !!(st->en_msk & (1 << DEV_AUX)),
-               !!(st->rc.user_ctrl & BIT_I2C_MST_EN),
+               !!(st->rc.user_ctrl & BIT_I2C_MST_EN), st->aux.dmp_en_msk,
                (st->rc.i2c_slv4_ctrl & BITS_I2C_MST_DLY),
                st->src[st->hal->dev[DEV_AUX]->src].period_us_src,
                a->ext_data_n, (st->rc.int_pin_cfg & BIT_BYPASS_EN),
@@ -1414,15 +1564,13 @@ static void nvi_aux_dbg(struct nvi_state *st, char *tag, int val)
 
 static void nvi_aux_ext_data_offset(struct nvi_state *st)
 {
-       int i;
-       unsigned short offset;
+       unsigned int i;
+       unsigned int offset = 0;
 
-       offset = 0;
        for (i = 0; i < AUX_PORT_IO; i++) {
-               if ((st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
-                                 (st->aux.port[i].nmp.addr & BIT_I2C_READ)) {
+               if (st->aux.port[i].nmp.addr & BIT_I2C_READ) {
                        st->aux.port[i].ext_data_offset = offset;
-                       offset += (st->aux.port[i].nmp.ctrl &
+                       offset += (st->rc.i2c_slv_ctrl[i] &
                                   BITS_I2C_SLV_CTRL_LEN);
                }
        }
@@ -1471,60 +1619,89 @@ static int nvi_aux_port_en(struct nvi_state *st, int port, bool en)
        struct aux_port *ap;
        u8 slv_ctrl;
        u8 val;
+       unsigned int dmp_ctrl_msk;
        int ret = 0;
 
-       st->aux.ext_data_n = 0;
        ap = &st->aux.port[port];
-       if ((!(st->rc.i2c_slv_addr[port])) && en) {
+       if (en && !st->rc.i2c_slv_addr[port]) {
                ret = nvi_aux_port_wr(st, port);
                if (!ret)
                        ap->hw_do = true;
        }
        if (en && !ap->hw_do)
                nvi_aux_port_data_out(st, port, ap->nmp.data_out);
-       slv_ctrl = st->rc.i2c_slv_ctrl[port];
        if (port == AUX_PORT_IO) {
                ret = nvi_wr_i2c_slv4_ctrl(st, en);
        } else {
-               if (en)
-                       val = (ap->nmp.ctrl | BIT_SLV_EN);
-               else
+               slv_ctrl = st->rc.i2c_slv_ctrl[port];
+               if (en) {
+                       dmp_ctrl_msk = st->aux.dmp_ctrl_msk;
+                       if (st->en_msk & (1 << DEV_DMP)) {
+                               val = ap->nmp.dmp_ctrl | BIT_SLV_EN;
+                               st->aux.dmp_ctrl_msk |= (1 << port);
+                       } else {
+                               val = ap->nmp.ctrl | BIT_SLV_EN;
+                               st->aux.dmp_ctrl_msk &= ~(1 << port);
+                       }
+                       if (ap->nmp.dmp_ctrl != ap->nmp.ctrl && dmp_ctrl_msk !=
+                                                         st->aux.dmp_ctrl_msk)
+                               /* AUX HW needs to be reset if slv_ctrl values
+                                * change other than enable bit.
+                                */
+                               st->aux.reset_i2c = true;
+               } else {
                        val = 0;
+                       st->aux.dmp_ctrl_msk &= ~(1 << port);
+               }
                ret = nvi_i2c_wr_rc(st, &st->hal->reg->i2c_slv_ctrl[port], val,
                                    __func__, &st->rc.i2c_slv_ctrl[port]);
+               if (slv_ctrl != st->rc.i2c_slv_ctrl[port])
+                       nvi_aux_ext_data_offset(st);
        }
-       if (slv_ctrl != st->rc.i2c_slv_ctrl[port])
-               nvi_aux_ext_data_offset(st);
        return ret;
 }
 
-static int nvi_aux_enable(struct nvi_state *st, bool en)
+int nvi_aux_enable(struct nvi_state *st, const char *fn,
+                  bool en_req, bool force)
 {
+       bool enable = en_req;
+       bool enabled = false;
+       bool en;
+       unsigned int msk_en;
        unsigned int i;
        int ret = 0;
 
        if (st->rc.int_pin_cfg & BIT_BYPASS_EN)
-               en = false;
+               enable = false;
        /* global enable is honored only if a port is enabled */
-       if (!st->snsr[DEV_AUX].enable)
-               en = false;
-       if (en == (bool)(st->en_msk & (1 << DEV_AUX)))
-               return 0;
-
-       if (en) {
-               st->en_msk |= (1 << DEV_AUX);
-               for (i = 0; i < AUX_PORT_MAX; i++) {
-                       if (st->snsr[DEV_AUX].enable & (1 << i))
-                               ret |= nvi_aux_port_en(st, i, true);
-               }
-       } else {
-               st->en_msk &= ~(1 << DEV_AUX);
-               for (i = 0; i < AUX_PORT_MAX; i++) {
-                       if (st->rc.i2c_slv_addr[i])
-                               nvi_aux_port_en(st, i, false);
+       msk_en = st->snsr[DEV_AUX].enable | st->aux.dmp_en_msk;
+       if (!msk_en)
+               enable = false;
+       if (st->en_msk & (1 << DEV_AUX))
+               enabled = true;
+       if (force || enable != enabled) {
+               if (enable) {
+                       st->en_msk |= (1 << DEV_AUX);
+                       for (i = 0; i < AUX_PORT_MAX; i++) {
+                               if (msk_en & (1 << i))
+                                       en = true;
+                               else
+                                       en = false;
+                               ret |= nvi_aux_port_en(st, i, en);
+                       }
+               } else {
+                       st->en_msk &= ~(1 << DEV_AUX);
+                       for (i = 0; i < AUX_PORT_MAX; i++) {
+                               if (st->rc.i2c_slv_addr[i])
+                                       nvi_aux_port_en(st, i, false);
+                       }
                }
+               if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG |
+                              NVI_DBG_SPEW_AUX))
+                       dev_info(&st->i2c->dev,
+                                "%s-%s en_req=%x enabled: %x->%x err=%d\n",
+                                __func__, fn, en_req, enabled, enable, ret);
        }
-
        return ret;
 }
 
@@ -1550,7 +1727,7 @@ static int nvi_aux_port_enable(struct nvi_state *st, int port, bool en)
                return 0;
 
        ret = nvi_aux_port_en(st, port, en);
-       ret |= nvi_aux_enable(st, true);
+       ret |= nvi_aux_enable(st, __func__, true, false);
        nvi_period_aux(st);
        ret |= nvi_en(st);
        return ret;
@@ -1560,12 +1737,13 @@ static int nvi_aux_port_free(struct nvi_state *st, int port)
 {
        memset(&st->aux.port[port], 0, sizeof(struct aux_port));
        st->snsr[DEV_AUX].enable &= ~(1 << port);
+       st->aux.dmp_en_msk &= ~(1 << port);
        if (st->rc.i2c_slv_addr[port]) {
                nvi_aux_port_wr(st, port);
                nvi_aux_port_en(st, port, false);
-               nvi_aux_enable(st, false);
+               nvi_aux_enable(st, __func__, false, false);
                nvi_user_ctrl_en(st, __func__, false, false, false, false);
-               nvi_aux_enable(st, true);
+               nvi_aux_enable(st, __func__, true, false);
                if (port != AUX_PORT_IO)
                        st->aux.reset_i2c = true;
                nvi_period_aux(st);
@@ -1597,6 +1775,8 @@ static int nvi_aux_port_alloc(struct nvi_state *st,
 
        memset(&st->aux.port[i], 0, sizeof(struct aux_port));
        memcpy(&st->aux.port[i].nmp, nmp, sizeof(struct nvi_mpu_port));
+       if (!st->aux.port[i].nmp.dmp_ctrl)
+               st->aux.port[i].nmp.dmp_ctrl = st->aux.port[i].nmp.ctrl;
        st->aux.port[i].period_us = st->aux.port[i].nmp.delay_us;
        return i;
 }
@@ -1611,7 +1791,7 @@ static int nvi_aux_bypass_enable(struct nvi_state *st, bool en)
 
        val = st->rc.int_pin_cfg;
        if (en) {
-               ret = nvi_aux_enable(st, false);
+               ret = nvi_aux_enable(st, __func__, false, false);
                ret |= nvi_user_ctrl_en(st, __func__,
                                        false, false, false, false);
                if (!ret) {
@@ -1624,7 +1804,7 @@ static int nvi_aux_bypass_enable(struct nvi_state *st, bool en)
                ret = nvi_i2c_wr_rc(st, &st->hal->reg->int_pin_cfg, val,
                                    __func__, &st->rc.int_pin_cfg);
                if (!ret)
-                       nvi_aux_enable(st, true);
+                       nvi_aux_enable(st, __func__, true, false);
        }
        nvi_period_aux(st);
        nvi_en(st);
@@ -1961,13 +2141,14 @@ int nvi_mpu_batch(int port, unsigned int period_us, unsigned int timeout_us)
                } else {
                        st->aux.port[port].period_us = period_us;
                        st->aux.port[port].timeout_us = timeout_us;
-                       if (st->en_msk & (1 << DEV_DMP)) {
+                       ret = nvi_period_aux(st);
+                       if (st->en_msk & (1 << DEV_DMP) &&
+                                                 st->hal->dmp->fn_dev_batch) {
                                /* batch can be done real-time with DMP on */
                                /* nvi_dd_batch */
                                ret = st->hal->dmp->fn_dev_batch(st, DEV_AUX,
                                                                 port);
                        } else {
-                               ret = nvi_period_aux(st);
                                if (ret > 0)
                                        /* timings changed */
                                        ret = nvi_en(st);
@@ -2100,14 +2281,16 @@ int nvi_reset(struct nvi_state *st, const char *fn,
 {
        s64 ts;
        u8 val;
+       bool rst_dmp = false;
        unsigned int i;
        int ret;
 
        ret = nvi_int_able(st, __func__, false);
        val = 0;
-       if (rst_i2c) {
+       if (rst_i2c || st->aux.reset_i2c) {
                st->aux.reset_i2c = false;
-               ret |= nvi_aux_enable(st, false);
+               rst_i2c = true;
+               ret |= nvi_aux_enable(st, __func__, false, false);
                val |= BIT_I2C_MST_RST;
        }
        if (rst_fifo) {
@@ -2115,16 +2298,16 @@ int nvi_reset(struct nvi_state *st, const char *fn,
                val |= BIT_FIFO_RST;
                if (st->en_msk & (1 << DEV_DMP)) {
                        val |= BIT_DMP_RST;
-                       rst_i2c = true;
-                       ret |= nvi_aux_enable(st, false);
+                       rst_dmp = true;
+                       ret |= nvi_aux_enable(st, __func__, false, false);
                }
        }
        ret |= nvi_user_ctrl_en(st, __func__,
                                !rst_fifo, !rst_fifo, !rst_i2c, false);
        val |= st->rc.user_ctrl;
        ret |= nvi_user_ctrl_rst(st, val);
-       if (rst_i2c)
-               ret |= nvi_aux_enable(st, true);
+       if (rst_i2c || rst_dmp)
+               ret |= nvi_aux_enable(st, __func__, true, false);
        ts = nvs_timestamp();
        if (rst_fifo) {
                for (i = 0; i < st->hal->src_n; i++) {
@@ -2139,45 +2322,94 @@ int nvi_reset(struct nvi_state *st, const char *fn,
                        st->snsr[i].ts_last = ts;
                        st->snsr[i].ts_n = 0;
                }
+
+               for (i = 0; i < AUX_PORT_MAX; i++) {
+                       st->aux.port[i].ts_reset = true;
+                       st->aux.port[i].ts_last = ts;
+               }
+
+               if (st->hal->dmp) {
+                       /* nvi_dmp_clk_n */
+                       ret |= st->hal->dmp->fn_clk_n(st, &st->dmp_clk_n);
+                       st->src[SRC_DMP].ts_reset = true;
+                       st->src[SRC_DMP].ts_1st = ts;
+                       st->src[SRC_DMP].ts_end = ts;
+                       st->src[SRC_DMP].ts_period =
+                                        st->src[SRC_DMP].period_us_src * 1000;
+               }
        }
 
        ret |= nvi_user_ctrl_en(st, __func__, true, true, true, en_irq);
        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG |
                       NVI_DBG_SPEW_FIFO | NVI_DBG_SPEW_TS))
                dev_info(&st->i2c->dev,
-                        "%s-%s FIFO=%x I2C=%x ts=%lld err=%d\n",
-                        __func__, fn, rst_fifo, rst_i2c, ts, ret);
+                        "%s-%s DMP=%x FIFO=%x I2C=%x ts=%lld err=%d\n",
+                        __func__, fn, rst_dmp, rst_fifo, rst_i2c, ts, ret);
        return ret;
 }
 
-static s64 nvi_ts_dev(struct nvi_state *st, unsigned int dev, s64 ts_now)
+s64 nvi_ts_dev(struct nvi_state *st, s64 ts_now,
+              unsigned int dev, unsigned int aux_port)
 {
-       int src;
        s64 ts;
+       int src;
 
-       if (ts_now)
-               src = st->hal->dev[dev]->src;
-       else
+       if (ts_now) {
+               if (st->en_msk & (1 << DEV_DMP))
+                       src = SRC_DMP;
+               else
+                       src = st->hal->dev[dev]->src;
+       } else {
                src = -1;
+       }
        if (src < 0) {
                ts = nvs_timestamp();
        } else {
-               if (st->snsr[dev].ts_reset) {
-                       st->snsr[dev].ts_reset = false;
-                       ts = st->src[src].ts_1st;
+               if (dev == DEV_AUX && aux_port < AUX_PORT_MAX) {
+                       if (st->aux.port[aux_port].ts_reset) {
+                               st->aux.port[aux_port].ts_reset = false;
+                               ts = st->src[src].ts_1st;
+                       } else {
+                               ts = st->src[src].ts_period;
+                               if (st->aux.port[aux_port].odr)
+                                       ts *= (st->aux.port[aux_port].odr + 1);
+                               ts += st->aux.port[aux_port].ts_last;
+                       }
                } else {
-                       ts = st->snsr[dev].ts_last + st->src[src].ts_period;
+                       if (st->snsr[dev].ts_reset) {
+                               st->snsr[dev].ts_reset = false;
+                               ts = st->src[src].ts_1st;
+                       } else {
+                               ts = st->src[src].ts_period;
+                               if (st->snsr[dev].odr)
+                                       ts *= (st->snsr[dev].odr + 1);
+                               ts += st->snsr[dev].ts_last;
+                       }
+               }
+               if (ts > ts_now) {
+                       if (st->sts & (NVI_DBG_SPEW_FIFO | NVI_DBG_SPEW_TS))
+                               dev_info(&st->i2c->dev,
+                                        "%s ts > ts_now (%lld > %lld)\n",
+                                        __func__, ts, ts_now);
+                       ts = ts_now;
                }
        }
-       if (ts < st->snsr[dev].ts_last)
-               ts = -1;
-       else
-               st->snsr[dev].ts_last = ts;
+       if (dev == DEV_AUX && aux_port < AUX_PORT_MAX) {
+               if (ts < st->aux.port[aux_port].ts_last)
+                       ts = -1;
+               else
+                       st->aux.port[aux_port].ts_last = ts;
+       } else {
+               if (ts < st->snsr[dev].ts_last)
+                       ts = -1;
+               else
+                       st->snsr[dev].ts_last = ts;
+       }
        if (ts < st->snsr[dev].ts_push_delay)
                ts = -1;
-       if (st->sts & NVI_DBG_SPEW_TS && src >= 0)
+       if (st->sts & NVI_DBG_SPEW_FIFO && src >= 0)
                dev_info(&st->i2c->dev,
-                        "src[%d]: ts_prd=%lld ts_end=%lld %s ts[%u]=%lld",
+                        "src[%d] ts_period=%lld ts_end=%lld %s ts[%u]=%lld\n",
                         src, st->src[src].ts_period, st->src[src].ts_end,
                         st->snsr[dev].cfg.name, st->snsr[dev].ts_n, ts);
        st->snsr[dev].ts_n++;
@@ -2202,7 +2434,7 @@ static void nvi_aux_rd(struct nvi_state *st)
        if (ret)
                return;
 
-       ts = nvi_ts_dev(st, DEV_AUX, 0);
+       ts = nvi_ts_dev(st, 0, DEV_AUX, -1);
        for (i = 0; i < AUX_PORT_IO; i++) {
                ap = &st->aux.port[i];
                if ((st->rc.i2c_slv_ctrl[i] & BIT_SLV_EN) &&
@@ -2226,9 +2458,9 @@ static s32 nvi_matrix(struct nvi_state *st, signed char *matrix,
                 (matrix[6 + axis] == -1 ? -z : 0)));
 }
 
-static int nvi_push(struct nvi_state *st, unsigned int dev, u8 *buf, s64 ts)
+int nvi_push(struct nvi_state *st, unsigned int dev, u8 *buf, s64 ts)
 {
-       u8 buf_le[16];
+       u8 buf_le[20];
        s32 val_le[4];
        s32 val[AXIS_N];
        u32 u_val;
@@ -2236,35 +2468,50 @@ static int nvi_push(struct nvi_state *st, unsigned int dev, u8 *buf, s64 ts)
        unsigned int buf_le_i;
        unsigned int ch;
        unsigned int ch_sz;
+       unsigned int m;
        unsigned int n;
        int i;
 
        ch_sz = abs(st->snsr[dev].cfg.ch_sz);
-       if (st->snsr[dev].buf_n)
+       m = 0;
+       if (st->snsr[dev].buf_n) {
                n = st->snsr[dev].buf_n / st->snsr[dev].cfg.ch_n;
-       else
+               m = st->snsr[dev].buf_n % st->snsr[dev].cfg.ch_n;
+               if (m)
+                       n++;
+       } else {
                n = ch_sz;
+       }
        /* convert big endian byte stream to little endian channel data */
        for (ch = 0; ch < st->snsr[dev].cfg.ch_n; ch++) {
                val_le[ch] = 0;
                if (st->snsr[dev].enable & (1 << ch)) {
-                       for (i = 0; i < n; i++) {
-                               val_le[ch] <<= 8;
-                               val_le[ch] |= (u8)*buf++;
+                       if (m && ch == (st->snsr[dev].cfg.ch_n - 1)) {
+                               /* handle last channel misalignment */
+                               for (i = 0; i < m; i++) {
+                                       val_le[ch] <<= 8;
+                                       val_le[ch] |= (u8)*buf++;
+                               }
+                               /* extend sign bit */
+                               i = (sizeof(val_le[ch]) - m) * 8;
+                               val_le[ch] <<= i;
+                               val_le[ch] >>= i;
+                       } else {
+                               for (i = 0; i < n; i++) {
+                                       val_le[ch] <<= 8;
+                                       val_le[ch] |= (u8)*buf++;
+                               }
+                               /* extend sign bit */
+                               i = (sizeof(val_le[ch]) - n) * 8;
+                               if (i) {
+                                       val_le[ch] <<= i;
+                                       val_le[ch] >>= i;
+                               }
                        }
                }
        }
 
-       /* extend sign bit */
-       i = (sizeof(val_le[0]) - n) * 8;
-       if (i) {
-               for (ch = 0; ch < st->snsr[dev].cfg.ch_n; ch++) {
-                       val_le[ch] <<= i;
-                       val_le[ch] >>= i;
-               }
-       }
-
-       /* shift HW data size to buffer size if needed */
+       /* shift HW data size to channel size if needed */
        if (st->snsr[dev].buf_shft) {
                if (st->snsr[dev].buf_shft < 0) {
                        n = abs(st->snsr[dev].buf_shft);
@@ -2287,7 +2534,7 @@ static int nvi_push(struct nvi_state *st, unsigned int dev, u8 *buf, s64 ts)
                                                val[AXIS_Z], ch);
        }
 
-       /* convert to little endian byte stream */
+       /* convert little endian channel data to little endian byte stream */
        buf_le_i = 0;
        for (ch = 0; ch < st->snsr[dev].cfg.ch_n; ch++) {
                u_val = (u32)val_le[ch];
@@ -2298,6 +2545,16 @@ static int nvi_push(struct nvi_state *st, unsigned int dev, u8 *buf, s64 ts)
                buf_le_i += ch_sz;
        }
 
+       /* add status if needed (no endian conversion) */
+       if (buf_le_i < st->snsr[dev].cfg.snsr_data_n) {
+               n = st->snsr[dev].cfg.snsr_data_n - buf_le_i;
+               u_val = st->snsr[dev].sts;
+               for (i = 0; i < n; i++) {
+                       buf_le[buf_le_i + i] = (u8)(u_val & 0xFF);
+                       u_val >>= 8;
+               }
+       }
+
        if (ts >= 0) {
                if (st->sts & (NVI_DBG_SPEW_SNSR << dev)) {
                        sts = st->sts;
@@ -2352,83 +2609,17 @@ static int nvi_dev_rd(struct nvi_state *st, unsigned int dev)
        ret = nvi_i2c_r(st, st->hal->reg->out_h[dev].bank,
                        st->hal->reg->out_h[dev].reg, len, buf);
        if (!ret)
-               ret = nvi_push(st, dev, buf, nvi_ts_dev(st, dev, 0));
+               ret = nvi_push(st, dev, buf, nvi_ts_dev(st, 0, dev, 0));
        return ret;
 }
 
-static int nvi_fifo_dmp(struct nvi_state *st, s64 ts, unsigned int n)
-{
-       const struct nvi_dmp_dev *dd;
-       struct aux_port *ap;
-       unsigned int dd_i;
-       unsigned int i;
-       u8 byte;
-
-       while (n > DMP_HDR_LEN_MAX) {
-               for (dd_i = 0; dd_i < st->hal->dmp->dd_n; dd_i++) {
-                       dd = &st->hal->dmp->dd[dd_i];
-                       if (!dd->hdr_n)
-                               continue;
-
-                       for (i = 0; i < dd->hdr_n; i++) {
-                               byte = st->buf[st->buf_i + i];
-                               byte &= dd->hdr_msk[i];
-                               if (byte != dd->hdr[i])
-                                       break;
-                       }
-                       if (i >= dd->hdr_n)
-                               break;
-               }
-               if (dd_i >= st->hal->dmp->dd_n) {
-                       /* unknown header: lost DMP sync so DMP reset */
-                       if (st->sts & NVI_DBG_SPEW_FIFO)
-                               dev_err(&st->i2c->dev,
-                                       "%s ERR: DMP sync  HDR: %x %x %x %x\n",
-                                       __func__, st->buf[st->buf_i],
-                                       st->buf[st->buf_i + 1],
-                                       st->buf[st->buf_i + 2],
-                                       st->buf[st->buf_i + 3]);
-                       nvi_err(st);
-                       return -1;
-               }
-
-               if (n > dd->data_n + i) {
-                       if (dd->dev == DEV_AUX) {
-                               if (st->sts & NVI_DBG_SPEW_FIFO)
-                                       dev_info(&st->i2c->dev,
-                                                "%s DMP HDR: AUX port=%u\n",
-                                                __func__, dd->aux_port);
-                               ap = &st->aux.port[dd->aux_port];
-                               ap->nmp.handler(&st->buf[st->buf_i + i],
-                                               dd->data_n,
-                                               nvi_ts_dev(st, dd->dev, 0),
-                                               ap->nmp.ext_driver);
-                       } else if (dd->dev < DEV_N) {
-                               if (st->sts & NVI_DBG_SPEW_FIFO)
-                                       dev_info(&st->i2c->dev,
-                                                "%s DMP HDR: %s\n", __func__,
-                                                st->snsr[dd->dev].cfg.name);
-                               nvi_push(st, dd->dev, &st->buf[st->buf_i + i],
-                                        nvi_ts_dev(st, dd->dev, 0));
-                       }
-                       i += dd->data_n;
-                       st->buf_i += i;
-                       n -= i;
-               } else {
-                       return 0;
-               }
-       }
-
-       return 0;
-}
-
 static int nvi_fifo_aux(struct nvi_state *st, s64 ts, unsigned int n)
 {
        struct aux_port *ap;
        unsigned int fifo_data_n;
        unsigned int port;
 
-       ts = nvi_ts_dev(st, DEV_AUX, ts);
+       ts = nvi_ts_dev(st, ts, DEV_AUX, -1);
        for (port = 0; port < AUX_PORT_IO; port++) {
                ap = &st->aux.port[port];
                if (st->rc.fifo_en & (1 << st->hal->bit->slv_fifo_en[port])) {
@@ -2441,6 +2632,8 @@ static int nvi_fifo_aux(struct nvi_state *st, s64 ts, unsigned int n)
                        st->buf_i += fifo_data_n;
                        n -= fifo_data_n;
                }
+               if (st->sts & (NVS_STS_SUSPEND | NVS_STS_SHUTDOWN))
+                       return -1;
        }
 
        return 1;
@@ -2449,10 +2642,13 @@ static int nvi_fifo_aux(struct nvi_state *st, s64 ts, unsigned int n)
 static int nvi_fifo_dev_rd(struct nvi_state *st, s64 ts, unsigned int n,
                           unsigned int dev)
 {
+       if (st->sts & (NVS_STS_SUSPEND | NVS_STS_SHUTDOWN))
+               return -1;
+
        if (st->hal->dev[dev]->fifo_data_n > n)
                return 0;
 
-       nvi_push(st, dev, &st->buf[st->buf_i], nvi_ts_dev(st, dev, ts));
+       nvi_push(st, dev, &st->buf[st->buf_i], nvi_ts_dev(st, ts, dev, 0));
        st->buf_i += st->hal->dev[dev]->fifo_data_n;
        return 1;
 }
@@ -2493,6 +2689,7 @@ static int nvi_fifo_rd(struct nvi_state *st, int src, unsigned int fifo_n_max,
                       int (*fn)(struct nvi_state *st, s64 ts, unsigned int n))
 {
        u16 fifo_count;
+       u32 dmp_clk_n = 0;
        s64 ts_period;
        s64 ts_now;
        s64 ts_end;
@@ -2500,10 +2697,13 @@ static int nvi_fifo_rd(struct nvi_state *st, int src, unsigned int fifo_n_max,
        unsigned int ts_n;
        unsigned int fifo_n;
        unsigned int buf_n;
-       int ret;
+       int ret = 0;
 
        ts_end = nvs_timestamp();
-       ret = nvi_i2c_rd(st, &st->hal->reg->fifo_count_h, (u8 *)&fifo_count);
+       if (src < 0)
+               /* nvi_dmp_clk_n */
+               ret = st->hal->dmp->fn_clk_n(st, &dmp_clk_n);
+       ret |= nvi_i2c_rd(st, &st->hal->reg->fifo_count_h, (u8 *)&fifo_count);
        if (ret || !fifo_count)
                return 0;
 
@@ -2514,20 +2714,34 @@ static int nvi_fifo_rd(struct nvi_state *st, int src, unsigned int fifo_n_max,
                sync = false;
        ts_end = atomic64_read(&st->ts_irq);
        fifo_n = (unsigned int)be16_to_cpu(fifo_count);
+       if (st->sts & NVS_STS_SPEW_IRQ)
+               dev_info(&st->i2c->dev,
+                        "src=%d sync=%x fifo_n=%u ts_clk_n=%u ts_diff=%lld\n",
+                        src, sync, fifo_n, dmp_clk_n, ts_now - st->ts_now);
+       st->ts_now = ts_now;
        if (src < 0) {
                /* DMP timing */
-               ts_n = 1;
+               if (dmp_clk_n > st->dmp_clk_n)
+                       ts_n = dmp_clk_n - st->dmp_clk_n;
+               else
+                       /* counter rolled over */
+                       ts_n = (~st->dmp_clk_n + 1) + dmp_clk_n;
+               /* ts_n is the number of DMP clock ticks since last time */
+               st->dmp_clk_n = dmp_clk_n;
+               src = SRC_DMP;
+               fifo_n_max = 0; /* DMP disables round-robin FIFOs */
        } else {
                /* FIFO timing */
                ts_n = fifo_n / st->src[src].fifo_data_n; /* TS's needed */
                if ((fifo_n % st->src[src].fifo_data_n) || !ts_n)
                        /* reset FIFO if doesn't divide cleanly */
                        return -1;
+       }
 
+       if (ts_n) {
                ts_period = st->src[src].period_us_src * 1000;
-               ts_period >>= 2;
                if (sync && ts_end > st->src[src].ts_end && ts_end < ts_now &&
-                                                ts_end > (ts_now - ts_period))
+                                         ts_end > (ts_now - (ts_period >> 2)))
                        /* ts_irq is within the rate so sync to IRQ */
                        ts_now = ts_end;
                if (st->src[src].ts_reset) {
@@ -2541,14 +2755,22 @@ static int nvi_fifo_rd(struct nvi_state *st, int src, unsigned int fifo_n_max,
                        ts_end = st->src[src].ts_period * ts_n;
                }
                ts_end += st->src[src].ts_end;
-               if (ts_end > ts_now || (sync &&
-                                       (ts_end < (ts_now - ts_period)))) {
+               /* ts_now will be sent to nvi_ts_dev where the timestamp is
+                * prevented from going into the future which allows some
+                * tolerance here for ts_end being a little more than ts_now.
+                * The more tolerance we have the less recalculating the period
+                * to avoid swing around the true period.  Plus, the clamp on
+                * ts_now in nvi_ts_dev has the benefit of "syncing" with the
+                * current calculations per device.
+                */
+               if (ts_end > (ts_now + (ts_period >> 3)) || (sync && (ts_end <
+                                              (ts_now - (ts_period >> 1))))) {
                        if (st->sts & (NVI_DBG_SPEW_FIFO | NVI_DBG_SPEW_TS)) {
                                dev_info(&st->i2c->dev,
                                         "sync=%x now=%lld end=%lld ts_n=%u\n",
                                         sync, ts_now, ts_end, ts_n);
                                dev_info(&st->i2c->dev,
-                                        "src[%d]: period=%lld ts_end=%lld\n",
+                                        "src=%d old period=%lld end=%lld\n",
                                         src, st->src[src].ts_period,
                                         st->src[src].ts_end);
                        }
@@ -2560,10 +2782,11 @@ static int nvi_fifo_rd(struct nvi_state *st, int src, unsigned int fifo_n_max,
                        ts_end += st->src[src].ts_end;
                        if (st->sts & (NVI_DBG_SPEW_FIFO | NVI_DBG_SPEW_TS))
                                dev_info(&st->i2c->dev,
-                                        "src[%d]: period=%lld ts_end=%lld\n",
+                                        "src=%d new period=%lld end=%lld\n",
                                         src, ts_period, ts_end);
                }
                if (fifo_n_max) {
+                       /* would only apply to FIFO timing (non-DMP) */
                        if (fifo_n_max < fifo_n) {
                                fifo_n = fifo_n_max;
                                ts_n = fifo_n / st->src[src].fifo_data_n;
@@ -2572,6 +2795,9 @@ static int nvi_fifo_rd(struct nvi_state *st, int src, unsigned int fifo_n_max,
                        }
                }
                st->src[src].ts_end = ts_end;
+       } else {
+               /* wasn't able to calculate TS */
+               ts_now = 0;
        }
 
        while (fifo_n) {
@@ -2629,7 +2855,8 @@ static int nvi_rd(struct nvi_state *st)
                                nvi_push_event(st, DEV_STP);
                }
                if (st->en_msk & st->hal->dmp->en_msk)
-                       return nvi_fifo_rd(st, -1, 0, nvi_fifo_dmp);
+                       /* nvi_dmp_rd */
+                       return nvi_fifo_rd(st, -1, 0, st->hal->dmp->fn_rd);
 
                nvi_en(st);
                return 0;
@@ -2695,7 +2922,9 @@ static int nvi_read(struct nvi_state *st, bool flush)
                nvi_en(st);
        } else if (!(st->sts & (NVS_STS_SUSPEND | NVS_STS_SHUTDOWN))) {
                ret = nvi_rd(st);
-               if (flush || ret < 0)
+               if (ret < 0)
+                       nvi_en(st); /* a little harder reset for ICM DMP */
+               else if (flush)
                        nvi_reset(st, __func__, true, false, true);
        } else if (flush) {
                nvi_flush_push(st);
@@ -2738,32 +2967,38 @@ static irqreturn_t nvi_handler(int irq, void *dev_id)
 static int nvi_enable(void *client, int snsr_id, int enable)
 {
        struct nvi_state *st = (struct nvi_state *)client;
-       unsigned int en_msk = st->en_msk;
 
        if (enable < 0)
+               /* return current enable request status */
                return st->snsr[snsr_id].enable;
 
-       st->snsr[snsr_id].enable = enable;
-       if (enable)
-               st->en_msk |= (1 << snsr_id);
-       else
-               st->en_msk &= ~(1 << snsr_id);
-       if (st->en_msk == en_msk)
+       if (st->snsr[snsr_id].enable == enable)
+               /* nothing has changed with enable request */
                return 0;
 
+       st->snsr[snsr_id].enable = enable;
+       if (!enable)
+               /* officially flagged as off here */
+               st->en_msk &= ~(1 << snsr_id);
        if (st->sts & NVS_STS_SUSPEND)
                /* speed up suspend/resume by not doing nvi_en for every dev */
                return 0;
 
        if (snsr_id == DEV_TMP)
+               /* this is a static sensor that will be read when gyro is on */
                return 0;
 
-       if (en_msk & (1 << DEV_DMP)) {
+       if (st->en_msk & (1 << DEV_DMP)) {
+               /* DMP is currently on */
                if (!(st->en_msk & st->hal->dmp->en_msk))
-                       /* DMP may get turned off */
+                       /* DMP may get turned off (may stay on due to batch) so
+                        * we update timings that may have changed while DMP
+                        * was on.
+                        */
                        nvi_period_all(st);
        } else {
                nvi_period_src(st, st->hal->dev[snsr_id]->src);
+               nvi_timeout(st);
        }
        return nvi_en(st);
 }
@@ -2783,12 +3018,13 @@ static int nvi_batch(void *client, int snsr_id, int flags,
        st->snsr[snsr_id].period_us = period;
        st->snsr[snsr_id].timeout_us = timeout;
        if (st->snsr[snsr_id].enable) {
-               if (st->en_msk & (1 << DEV_DMP))
+               ret = nvi_timeout(st);
+               if (st->en_msk & (1 << DEV_DMP) && st->hal->dmp->fn_dev_batch)
                        /* batch can be done in real-time with the DMP on */
                        /* nvi_dd_batch */
                        return st->hal->dmp->fn_dev_batch(st, snsr_id, -1);
 
-               ret = nvi_period_src(st, st->hal->dev[snsr_id]->src);
+               ret |= nvi_period_src(st, st->hal->dev[snsr_id]->src);
                if (ret > 0)
                        nvi_en(st);
        }
@@ -2938,7 +3174,7 @@ static int nvi_self_test(void *client, int snsr_id, char *buf)
        int ret;
 
        nvi_pm(st, __func__, NVI_PM_ON);
-       nvi_aux_enable(st, false);
+       nvi_aux_enable(st, __func__, false, false);
        nvi_user_ctrl_en(st, __func__, false, false, false, false);
        if (snsr_id == DEV_ACC)
                ret = st->hal->fn->st_acc(st);
@@ -2946,7 +3182,7 @@ static int nvi_self_test(void *client, int snsr_id, char *buf)
                ret = st->hal->fn->st_gyr(st);
        else
                ret = 0;
-       nvi_aux_enable(st, true);
+       nvi_aux_enable(st, __func__, true, false);
        nvi_period_all(st);
        nvi_en(st);
        if (ret)
@@ -3058,6 +3294,9 @@ static int nvi_nvs_read(void *client, int snsr_id, char *buf)
 
        case NVI_INFO_DBG:
                t = sprintf(buf, "en_msk=%x\n", st->en_msk);
+               t += sprintf(buf + t, "sts=%x\n", st->sts);
+               t += sprintf(buf + t, "pm=%d\n", st->pm);
+               t += sprintf(buf + t, "bm_timeout_us=%u\n", st->bm_timeout_us);
                t += sprintf(buf + t, "fifo_src=%d\n", st->fifo_src);
                t += sprintf(buf + t, "ts_irq=%lld\n",
                             atomic64_read(&st->ts_irq));
@@ -3070,18 +3309,36 @@ static int nvi_nvs_read(void *client, int snsr_id, char *buf)
                                     st->snsr[i].period_us);
                        t += sprintf(buf + t, "timeout_us=%u\n",
                                     st->snsr[i].timeout_us);
-                       t += sprintf(buf + t, "fsync=%x\n",
-                                    st->snsr[i].fsync);
-                       t += sprintf(buf + t, "ts_push_delay=%lld\n",
-                                    st->snsr[i].ts_push_delay);
-                       t += sprintf(buf + t, "push_delay_ns=%lld\n",
-                                    st->snsr[i].push_delay_ns);
+                       t += sprintf(buf + t, "odr=%u\n",
+                                    st->snsr[i].odr);
                        t += sprintf(buf + t, "ts_last=%lld\n",
                                     st->snsr[i].ts_last);
+                       t += sprintf(buf + t, "ts_reset=%x\n",
+                                    st->snsr[i].ts_reset);
                        t += sprintf(buf + t, "flush=%x\n",
                                     st->snsr[i].flush);
+                       t += sprintf(buf + t, "matrix=%x\n",
+                                    st->snsr[i].matrix);
+                       t += sprintf(buf + t, "buf_shft=%d\n",
+                                    st->snsr[i].buf_shft);
+                       t += sprintf(buf + t, "buf_n=%u\n",
+                                    st->snsr[i].buf_n);
+               }
+
+               if (st->hal->dmp) {
+                       /* nvi_dmp_clk_n */
+                       st->hal->dmp->fn_clk_n(st, &n);
+                       t += sprintf(buf + t, "nvi_dmp_clk_n=%u\n", n);
+                       t += sprintf(buf + t, "st->dmp_clk_n=%u\n",
+                                    st->dmp_clk_n);
+                       n = SRC_DMP;
+               } else {
+                       n = 0;
                }
-               for (i = 0; i < st->hal->src_n; i++) {
+               for (i = 0; i < SRC_N; i++) {
+                       if (i >= st->hal->src_n && i != SRC_DMP)
+                               continue;
+
                        t += sprintf(buf + t, "src[%u]:\n", i);
                        t += sprintf(buf + t, "ts_reset=%x\n",
                                     st->src[i].ts_reset);
@@ -3145,6 +3402,7 @@ static int nvi_nvs_read(void *client, int snsr_id, char *buf)
                return t;
 
        case NVI_INFO_MEM_WR:
+               st->mc_dis = true;
                buf_rw[0] = (u8)(info >> 8);
                ret = nvi_mem_wr(st, info >> 16, 1, buf_rw, true);
                return sprintf(buf, "MEM WR: a=%04x d=%02x ERR=%d\n",
@@ -3452,9 +3710,9 @@ static struct sensor_cfg nvi_cfg_dflt[] = {
                .name                   = "accelerometer",
                .snsr_id                = DEV_ACC,
                .kbuf_sz                = KBUF_SZ,
-               .snsr_data_n            = 8,
+               .snsr_data_n            = 14,
                .ch_n                   = AXIS_N,
-               .ch_sz                  = -2,
+               .ch_sz                  = -4,
                .vendor                 = NVI_VENDOR,
                .float_significance     = NVS_FLOAT_NANO,
                .ch_n_max               = AXIS_N,
@@ -3464,9 +3722,9 @@ static struct sensor_cfg nvi_cfg_dflt[] = {
                .name                   = "gyroscope",
                .snsr_id                = DEV_GYR,
                .kbuf_sz                = KBUF_SZ,
-               .snsr_data_n            = 8,
+               .snsr_data_n            = 14,
                .ch_n                   = AXIS_N,
-               .ch_sz                  = -2,
+               .ch_sz                  = -4,
                .vendor                 = NVI_VENDOR,
                .max_range              = {
                        .ival           = 3,
@@ -3518,6 +3776,26 @@ static struct sensor_cfg nvi_cfg_dflt[] = {
                .delay_us_min           = 10000,
                .delay_us_max           = 255000,
        },
+       {
+               .name                   = "geomagnetic_rotation_vector",
+               .snsr_id                = DEV_GMR,
+               .kbuf_sz                = KBUF_SZ,
+               .ch_n                   = 4,
+               .ch_sz                  = -4,
+               .vendor                 = NVI_VENDOR,
+               .delay_us_min           = 10000,
+               .delay_us_max           = 255000,
+       },
+       {
+               .name                   = "gyroscope_uncalibrated",
+               .snsr_id                = DEV_GYU,
+               .kbuf_sz                = KBUF_SZ,
+               .ch_n                   = AXIS_N,
+               .ch_sz                  = -2,
+               .vendor                 = NVI_VENDOR,
+               .delay_us_min           = 10000,
+               .delay_us_max           = 255000,
+       },
 };
 
 static int nvi_of_dt(struct nvi_state *st, struct device_node *dn)
@@ -3588,12 +3866,16 @@ static int nvi_init(struct nvi_state *st,
                        return -ENODEV;
                }
        }
+       for (i = DEV_STP; i < DEV_N; i++)
+               memcpy(&st->snsr[i].cfg.matrix, &st->snsr[DEV_GYR].cfg.matrix,
+                      sizeof(st->snsr[i].cfg.matrix));
+
        nvi_pm_init(st);
        ret = nvi_id_dev(st, i2c_dev_id);
        if (ret)
                return ret;
 
-       if (st->en_msk & (1 < FW_LOADED))
+       if (st->en_msk & (1 << FW_LOADED))
                ret = 0;
        else
                ret = nvi_dmp_fw(st);
index 1ce6284..f3b8d56 100644 (file)
@@ -36,7 +36,8 @@
 #define SRC_GYR                                (0)
 #define SRC_ACC                                (1)
 #define SRC_AUX                                (2)
-#define SRC_N                          (3)
+#define SRC_DMP                                (3)
+#define SRC_N                          (4)
 
 #define AXIS_X                         (0)
 #define AXIS_Y                         (1)
 #define DEV_SM                         (3)
 #define DEV_STP                                (4)
 #define DEV_QTN                                (5)
-#define DEV_N                          (6)
-#define DEV_AUX                                (6)
-#define DEV_N_AUX                      (7)
-#define DEV_DMP                                (8)
+#define DEV_GMR                                (6)
+#define DEV_GYU                                (7)
+#define DEV_N                          (8)
+#define DEV_AUX                                (8)
+#define DEV_N_AUX                      (9)
+#define DEV_DMP                                (9)
 #define FW_LOADED                      (16)
 #define EN_STDBY                       (17)
 #define EN_LP                          (18)
                                         (1 << DEV_TMP))
 #define MSK_DEV_DMP                    ((1 << DEV_SM) | \
                                         (1 << DEV_STP) | \
-                                        (1 << DEV_QTN))
+                                        (1 << DEV_QTN) | \
+                                        (1 << DEV_GMR) | \
+                                        (1 << DEV_GYU))
 #define MSK_DEV_SNSR                   (MSK_DEV_MPU | MSK_DEV_DMP)
 #define MSK_DEV_ALL                    (MSK_DEV_SNSR | (1 << DEV_AUX))
-#define MSK_PM_ON_FULL                 (1 << DEV_GYR)
+#define MSK_PM_ON_FULL                 ((1 << DEV_GYR) | (1 << DEV_GYU))
 #define MSK_PM_ON                      ((1 << DEV_TMP) | \
                                         (1 << DEV_AUX) | \
                                         MSK_DEV_DMP)
 #define BIT_ZMOT_EN                    (0x20)
 #define BIT_MOT_EN                     (0x40)
 #define BIT_6500_WOM_EN                        (0x40)
-#define BIT_SLV0_DLY_EN                        (0x01)
-#define BIT_SLV1_DLY_EN                        (0x02)
-#define BIT_SLV2_DLY_EN                        (0x04)
-#define BIT_SLV3_DLY_EN                        (0x08)
 #define BIT_DELAY_ES_SHADOW            (0x80)
 #define BIT_ACCEL_INTEL_MODE           (0x40)
 #define BIT_ACCEL_INTEL_ENABLE         (0x80)
 #define AUX_DEV_VALID_READ_LOOP_MAX    (20)
 #define AUX_DEV_VALID_READ_DELAY_MS    (5)
 
-#define DMP_HDR_LEN_MAX                        (4)
-
 struct nvi_state;
 
 struct nvi_rr {
@@ -285,35 +284,37 @@ struct nvi_rc {
        u8 reg_bank;
 };
 
-#define DMP_DEV_ABLE_LEN               (2)
-
-struct nvi_dmp_icm {
-       u16 en_addr;
-       u16 en_msk;
-       u16 odr_cfg;
-       u16 odr_cntr;
+struct nvi_mc_icm {
+       u32 data_out_ctl;
+       u32 data_intr_ctl;
+       u32 motion_event_ctl;
+       u32 accel_cal_rate;
+       u32 ped_rate;
+       u32 accel_alpha_var;
+       u32 accel_a_var;
+       u32 accel_only_gain;
+       u32 cpass_time_buffer;
+       u32 cpass_alpha_var;
+       u32 cpass_a_var;
+       u32 cpass_rad_3d_thr;
+       u32 cpass_nomot_var_thr;
+       u32 smd_mot_thld;
+       u32 smd_delay_thld;
+       u32 smd_delay2_thld;
+       u32 wom_enable;
 };
 
-struct nvi_dmp_mpu {
-       u16 en_addr;
-       u8 en_len;
-       u8 en[DMP_DEV_ABLE_LEN];
-       u8 dis[DMP_DEV_ABLE_LEN];
-       u16 odr_cfg;
-       u16 odr_cntr;
+struct nvi_mc_mpu {
+       u32 cfg_fifo_int;
+       u32 d_smd_mot_thld;
+       u32 d_smd_delay_thld;
+       u32 d_smd_delay2_thld;
 };
 
-struct nvi_dmp_dev {
-       unsigned int dev;
-       unsigned int data_n;
-       unsigned int aux_port;
-       unsigned int hdr_n;
-       u8 hdr[DMP_HDR_LEN_MAX];
-       u8 hdr_msk[DMP_HDR_LEN_MAX];
-       int (*fn_init)(struct nvi_state *st);
+struct nvi_mc {
        union {
-               struct nvi_dmp_icm icm;
-               struct nvi_dmp_mpu mpu;
+               struct nvi_mc_icm icm;
+               struct nvi_mc_mpu mpu;
        };
 };
 
@@ -323,15 +324,16 @@ struct nvi_dmp {
        unsigned int fw_crc32;
        unsigned int fw_mem_addr;
        unsigned int fw_start;
-       unsigned int dmp_period_us;
+       unsigned int dmp_reset_delay_ms;
        unsigned int dev_msk;
        unsigned int en_msk;
        unsigned int dd_n;
        const struct nvi_dmp_dev *dd;
+       int (*fn_rd)(struct nvi_state *st, s64 ts, unsigned int n);
+       int (*fn_clk_n)(struct nvi_state *st, u32 *clk_n);
        int (*fn_init)(struct nvi_state *st);
        int (*fn_en)(struct nvi_state *st);
        int (*fn_dev_init)(struct nvi_state *st, unsigned int dev);
-       int (*fn_dev_enable)(struct nvi_state *st, unsigned int dev, int port);
        int (*fn_dev_batch)(struct nvi_state *st, unsigned int dev, int port);
 };
 
@@ -370,7 +372,8 @@ struct nvi_snsr {
        unsigned int enable;
        unsigned int period_us;
        unsigned int timeout_us;
-       unsigned int fsync;
+       unsigned int sts;
+       unsigned int odr;
        unsigned int ts_n;
        s64 ts_push_delay;
        s64 push_delay_ns;
@@ -382,21 +385,26 @@ struct nvi_snsr {
 
 struct aux_port {
        struct nvi_mpu_port nmp;
-       unsigned short ext_data_offset;
+       unsigned int ext_data_offset;
+       unsigned int period_us;
+       unsigned int timeout_us;
+       unsigned int odr;
+       s64 ts_last;
+       bool ts_reset;
+       bool flush;
        bool hw_valid;
        bool hw_en;
        bool hw_do;
-       bool flush;
-       unsigned int period_us;
-       unsigned int timeout_us;
 };
 
 struct aux_ports {
        struct aux_port port[AUX_PORT_MAX];
        s64 bypass_timeout_ns;
        unsigned int bypass_lock;
+       unsigned int dmp_en_msk;
+       unsigned int dmp_ctrl_msk;
+       unsigned int ext_data_n;
        u8 delay_hw;
-       unsigned short ext_data_n;
        unsigned char ext_data[AUX_EXT_DATA_REG_MAX];
        unsigned char clock_i2c;
        bool reset_i2c;
@@ -430,21 +438,23 @@ struct nvi_state {
        struct notifier_block nb_vreg[2];
        const struct nvi_hal *hal;
        struct nvi_rc rc;
+       struct nvi_mc mc;
        struct aux_ports aux;
        unsigned int sts;
        unsigned int errs;
        unsigned int info;
        unsigned int en_msk;
+       unsigned int bm_timeout_us;
        struct nvi_snsr snsr[DEV_N_AUX];
        struct nvi_src src[SRC_N];
        int fifo_src;
-
-       unsigned int src_timeout_us[SRC_N];
-
        bool rc_dis;
+       bool mc_dis;
        bool irq_dis;
        bool irq_set_irq_wake;
        int pm;
+       u32 dmp_clk_n;
+       s64 ts_now;
        s64 ts_vreg_en[2];
        atomic64_t ts_irq;
 
@@ -468,10 +478,13 @@ int nvi_i2c_write_rc(struct nvi_state *st, const struct nvi_br *br, u32 val,
                     const char *fn, u8 *rc, bool be);
 int nvi_i2c_r(struct nvi_state *st, u8 bank, u8 reg, u16 len, u8 *buf);
 int nvi_i2c_rd(struct nvi_state *st, const struct nvi_br *br, u8 *buf);
-int nvi_mem_wr(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data,
+int nvi_mem_wr(struct nvi_state *st, u16 addr, u16 len, u8 *data,
               bool validate);
-int nvi_mem_wr_be(struct nvi_state *st, u16 mem_addr, u16 len, u32 val);
-int nvi_mem_rd(struct nvi_state *st, u16 mem_addr, u16 len, u8 *data);
+int nvi_mem_wr_be(struct nvi_state *st, u16 addr, u16 len, u32 val);
+int nvi_mem_wr_be_mc(struct nvi_state *st, u16 addr, u16 len,
+                    u32 val, u32 *mc);
+int nvi_mem_rd(struct nvi_state *st, u16 addr, u16 len, u8 *data);
+int nvi_mem_rd_le(struct nvi_state *st, u16 addr, u16 len, u32 *val);
 int nvi_wr_accel_offset(struct nvi_state *st, unsigned int axis, u16 offset);
 int nvi_wr_gyro_offset(struct nvi_state *st, unsigned int axis, u16 offset);
 int nvi_wr_fifo_cfg(struct nvi_state *st, int fifo);
@@ -482,8 +495,15 @@ int nvi_user_ctrl_en(struct nvi_state *st, const char *fn,
                     bool en_dmp, bool en_fifo, bool en_i2c, bool en_irq);
 int nvi_wr_pm1(struct nvi_state *st, const char *fn, u8 pm1);
 int nvi_pm_wr(struct nvi_state *st, const char *fn, u8 pm1, u8 pm2, u8 lp);
+int nvi_aux_enable(struct nvi_state *st, const char *fn,
+                  bool en_req, bool force);
+int nvi_period_aux(struct nvi_state *st);
 int nvi_aux_delay(struct nvi_state *st, const char *fn);
 void nvi_push_delay(struct nvi_state *st);
+int nvi_push(struct nvi_state *st, unsigned int dev, u8 *buf, s64 ts);
+s64 nvi_ts_dev(struct nvi_state *st, s64 ts_now,
+              unsigned int dev, unsigned int aux_port);
+void nvi_err(struct nvi_state *st);
 
 extern const struct nvi_hal nvi_hal_20628;
 extern const struct nvi_hal nvi_hal_6515;
index 5359ca4..9c5b733 100644 (file)
 #include "nvi.h"
 #include "nvi_dmp_icm.h"
 
-#define DMP_FREQ                       (102)
-#define DMP_PERIOD_US                  (9804)
-#define PEDOMETER_FREQ                 (DMP_FREQ >> 1)
-#define DEFAULT_ACCEL_GAIN             33554432L
-#define BASE_SAMPLE_RATE               (1125)
-#define DMP_DIVIDER                    (BASE_SAMPLE_RATE / DMP_FREQ)
+#define MPL520                         (1)
 
+#define DEFAULT_ACCEL_GAIN             (0x02000000)
+#define PED_ACCEL_GAIN                 (0x04000000)
+#define DMP_MULTI_SHIFT                        (30)
 
-static int nvi_mem_rdwr(struct nvi_state *st, u16 addr, u16 len,
-                       u32 val, u32 msk)
+#define DMP_HDR_LEN_MAX                        (4)
+#define DMP_HDR1_HDR2_MSK              (0x0008)
+#define DMP_HDR1_PUSH_MSK              (0xFEF0)
+#define DMP_DATA_OUT_CTL_HDR2_MSK      (0x0000FFFF)
+#define DMP_DATA_OUT_CTL_HDR2_BIT      (0x00080000)
+
+
+struct nvi_dmp_dev {
+       unsigned int dev;
+       unsigned int aux_port;
+       unsigned int depend_msk;        /* sensor dependencies */
+       unsigned int src_msk;
+       unsigned int period_us_dflt;
+       unsigned int buf_n;
+       int buf_shft;
+       bool matrix;
+       u32 out_ctl;
+       u16 int_ctl;
+       u16 odr_cfg;
+       u16 odr_cntr;
+       int (*fn_init)(struct nvi_state *st, unsigned int en_msk,
+                      u32 *out_ctl);
+};
+
+struct nvi_dmp_hdr {
+       unsigned int dev;
+       unsigned int aux_port;
+       unsigned int data_n;
+       u16 hdr_msk;
+};
+
+
+struct inv_dmp_acc_wtf {
+       unsigned int period_us;
+       unsigned int rate;
+       unsigned int gain;
+       unsigned int alpha;
+       unsigned int a;
+};
+
+static struct inv_dmp_acc_wtf inv_dmp_acc_wtfs[] = {
+       {
+               .period_us              = 1000,
+       },
+       {
+               .period_us              = 4444,
+               .rate                   = 3,
+               .gain                   = DEFAULT_ACCEL_GAIN,
+               .alpha                  = 0x33333333,
+               .a                      = 0x0CCCCCCD,
+       },
+       {
+               .period_us              = 9804,
+               .rate                   = 1,
+               .gain                   = DEFAULT_ACCEL_GAIN,
+               .alpha                  = 0x33333333,
+               .a                      = 0x0CCCCCCD,
+       },
+       {
+               .period_us              = 4902,
+               .rate                   = 0,
+               .gain                   = PED_ACCEL_GAIN,
+               .alpha                  = 0x33333333,
+               .a                      = 0x0CCCCCCD,
+       },
+       {
+               .period_us              = 66667,
+               .rate                   = 0,
+               .gain                   = DEFAULT_ACCEL_GAIN,
+               .alpha                  = 0x15555555,
+               .a                      = 0x2AAAAAAB,
+       },
+       {
+               .period_us              = 200000,
+               .rate                   = 0,
+               .gain                   = DEFAULT_ACCEL_GAIN,
+               .alpha                  = 0x06666666,
+               .a                      = 0x3999999A,
+       },
+};
+
+static int nvi_dmp_acc_init(struct nvi_state *st, unsigned int en_msk,
+                           u32 *out_ctl)
 {
-       u8 buf_rd[4];
-       u32 buf_wr;
-       unsigned int i;
+       unsigned int i = 0;
+       unsigned int j;
+       int min_diff;
+       int tmp;
        int ret;
 
-       ret = nvi_mem_rd(st, addr, len, buf_rd);
-       if (!ret) {
-               buf_wr = 0;
-               /* convert to little endian */
-               for (i = 0; i < len; i++) {
-                       buf_wr <<= 8;
-                       buf_wr |= buf_rd[i];
+       ret = st->hal->fn->en_acc(st);
+       min_diff =  inv_dmp_acc_wtfs[0].period_us;
+       for (j = 0; j < ARRAY_SIZE(inv_dmp_acc_wtfs); j++) {
+               tmp = abs(inv_dmp_acc_wtfs[j].period_us -
+                         st->src[SRC_ACC].period_us_src);
+               if (tmp < min_diff) {
+                       min_diff = tmp;
+                       i = j;
                }
-
-               buf_wr &= ~msk;
-               buf_wr |= val;
-               ret = nvi_mem_wr_be(st, addr, len, buf_wr);
        }
+
+       ret |= nvi_mem_wr_be_mc(st, ACCEL_CAL_RATE, 2,
+                               inv_dmp_acc_wtfs[i].rate,
+                               &st->mc.icm.accel_cal_rate);
+       ret |= nvi_mem_wr_be_mc(st, PED_RATE, 4, inv_dmp_acc_wtfs[i].rate,
+                               &st->mc.icm.ped_rate);
+       ret |= nvi_mem_wr_be_mc(st, ACCEL_ALPHA_VAR, 4,
+                               inv_dmp_acc_wtfs[i].alpha,
+                               &st->mc.icm.accel_alpha_var);
+       ret |= nvi_mem_wr_be_mc(st, ACCEL_A_VAR, 4, inv_dmp_acc_wtfs[i].a,
+                               &st->mc.icm.accel_a_var);
+       ret |= nvi_mem_wr_be_mc(st, ACCEL_ONLY_GAIN, 4,
+                               inv_dmp_acc_wtfs[i].gain,
+                               &st->mc.icm.accel_only_gain);
        return ret;
 }
 
-static int nvi_dmp_cntl(struct nvi_state *st, bool en, u16 addr, u32 msk)
+struct inv_dmp_gmf_wtf {
+       unsigned int period_us;
+       unsigned int ct;
+       unsigned int alpha_c;
+       unsigned int a_c;
+       unsigned int rad_3d;
+       unsigned int nomot_var_thr;
+};
+
+static struct inv_dmp_gmf_wtf inv_dmp_gmf_wtfs[] = {
+       {
+               .period_us              = 5000,
+               .ct                     = 200,
+       },
+       {
+               .period_us              = 14286,
+               .ct                     = 70,
+               .alpha_c                = 0x36DB6DB7,
+               .a_c                    = 0x09249249,
+               .rad_3d                 = 0x00000E00,
+               .nomot_var_thr          = 0x0000000C,
+       },
+       {
+               .period_us              = 28571,
+               .ct                     = 35,
+               .alpha_c                = 0x2DB6DB6E,
+               .a_c                    = 0x12492492,
+               .rad_3d                 = 0x00000E00,
+               .nomot_var_thr          = 0x0000000C,
+       },
+       {
+               .period_us              = 66667,
+               .ct                     = 15,
+               .alpha_c                = 0x15555555,
+               .a_c                    = 0x2AAAAAAB,
+               .rad_3d                 = 0x00000E00,
+               .nomot_var_thr          = 0x00000008,
+       },
+       {
+               .period_us              = 125000,
+               .ct                     = 8,
+               .alpha_c                = 0x06666666,
+               .a_c                    = 0x3999999A,
+               .rad_3d                 = 0x00000E00,
+               .nomot_var_thr          = 0x00000001,
+       },
+       {
+               .period_us              = 250000,
+               .ct                     = 4,
+               .alpha_c                = 0x06666666,
+               .a_c                    = 0x3999999A,
+               .rad_3d                 = 0x00000E00,
+               .nomot_var_thr          = 0x00000001,
+       },
+};
+
+/* gmf = GeoMagnetic Field (compass) */
+static int nvi_dmp_gmf_init(struct nvi_state *st, unsigned int en_msk,
+                           u32 *out_ctl)
 {
-       u32 val = 0;
+       unsigned int i = 0;
+       unsigned int j;
+       int min_diff;
+       int tmp;
+       int ret;
 
-       if (en)
-               val = msk;
-       return nvi_mem_rdwr(st, addr, 2, val, msk);
+       if (st->aux.port[0].nmp.type != SECONDARY_SLAVE_TYPE_COMPASS)
+               /* DMP shouldn't run if AUX device not supported */
+               return -EINVAL;
+
+       min_diff =  inv_dmp_gmf_wtfs[0].period_us;
+       for (j = 0; j < ARRAY_SIZE(inv_dmp_gmf_wtfs); j++) {
+               tmp = abs(inv_dmp_gmf_wtfs[j].period_us -
+                         st->src[SRC_AUX].period_us_src);
+               if (tmp < min_diff) {
+                       min_diff = tmp;
+                       i = j;
+               }
+       }
+
+       ret = nvi_mem_wr_be_mc(st, CPASS_TIME_BUFFER, 2,
+                              inv_dmp_gmf_wtfs[i].ct,
+                              &st->mc.icm.cpass_time_buffer);
+       ret |= nvi_mem_wr_be_mc(st, CPASS_ALPHA_VAR, 4,
+                               inv_dmp_gmf_wtfs[i].alpha_c,
+                               &st->mc.icm.cpass_alpha_var);
+       ret |= nvi_mem_wr_be_mc(st, CPASS_A_VAR, 4,
+                               inv_dmp_gmf_wtfs[i].a_c,
+                               &st->mc.icm.cpass_a_var);
+       ret |= nvi_mem_wr_be_mc(st,
+                               CPASS_RADIUS_3D_THRESH_ANOMALY, 4,
+                               inv_dmp_gmf_wtfs[i].rad_3d,
+                               &st->mc.icm.cpass_rad_3d_thr);
+       ret |= nvi_mem_wr_be_mc(st, CPASS_NOMOT_VAR_THRESH, 4,
+                               inv_dmp_gmf_wtfs[i].nomot_var_thr,
+                               &st->mc.icm.cpass_nomot_var_thr);
+       return ret;
 }
 
-static int nvi_dmp_press_init(struct nvi_state *st)
+/* prs = pressure */
+static int nvi_dmp_prs_init(struct nvi_state *st, unsigned int en_msk,
+                           u32 *out_ctl)
 {
        return 1;
 }
 
-static int nvi_dmp_cpass_init(struct nvi_state *st)
+static int nvi_dmp_gyr_init(struct nvi_state *st, unsigned int en_msk,
+                           u32 *out_ctl)
 {
-       int ret = 1;
-
-       if (st->aux.port[0].nmp.type == SECONDARY_SLAVE_TYPE_COMPASS) {
-               if (st->snsr[DEV_AUX].enable & (1 << 0))
-                       ret = nvi_aux_delay(st, __func__);
-       }
-       return ret;
+       st->snsr[DEV_GYR].sts = 1;
+       return st->hal->fn->en_gyr(st);
 }
 
-static int nvi_dmp_sm_init(struct nvi_state *st)
+static int nvi_dmp_sm_init(struct nvi_state *st, unsigned int en_msk,
+                          u32 *out_ctl)
 {
        int ret;
 
-       ret = nvi_mem_wr_be(st, SMD_MOT_THLD, 4,
-                           st->snsr[DEV_SM].cfg.thresh_lo << 16);
-       ret |= nvi_mem_wr_be(st, SMD_DELAY_THLD, 4,
-                            st->snsr[DEV_SM].cfg.thresh_hi);
-       ret |= nvi_mem_wr_be(st, SMD_DELAY2_THLD, 4,
-                            st->snsr[DEV_SM].cfg.delay_us_max);
+       ret = nvi_mem_wr_be_mc(st, SMD_MOT_THLD, 4,
+                              st->snsr[DEV_SM].cfg.thresh_lo << 16,
+                              &st->mc.icm.smd_mot_thld);
+       ret |= nvi_mem_wr_be_mc(st, SMD_DELAY_THLD, 4,
+                               st->snsr[DEV_SM].cfg.thresh_hi,
+                               &st->mc.icm.smd_delay_thld);
+       ret |= nvi_mem_wr_be_mc(st, SMD_DELAY2_THLD, 4,
+                               st->snsr[DEV_SM].cfg.delay_us_max,
+                               &st->mc.icm.smd_delay2_thld);
        return ret;
 }
 
+static int nvi_dmp_gmr_init(struct nvi_state *st, unsigned int en_msk,
+                           u32 *out_ctl)
+{
+       if (out_ctl) {
+               if (en_msk & ((1 << DEV_GYR) | (1 << DEV_GYU)))
+                       /* if gyro is enabled GMR becomes QTN 9-axis */
+                       *out_ctl |= 0x04000000;
+               else
+                       *out_ctl |= 0x04000200;
+       }
+       return 0;
+}
+
 static struct nvi_dmp_dev nvi_dmp_devs[] = {
        {
                .dev                    = DEV_ACC,
-               .data_n                 = 12,
-               .hdr_n                  = 2,
-               .hdr                    = { 0x80, 0x00, 0x00, 0x00 },
-               .hdr_msk                = { 0xFF, 0xF0, 0x00, 0x00 },
-               .icm                    = {
-                       .en_addr        = DATA_OUT_CTL1,
-                       .en_msk         = 0x8000,
-                       .en_msk         = 0,
-                       .odr_cfg        = ODR_ACCEL,
-                       .odr_cntr       = ODR_CNTR_ACCEL,
-               },
+               .buf_n                  = 12,
+               .buf_shft               = -11,
+               .matrix                 = true,
+               .out_ctl                = 0x80000000,
+               .int_ctl                = ACCEL_SET,
+               .odr_cfg                = ODR_ACCEL,
+               .odr_cntr               = ODR_CNTR_ACCEL,
+               .fn_init                = &nvi_dmp_acc_init,
        },
        {
                .dev                    = DEV_GYR,
-               .data_n                 = 6,
-               .hdr_n                  = 2,
-               .hdr                    = { 0x40, 0x00 },
-               .hdr_msk                = { 0xFF, 0xF0 },
-               .icm                    = {
-                       .en_addr        = DATA_OUT_CTL1,
-                       .en_msk         = 0x4000,
-                       .odr_cfg        = ODR_GYRO,
-                       .odr_cntr       = ODR_CNTR_GYRO,
-               },
+               .depend_msk             = (1 << DEV_GYU),
+#ifdef MPL520
+               .buf_n                  = 6,
+#else /* MPL520 */
+               .buf_n                  = 12,
+#endif /* MPL520 */
+               .matrix                 = true,
+               .out_ctl                = 0x00400000,
+               .int_ctl                = GYRO_CALIBR_SET,
+               .odr_cfg                = ODR_GYRO_CALIBR,
+               .odr_cntr               = ODR_CNTR_GYRO_CALIBR,
+               .fn_init                = &nvi_dmp_gyr_init,
+       },
+       {
+               .dev                    = DEV_GYU,
+               .depend_msk             = (1 << DEV_GYR),
+               .buf_n                  = 6,
+               .matrix                 = true,
+               .out_ctl                = 0x40000000,
+               .int_ctl                = GYRO_SET,
+               .odr_cfg                = ODR_GYRO,
+               .odr_cntr               = ODR_CNTR_GYRO,
        },
        {
                .dev                    = DEV_QTN,
-               .data_n                 = 12,
-               .hdr_n                  = 4,
-               .hdr                    = { 0x08, 0x00, 0x04, 0x00 },
-               .hdr_msk                = { 0xFF, 0xF0, 0xFF, 0xF0 },
-               .icm                    = {
-                       .en_addr        = DATA_OUT_CTL1,
-                       .en_msk         = 0x0800,
-                       .en_msk         = 0,
-                       .odr_cfg        = ODR_QUAT6,
-                       .odr_cntr       = ODR_CNTR_QUAT6,
-               },
+               .depend_msk             = (1 << DEV_ACC) |
+                                         (1 << DEV_GYR) |
+                                         (1 << DEV_GYU),
+               .src_msk                = (1 << SRC_ACC) |
+                                         (1 << SRC_GYR) |
+                                         (1 << SRC_AUX),
+               .period_us_dflt         = ICM_DMP_PERIOD_US,
+               .buf_n                  = 12,
+               .matrix                 = true,
+               .out_ctl                = 0x08000000,
+               .int_ctl                = QUAT6_SET,
+               .odr_cfg                = ODR_QUAT6,
+               .odr_cntr               = ODR_CNTR_QUAT6,
+       },
+       {
+               .dev                    = DEV_GMR,
+               .depend_msk             = (1 << DEV_ACC) |
+                                         (0x03 << DEV_N_AUX),
+               .src_msk                = (1 << SRC_ACC) |
+                                         (1 << SRC_GYR) |
+                                         (1 << SRC_AUX),
+               .period_us_dflt         = ICM_DMP_PERIOD_US,
+               .buf_n                  = 14,
+               .int_ctl                = QUAT9_SET,
+               .odr_cfg                = ODR_QUAT9,
+               .odr_cntr               = ODR_CNTR_QUAT9,
+               .fn_init                = &nvi_dmp_gmr_init,
        },
        {
                .dev                    = DEV_SM,
-               .data_n                 = 1,
-               .hdr_n                  = 0,
+               .depend_msk             = (1 << DEV_ACC),
+               .src_msk                = (1 << SRC_ACC),
+               .period_us_dflt         = ICM_DMP_PERIOD_US,
+               .buf_n                  = 1,
+               .out_ctl                = 0x80000000,
                .fn_init                = &nvi_dmp_sm_init,
-               .icm                    = {
-                       .en_addr        = DATA_OUT_CTL1,
-                       .en_msk         = 0,
-                       .odr_cfg        = 0,
-                       .odr_cntr       = 0,
-               },
        },
        {
                .dev                    = DEV_AUX,
+               .aux_port               = 0,
+               .depend_msk             = (0x03 << DEV_N_AUX),
+               .buf_n                  = 6,
+               .out_ctl                = 0x20000000,
+               .int_ctl                = CPASS_SET,
+               .odr_cfg                = ODR_CPASS,
+               .odr_cntr               = ODR_CNTR_CPASS,
+               .fn_init                = &nvi_dmp_gmf_init,
+       },
+       {
+               .dev                    = DEV_AUX,
+               .aux_port               = 2,
+               .depend_msk             = (0x0C << DEV_N_AUX),
+               .buf_n                  = 6,
+               .out_ctl                = 0x00800000,
+               .int_ctl                = PRESSURE_SET,
+               .odr_cfg                = ODR_PRESSURE,
+               .odr_cntr               = ODR_CNTR_PRESSURE,
+               .fn_init                = &nvi_dmp_prs_init,
+       },
+};
+
+/* devices must be in the order the data comes out of the DMP */
+static struct nvi_dmp_hdr nvi_dmp_hdr2s[] = {
+       {
+               .dev                    = DEV_ACC,
+               .data_n                 = 2,
+               .hdr_msk                = 0x4000,
+       },
+       {
+               .dev                    = DEV_GYR,
+               .data_n                 = 2,
+               .hdr_msk                = 0x2000,
+       },
+       {
+               .dev                    = DEV_AUX,
+               .aux_port               = 0,
+               .data_n                 = 2,
+               .hdr_msk                = 0x1000,
+       },
+       {
+               .dev                    = -1,
                .data_n                 = 6,
+               .hdr_msk                = 0x0080,
+       },
+};
+
+/* devices must be in the order the data comes out of the DMP */
+static struct nvi_dmp_hdr nvi_dmp_hdr1s[] = {
+       {
+               .dev                    = DEV_ACC,
+               .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,
-               .hdr_n                  = 2,
-               .hdr                    = { 0x20, 0x00 },
-               .hdr_msk                = { 0xFF, 0xF0 },
-               .fn_init                = &nvi_dmp_cpass_init,
-               .icm                    = {
-                       .en_addr        = DATA_OUT_CTL1,
-                       .en_msk         = 0x2000,
-                       .odr_cfg        = ODR_CPASS,
-                       .odr_cntr       = ODR_CNTR_CPASS,
-               },
+               .data_n                 = 6,
+               .hdr_msk                = 0x2000,
        },
        {
                .dev                    = DEV_AUX,
+               .aux_port               = -1,
+               .data_n                 = 8,
+               .hdr_msk                = 0x1000,
+       },
+       {
+               .dev                    = DEV_QTN,
+               .data_n                 = 12,
+               .hdr_msk                = 0x0800,
+       },
+       {
+               .dev                    = DEV_GMR,
+               .data_n                 = 14,
+               .hdr_msk                = 0x0400,
+       },
+       {
+               .dev                    = -1, /* PQUAT6 */
                .data_n                 = 6,
+               .hdr_msk                = 0x0200,
+       },
+       {
+               .dev                    = DEV_AUX,
                .aux_port               = 2,
-               .hdr_n                  = 2,
-               .hdr                    = { 0x80, 0x00 },
-               .hdr_msk                = { 0xFF, 0xF0 },
-               .fn_init                = &nvi_dmp_press_init,
-               .icm                    = {
-                       .en_addr        = DATA_OUT_CTL1,
-                       .en_msk         = 0x0080,
-                       .odr_cfg        = ODR_PRESSURE,
-                       .odr_cntr       = ODR_CNTR_PRESSURE,
-               },
+               .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,
+               .hdr_msk                = 0x0020,
+       },
+       {
+               .dev                    = DEV_STP,
+               .data_n                 = 4,
+               .hdr_msk                = 0x0010,
        },
 };
 
-static int nvi_dd_odr(struct nvi_state *st, struct nvi_dmp_dev *dd)
+static int nvi_dmp_rd(struct nvi_state *st, s64 ts, unsigned int n)
 {
-       u16 odr_cfg;
-       int ret;
+       struct nvi_dmp_hdr *dh;
+       struct aux_port *ap;
+       unsigned int data_n;
+       unsigned int hdr1_i;
+       unsigned int hdr2_i;
+       unsigned int i;
+       unsigned int j;
+       u16 hdr1;
+       u16 hdr2;
 
-       if (dd->dev == DEV_AUX)
-               odr_cfg = (st->aux.port[dd->aux_port].period_us /
-                          DMP_PERIOD_US) - 1;
-       else
-               odr_cfg = (st->snsr[dd->dev].period_us / DMP_PERIOD_US) - 1;
-       ret = nvi_mem_wr_be(st, dd->icm.odr_cntr, 2, 0);
-       ret |= nvi_mem_wr_be(st, dd->icm.odr_cfg, 2, odr_cfg);
-       ret |= nvi_mem_wr_be(st, dd->icm.odr_cntr, 2, 0);
-       return ret;
+       while (n > DMP_HDR_LEN_MAX) {
+               if (st->sts & (NVS_STS_SUSPEND | NVS_STS_SHUTDOWN))
+                       return -1;
+
+               hdr1 = (u16)be16_to_cpup((__be16 *)(&st->buf[st->buf_i]));
+               data_n = sizeof(hdr1);
+               hdr1_i = sizeof(hdr1);
+               /* make sure we have all the data for this header */
+               for (i = 0; i < ARRAY_SIZE(nvi_dmp_hdr1s); i++) {
+                       if (hdr1 & nvi_dmp_hdr1s[i].hdr_msk)
+                               data_n += nvi_dmp_hdr1s[i].data_n;
+               }
+               if (hdr1 & DMP_HDR1_HDR2_MSK) {
+                       hdr2 = (u16)be16_to_cpup((__be16 *)
+                                                (&st->buf[st->buf_i +
+                                                          sizeof(hdr1)]));
+                       data_n += sizeof(hdr2);
+                       hdr2_i = data_n; /* index to header2 data */
+                       hdr1_i += sizeof(hdr2); /* index to header1 data */
+                       for (i = 0; i < ARRAY_SIZE(nvi_dmp_hdr2s); i++) {
+                               if (hdr2 & nvi_dmp_hdr2s[i].hdr_msk)
+                                       data_n += nvi_dmp_hdr2s[i].data_n;
+                       }
+               } else {
+                       hdr2 = 0;
+               }
+
+               if (n < data_n)
+                       /* not enough data to process */
+                       return 0;
+
+               if (st->sts & NVI_DBG_SPEW_FIFO)
+                       dev_info(&st->i2c->dev, "%s DMP HDR1=%04x HDR2=%04x\n",
+                                __func__, hdr1, hdr2);
+               /* we process header2 accuracy data first so that it becomes
+                * part of the header1 status data when pushed.
+                */
+               for (i = 0; i < ARRAY_SIZE(nvi_dmp_hdr2s) && hdr2; i++) {
+                       dh = &nvi_dmp_hdr2s[i];
+                       if (hdr2 & dh->hdr_msk) {
+                               hdr2 &= ~dh->hdr_msk;
+                               if (dh->dev < DEV_N) {
+                                       st->snsr[dh->dev].sts = 0;
+                                       for (j = 0; j < dh->data_n; j++) {
+                                               st->snsr[dh->dev].sts <<= 8;
+                                               st->snsr[dh->dev].sts |=
+                                                           st->buf[st->buf_i +
+                                                                   hdr2_i];
+                                               hdr2_i++;
+                                       }
+                               } else {
+                                       hdr2_i += dh->data_n;
+                               }
+                       }
+               }
+
+               if (hdr2) {
+                       /* unknown header: lost DMP sync so DMP reset */
+                       if (st->sts & NVI_DBG_SPEW_FIFO)
+                               dev_err(&st->i2c->dev,
+                                       "%s ERR: DMP sync HDR2=%hx\n",
+                                       __func__, hdr2);
+                       nvi_err(st);
+                       return -1;
+               }
+
+               /* process push data */
+               hdr1 &= DMP_HDR1_PUSH_MSK;
+               for (i = 0; i < ARRAY_SIZE(nvi_dmp_hdr1s) && hdr1; i++) {
+                       dh = &nvi_dmp_hdr1s[i];
+                       if (hdr1 & dh->hdr_msk) {
+                               hdr1 &= ~dh->hdr_msk;
+                               if (dh->dev == DEV_AUX &&
+                                                dh->aux_port < AUX_PORT_MAX &&
+                                     st->aux.port[dh->aux_port].nmp.handler) {
+                                       ap = &st->aux.port[dh->aux_port];
+                                       ap->nmp.handler(&st->buf[st->buf_i +
+                                                                hdr1_i],
+                                                       dh->data_n,
+                                                       nvi_ts_dev(st, 0,
+                                                                  dh->dev,
+                                                                dh->aux_port),
+                                                       ap->nmp.ext_driver);
+                               } else if (dh->dev < DEV_N) {
+                                       nvi_push(st, dh->dev,
+                                                &st->buf[st->buf_i + hdr1_i],
+                                                nvi_ts_dev(st, ts,
+                                                           dh->dev, 0));
+                               }
+                               hdr1_i += dh->data_n;
+                       }
+               }
+
+               st->buf_i += data_n;
+               n -= data_n;
+               if (hdr1) {
+                       /* unknown header: lost DMP sync so DMP reset */
+                       if (st->sts & NVI_DBG_SPEW_FIFO)
+                               dev_err(&st->i2c->dev,
+                                       "%s ERR: DMP sync HDR1: %x\n",
+                                       __func__, hdr1);
+                       nvi_err(st);
+                       return -1;
+               }
+       }
+
+       return 0;
+}
+
+static int nvi_dmp_clk_n(struct nvi_state *st, u32 *clk_n)
+{
+       return nvi_mem_rd_le(st, DMPRATE_CNTR, 4, clk_n);
 }
 
-static int nvi_dd_en(struct nvi_state *st, struct nvi_dmp_dev *dd)
+static int nvi_dmp_period(struct nvi_state *st, unsigned int en_msk)
 {
+       struct nvi_dmp_dev *dd;
+       u16 int_ctl = 0;
+       u16 odr_cfg;
+       unsigned int period_us_req[SRC_N];
+       unsigned int period_us;
+       unsigned int period_us_int = -1;
+       unsigned int src_msk;
+       unsigned int src;
+       unsigned int i;
+       unsigned int j = -1;
        int ret;
+       int ret_t = 0;
 
-       if (st->snsr[dd->dev].enable) {
-               if (dd->fn_init)
-                       ret = dd->fn_init(st);
-               else
-                       ret = 0;
-               if (ret > 0) {
-                       /* ignore without error */
-                       ret = 0;
-               } else if (!ret) {
-                       ret = nvi_dd_odr(st, dd);
-                       ret |= nvi_dmp_cntl(st, true, dd->icm.en_addr,
-                                           dd->icm.en_msk);
+       /* initialize source period */
+       for (src = 0; src < st->hal->src_n; src++)
+               period_us_req[src] = st->hal->src[src].period_us_max;
+
+       /* set source's period_us_req[] to fastest DMP device */
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if ((!st->snsr[dd->dev].period_us) || !(en_msk &
+                                                       (1 << dd->dev)))
+                       continue;
+
+               src_msk = dd->src_msk; /* use all of device's sources */
+               if (st->hal->dev[dd->dev]->src >= 0)
+                       src_msk |= (1 << st->hal->dev[dd->dev]->src);
+               for (src = 0; src < st->hal->src_n; src++) {
+                       if (!(src_msk & (1 << src)))
+                               continue;
+
+                       if (st->snsr[dd->dev].period_us < period_us_req[src])
+                               period_us_req[src] =
+                                                  st->snsr[dd->dev].period_us;
                }
-               if (ret && (dd->dev != DEV_AUX))
-                       st->en_msk &= ~(1 << dd->dev);
-       } else {
-               ret = nvi_dmp_cntl(st, true, dd->icm.en_addr, dd->icm.en_msk);
        }
-       return ret;
+
+       /* override above fastest speeds with any faster defaults */
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if ((!dd->period_us_dflt) || !(en_msk & (1 << dd->dev)))
+                       continue;
+
+               src_msk = dd->src_msk; /* use all of device's sources */
+               if (st->hal->dev[dd->dev]->src >= 0)
+                       src_msk |= (1 << st->hal->dev[dd->dev]->src);
+               for (src = 0; src < st->hal->src_n; src++) {
+                       if (src_msk & (1 << src) && (dd->period_us_dflt <
+                                                    period_us_req[src]))
+                               period_us_req[src] = dd->period_us_dflt;
+               }
+       }
+
+       /* WAR: SRC_ACC && SRC_GYR must be same speed for timestamps to sync.
+        * SRC_AUX has timestamps set to ts_now = 0 since SRC_AUX has fixed
+        * rates and can't sync with the other sources.
+        */
+       period_us = -1;
+       for (src = 0; src < st->hal->src_n; src++) {
+               if (period_us_req[src] < period_us)
+                       period_us = period_us_req[src];
+       }
+       for (src = 0; src < st->hal->src_n; src++)
+               period_us_req[src] = period_us;
+       /* WAR: end */
+
+       /* program the sources */
+       for (src = 0; src < st->hal->src_n; src++) {
+               if (!(st->hal->src[src].dev_msk & en_msk))
+                       continue;
+
+               if (period_us_req[src] < st->hal->src[src].period_us_min)
+                       period_us_req[src] = st->hal->src[src].period_us_min;
+               if (period_us_req[src] > st->hal->src[src].period_us_max)
+                       period_us_req[src] = st->hal->src[src].period_us_max;
+               st->src[src].period_us_req = period_us_req[src];
+               switch (src) {
+               case SRC_GYR:
+               case SRC_ACC:
+                       i = (st->src[src].period_us_req * 1000) /
+                                                          st->src[src].base_t;
+                       if (i)
+                               i--;
+                       ret = nvi_i2c_write_rc(st, &st->hal->reg->smplrt[src],
+                                              i, __func__,
+                                             (u8 *)&st->rc.smplrt[src], true);
+                       if (ret)
+                               ret_t |= ret;
+                       else
+                               st->src[src].period_us_src = ((i + 1) *
+                                                  st->src[src].base_t) / 1000;
+                       break;
+
+               case SRC_AUX:
+                       ret_t |= st->hal->src[SRC_AUX].fn_period(st);
+                       break;
+               }
+       }
+
+       /* now set each DMP device's ODR based on their source */
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if (dd->dev >= DEV_N_AUX || !(dd->odr_cfg | dd->odr_cntr))
+                       /* dd (DMP Device) is not ODR configurable */
+                       continue;
+
+               if (!(en_msk & (1 << dd->dev)))
+                       continue;
+
+               if (dd->dev == DEV_AUX) {
+                       if (!(en_msk & (1 << (dd->aux_port + DEV_N_AUX))))
+                               continue;
+
+                       period_us = st->aux.port[dd->aux_port].period_us;
+               } else {
+                       period_us = st->snsr[dd->dev].period_us;
+               }
+               if (st->bm_timeout_us) {
+                       /* batch mode on */
+                       int_ctl |= dd->int_ctl;
+               } else {
+                       if (dd->int_ctl && period_us) {
+                               /* IRQ on fastest period */
+                               if (period_us < period_us_int) {
+                                       period_us_int = period_us;
+                                       j = i;
+                               }
+                       }
+               }
+               src = st->hal->dev[dd->dev]->src;
+               if (src < st->hal->src_n) {
+                       odr_cfg = period_us / st->src[src].period_us_src;
+                       if (odr_cfg)
+                               odr_cfg--;
+               } else {
+                       odr_cfg = -1;
+               }
+               if (dd->dev == DEV_AUX)
+                       st->aux.port[dd->aux_port].odr = odr_cfg;
+               else
+                       st->snsr[dd->dev].odr = odr_cfg;
+               ret_t |= nvi_mem_wr_be(st, dd->odr_cntr, 2, 0);
+               ret_t |= nvi_mem_wr_be(st, dd->odr_cfg, 2, odr_cfg);
+               ret_t |= nvi_mem_wr_be(st, dd->odr_cntr, 2, 0);
+       }
+
+       if (j != -1)
+               int_ctl |= nvi_dmp_devs[j].int_ctl;
+       if (en_msk & (1 << DEV_STP))
+               int_ctl |= PED_STEPDET_SET;
+       ret_t |= nvi_mem_wr_be_mc(st, DATA_INTR_CTL, 2, int_ctl,
+                                 &st->mc.icm.data_intr_ctl);
+       /* WAR: DMPRATE_CNTR only runs off of SRC_ACC.  If SRC_ACC is off then
+        * timestamps will switch to realtime when DMPRATE_CNTR doesn't tick.
+        */
+       st->src[SRC_DMP].period_us_src = st->src[SRC_ACC].period_us_src;
+       return ret_t;
+}
+
+static int nvi_dmp_irq(struct nvi_state *st)
+{
+       u32 able;
+
+       if ((st->en_msk & MSK_DEV_ALL) & ~((1 << DEV_SM) | (1 << DEV_STP)))
+               /* DMP requires FIFO IRQ */
+               able = 0;
+       else
+               /* DMP IRQ is in event mode */
+               able = 1;
+       return nvi_mem_wr_be_mc(st, WOM_ENABLE, 4, able,
+                               &st->mc.icm.wom_enable);
 }
 
 static struct nvi_dmp_dev *nvi_dd(struct nvi_state *st,
@@ -229,28 +802,6 @@ static struct nvi_dmp_dev *nvi_dd(struct nvi_state *st,
        return NULL;
 }
 
-static int nvi_dd_batch(struct nvi_state *st, unsigned int dev, int port)
-{
-       struct nvi_dmp_dev *dd;
-
-       dd = nvi_dd(st, dev, port);
-       if (dd == NULL)
-               return -EINVAL;
-
-       return nvi_dd_odr(st, dd);
-}
-
-static int nvi_dd_enable(struct nvi_state *st, unsigned int dev, int port)
-{
-       struct nvi_dmp_dev *dd;
-
-       dd = nvi_dd(st, dev, port);
-       if (dd == NULL)
-               return -EINVAL;
-
-       return nvi_dd_en(st, dd);
-}
-
 static int nvi_dd_init(struct nvi_state *st, unsigned int dev)
 {
        struct nvi_dmp_dev *dd;
@@ -261,136 +812,73 @@ static int nvi_dd_init(struct nvi_state *st, unsigned int dev)
                return -EINVAL;
 
        if (dd->fn_init)
-               ret = dd->fn_init(st);
+               ret = dd->fn_init(st, st->en_msk | (st->aux.dmp_en_msk <<
+                                                   DEV_N_AUX), NULL);
        return ret;
 }
 
-#define PED_ACCEL_GAIN           67108864L
-
-struct inv_accel_cal_params {
-       unsigned int freq;
-       unsigned int rate;
-       unsigned int gain;
-       unsigned int alpha;
-       unsigned int a;
+static const u16 nvi_dmp_gmf_mtx_addrs[] = {
+       CPASS_MTX_00,
+       CPASS_MTX_01,
+       CPASS_MTX_02,
+       CPASS_MTX_10,
+       CPASS_MTX_11,
+       CPASS_MTX_12,
+       CPASS_MTX_20,
+       CPASS_MTX_21,
+       CPASS_MTX_22,
 };
 
-static struct inv_accel_cal_params accel_cal_para[] = {
-       {
-               .freq = 1000,
-       },
-       {
-               .freq = 225,
-               .rate = 3,
-               .gain = DEFAULT_ACCEL_GAIN,
-               .alpha = 858993459,
-               .a     = 214748365,
-       },
-       {
-               .freq = 102,
-               .gain = DEFAULT_ACCEL_GAIN,
-               .rate = 1,
-               .alpha = 858993459,
-               .a     = 214748365,
-       },
-       {
-               .freq = PEDOMETER_FREQ,
-               .gain = PED_ACCEL_GAIN,
-               .rate = 0,
-               .alpha = 858993459,
-               .a     = 214748365,
-       },
-       {
-               .freq = 15,
-               .gain = DEFAULT_ACCEL_GAIN,
-               .rate = 0,
-               .alpha = 357913941,
-               .a     = 715827883,
-       },
-       {
-               .freq = 5,
-               .gain = DEFAULT_ACCEL_GAIN,
-               .rate = 0,
-               .alpha = 107374182,
-               .a     = 966367642,
-       },
-};
-
-static int nvi_dmp_en(struct nvi_state *st)
+static int nvi_dmp_gmf_mtx(struct nvi_state *st, u32 *mtx)
 {
        unsigned int i;
        int ret = 0;
 
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_gmf_mtx_addrs); i++)
+               ret |= nvi_mem_wr_be(st, nvi_dmp_gmf_mtx_addrs[i], 4, mtx[i]);
 
+       return ret;
+}
 
-       st->sts ^= NVI_DBG_SPEW_MSG;
-       st->rc_dis = true;
-
-
-       nvi_wr_pm1(st, __func__, 0x01);
-       nvi_i2c_wr_rc(st, &st->hal->reg->pm2, 0x00, __func__, &st->rc.pm2);
-       nvi_i2c_wr_rc(st, &st->hal->reg->fifo_cfg, 0x01,
-                     __func__, &st->rc.fifo_cfg);
-       nvi_i2c_wr_rc(st, &st->hal->reg->fifo_sz, 0x03,
-                     __func__, &st->rc.fifo_sz);
-
-
-       ret |= nvi_i2c_write_rc(st, &st->hal->reg->fifo_en, 0,
-                               __func__, (u8 *)&st->rc.fifo_en, false);
-
-
-       nvi_i2c_wr(st, &st->hal->reg->fifo_rst, 0x1F, __func__);
-       nvi_i2c_wr(st, &st->hal->reg->fifo_rst, 0x1E, __func__);
-
-
-       /* reset DMP and FIFO */
-       nvi_i2c_wr_rc(st, &st->hal->reg->user_ctrl, 0x0C,
-                     __func__, &st->rc.user_ctrl);
-
-       msleep(20);
-
-       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++)
-               ret |= nvi_dd_en(st, &nvi_dmp_devs[i]);
-
+static int nvi_dmp_init_gmf(struct nvi_state *st)
+{
+       struct nvi_mpu_port *nmp;
+       s8 mtrx[AXIS_N * 3];
+       s32 mtx[ARRAY_SIZE(nvi_dmp_gmf_mtx_addrs)];
+       int adj[ARRAY_SIZE(nvi_dmp_gmf_mtx_addrs)];
+       unsigned int i;
+       unsigned int j;
+       unsigned int k;
 
-       ret |= nvi_mem_wr_be(st, DATA_INTR_CTL, 2, 0xC000);
+       if (st->aux.port[0].nmp.type != SECONDARY_SLAVE_TYPE_COMPASS)
+               return -EINVAL;
 
-       ret |= nvi_mem_wr_be(st, ACCEL_CAL_RATE, 2, accel_cal_para[i].rate);
-       ret |= nvi_mem_wr_be(st, PED_RATE, 4, accel_cal_para[i].rate);
-       ret |= nvi_mem_wr_be(st, ACCEL_ALPHA_VAR, 4, accel_cal_para[i].alpha);
-       ret |= nvi_mem_wr_be(st, ACCEL_A_VAR, 4, accel_cal_para[i].a);
-       ret |= nvi_mem_wr_be(st, ACCEL_ONLY_GAIN, 4, accel_cal_para[i].gain);
-       /* inv_enable_accel_cal_V3 */
-       ret |= nvi_dmp_cntl(st, st->en_msk & (1 << DEV_ACC),
-                           MOTION_EVENT_CTL, ACCEL_CAL_EN << 8);
-       /* inv_enable_gyro_cal_V3 */
-       ret |= nvi_dmp_cntl(st, st->en_msk & (1 << DEV_GYR),
-                           MOTION_EVENT_CTL, GYRO_CAL_EN << 8);
-       /* inv_enable_compass_cal_V3 */
-       ret |= nvi_dmp_cntl(st, 0, MOTION_EVENT_CTL, COMPASS_CAL_EN); /* dis */
-       /* inv_enable_9axes_V3 */
-       ret |= nvi_dmp_cntl(st, 1, MOTION_EVENT_CTL, NINE_AXIS_EN); /* en */
-       /* inv_setup_events */
-       ret |= nvi_dmp_cntl(st, st->en_msk & (1 << DEV_STP),
-                           MOTION_EVENT_CTL, PEDOMETER_EN << 8);
-       ret |= nvi_dmp_cntl(st, st->en_msk & (1 << DEV_SM),
-                           MOTION_EVENT_CTL, SMD_EN << 8);
-       /* inv_set_wom */
-       ret |= nvi_mem_wr_be(st, WOM_ENABLE, 4,
-                            !!(st->en_msk & (1 << DEV_SM)));
+       nmp = &st->aux.port[0].nmp;
+       for (i = 0; i < AXIS_N; i++) {
+               for (j = 0; j < AXIS_N; j++)
+                       mtrx[AXIS_N * j + i] =
+                                 st->snsr[DEV_ACC].cfg.matrix[AXIS_N * i + j];
+       }
 
+       for (i = 0; i < 9; i++) {
+               adj[i] = nmp->matrix[i] * (nmp->q30[i % AXIS_N] >>
+                                          DMP_MULTI_SHIFT);
+               mtx[i] = 0;
+       }
 
-       st->en_msk |= (1 << DEV_DMP);
-       nvi_int_able(st, __func__, true); /* 0x38 = 0x02 */
+       for (i = 0; i < AXIS_N; i++) {
+               for (j = 0; j < AXIS_N; j++) {
+                       for (k = 0; k < AXIS_N; k++)
+                               mtx[AXIS_N * i + j] += mtrx[AXIS_N * i + k] *
+                                                      adj[AXIS_N * k + j];
+               }
+       }
 
-       nvi_i2c_wr_rc(st, &st->hal->reg->user_ctrl, 0xC0,
-                     __func__, &st->rc.user_ctrl);
-       return 0;
+       return nvi_dmp_gmf_mtx(st, mtx);
 }
 
-static int nvi_dmp_init(struct nvi_state *st)
+static int nvi_dmp_init_gyr(struct nvi_state *st)
 {
-       u8 init[] = { 0, 0 };
        u8 val;
        s8 pll;
        s32 gryo_sf;
@@ -399,15 +887,6 @@ static int nvi_dmp_init(struct nvi_state *st)
        int t;
        int ret;
 
-       /* initialize DMP control registers */
-       ret = nvi_mem_wr(st, DATA_OUT_CTL1, sizeof(init), init, false);
-       ret |= nvi_mem_wr(st, DATA_OUT_CTL2, sizeof(init), init, false);
-       ret |= nvi_mem_wr(st, DATA_INTR_CTL, sizeof(init), init, false);
-       ret |= nvi_mem_wr(st, MOTION_EVENT_CTL, sizeof(init), init, false);
-       if (ret)
-               return ret;
-
-       /* initialize GYRO_SF */
        ret = nvi_i2c_rd(st, &st->hal->reg->tbc_pll, &val);
        pll = abs(val & 0x7F);
        if (val & 0x80)
@@ -415,15 +894,153 @@ static int nvi_dmp_init(struct nvi_state *st)
        t = 102870L + 81L * pll;
        a = (1L << 30) / t;
        r = (1L << 30) - a * t;
-       gryo_sf = a * 797 * DMP_DIVIDER;
-       gryo_sf += (s64)((a * 1011387LL * DMP_DIVIDER) >> 20);
-       gryo_sf += r * 797L * DMP_DIVIDER / t;
-       gryo_sf += (s32)((s64)((r * 1011387LL * DMP_DIVIDER) >> 20)) / t;
+       gryo_sf = a * 797 * ICM_DMP_DIVIDER;
+       gryo_sf += (s64)((a * 1011387LL * ICM_DMP_DIVIDER) >> 20);
+       gryo_sf += r * 797L * ICM_DMP_DIVIDER / t;
+       gryo_sf += (s32)((s64)((r * 1011387LL * ICM_DMP_DIVIDER) >> 20)) / t;
        gryo_sf <<= 1;
        ret |= nvi_mem_wr_be(st, GYRO_SF, sizeof(gryo_sf), gryo_sf);
+       return ret;
+}
+
+static int nvi_dmp_init(struct nvi_state *st)
+{
+       int ret;
+
+       st->mc_dis = true;
+       memset(&st->mc, 0, sizeof(st->mc));
+       /* initialize DMP control registers */
+       ret = nvi_mem_wr_be_mc(st, DATA_OUT_CTL1, 4, 0,
+                              &st->mc.icm.data_out_ctl);
+       ret |= nvi_mem_wr_be_mc(st, DATA_INTR_CTL, 2, 0,
+                               &st->mc.icm.data_intr_ctl);
+       ret |= nvi_mem_wr_be_mc(st, MOTION_EVENT_CTL, 2, 0,
+                               &st->mc.icm.motion_event_ctl);
+       st->mc_dis = false;
+       if (ret)
+               return ret;
+
+       ret = nvi_dmp_init_gyr(st);
+       nvi_dmp_init_gmf(st);
+       return ret;
+}
+
+static int nvi_dmp_en(struct nvi_state *st)
+{
+       struct nvi_dmp_dev *dd;
+       bool en;
+       unsigned int i;
+       unsigned int en_msk = 0;
+       int ret = 0;
+       u32 out_ctl = 0;
+       u16 evnt_ctl = 0;
+
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if (dd->dev == DEV_AUX) {
+                       if (st->snsr[DEV_AUX].enable & (1 << dd->aux_port)) {
+                               en_msk |= (1 << DEV_AUX);
+                               en_msk |= (1 << (dd->aux_port + DEV_N_AUX));
+                               en_msk |= dd->depend_msk;
+                       }
+               } else if (dd->dev < DEV_AUX) {
+                       if (st->snsr[dd->dev].enable) {
+                               en_msk |= (1 << dd->dev);
+                               en_msk |= dd->depend_msk;
+                       }
+               }
+       }
+
+
+#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;
 
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if (dd->dev > DEV_AUX)
+                       continue;
+
+               en = false;
+               if (dd->dev == DEV_AUX) {
+                       if (en_msk & (1 << (dd->aux_port + DEV_N_AUX)))
+                               en = true;
+               } else if (dd->dev < DEV_AUX) {
+                       if (en_msk & (1 << dd->dev))
+                               en = true;
+               }
+               if (en) {
+                       if (dd->fn_init)
+                               ret = dd->fn_init(st, en_msk, &out_ctl);
+                       else
+                               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);
+                       } else if (ret < 0) {
+                               return ret;
+                       } else {
+                               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;
+                       }
+               }
+       }
+
+       st->aux.dmp_en_msk = en_msk >> DEV_N_AUX;
+       if (st->aux.dmp_en_msk) {
+               ret = nvi_aux_enable(st, __func__, true, true);
+               if (ret)
+                       return ret;
+       }
+
+       if (out_ctl & DMP_DATA_OUT_CTL_HDR2_MSK)
+               /* header 2 needs to be enabled */
+               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));
+               return ret;
+       } else {
+               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))
+               evnt_ctl |= (ACCEL_CAL_EN | GYRO_CAL_EN);
+       /* inv_enable_compass_cal_V3 */
+       if (en_msk & (0x03 << DEV_N_AUX))
+               evnt_ctl |= COMPASS_CAL_EN;
+       /* inv_enable_9axes_V3 */
+       if (en_msk & (1 << DEV_GMR))
+               evnt_ctl |= NINE_AXIS_EN;
+       /* inv_setup_events */
+       if (en_msk & (1 << DEV_STP))
+               evnt_ctl |= PEDOMETER_EN;
+       if (en_msk & (1 << DEV_SM))
+               evnt_ctl |= (SMD_EN | PEDOMETER_EN);
+       ret |= nvi_mem_wr_be_mc(st, MOTION_EVENT_CTL, 2, evnt_ctl,
+                               &st->mc.icm.motion_event_ctl);
+       /* inv_set_wom */
+       ret |= nvi_dmp_irq(st);
+       if (!ret) {
+               st->en_msk |= (1 << DEV_DMP);
+               ret = nvi_i2c_wr(st, &st->hal->reg->pm2, 0, __func__);
+               nvi_push_delay(st);
+               ret |= nvi_reset(st, __func__, true, false, true);
+       }
        return ret;
 }
 
@@ -1341,16 +1958,24 @@ struct nvi_dmp nvi_dmp_icm = {
        .fw_crc32                       = 0x12F362A6,
        .fw_mem_addr                    = 0x90,
        .fw_start                       = 0x08D0,
-       .dmp_period_us                  = DMP_PERIOD_US,
-       .dev_msk                        = (1 << DEV_SM) | (1 << DEV_QTN),
-       .en_msk                         = (1 << DEV_SM) | (1 << DEV_QTN),
+       .dmp_reset_delay_ms             = 25,
+#ifdef MPL520
+       .dev_msk                        = (1 << DEV_SM) |
+                                         (1 << DEV_QTN),
+#else /* MPL520 */
+       .dev_msk                        = (1 << DEV_SM) |
+                                         (1 << DEV_QTN) |
+                                         (1 << DEV_GMR) |
+                                         (1 << DEV_GYU),
+#endif /* MPL520 */
+       .en_msk                         = MSK_DEV_ALL,
        .dd_n                           = ARRAY_SIZE(nvi_dmp_devs),
        .dd                             = nvi_dmp_devs,
+       .fn_rd                          = &nvi_dmp_rd,
+       .fn_clk_n                       = &nvi_dmp_clk_n,
        .fn_init                        = &nvi_dmp_init,
        .fn_en                          = &nvi_dmp_en,
        .fn_dev_init                    = &nvi_dd_init,
-       .fn_dev_enable                  = &nvi_dd_enable,
-       .fn_dev_batch                   = &nvi_dd_batch,
 };
 EXPORT_SYMBOL(nvi_dmp_icm);
 
index 711f2fc..564d3b6 100644 (file)
 #ifndef _NVI_DMP_ICM_H_
 #define _NVI_DMP_ICM_H_
 
+#define ICM_DMP_FREQ                   (102)
+#define ICM_DMP_PERIOD_US              (9804)
+#define ICM_BASE_SAMPLE_RATE           (1125)
+#define ICM_DMP_DIVIDER                        (ICM_BASE_SAMPLE_RATE / ICM_DMP_FREQ)
+
 #define DATA_OUT_CTL1                  (4 * 16)        /* 0x0040 */
 #define DATA_OUT_CTL2                  (4 * 16 + 2)    /* 0x0042 */
 #define DATA_INTR_CTL                  (4 * 16 + 12)   /* 0x004C */
 
 #define MOTION_EVENT_CTL               (4 * 16 + 14)   /* 0x004E */
 
-#define BM_BATCH_CNTR                  (27 * 16)
-#define BM_BATCH_THLD                  (19 * 16 + 12)
-#define BM_BATCH_MASK                  (21 * 16 + 14)
-
-#define ODR_ACCEL                      (11 * 16 + 14)
-#define ODR_GYRO                       (11 * 16 + 10)
-#define ODR_CPASS                      (11 * 16 +  6)
-#define ODR_ALS                                (11 * 16 +  2)
-#define ODR_QUAT6                      (10 * 16 + 12)
-#define ODR_QUAT9                      (10 * 16 +  8)
-#define ODR_PQUAT6                     (10 * 16 +  4)
-#define ODR_PRESSURE                   (11 * 16 + 12)
-#define ODR_GYRO_CALIBR                        (11 * 16 +  8)
-#define ODR_CPASS_CALIBR               (11 * 16 +  4)
-
-#define ODR_CNTR_ACCEL                 (9 * 16 + 14)
-#define ODR_CNTR_GYRO                  (9 * 16 + 10)
-#define ODR_CNTR_CPASS                 (9 * 16 +  6)
-#define ODR_CNTR_ALS                   (9 * 16 +  2)
-#define ODR_CNTR_QUAT6                 (8 * 16 + 12)
-#define ODR_CNTR_QUAT9                 (8 * 16 +  8)
-#define ODR_CNTR_PQUAT6                        (8 * 16 +  4)
-#define ODR_CNTR_PRESSURE              (9 * 16 + 12)
-#define ODR_CNTR_GYRO_CALIBR           (9 * 16 +  8)
-#define ODR_CNTR_CPASS_CALIBR          (9 * 16 +  4)
+#define BM_BATCH_CNTR                  (27 * 16)       /* 0x01B0 */
+#define BM_BATCH_THLD                  (19 * 16 + 12)  /* 0x013C */
+#define BM_BATCH_MASK                  (21 * 16 + 14)  /* 0x015E */
+
+#define ODR_ACCEL                      (11 * 16 + 14)  /* 0x00BE */
+#define ODR_GYRO                       (11 * 16 + 10)  /* 0x00BA */
+#define ODR_CPASS                      (11 * 16 +  6)  /* 0x00B6 */
+#define ODR_ALS                                (11 * 16 +  2)  /* 0x00B2 */
+#define ODR_QUAT6                      (10 * 16 + 12)  /* 0x00AC */
+#define ODR_QUAT9                      (10 * 16 +  8)  /* 0x00A8 */
+#define ODR_PQUAT6                     (10 * 16 +  4)  /* 0x00A4 */
+#define ODR_PRESSURE                   (11 * 16 + 12)  /* 0x00BC */
+#define ODR_GYRO_CALIBR                        (11 * 16 +  8)  /* 0x00B8 */
+#define ODR_CPASS_CALIBR               (11 * 16 +  4)  /* 0x00B4 */
+
+#define ODR_CNTR_ACCEL                 (9 * 16 + 14)   /* 0x009E */
+#define ODR_CNTR_GYRO                  (9 * 16 + 10)   /* 0x009A */
+#define ODR_CNTR_CPASS                 (9 * 16 +  6)   /* 0x0096 */
+#define ODR_CNTR_ALS                   (9 * 16 +  2)   /* 0x0092 */
+#define ODR_CNTR_QUAT6                 (8 * 16 + 12)   /* 0x008C */
+#define ODR_CNTR_QUAT9                 (8 * 16 +  8)   /* 0x0088 */
+#define ODR_CNTR_PQUAT6                        (8 * 16 +  4)   /* 0x0084 */
+#define ODR_CNTR_PRESSURE              (9 * 16 + 12)   /* 0x009C */
+#define ODR_CNTR_GYRO_CALIBR           (9 * 16 +  8)   /* 0x0098 */
+#define ODR_CNTR_CPASS_CALIBR          (9 * 16 +  4)   /* 0x0094 */
 
 #define CPASS_MTX_00                   (23 * 16)
 #define CPASS_MTX_01                   (23 * 16 + 4)
 #define ACCEL_ACCURACY                 (97 * 16)
 #define ACCEL_CAL_RESET                        (77 * 16)
 #define ACCEL_VARIANCE_THRESH          (93 * 16)
-#define ACCEL_CAL_RATE                 (94 * 16 + 4)
+#define ACCEL_CAL_RATE                 (94 * 16 + 4)   /* 0x05E4 */
 #define ACCEL_PRE_SENSOR_DATA          (97 * 16 + 4)
 #define ACCEL_COVARIANCE               (101 * 16 + 8)
-#define ACCEL_ALPHA_VAR                        (91 * 16)
+#define ACCEL_ALPHA_VAR                        (91 * 16)       /* 0x05B0 */
 #define ACCEL_A_VAR                    (92 * 16)
 
 #define CPASS_BIAS_X                   (126 * 16 +  4)
 #define PEDSTD_DRIVE_STATE             (43 * 16 + 10)
 #define PED_RATE                       (58 * 16 +  4)
 
-#define SMD_MOT_THLD                   (72 * 16 + 12)
-#define SMD_DELAY_THLD                 (76 * 16 + 8)
-#define SMD_DELAY2_THLD                        (76 * 16 + 12)
+#define SMD_MOT_THLD                   (72 * 16 + 12)  /* 0x048C */
+#define SMD_DELAY_THLD                 (76 * 16 + 8)   /* 0x04C8 */
+#define SMD_DELAY2_THLD                        (76 * 16 + 12)  /* 0x04CC */
 
-#define WOM_ENABLE                     (64 * 16 + 14)
+#define WOM_ENABLE                     (64 * 16 + 14)  /* 0x040E */
 #define WOM_STATUS                     (64 * 16 + 6)
 #define WOM_THRESHOLD                  (64 * 16)
 #define WOM_CNTR_TH                    (64 * 16 + 12)
 #define BATCH_MODE_EN                  0x0100
 #define ACT_RECOG_SET                  0x0080
 
-#define PEDOMETER_EN                   0x40
-#define PEDOMETER_INT_EN               0x20
-#define SMD_EN                         0x08
-#define ACCEL_CAL_EN                   0x02
-#define GYRO_CAL_EN                    0x01
-#define COMPASS_CAL_EN                 0x80
-#define NINE_AXIS_EN                   0x40
+#define PEDOMETER_EN                   0x4000
+#define PEDOMETER_INT_EN               0x2000
+#define SMD_EN                         0x0800
+#define ACCEL_CAL_EN                   0x0200
+#define GYRO_CAL_EN                    0x0100
+#define COMPASS_CAL_EN                 0x0080
+#define NINE_AXIS_EN                   0x0040
 
 #define HEADER_SZ                      2
 #define ACCEL_DATA_SZ                  12
index 978343e..cdf92c7 100644 (file)
 #include "nvi.h"
 #include "nvi_dmp_mpu.h"
 
-#define DMP_PERIOD_US                  (5000)
+#define MPU_DMP_PERIOD_US              (5000)
 
+#define DMP_HDR_LEN_MAX                        (4)
+#define DMP_DEV_ABLE_LEN               (2)
 
-static int nvi_dmp_press_init(struct nvi_state *st)
+struct nvi_dmp_mpu {
+};
+
+struct nvi_dmp_dev {
+       unsigned int dev;
+       unsigned int aux_port;
+       unsigned int depend_msk;        /* sensor dependencies */
+       unsigned int data_n;
+       unsigned int hdr_n;
+       u8 hdr[DMP_HDR_LEN_MAX];
+       u8 hdr_msk[DMP_HDR_LEN_MAX];
+       u16 en_addr;
+       u8 en_len;
+       u8 en[DMP_DEV_ABLE_LEN];
+       u8 dis[DMP_DEV_ABLE_LEN];
+       u16 odr_cfg;
+       u16 odr_cntr;
+       int (*fn_init)(struct nvi_state *st);
+};
+
+
+/* prs = pressure */
+static int nvi_dmp_prs_init(struct nvi_state *st)
 {
        return 1;
 }
@@ -25,25 +49,26 @@ static int nvi_dmp_press_init(struct nvi_state *st)
 /* gmf = GeoMagnetic Field (compass) */
 static int nvi_dmp_gmf_init(struct nvi_state *st)
 {
-       int ret = 1;
+       if (st->aux.port[0].nmp.type != SECONDARY_SLAVE_TYPE_COMPASS)
+               /* DMP shouldn't run if AUX device not supported */
+               return -EINVAL;
 
-       if (st->aux.port[0].nmp.type == SECONDARY_SLAVE_TYPE_COMPASS) {
-               if (st->snsr[DEV_AUX].enable & (1 << 0))
-                       ret = nvi_aux_delay(st, __func__);
-       }
-       return ret;
+       return 0;
 }
 
 static int nvi_dmp_sm_init(struct nvi_state *st)
 {
        int ret;
 
-       ret = nvi_mem_wr_be(st, D_SMD_MOT_THLD, 4,
-                           st->snsr[DEV_SM].cfg.thresh_lo << 16);
-       ret |= nvi_mem_wr_be(st, D_SMD_DELAY_THLD, 4,
-                            st->snsr[DEV_SM].cfg.thresh_hi);
-       ret |= nvi_mem_wr_be(st, D_SMD_DELAY2_THLD, 4,
-                            st->snsr[DEV_SM].cfg.delay_us_max);
+       ret = nvi_mem_wr_be_mc(st, D_SMD_MOT_THLD, 4,
+                              st->snsr[DEV_SM].cfg.thresh_lo << 16,
+                              &st->mc.mpu.d_smd_mot_thld);
+       ret |= nvi_mem_wr_be_mc(st, D_SMD_DELAY_THLD, 4,
+                               st->snsr[DEV_SM].cfg.thresh_hi,
+                               &st->mc.mpu.d_smd_delay_thld);
+       ret |= nvi_mem_wr_be_mc(st, D_SMD_DELAY2_THLD, 4,
+                               st->snsr[DEV_SM].cfg.delay_us_max,
+                               &st->mc.mpu.d_smd_delay2_thld);
        return ret;
 }
 
@@ -54,14 +79,12 @@ static struct nvi_dmp_dev nvi_dmp_devs[] = {
                .hdr_n                  = 2,
                .hdr                    = { 0x40, 0x00 },
                .hdr_msk                = { 0xFF, 0xF0 },
-               .mpu                    = {
-                       .en_addr        = CFG_OUT_ACCL,
-                       .en_len         = 2,
-                       .en             = { 0xA3, 0xA3 },
-                       .dis            = { 0xF4, 0x12 },
-                       .odr_cfg        = KEY_CFG_ACCL_ODR,
-                       .odr_cntr       = KEY_ODR_CNTR_ACCL,
-               },
+               .en_addr                = CFG_OUT_ACCL,
+               .en_len                 = 2,
+               .en                     = { 0xA3, 0xA3 },
+               .dis                    = { 0xF4, 0x12 },
+               .odr_cfg                = KEY_CFG_ACCL_ODR,
+               .odr_cntr               = KEY_ODR_CNTR_ACCL,
        },
        {
                .dev                    = DEV_GYR,
@@ -69,91 +92,160 @@ static struct nvi_dmp_dev nvi_dmp_devs[] = {
                .hdr_n                  = 2,
                .hdr                    = { 0x20, 0x00 },
                .hdr_msk                = { 0xFF, 0xF0 },
-               .mpu                    = {
-                       .en_addr        = CFG_OUT_GYRO,
-                       .en_len         = 2,
-                       .en             = { 0xA3, 0xA3 },
-                       .dis            = { 0xF4, 0x12 },
-                       .odr_cfg        = KEY_CFG_GYRO_ODR,
-                       .odr_cntr       = KEY_ODR_CNTR_GYRO,
-               },
+               .en_addr                = CFG_OUT_GYRO,
+               .en_len                 = 2,
+               .en                     = { 0xA3, 0xA3 },
+               .dis                    = { 0xF4, 0x12 },
+               .odr_cfg                = KEY_CFG_GYRO_ODR,
+               .odr_cntr               = KEY_ODR_CNTR_GYRO,
        },
        {
                .dev                    = DEV_QTN,
+               .depend_msk             = (1 << DEV_ACC) |
+                                         (1 << DEV_GYR),
                .data_n                 = 12,
                .hdr_n                  = 4,
                .hdr                    = { 0x04, 0x00, 0x04, 0x00 },
                .hdr_msk                = { 0xFF, 0xF0, 0xFF, 0xF0 },
-               .mpu                    = {
-                       .en_addr        = CFG_OUT_6QUAT,
-                       .en_len         = 2,
-                       .en             = { 0xA3, 0xA3 },
-                       .dis            = { 0xF4, 0x13 },
-                       .odr_cfg        = KEY_CFG_6QUAT_ODR,
-                       .odr_cntr       = KEY_ODR_CNTR_6QUAT,
-               },
+               .en_addr                = CFG_OUT_6QUAT,
+               .en_len                 = 2,
+               .en                     = { 0xA3, 0xA3 },
+               .dis                    = { 0xF4, 0x13 },
+               .odr_cfg                = KEY_CFG_6QUAT_ODR,
+               .odr_cntr               = KEY_ODR_CNTR_6QUAT,
        },
        {
                .dev                    = DEV_SM,
+               .depend_msk             = (1 << DEV_ACC),
                .data_n                 = 1,
                .hdr_n                  = 0,
                .fn_init                = &nvi_dmp_sm_init,
-               .mpu                    = {
-                       .en_addr        = D_SMD_ENABLE,
-                       .en_len         = 2,
-                       .en             = { 0x00, 0x01 },
-                       .dis            = { 0x00, 0x00 },
-               },
+               .en_addr                = D_SMD_ENABLE,
+               .en_len                 = 2,
+               .en                     = { 0x00, 0x01 },
+               .dis                    = { 0x00, 0x00 },
        },
        {
                .dev                    = DEV_AUX,
-               .data_n                 = 6,
                .aux_port               = 0,
+               .depend_msk             = (0x03 << DEV_N_AUX),
+               .data_n                 = 6,
                .hdr_n                  = 2,
                .hdr                    = { 0x10, 0x00 },
                .hdr_msk                = { 0xFF, 0xF0 },
                .fn_init                = &nvi_dmp_gmf_init,
-               .mpu                    = {
-                       .en_addr        = CFG_OUT_CPASS,
-                       .en_len         = 2,
-                       .en             = { 0xA3, 0xA3 },
-                       .dis            = { 0xF4, 0x12 },
-                       .odr_cfg        = KEY_CFG_CPASS_ODR,
-                       .odr_cntr       = KEY_ODR_CNTR_CPASS,
-               },
+               .en_addr                = CFG_OUT_CPASS,
+               .en_len                 = 2,
+               .en                     = { 0xA3, 0xA3 },
+               .dis                    = { 0xF4, 0x12 },
+               .odr_cfg                = KEY_CFG_CPASS_ODR,
+               .odr_cntr               = KEY_ODR_CNTR_CPASS,
        },
        {
                .dev                    = DEV_AUX,
-               .data_n                 = 6,
                .aux_port               = 2,
+               .depend_msk             = (0x0C << DEV_N_AUX),
+               .data_n                 = 6,
                .hdr_n                  = 2,
                .hdr                    = { 0x80, 0x00 },
                .hdr_msk                = { 0xFF, 0xF0 },
-               .fn_init                = &nvi_dmp_press_init,
-               .mpu                    = {
-                       .en_addr        = CFG_OUT_PRESS,
-                       .en_len         = 2,
-                       .en             = { 0xA3, 0xA3 },
-                       .dis            = { 0xF4, 0x12 },
-                       .odr_cfg        = KEY_CFG_PRESS_ODR,
-                       .odr_cntr       = KEY_ODR_CNTR_PRESS,
-               },
+               .fn_init                = &nvi_dmp_prs_init,
+               .en_addr                = CFG_OUT_PRESS,
+               .en_len                 = 2,
+               .en                     = { 0xA3, 0xA3 },
+               .dis                    = { 0xF4, 0x12 },
+               .odr_cfg                = KEY_CFG_PRESS_ODR,
+               .odr_cntr               = KEY_ODR_CNTR_PRESS,
        },
 };
 
-static int nvi_dmp_fifo_irq(struct nvi_state *st)
+static int nvi_dmp_rd(struct nvi_state *st, s64 ts, unsigned int n)
 {
-       u8 en[] = {0xFE};
-       u8 dis[] = {0xA3};
-       u8 *able;
+       const struct nvi_dmp_dev *dd;
+       struct aux_port *ap;
+       unsigned int dd_i;
+       unsigned int i;
+       u8 byte;
+
+       while (n > DMP_HDR_LEN_MAX) {
+               if (st->sts & (NVS_STS_SUSPEND | NVS_STS_SHUTDOWN))
+                       return -1;
+
+               for (dd_i = 0; dd_i < st->hal->dmp->dd_n; dd_i++) {
+                       dd = &st->hal->dmp->dd[dd_i];
+                       if (!dd->hdr_n)
+                               continue;
+
+                       for (i = 0; i < dd->hdr_n; i++) {
+                               byte = st->buf[st->buf_i + i];
+                               byte &= dd->hdr_msk[i];
+                               if (byte != dd->hdr[i])
+                                       break;
+                       }
+                       if (i >= dd->hdr_n)
+                               break;
+               }
+               if (dd_i >= st->hal->dmp->dd_n) {
+                       /* unknown header: lost DMP sync so DMP reset */
+                       if (st->sts & NVI_DBG_SPEW_FIFO)
+                               dev_err(&st->i2c->dev,
+                                       "%s ERR: DMP sync  HDR: %x %x %x %x\n",
+                                       __func__, st->buf[st->buf_i],
+                                       st->buf[st->buf_i + 1],
+                                       st->buf[st->buf_i + 2],
+                                       st->buf[st->buf_i + 3]);
+                       nvi_err(st);
+                       return -1;
+               }
+
+               if (n > dd->data_n + i) {
+                       if (dd->dev == DEV_AUX) {
+                               if (st->sts & NVI_DBG_SPEW_FIFO)
+                                       dev_info(&st->i2c->dev,
+                                                "%s DMP HDR: AUX port=%u\n",
+                                                __func__, dd->aux_port);
+                               ap = &st->aux.port[dd->aux_port];
+                               ap->nmp.handler(&st->buf[st->buf_i + i],
+                                               dd->data_n,
+                                               nvi_ts_dev(st, ts, dd->dev,
+                                                          dd->aux_port),
+                                               ap->nmp.ext_driver);
+                       } else if (dd->dev < DEV_N) {
+                               if (st->sts & NVI_DBG_SPEW_FIFO)
+                                       dev_info(&st->i2c->dev,
+                                                "%s DMP HDR: %s\n", __func__,
+                                                st->snsr[dd->dev].cfg.name);
+                               nvi_push(st, dd->dev, &st->buf[st->buf_i + i],
+                                        nvi_ts_dev(st, ts, dd->dev, 0));
+                       }
+                       i += dd->data_n;
+                       st->buf_i += i;
+                       n -= i;
+               } else {
+                       return 0;
+               }
+       }
+
+       return 0;
+}
+
+static int nvi_dmp_clk_n(struct nvi_state *st, u32 *clk_n)
+{
+       return nvi_mem_rd_le(st, D_DMP_RUN_CNTR, 4, clk_n);
+}
+
+static int nvi_dmp_irq(struct nvi_state *st)
+{
+       u32 able;
 
        if ((st->en_msk & MSK_DEV_ALL) & ~((1 << DEV_SM) | (1 << DEV_STP)))
                /* DMP requires FIFO IRQ */
-               able = en;
+               able = 0xFE;
        else
                /* DMP IRQ is in event mode */
-               able = dis;
-       return nvi_mem_wr(st, CFG_FIFO_INT, 1, able, false);
+               able = 0xA3;
+       return nvi_mem_wr_be_mc(st, CFG_FIFO_INT, 1, able,
+                               &st->mc.mpu.cfg_fifo_int);
 }
 
 static int nvi_dd_odr(struct nvi_state *st, struct nvi_dmp_dev *dd)
@@ -162,31 +254,38 @@ static int nvi_dd_odr(struct nvi_state *st, struct nvi_dmp_dev *dd)
        unsigned int period_us;
        int ret;
 
-       if (!(dd->mpu.odr_cfg | dd->mpu.odr_cntr))
+       if (!(dd->odr_cfg | dd->odr_cntr))
                return 0;
 
        if (dd->dev == DEV_AUX)
                period_us = st->aux.port[dd->aux_port].period_us;
        else
                period_us = st->snsr[dd->dev].period_us;
-       if (period_us)
-               odr_cfg = (period_us / DMP_PERIOD_US) - 1;
+       if (period_us) {
+               odr_cfg = period_us / MPU_DMP_PERIOD_US;
+               if (odr_cfg)
+                       odr_cfg--;
+       } else {
+               odr_cfg = 0;
+       }
+       if (dd->dev == DEV_AUX)
+               st->aux.port[dd->aux_port].odr = odr_cfg;
        else
-               odr_cfg = 1;
-       ret = nvi_mem_wr_be(st, dd->mpu.odr_cntr, 2, 0);
-       ret |= nvi_mem_wr_be(st, dd->mpu.odr_cfg, 2, odr_cfg);
-       ret |= nvi_mem_wr_be(st, dd->mpu.odr_cntr, 2, 0);
+               st->snsr[dd->dev].odr = odr_cfg;
+       ret = nvi_mem_wr_be(st, dd->odr_cntr, 2, 0);
+       ret |= nvi_mem_wr_be(st, dd->odr_cfg, 2, odr_cfg);
+       ret |= nvi_mem_wr_be(st, dd->odr_cntr, 2, 0);
        return ret;
 }
 
-static int nvi_dd_en(struct nvi_state *st, struct nvi_dmp_dev *dd)
+static int nvi_dd_able(struct nvi_state *st, struct nvi_dmp_dev *dd, bool en)
 {
        int ret;
 
        if (dd->dev >= DEV_N_AUX)
                return 0;
 
-       if (st->snsr[dd->dev].enable) {
+       if (en) {
                if (dd->fn_init)
                        ret = dd->fn_init(st);
                else
@@ -196,18 +295,20 @@ static int nvi_dd_en(struct nvi_state *st, struct nvi_dmp_dev *dd)
                        ret = 0;
                } else if (!ret) {
                        ret = nvi_dd_odr(st, dd);
-                       ret |= nvi_mem_wr(st, dd->mpu.en_addr, dd->mpu.en_len,
-                                         &dd->mpu.en[0], false);
+                       ret |= nvi_mem_wr(st, dd->en_addr, dd->en_len,
+                                         &dd->en[0], false);
                }
                if (dd->dev != DEV_AUX) {
-                       if (ret)
+                       if (ret) {
                                st->en_msk &= ~(1 << dd->dev);
-                       else
+                       } else {
+                               st->en_msk |= (1 << dd->dev);
                                st->snsr[dd->dev].buf_n = dd->data_n;
+                       }
                }
        } else {
-               ret = nvi_mem_wr(st, dd->mpu.en_addr, dd->mpu.en_len,
-                                &dd->mpu.dis[0], false);
+               ret = nvi_mem_wr(st, dd->en_addr, dd->en_len,
+                                &dd->dis[0], false);
        }
        return ret;
 }
@@ -241,20 +342,6 @@ static int nvi_dd_batch(struct nvi_state *st, unsigned int dev, int port)
        return nvi_dd_odr(st, dd);
 }
 
-static int nvi_dd_enable(struct nvi_state *st, unsigned int dev, int port)
-{
-       struct nvi_dmp_dev *dd;
-       int ret;
-
-       dd = nvi_dd(st, dev, port);
-       if (dd == NULL)
-               return -EINVAL;
-
-       ret = nvi_dd_en(st, dd);
-       ret |= nvi_dmp_fifo_irq(st);
-       return ret;
-}
-
 static int nvi_dd_init(struct nvi_state *st, unsigned int dev)
 {
        struct nvi_dmp_dev *dd;
@@ -350,6 +437,7 @@ static int nvi_dmp_init(struct nvi_state *st)
        int i;
        int ret;
 
+       memset(&st->mc, 0, sizeof(st->mc));
        i = nvi_dmp_matrix_i(st->snsr[DEV_GYR].cfg.matrix);
        if (i < 0)
                return -EINVAL;
@@ -370,17 +458,38 @@ static int nvi_dmp_init(struct nvi_state *st)
                return ret;
 
        msleep(400);
-       ret |= nvi_user_ctrl_en(st, __func__, false, false, false, false);
-       return ret;
+       return nvi_user_ctrl_en(st, __func__, false, false, false, false);
 }
 
 static int nvi_dmp_en(struct nvi_state *st)
 {
+       struct nvi_dmp_dev *dd;
+       bool en;
        unsigned int i;
+       unsigned int en_msk = 0;
        int ret = 0;
 
+       st->en_msk |= (1 << DEV_DMP);
        st->snsr[DEV_ACC].matrix = true;
        st->snsr[DEV_GYR].matrix = true;
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if (dd->dev == DEV_AUX) {
+                       if (st->snsr[DEV_AUX].enable & (1 << dd->aux_port)) {
+                               en_msk |= (1 << DEV_AUX);
+                               en_msk |= (1 << dd->aux_port);
+                               en_msk |= dd->depend_msk;
+                       }
+               } else if (dd->dev < DEV_AUX) {
+                       if (st->snsr[dd->dev].enable) {
+                               en_msk |= (1 << dd->dev);
+                               en_msk |= dd->depend_msk;
+                       }
+               }
+       }
+
+       st->aux.dmp_en_msk = en_msk >> DEV_N_AUX;
+       st->src[SRC_DMP].period_us_src = MPU_DMP_PERIOD_US;
        ret |= nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config1, 0x03,
                             __func__, &st->rc.gyro_config1);
        ret |= nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config2, 0x18,
@@ -389,11 +498,33 @@ static int nvi_dmp_en(struct nvi_state *st)
                             __func__, &st->rc.accel_config);
        ret |= nvi_i2c_wr_rc(st, &st->hal->reg->smplrt[0], 0x04,
                             __func__, (u8 *)&st->rc.smplrt[0]);
-       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++)
-               ret |= nvi_dd_en(st, &nvi_dmp_devs[i]);
-       ret |= nvi_dmp_fifo_irq(st);
-       nvi_push_delay(st);
-       ret |= nvi_reset(st, __func__, true, false, true);
+       if (ret)
+               return ret;
+
+       for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
+               dd = &nvi_dmp_devs[i];
+               if (dd->dev > DEV_AUX)
+                       continue;
+
+               en = false;
+               if (dd->dev == DEV_AUX) {
+                       if (en_msk & (1 << (dd->aux_port + DEV_N_AUX)))
+                               en = true;
+               } else if (dd->dev < DEV_AUX) {
+                       if (en_msk & (1 << dd->dev))
+                               en = true;
+               }
+               ret = nvi_dd_able(st, dd, en);
+               if (ret)
+                       return ret;
+       }
+
+       ret = nvi_dmp_irq(st);
+       ret |= nvi_aux_delay(st, __func__);
+       if (!ret) {
+               nvi_push_delay(st);
+               ret = nvi_reset(st, __func__, true, false, true);
+       }
        return ret;
 }
 
@@ -724,15 +855,15 @@ struct nvi_dmp nvi_dmp_mpu = {
        .fw_crc32                       = 0x972AAE92,
        .fw_mem_addr                    = 0x20,
        .fw_start                       = 0x0400,
-       .dmp_period_us                  = DMP_PERIOD_US,
        .dev_msk                        = (1 << DEV_SM) | (1 << DEV_QTN),
        .en_msk                         = (1 << DEV_SM) | (1 << DEV_QTN),
        .dd_n                           = ARRAY_SIZE(nvi_dmp_devs),
        .dd                             = nvi_dmp_devs,
+       .fn_rd                          = &nvi_dmp_rd,
+       .fn_clk_n                       = &nvi_dmp_clk_n,
        .fn_init                        = &nvi_dmp_init,
        .fn_en                          = &nvi_dmp_en,
        .fn_dev_init                    = &nvi_dd_init,
-       .fn_dev_enable                  = &nvi_dd_enable,
        .fn_dev_batch                   = &nvi_dd_batch,
 };
 EXPORT_SYMBOL(nvi_dmp_mpu);
index 4dbfd48..0fb4598 100644 (file)
@@ -14,7 +14,7 @@
 #include <linux/time.h>
 #include <linux/delay.h>
 #include "nvi.h"
-
+#include "nvi_dmp_icm.h"
 
 #define BYTES_PER_SENSOR               6
 /* full scale and LPF setting */
@@ -574,8 +574,8 @@ static int nvi_pm_icm(struct nvi_state *st, u8 pm1, u8 pm2, u8 lp)
 
        ret = nvi_i2c_wr_rc(st, &st->hal->reg->lp_config, 0x70,
                            __func__, &st->rc.lp_config);
-       ret |= nvi_i2c_wr_rc(st, &st->hal->reg->pm2, pm2,
-                            __func__, &st->rc.pm2);
+       /* the DMP changes pm2 so we can't use the cache */
+       ret |= nvi_i2c_wr(st, &st->hal->reg->pm2, pm2, __func__);
        ret |= nvi_i2c_wr_rc(st, &st->hal->reg->pm1, pm1,
                             __func__, &st->rc.pm1);
        return ret;
@@ -588,6 +588,7 @@ static int nvi_en_gyr(struct nvi_state *st)
        int ret = 0;
 
        st->snsr[DEV_GYR].matrix = true;
+       st->snsr[DEV_GYR].buf_n = 6;
        for (i = 0; i < AXIS_N; i++)
                ret |= nvi_wr_gyro_offset(st, i,
                                          (u16)(st->rom_offset[DEV_GYR][i] +
@@ -625,6 +626,7 @@ static int nvi_init(struct nvi_state *st)
 {
        u8 val;
        s8 t;
+       unsigned int src;
        int ret;
 
        st->snsr[DEV_ACC].cfg.thresh_hi = 0; /* no ACC LP on ICM */
@@ -635,11 +637,13 @@ static int nvi_init(struct nvi_state *st)
        t = abs(val & 0x7F);
        if (val & 0x80)
                t = -t;
-
-       st->src[SRC_ACC].base_t = NSEC_PER_SEC;
        st->src[SRC_GYR].base_t = NSEC_PER_SEC - t * 769903 + ((t * 769903) /
                                                               1270) * t;
+       st->src[SRC_ACC].base_t = NSEC_PER_SEC;
        st->src[SRC_AUX].base_t = NSEC_PER_SEC;
+       for (src = 0; src < st->hal->src_n; src++)
+               st->src[src].base_t /= ICM_BASE_SAMPLE_RATE;
+
        return 0;
 }
 
@@ -662,50 +666,34 @@ struct nvi_fn nvi_fn_icm = {
 };
 
 
-static int nvi_period_gyr(struct nvi_state *st)
+static int nvi_src(struct nvi_state *st, unsigned int src)
 {
-       u16 rate;
-       unsigned int us;
+       unsigned int rate;
        int ret;
 
-       us = st->src[SRC_GYR].period_us_req;
-       if (us < st->hal->src[SRC_GYR].period_us_min)
-               us = st->hal->src[SRC_GYR].period_us_min;
-       if (us > st->hal->src[SRC_GYR].period_us_max)
-               us = st->hal->src[SRC_GYR].period_us_max;
-       rate = us / 1125 - 1;
-       st->src[SRC_GYR].period_us_src = us;
-       ret = nvi_i2c_write_rc(st, &st->hal->reg->smplrt[SRC_GYR], rate,
-                              __func__, (u8 *)&st->rc.smplrt[SRC_GYR], true);
+       rate = st->src[src].period_us_req / ICM_BASE_SAMPLE_RATE;
+       if (rate)
+               rate--;
+       ret = nvi_i2c_write_rc(st, &st->hal->reg->smplrt[src], rate,
+                              __func__, (u8 *)&st->rc.smplrt[src], true);
+       if (!ret)
+               st->src[src].period_us_src = (rate + 1) * ICM_BASE_SAMPLE_RATE;
        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
                dev_info(&st->i2c->dev,
-                        "%s src[SRC_GYR]: period=%u timeout=%u err=%d\n",
-                        __func__, st->src[SRC_GYR].period_us_req,
-                        st->src_timeout_us[SRC_GYR], ret);
+                        "%s src[%u] period_req=%u period_src=%u err=%d\n",
+                        __func__, src, st->src[src].period_us_req,
+                        st->src[src].period_us_src, ret);
        return ret;
 }
 
-static int nvi_period_acc(struct nvi_state *st)
+static int nvi_src_gyr(struct nvi_state *st)
 {
-       u16 rate;
-       unsigned int us;
-       int ret;
+       return nvi_src(st, SRC_GYR);
+}
 
-       us = st->src[SRC_ACC].period_us_req;
-       if (us < st->hal->src[SRC_ACC].period_us_min)
-               us = st->hal->src[SRC_ACC].period_us_min;
-       if (us > st->hal->src[SRC_ACC].period_us_max)
-               us = st->hal->src[SRC_ACC].period_us_max;
-       rate = us / 1125 - 1;
-       st->src[SRC_ACC].period_us_src = us;
-       ret = nvi_i2c_write_rc(st, &st->hal->reg->smplrt[SRC_ACC], rate,
-                              __func__, (u8 *)&st->rc.smplrt[SRC_ACC], true);
-       if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
-               dev_info(&st->i2c->dev,
-                        "%s src[SRC_ACC]: period=%u timeout=%u err=%d\n",
-                        __func__, st->src[SRC_ACC].period_us_req,
-                        st->src_timeout_us[SRC_ACC], ret);
-       return ret;
+static int nvi_src_acc(struct nvi_state *st)
+{
+       return nvi_src(st, SRC_ACC);
 }
 
 static unsigned int nvi_aux_period_us[] = {
@@ -723,52 +711,53 @@ static unsigned int nvi_aux_period_us[] = {
        14222,
 };
 
-static int nvi_period_aux(struct nvi_state *st)
+static int nvi_src_aux(struct nvi_state *st)
 {
        u8 val;
+       unsigned int i;
        int ret;
 
-       for (val = 0; val < ARRAY_SIZE(nvi_aux_period_us); val++) {
-               if (st->src[SRC_AUX].period_us_req >= nvi_aux_period_us[val])
+       for (i = 0; i < ARRAY_SIZE(nvi_aux_period_us); i++) {
+               if (st->src[SRC_AUX].period_us_req >= nvi_aux_period_us[i])
                        break;
        }
-       if (val >= ARRAY_SIZE(nvi_aux_period_us))
-               val = ARRAY_SIZE(nvi_aux_period_us) - 1;
-       st->src[SRC_AUX].period_us_src = nvi_aux_period_us[val];
-       val = 0x0F - val;
+       if (i >= ARRAY_SIZE(nvi_aux_period_us))
+               i = ARRAY_SIZE(nvi_aux_period_us) - 1;
+       val = 0x0F - i;
        ret = nvi_i2c_wr_rc(st, &st->hal->reg->i2c_mst_odr_config, val,
                            __func__, &st->rc.i2c_mst_odr_config);
-       ret |= nvi_aux_delay(st, __func__);
+       if (!ret) {
+               st->src[SRC_AUX].period_us_src = nvi_aux_period_us[i];
+               ret = nvi_aux_delay(st, __func__);
+       }
        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
-               dev_info(&st->i2c->dev,
-                        "%s src[SRC_AUX]: period=%u timeout=%u err=%d\n",
-                        __func__, st->src[SRC_AUX].period_us_req,
-                        st->src_timeout_us[SRC_AUX], ret);
+               dev_info(&st->i2c->dev, "%s src[SRC_AUX]: period=%u err=%d\n",
+                        __func__, st->src[SRC_AUX].period_us_req, ret);
        return ret;
 }
 
 static const struct nvi_hal_src src[] = {
        [SRC_GYR]                       {
-               .dev_msk                = (1 << DEV_GYR),
+               .dev_msk                = (1 << DEV_GYR) | (1 << DEV_GYU),
                .period_us_min          = 10000,
                .period_us_max          = 256000,
-               .fn_period              = nvi_period_gyr,
+               .fn_period              = nvi_src_gyr,
        },
        [SRC_ACC]                       {
                .dev_msk                = (1 << DEV_ACC),
                .period_us_min          = 10000,
                .period_us_max          = 256000,
-               .fn_period              = nvi_period_acc,
+               .fn_period              = nvi_src_acc,
        },
        [SRC_AUX]                       {
                .dev_msk                = (1 << DEV_AUX),
                .period_us_min          = 14222,
                .period_us_max          = 29127111,
-               .fn_period              = nvi_period_aux,
+               .fn_period              = nvi_src_aux,
        },
 };
 
-static int nvi_fifo_dev[] = {
+static int nvi_fifo_devs[] = {
        DEV_GYR,
        DEV_ACC,
        -1,
@@ -958,7 +947,7 @@ static const struct nvi_hal_dev nvi_hal_aux = {
 
 static const struct nvi_hal_dev nvi_hal_dmp = {
        .version                        = 1,
-       .src                            = -1,
+       .src                            = SRC_GYR,
        .rr_0n                          = ARRAY_SIZE(nvi_rr_dmp) - 1,
        .rr                             = nvi_rr_dmp,
        .milliamp                       = {
@@ -1302,8 +1291,8 @@ const struct nvi_hal nvi_hal_20628 = {
        .reg_bank_n                     = 4,
        .src                            = src,
        .src_n                          = ARRAY_SIZE(src),
-       .fifo_dev                       = nvi_fifo_dev,
-       .fifo_n                         = ARRAY_SIZE(nvi_fifo_dev),
+       .fifo_dev                       = nvi_fifo_devs,
+       .fifo_n                         = ARRAY_SIZE(nvi_fifo_devs),
        .lp_tbl                         = nvi_lp_dly_us_tbl,
        .lp_tbl_n                       = ARRAY_SIZE(nvi_lp_dly_us_tbl),
        .dev[DEV_ACC]                   = &nvi_hal_acc,
@@ -1312,11 +1301,13 @@ const struct nvi_hal nvi_hal_20628 = {
        .dev[DEV_SM]                    = &nvi_hal_dmp,
        .dev[DEV_STP]                   = &nvi_hal_dmp,
        .dev[DEV_QTN]                   = &nvi_hal_dmp,
+       .dev[DEV_GMR]                   = &nvi_hal_dmp,
+       .dev[DEV_GYU]                   = &nvi_hal_gyr,
        .dev[DEV_AUX]                   = &nvi_hal_aux,
        .reg                            = &nvi_hal_reg_icm,
        .bit                            = &nvi_hal_bit_icm,
        .fn                             = &nvi_fn_icm,
-       .dmp                            = NULL, /* &nvi_dmp_icm, */
+       .dmp                            = &nvi_dmp_icm,
 };
 EXPORT_SYMBOL(nvi_hal_20628);
 
index 7b75d69..67fabe6 100644 (file)
@@ -1027,6 +1027,7 @@ static int nvi_en_gyr(struct nvi_state *st)
        u8 val = st->snsr[DEV_GYR].usr_cfg << 3;
 
        st->snsr[DEV_GYR].matrix = true;
+       st->snsr[DEV_GYR].buf_n = 6;
        return nvi_i2c_wr_rc(st, &st->hal->reg->gyro_config2, val,
                             __func__, &st->rc.gyro_config2);
 }
@@ -1060,7 +1061,7 @@ static const unsigned int nvi_lpf_us_tbl[] = {
        /* 200000, 5Hz */
 };
 
-static int nvi_period(struct nvi_state *st)
+static int nvi_src(struct nvi_state *st)
 {
        u8 lpf;
        u16 rate;
@@ -1068,10 +1069,6 @@ static int nvi_period(struct nvi_state *st)
        int ret;
 
        us = st->src[SRC_MPU].period_us_req;
-       if (us < st->hal->src[SRC_MPU].period_us_min)
-               us = st->hal->src[SRC_MPU].period_us_min;
-       if (us > st->hal->src[SRC_MPU].period_us_max)
-               us = st->hal->src[SRC_MPU].period_us_max;
        /* calculate rate */
        rate = us / 1000 - 1;
        st->src[SRC_MPU].period_us_src = us;
@@ -1088,10 +1085,8 @@ static int nvi_period(struct nvi_state *st)
                             __func__, &st->rc.gyro_config1);
        ret |= nvi_aux_delay(st, __func__);
        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
-               dev_info(&st->i2c->dev,
-                        "%s src[SRC_MPU]: period=%u timeout=%u err=%d\n",
-                        __func__, st->src[SRC_MPU].period_us_req,
-                        st->src_timeout_us[SRC_MPU], ret);
+               dev_info(&st->i2c->dev, "%s src[SRC_MPU]: period=%u err=%d\n",
+                        __func__, st->src[SRC_MPU].period_us_req, ret);
        return ret;
 }
 
@@ -1108,7 +1103,7 @@ static const struct nvi_hal_src src[] = {
                                           (1 << DEV_AUX)),
                .period_us_min          = 10000,
                .period_us_max          = 256000,
-               .fn_period              = nvi_period,
+               .fn_period              = nvi_src,
        },
 };
 
@@ -1399,7 +1394,6 @@ static const struct nvi_hal_reg nvi_hal_reg_6050 = {
        },
        .fifo_en                        = {
                .reg                    = 0x23,
-               .dflt                   = 0x4000,
        },
        .fifo_count_h                   = {
                .reg                    = 0x72,
@@ -1524,6 +1518,8 @@ const struct nvi_hal nvi_hal_6050 = {
        .dev[DEV_SM]                    = &nvi_hal_dmp,
        .dev[DEV_STP]                   = &nvi_hal_dmp,
        .dev[DEV_QTN]                   = &nvi_hal_dmp,
+       .dev[DEV_GMR]                   = &nvi_hal_dmp,
+       .dev[DEV_GYU]                   = &nvi_hal_gyr,
        .dev[DEV_AUX]                   = &nvi_hal_aux,
        .reg                            = &nvi_hal_reg_6050,
        .bit                            = &nvi_hal_bit_6050,
@@ -1690,7 +1686,6 @@ static const struct nvi_hal_reg nvi_hal_reg_6500 = {
        },
        .fifo_en                        = {
                .reg                    = 0x23,
-               .dflt                   = 0x4000,
        },
        .fifo_count_h                   = {
                .reg                    = 0x72,
@@ -1789,6 +1784,8 @@ const struct nvi_hal nvi_hal_6500 = {
        .dev[DEV_SM]                    = &nvi_hal_dmp,
        .dev[DEV_STP]                   = &nvi_hal_dmp,
        .dev[DEV_QTN]                   = &nvi_hal_dmp,
+       .dev[DEV_GMR]                   = &nvi_hal_dmp,
+       .dev[DEV_GYU]                   = &nvi_hal_gyr,
        .dev[DEV_AUX]                   = &nvi_hal_aux,
        .reg                            = &nvi_hal_reg_6500,
        .bit                            = &nvi_hal_bit_6050,
@@ -1809,6 +1806,8 @@ const struct nvi_hal nvi_hal_6515 = {
        .dev[DEV_SM]                    = &nvi_hal_dmp,
        .dev[DEV_STP]                   = &nvi_hal_dmp,
        .dev[DEV_QTN]                   = &nvi_hal_dmp,
+       .dev[DEV_GMR]                   = &nvi_hal_dmp,
+       .dev[DEV_GYU]                   = &nvi_hal_gyr,
        .dev[DEV_AUX]                   = &nvi_hal_aux,
        .reg                            = &nvi_hal_reg_6500,
        .bit                            = &nvi_hal_bit_6050,
index 9485bcb..57d0c31 100644 (file)
 #include <linux/mpu_iio.h>
 #endif /* AKM_NVI_MPU_SUPPORT */
 
-#define AKM_DRIVER_VERSION             (301)
+#define AKM_DRIVER_VERSION             (304)
 #define AKM_VENDOR                     "AsahiKASEI"
 #define AKM_NAME                       "ak89xx"
 #define AKM_NAME_AK8963                        "ak8963"
 #define AKM_NAME_AK8975                        "ak8975"
 #define AKM_NAME_AK09911               "ak09911"
 #define AKM_KBUF_SIZE                  (32)
-#define AKM_OFFSET_IVAL                        (0)
-#define AKM_OFFSET_MICRO               (0)
 #define AKM_DELAY_US_MAX               (255000)
 #define AKM_HW_DELAY_POR_MS            (50)
 #define AKM_HW_DELAY_TSM_MS            (10) /* Time Single Measurement */
@@ -131,10 +129,9 @@ static unsigned short akm_i2c_addrs[] = {
        0x0F,
 };
 
-struct akm_scale {
+struct akm_rr {
        struct nvs_float max_range;
        struct nvs_float resolution;
-       struct nvs_float scale;
        s16 range_lo[3];
        s16 range_hi[3];
 };
@@ -152,16 +149,18 @@ struct akm_state {
        struct regulator_bulk_data vreg[ARRAY_SIZE(akm_vregs)];
        struct delayed_work dw;
        struct akm_hal *hal;            /* Hardware Abstraction Layer */
-       u8 asa[3];                      /* axis sensitivity adjustment */
+       u8 asa[AXIS_N];                 /* axis sensitivity adjustment */
+       u64 asa_q30[AXIS_N];            /* Q30 axis sensitivity adjustment */
        unsigned int sts;               /* status flags */
        unsigned int errs;              /* error count */
        unsigned int enabled;           /* enable status */
        unsigned int poll_delay_us;     /* requested sampling delay (us) */
-       unsigned int scale_i;           /* scale index */
+       unsigned int rr_i;              /* resolution/range index */
        u16 i2c_addr;                   /* I2C address */
        u8 dev_id;                      /* device ID */
        bool irq_dis;                   /* interrupt host disable flag */
        bool initd;                     /* set if initialized */
+       bool matrix_en;                 /* handle matrix internally */
        bool mpu_en;                    /* if device behind MPU */
        bool port_en[2];                /* enable status of MPU write port */
        int port_id[2];                 /* MPU port ID */
@@ -174,10 +173,11 @@ struct akm_state {
 struct akm_hal {
        const char *part;
        int version;
-       struct akm_scale *scale;
-       u8 scale_i_max;
+       struct akm_rr *rr;
+       u8 rr_i_max;
        struct nvs_float milliamp;
        unsigned int delay_us_min;
+       unsigned int asa_shift;
        u8 reg_start_rd;
        u8 reg_st1;
        u8 reg_st2;
@@ -481,7 +481,7 @@ static int akm_mode(struct akm_state *st)
                        }
                }
        }
-       if (st->scale_i)
+       if (st->rr_i)
                mode |= AKM_BIT_BITM;
        ret = akm_mode_wr(st, mode);
        return ret;
@@ -498,19 +498,23 @@ static s16 akm_matrix(struct akm_state *st,
                 (st->cfg.matrix[6 + axis] == -1 ? -z : 0)));
 }
 
-static void akm_calc(struct akm_state *st, s16 *data, bool matrix)
+static void akm_calc(struct akm_state *st, s16 *data, bool be, bool matrix)
 {
        s16 x;
        s16 y;
        s16 z;
        unsigned int axis;
 
-       st->magn_uc[AXIS_X] = data[AXIS_X];
-       st->magn_uc[AXIS_Y] = data[AXIS_Y];
-       st->magn_uc[AXIS_Z] = data[AXIS_Z];
-       x = (((int)st->magn_uc[AXIS_X] * (st->asa[AXIS_X] + 128)) >> 8);
-       y = (((int)st->magn_uc[AXIS_Y] * (st->asa[AXIS_Y] + 128)) >> 8);
-       z = (((int)st->magn_uc[AXIS_Z] * (st->asa[AXIS_Z] + 128)) >> 8);
+       for (axis = 0; axis < AXIS_N; axis++) {
+               if (be)
+                       st->magn_uc[axis] = be16_to_cpup(&data[axis]);
+               else
+                       st->magn_uc[axis] = data[axis];
+       }
+
+       x = (((s64)st->magn_uc[AXIS_X] * st->asa_q30[AXIS_X]) >> 30);
+       y = (((s64)st->magn_uc[AXIS_Y] * st->asa_q30[AXIS_Y]) >> 30);
+       z = (((s64)st->magn_uc[AXIS_Z] * st->asa_q30[AXIS_Z]) >> 30);
        if (matrix) {
                for (axis = 0; axis < AXIS_N; axis++)
                        st->magn[axis] = akm_matrix(st, x, y, z, axis);
@@ -531,11 +535,16 @@ static int akm_read_sts(struct akm_state *st, u8 *data)
        st2 = st->hal->reg_st2 - st->hal->reg_start_rd;
        if (data[st2] & (AKM_BIT_HOFL | AKM_BIT_DERR)) {
                if (st->sts & NVS_STS_SPEW_MSG)
-                       dev_info(&st->i2c->dev, "%s ERR\n", __func__);
+                       dev_info(&st->i2c->dev, "%s ERR: STS2=0x%02x\n",
+                                __func__, data[st2]);
                akm_err(st);
                ret = -1; /* error */
-       } else if (data[st1] & AKM_BIT_DRDY) {
-               ret = 1; /* data ready to be reported */
+       } else if (data[st1]) {
+               if (data[st1] & AKM_BIT_DOR && st->sts & NVS_STS_SPEW_MSG)
+                       dev_info(&st->i2c->dev, "%s ERR: STS1=0x%02x\n",
+                                __func__, data[st1]);
+               if (data[st1] & AKM_BIT_DRDY)
+                       ret = 1; /* data ready to be reported */
        }
        return ret;
 }
@@ -555,7 +564,7 @@ static int akm_read(struct akm_state *st)
        ret = akm_read_sts(st, data);
        if (ret > 0) {
                i = st->hal->reg_st1 - st->hal->reg_start_rd + 1;
-               akm_calc(st, (s16 *)&data[i], false);
+               akm_calc(st, (s16 *)&data[i], false, st->matrix_en);
                st->nvs->handler(st->nvs_st, &st->magn, ts);
        }
        return ret;
@@ -565,6 +574,7 @@ static int akm_read(struct akm_state *st)
 static void akm_mpu_handler(u8 *data, unsigned int len, s64 ts, void *p_val)
 {
        struct akm_state *st = (struct akm_state *)p_val;
+       bool be = false;
        unsigned int i;
        int ret;
 
@@ -582,13 +592,14 @@ static void akm_mpu_handler(u8 *data, unsigned int len, s64 ts, void *p_val)
                if (len == 6) {
                        /* this data is from the DMP */
                        i = 0;
+                       be = true;
                        ret = 1;
                } else {
                        i = st->hal->reg_st1 - st->hal->reg_start_rd + 1;
                        ret = akm_read_sts(st, data);
                }
                if (ret > 0) {
-                       akm_calc(st, (s16 *)&data[i], true);
+                       akm_calc(st, (s16 *)&data[i], be, st->matrix_en);
                        st->nvs->handler(st->nvs_st, &st->magn, ts);
                }
        }
@@ -644,7 +655,7 @@ static int akm_self_test(struct akm_state *st)
                udelay(AKM_HW_DELAY_US);
        }
        mode = st->hal->mode_self_test;
-       if (st->scale_i)
+       if (st->rr_i)
                mode |= AKM_BIT_BITM;
        ret_t |= akm_i2c_wr(st, st->hal->reg_mode, mode);
        mdelay(AKM_HW_DELAY_TSM_MS);
@@ -653,7 +664,7 @@ static int akm_self_test(struct akm_state *st)
                ret = akm_read_sts(st, data);
                if (ret > 0) {
                        i = st->hal->reg_st1 - st->hal->reg_start_rd + 1;
-                       akm_calc(st, (s16 *)&data[i], false);
+                       akm_calc(st, (s16 *)&data[i], false, false);
                        ret = 0;
                } else {
                        ret = -EBUSY;
@@ -667,19 +678,19 @@ static int akm_self_test(struct akm_state *st)
                        __func__, ret_t);
        } else {
                if ((st->magn[AXIS_X] <
-                              st->hal->scale[st->scale_i].range_lo[AXIS_X]) ||
+                              st->hal->rr[st->rr_i].range_lo[AXIS_X]) ||
                                (st->magn[AXIS_X] >
-                                st->hal->scale[st->scale_i].range_hi[AXIS_X]))
+                                st->hal->rr[st->rr_i].range_hi[AXIS_X]))
                        ret_t |= 1 << AXIS_X;
                if ((st->magn[AXIS_Y] <
-                              st->hal->scale[st->scale_i].range_lo[AXIS_Y]) ||
+                              st->hal->rr[st->rr_i].range_lo[AXIS_Y]) ||
                                (st->magn[AXIS_Y] >
-                                st->hal->scale[st->scale_i].range_hi[AXIS_Y]))
+                                st->hal->rr[st->rr_i].range_hi[AXIS_Y]))
                        ret_t |= 1 << AXIS_Y;
                if ((st->magn[AXIS_Z] <
-                              st->hal->scale[st->scale_i].range_lo[AXIS_Z]) ||
+                              st->hal->rr[st->rr_i].range_lo[AXIS_Z]) ||
                                (st->magn[AXIS_Z] >
-                                st->hal->scale[st->scale_i].range_hi[AXIS_Z]))
+                                st->hal->rr[st->rr_i].range_hi[AXIS_Z]))
                        ret_t |= 1 << AXIS_Z;
                if (ret_t) {
                        dev_err(&st->i2c->dev, "%s ERR: out_of_range %x\n",
@@ -691,6 +702,7 @@ static int akm_self_test(struct akm_state *st)
 
 static int akm_init_hw(struct akm_state *st)
 {
+       unsigned int i;
        int ret;
 
        ret = akm_nvi_mpu_bypass_request(st);
@@ -699,14 +711,21 @@ static int akm_init_hw(struct akm_state *st)
                                 st->hal->mode_rom_read);
                udelay(AKM_HW_DELAY_ROM_ACCESS_US);
                ret |= akm_i2c_rd(st, st->hal->reg_asa, 3, st->asa);
-               ret |= akm_i2c_wr(st, st->hal->reg_mode, AKM_MODE_POWERDOWN);
+               akm_i2c_wr(st, st->hal->reg_mode, AKM_MODE_POWERDOWN);
                akm_self_test(st);
                akm_nvi_mpu_bypass_release(st);
        }
-       if (ret)
+       if (ret) {
                dev_err(&st->i2c->dev, "%s ERR %d\n", __func__, ret);
-       else
+       } else {
                st->initd = true;
+               for (i = 0; i < AXIS_N; i++) {
+                       if (!st->asa_q30[i])
+                               /* use HW setting if no DT override */
+                               st->asa_q30[i] = st->asa[i] + 128;
+                       st->asa_q30[i] <<= 30 - st->hal->asa_shift;
+               }
+       }
        return ret;
 }
 
@@ -831,25 +850,31 @@ static int akm_flush(void *client, int snsr_id)
 static int akm_resolution(void *client, int snsr_id, int resolution)
 {
        struct akm_state *st = (struct akm_state *)client;
-       unsigned int scale_i = st->scale_i;
+       unsigned int rr_i = st->rr_i;
        int ret = 0;
 
-       if (resolution < 0 || resolution > st->hal->scale_i_max)
+       if (st->mpu_en)
+               /* can't change resolutions at runtime when behind the MPU
+                * since DMP has already been configured with the resolution.
+                */
+               return -EINVAL;
+
+       if (resolution < 0 || resolution > st->hal->rr_i_max)
                return -EINVAL;
 
-       st->scale_i = resolution;
-       if (st->enabled && (resolution != scale_i)) {
+       st->rr_i = resolution;
+       if (st->enabled && (resolution != rr_i)) {
                ret = akm_mode(st);
                if (ret < 0)
-                       st->scale_i = scale_i;
+                       st->rr_i = rr_i;
        }
 
-       st->cfg.max_range.ival = st->hal->scale[st->scale_i].max_range.ival;
-       st->cfg.max_range.fval = st->hal->scale[st->scale_i].max_range.fval;
-       st->cfg.resolution.ival = st->hal->scale[st->scale_i].resolution.ival;
-       st->cfg.resolution.fval = st->hal->scale[st->scale_i].resolution.fval;
-       st->cfg.scale.ival = st->hal->scale[st->scale_i].scale.ival;
-       st->cfg.scale.fval = st->hal->scale[st->scale_i].scale.fval;
+       st->cfg.max_range.ival = st->hal->rr[st->rr_i].max_range.ival;
+       st->cfg.max_range.fval = st->hal->rr[st->rr_i].max_range.fval;
+       st->cfg.resolution.ival = st->hal->rr[st->rr_i].resolution.ival;
+       st->cfg.resolution.fval = st->hal->rr[st->rr_i].resolution.fval;
+       st->cfg.scale.ival = st->hal->rr[st->rr_i].resolution.ival;
+       st->cfg.scale.fval = st->hal->rr[st->rr_i].resolution.fval;
        return ret;
 }
 
@@ -961,6 +986,9 @@ static int akm_nvs_read(void *client, int snsr_id, char *buf)
        t += sprintf(buf + t, "irq=%d\n", st->i2c->irq);
        t += sprintf(buf + t, "mpu_en=%x\n", st->mpu_en);
        t += sprintf(buf + t, "nvi_config=%hhu\n", st->nvi_config);
+       t += sprintf(buf + t, "asa_q30_x=%llu\n", st->asa_q30[AXIS_X]);
+       t += sprintf(buf + t, "asa_q30_y=%llu\n", st->asa_q30[AXIS_Y]);
+       t += sprintf(buf + t, "asa_q30_z=%llu\n", st->asa_q30[AXIS_Z]);
        return t;
 }
 
@@ -1034,7 +1062,7 @@ static int akm_remove(struct i2c_client *client)
        return 0;
 }
 
-static struct akm_scale akm_scale_09911[] = {
+static struct akm_rr akm_rr_09911[] = {
        {
                .max_range              = {
                        .ival           = 9825,
@@ -1044,10 +1072,6 @@ static struct akm_scale akm_scale_09911[] = {
                        .ival           = 0,
                        .fval           = 600000,
                },
-               .scale                  = {
-                       .ival           = 0,
-                       .fval           = 600000,
-               },
                .range_lo[AXIS_X]       = -30,
                .range_hi[AXIS_X]       = 30,
                .range_lo[AXIS_Y]       = -30,
@@ -1080,13 +1104,14 @@ static struct akm_cmode akm_cmode_09911[] = {
 static struct akm_hal akm_hal_09911 = {
        .part                           = AKM_NAME_AK09911,
        .version                        = 2,
-       .scale                          = akm_scale_09911,
-       .scale_i_max                    = ARRAY_SIZE(akm_scale_09911) - 1,
+       .rr                             = akm_rr_09911,
+       .rr_i_max                       = ARRAY_SIZE(akm_rr_09911) - 1,
        .milliamp                       = {
                .ival                   = 2,
                .fval                   = 400000,
        },
        .delay_us_min                   = 10000,
+       .asa_shift                      = 7,
        .reg_start_rd                   = 0x10,
        .reg_st1                        = 0x10,
        .reg_st2                        = 0x18,
@@ -1105,7 +1130,7 @@ static struct akm_hal akm_hal_09911 = {
 #endif /* AKM_NVI_MPU_SUPPORT */
 };
 
-static struct akm_scale akm_scale_8975[] = {
+static struct akm_rr akm_rr_8975[] = {
        {
                .max_range              = {
                        .ival           = 2459,
@@ -1115,10 +1140,6 @@ static struct akm_scale akm_scale_8975[] = {
                        .ival           = 0,
                        .fval           = 300000,
                },
-               .scale                  = {
-                       .ival           = 0,
-                       .fval           = 300000,
-               },
                .range_lo[AXIS_X]       = -100,
                .range_hi[AXIS_X]       = 100,
                .range_lo[AXIS_Y]       = -100,
@@ -1131,13 +1152,14 @@ static struct akm_scale akm_scale_8975[] = {
 static struct akm_hal akm_hal_8975 = {
        .part                           = AKM_NAME_AK8975,
        .version                        = 2,
-       .scale                          = akm_scale_8975,
-       .scale_i_max                    = ARRAY_SIZE(akm_scale_8975) - 1,
+       .rr                             = akm_rr_8975,
+       .rr_i_max                       = ARRAY_SIZE(akm_rr_8975) - 1,
        .milliamp                       = {
                .ival                   = 3,
                .fval                   = 0,
        },
        .delay_us_min                   = 10000,
+       .asa_shift                      = 8,
        .reg_start_rd                   = 0x01,
        .reg_st1                        = 0x02,
        .reg_st2                        = 0x09,
@@ -1156,7 +1178,7 @@ static struct akm_hal akm_hal_8975 = {
 #endif /* AKM_NVI_MPU_SUPPORT */
 };
 
-static struct akm_scale akm_scale_8963[] = {
+static struct akm_rr akm_rr_8963[] = {
        {
                .max_range              = {
                        .ival           = 9825,
@@ -1166,10 +1188,6 @@ static struct akm_scale akm_scale_8963[] = {
                        .ival           = 0,
                        .fval           = 600000,
                },
-               .scale                  = {
-                       .ival           = 0,
-                       .fval           = 600000,
-               },
                .range_lo[AXIS_X]       = -50,
                .range_hi[AXIS_X]       = 50,
                .range_lo[AXIS_Y]       = -50,
@@ -1186,10 +1204,6 @@ static struct akm_scale akm_scale_8963[] = {
                        .ival           = 0,
                        .fval           = 150000,
                },
-               .scale                  = {
-                       .ival           = 0,
-                       .fval           = 150000,
-               },
                .range_lo[AXIS_X]       = -200,
                .range_hi[AXIS_X]       = 200,
                .range_lo[AXIS_Y]       = -200,
@@ -1214,13 +1228,14 @@ static struct akm_cmode akm_cmode_8963[] = {
 static struct akm_hal akm_hal_8963 = {
        .part                           = AKM_NAME_AK8963,
        .version                        = 2,
-       .scale                          = akm_scale_8963,
-       .scale_i_max                    = ARRAY_SIZE(akm_scale_8963) - 1,
+       .rr                             = akm_rr_8963,
+       .rr_i_max                       = ARRAY_SIZE(akm_rr_8963) - 1,
        .milliamp                       = {
                .ival                   = 2,
                .fval                   = 800000,
        },
        .delay_us_min                   = 10000,
+       .asa_shift                      = 8,
        .reg_start_rd                   = 0x01,
        .reg_st1                        = 0x02,
        .reg_st2                        = 0x09,
@@ -1260,7 +1275,7 @@ static int akm_id_hal(struct akm_state *st, u8 dev_id)
                st->hal = &akm_hal_8975;
                ret = -ENODEV;
        }
-       st->scale_i = st->hal->scale_i_max;
+       st->rr_i = st->hal->rr_i_max;
        st->cfg.name = "magnetic_field";
        st->cfg.kbuf_sz = AKM_KBUF_SIZE;
        st->cfg.ch_n = 3;
@@ -1268,18 +1283,16 @@ static int akm_id_hal(struct akm_state *st, u8 dev_id)
        st->cfg.part = st->hal->part;
        st->cfg.vendor = AKM_VENDOR;
        st->cfg.version = st->hal->version;
-       st->cfg.max_range.ival = st->hal->scale[st->scale_i].max_range.ival;
-       st->cfg.max_range.fval = st->hal->scale[st->scale_i].max_range.fval;
-       st->cfg.resolution.ival = st->hal->scale[st->scale_i].resolution.ival;
-       st->cfg.resolution.fval = st->hal->scale[st->scale_i].resolution.fval;
+       st->cfg.max_range.ival = st->hal->rr[st->rr_i].max_range.ival;
+       st->cfg.max_range.fval = st->hal->rr[st->rr_i].max_range.fval;
+       st->cfg.resolution.ival = st->hal->rr[st->rr_i].resolution.ival;
+       st->cfg.resolution.fval = st->hal->rr[st->rr_i].resolution.fval;
        st->cfg.milliamp.ival = st->hal->milliamp.ival;
        st->cfg.milliamp.fval = st->hal->milliamp.fval;
        st->cfg.delay_us_min = st->hal->delay_us_min;
        st->cfg.delay_us_max = AKM_DELAY_US_MAX;
-       st->cfg.scale.ival = st->hal->scale[st->scale_i].scale.ival;
-       st->cfg.scale.fval = st->hal->scale[st->scale_i].scale.fval;
-       st->cfg.offset.ival = AKM_OFFSET_IVAL;
-       st->cfg.offset.fval = AKM_OFFSET_MICRO;
+       st->cfg.scale.ival = st->hal->rr[st->rr_i].resolution.ival;
+       st->cfg.scale.fval = st->hal->rr[st->rr_i].resolution.fval;
        nvs_of_dt(st->i2c->dev.of_node, &st->cfg, NULL);
        return ret;
 }
@@ -1346,6 +1359,8 @@ static int akm_id_dev(struct akm_state *st, const char *name)
 {
 #if AKM_NVI_MPU_SUPPORT
        struct nvi_mpu_port nmp;
+       unsigned int i;
+       u64 q30;
        u8 config_boot;
 #endif /* AKM_NVI_MPU_SUPPORT */
        u8 val = 0;
@@ -1386,6 +1401,7 @@ static int akm_id_dev(struct akm_state *st, const char *name)
                        nmp.addr = st->i2c_addr | 0x80;
                        nmp.reg = st->hal->reg_start_rd;
                        nmp.ctrl = 10; /* MPU FIFO can't handle odd size */
+                       nmp.dmp_ctrl = 0x59; /* switch to big endian */
                        nmp.data_out = 0;
                        nmp.delay_ms = 0;
                        nmp.delay_us = st->poll_delay_us;
@@ -1398,8 +1414,17 @@ static int akm_id_dev(struct akm_state *st, const char *name)
                        memcpy(nmp.matrix, st->cfg.matrix, sizeof(nmp.matrix));
                        nmp.type = SECONDARY_SLAVE_TYPE_COMPASS;
                        nmp.id = st->hal->mpu_id;
-                       memcpy(nmp.asa, st->asa, sizeof(nmp.asa));
-                       nmp.rate_scale = 10;
+                       for (i = 0; i < AXIS_N; i++) {
+                               q30 = st->asa_q30[i];
+                               q30 *= st->hal->rr[st->rr_i].resolution.fval;
+                               if (st->cfg.float_significance)
+                                       do_div(q30,
+                                              NVS_FLOAT_SIGNIFICANCE_NANO);
+                               else
+                                       do_div(q30,
+                                              NVS_FLOAT_SIGNIFICANCE_MICRO);
+                               nmp.q30[i] = q30;
+                       }
                        ret = nvi_mpu_port_alloc(&nmp);
                        dev_dbg(&st->i2c->dev, "%s MPU port/ret=%d\n",
                                __func__, ret);
@@ -1512,6 +1537,17 @@ static int akm_of_dt(struct akm_state *st, struct device_node *dn)
                }
        }
 
+       /* option to handle matrix internally */
+       cfg = 0;
+       of_property_read_u8(dn, "internal_matrix_enable", &cfg);
+       if (cfg)
+               st->matrix_en = true;
+       else
+               st->matrix_en = false;
+       /* axis sensitivity adjustment overrides */
+       of_property_read_u32(dn, "ara_q30_x", (u32 *)&st->asa_q30[AXIS_X]);
+       of_property_read_u32(dn, "ara_q30_y", (u32 *)&st->asa_q30[AXIS_Y]);
+       of_property_read_u32(dn, "ara_q30_z", (u32 *)&st->asa_q30[AXIS_Z]);
        return 0;
 }
 
@@ -1571,8 +1607,7 @@ static int akm_probe(struct i2c_client *client,
                goto akm_probe_err;
        }
 
-       if (st->mpu_en) {
-               /* Due to DMP, matrix handled at kernel so remove from NVS */
+       if (st->matrix_en) {
                memcpy(matrix, st->cfg.matrix, sizeof(matrix));
                memset(st->cfg.matrix, 0, sizeof(st->cfg.matrix));
        }
@@ -1584,9 +1619,9 @@ static int akm_probe(struct i2c_client *client,
                goto akm_probe_err;
        }
 
-       if (st->mpu_en)
+       if (st->matrix_en)
                memcpy(st->cfg.matrix, matrix, sizeof(st->cfg.matrix));
-       else
+       if (!st->mpu_en)
                INIT_DELAYED_WORK(&st->dw, akm_work);
        if ((st->i2c->irq > 0) && !st->mpu_en) {
                ret = request_threaded_irq(st->i2c->irq, NULL, akm_irq_thread,
index fb9d96b..a03366f 100644 (file)
@@ -60,7 +60,7 @@
 #include <linux/mpu_iio.h>
 #endif /* BMP_NVI_MPU_SUPPORT */
 
-#define BMP_DRIVER_VERSION             (300)
+#define BMP_DRIVER_VERSION             (301)
 #define BMP_VENDOR                     "Bosch"
 #define BMP_NAME                       "bmpX80"
 #define BMP180_NAME                    "bmp180"
@@ -1020,7 +1020,7 @@ static int bmp_batch(void *client, int snsr_id, int flags,
                     unsigned int period, unsigned int timeout)
 {
        struct bmp_state *st = (struct bmp_state *)client;
-       unsigned int old_peroid;
+       unsigned int old_period;
        unsigned int old_timeout;
        int ret;
 
@@ -1028,7 +1028,7 @@ static int bmp_batch(void *client, int snsr_id, int flags,
                /* timeout not supported (no HW FIFO) */
                return -EINVAL;
 
-       old_peroid = st->delay_us[snsr_id];
+       old_period = st->delay_us[snsr_id];
        old_timeout = st->timeout_us[snsr_id];
        st->delay_us[snsr_id] = period;
        st->timeout_us[snsr_id] = timeout;
@@ -1036,7 +1036,7 @@ static int bmp_batch(void *client, int snsr_id, int flags,
        if (st->enabled && st->i2c->irq && !ret)
                ret = bmp_mode(st);
        if (ret) {
-               st->delay_us[snsr_id] = old_peroid;
+               st->delay_us[snsr_id] = old_period;
                st->timeout_us[snsr_id] = old_timeout;
        }
        return 0;
@@ -1616,7 +1616,7 @@ static int bmp_id_hal(struct bmp_state *st)
 
        default:
                dev_err(&st->i2c->dev, "%s ERR: Unknown device\n", __func__);
-               st->hal = &bmp_hal_180; /* to prevent NULL pointers */
+               st->hal = &bmp_hal_280; /* to prevent NULL pointers */
                ret = -ENODEV;
                break;
        }
@@ -1742,7 +1742,6 @@ static int bmp_id_dev(struct bmp_state *st, const char *name)
                        nmp.ctrl = 10; /* MPU FIFO can't handle odd size */
                        nmp.handler = &bmp_mpu_handler_280;
                }
-               nmp.rate_scale = 34;
                ret = nvi_mpu_port_alloc(&nmp);
                dev_dbg(&st->i2c->dev, "%s MPU port/ret=%d\n",
                        __func__, ret);
@@ -1788,14 +1787,14 @@ static int bmp_id_dev(struct bmp_state *st, const char *name)
 #endif /* BMP_NVI_MPU_SUPPORT */
        /* NVI_CONFIG_BOOT_HOST */
        st->mpu_en = false;
-       ret = bmp_i2c_rd(st, BMP_REG_ID, 1, &val);
-       dev_dbg(&st->i2c->dev, "%s Host read ID=%x ret=%d\n",
-               __func__, val, ret);
-       if (!ret) {
-               ret = bmp_id_compare(st, val, name);
+       if (!st->dev_id) {
+               ret = bmp_i2c_rd(st, BMP_REG_ID, 1, &val);
+               dev_dbg(&st->i2c->dev, "%s Host read ID=%x ret=%d\n",
+                       __func__, val, ret);
                if (!ret)
-                       ret = bmp_id_hal(st);
+                       bmp_id_compare(st, val, name);
        }
+       ret = bmp_id_hal(st);
        return ret;
 }
 
index 6bf8e5d..24d515e 100644 (file)
@@ -149,6 +149,11 @@ struct mpu_platform_data {
  *      3:0. If the port is to do write transactions then this
  *      value must be 1.  See MPU documentation for the other
  *      bits in I2C_SLVx_CTRL that can be applied by this byte.
+ * - dmp_ctrl: When the DMP in enabled, the number of
+ *      consecutive registers to read in 3:0. If the port is to
+ *      do write transactions then this value must be 1.  See
+ *      MPU documentation for the other bits in I2C_SLVx_CTRL
+ *      that can be applied by this byte.
  * - data_out: The data byte written if the port is configured
  *      to do writes (addr 7:7 = 0).
  * - delay_ms: The polling delay time between I2C transactions
@@ -185,6 +190,7 @@ struct nvi_mpu_port {
        u8 addr;
        u8 reg;
        u8 ctrl;
+       u8 dmp_ctrl;
        u8 data_out;
        unsigned int delay_ms;
        unsigned long delay_us;
@@ -195,8 +201,7 @@ struct nvi_mpu_port {
        signed char matrix[9];
        enum secondary_slave_type type;
        enum ext_slave_id id;
-       u8 asa[3];
-       int rate_scale;
+       u64 q30[3];
 };
 
 /**