iio: imu: nvi v.334 ICM SMD
Erik Lilliebjerg [Thu, 7 Jul 2016 03:53:55 +0000 (20:53 -0700)]
- Fix ICM DMP FW v.2 significant motion default parameters.
- Add realtime sensor configuration for significant motion.
- Fix ICM DMP FW v.2 maximum period by limiting accelerometer slowest clock
  setting to gyros since the FW v.2 WAR requires the same speed.

Bug 1768847

Change-Id: I8e3d2574019da6e2e9a0e0e574cc33cab363d490
Signed-off-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Reviewed-on: http://git-master/r/1177151
(cherry picked from commit 24b210f89d2856b8ecedb5443e604788e3964680)
Reviewed-on: http://git-master/r/1185053
GVS: Gerrit_Virtual_Submit
Tested-by: Robert Collins <rcollins@nvidia.com>
Reviewed-by: Akhilesh Khumbum <akhumbum@nvidia.com>
Reviewed-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_dmp_mpu.h
drivers/iio/imu/nvi_mpu/nvi_icm.c
drivers/iio/imu/nvi_mpu/nvi_mpu.c
include/linux/nvs.h

index 01b2a54..f179aeb 100644 (file)
@@ -76,7 +76,7 @@
 #include <linux/iio/trigger.h>
 #include <linux/nvs.h>
 
-#define NVS_IIO_DRIVER_VERSION         (213)
+#define NVS_IIO_DRIVER_VERSION         (214)
 
 enum NVS_ATTR {
        NVS_ATTR_ENABLE,
@@ -399,28 +399,40 @@ static ssize_t nvs_dbg_cfg(struct iio_dev *indio_dev, char *buf)
        unsigned int i;
        ssize_t t;
 
-       t = sprintf(buf, "name=%s\n", st->cfg->name);
-       t += sprintf(buf + t, "snsr_id=%d\n", st->cfg->snsr_id);
-       t += sprintf(buf + t, "timestamp_sz=%d\n", st->cfg->timestamp_sz);
-       t += sprintf(buf + t, "snsr_data_n=%d\n", st->cfg->snsr_data_n);
-       t += sprintf(buf + t, "kbuf_sz=%d\n", st->cfg->kbuf_sz);
-       t += sprintf(buf + t, "ch_n=%u\n", st->cfg->ch_n);
-       t += sprintf(buf + t, "ch_sz=%d\n", st->cfg->ch_sz);
-       t += sprintf(buf + t, "ch_inf=%p\n", st->cfg->ch_inf);
-       t += sprintf(buf + t, "delay_us_min=%u\n", st->cfg->delay_us_min);
-       t += sprintf(buf + t, "delay_us_max=%u\n", st->cfg->delay_us_max);
-       t += sprintf(buf + t, "matrix: ");
+       t = snprintf(buf, PAGE_SIZE, "name=%s\n", st->cfg->name);
+       t += snprintf(buf + t, PAGE_SIZE - t, "snsr_id=%d\n",
+                     st->cfg->snsr_id);
+       t += snprintf(buf + t, PAGE_SIZE - t, "timestamp_sz=%d\n",
+                     st->cfg->timestamp_sz);
+       t += snprintf(buf + t, PAGE_SIZE - t, "snsr_data_n=%d\n",
+                     st->cfg->snsr_data_n);
+       t += snprintf(buf + t, PAGE_SIZE - t, "kbuf_sz=%d\n",
+                     st->cfg->kbuf_sz);
+       t += snprintf(buf + t, PAGE_SIZE - t, "ch_n=%u\n", st->cfg->ch_n);
+       t += snprintf(buf + t, PAGE_SIZE - t, "ch_sz=%d\n", st->cfg->ch_sz);
+       t += snprintf(buf + t, PAGE_SIZE - t, "ch_inf=%p\n", st->cfg->ch_inf);
+       t += snprintf(buf + t, PAGE_SIZE - t, "delay_us_min=%u\n",
+                     st->cfg->delay_us_min);
+       t += snprintf(buf + t, PAGE_SIZE - t, "delay_us_max=%u\n",
+                     st->cfg->delay_us_max);
+       t += snprintf(buf + t, PAGE_SIZE - t, "matrix: ");
        for (i = 0; i < 9; i++)
-               t += sprintf(buf + t, "%hhd ", st->cfg->matrix[i]);
-       t += sprintf(buf + t, "\nuncal_lo=%d\n", st->cfg->uncal_lo);
-       t += sprintf(buf + t, "uncal_hi=%d\n", st->cfg->uncal_hi);
-       t += sprintf(buf + t, "cal_lo=%d\n", st->cfg->cal_lo);
-       t += sprintf(buf + t, "cal_hi=%d\n", st->cfg->cal_hi);
-       t += sprintf(buf + t, "thresh_lo=%d\n", st->cfg->thresh_lo);
-       t += sprintf(buf + t, "thresh_hi=%d\n", st->cfg->thresh_hi);
-       t += sprintf(buf + t, "report_n=%d\n", st->cfg->report_n);
-       t += sprintf(buf + t, "float_significance=%s\n",
-                    nvs_float_significances[st->cfg->float_significance]);
+               t += snprintf(buf + t, PAGE_SIZE - t, "%hhd ",
+                             st->cfg->matrix[i]);
+       t += snprintf(buf + t, PAGE_SIZE - t, "\nuncal_lo=%d\n",
+                     st->cfg->uncal_lo);
+       t += snprintf(buf + t, PAGE_SIZE - t, "uncal_hi=%d\n",
+                     st->cfg->uncal_hi);
+       t += snprintf(buf + t, PAGE_SIZE - t, "cal_lo=%d\n", st->cfg->cal_lo);
+       t += snprintf(buf + t, PAGE_SIZE - t, "cal_hi=%d\n", st->cfg->cal_hi);
+       t += snprintf(buf + t, PAGE_SIZE - t, "thresh_lo=%d\n",
+                     st->cfg->thresh_lo);
+       t += snprintf(buf + t, PAGE_SIZE - t, "thresh_hi=%d\n",
+                     st->cfg->thresh_hi);
+       t += snprintf(buf + t, PAGE_SIZE - t, "report_n=%d\n",
+                     st->cfg->report_n);
+       t += snprintf(buf + t, PAGE_SIZE - t, "float_significance=%s\n",
+                     nvs_float_significances[st->cfg->float_significance]);
        return t;
 }
 
@@ -454,7 +466,7 @@ static ssize_t nvs_dbg_data(struct iio_dev *indio_dev, char *buf)
        unsigned int i;
        u64 data;
 
-       t = sprintf(buf, "%s: ", st->cfg->name);
+       t = snprintf(buf, PAGE_SIZE, "%s: ", st->cfg->name);
        n = 0;
        for (i = 0; i < indio_dev->num_channels - 1; i++) {
                ch = &indio_dev->channels[i];
@@ -462,7 +474,7 @@ static ssize_t nvs_dbg_data(struct iio_dev *indio_dev, char *buf)
                        ch_n = ch->scan_type.storagebits / 8;
                        buf_i = nvs_buf_index(ch_n, &n);
                } else {
-                       t += sprintf(buf + t, "disabled ");
+                       t += snprintf(buf + t, PAGE_SIZE - t, "disabled ");
                        continue;
                }
 
@@ -471,16 +483,18 @@ static ssize_t nvs_dbg_data(struct iio_dev *indio_dev, char *buf)
                        memcpy(&data, &st->buf[buf_i], ch_n);
                        if (ch->scan_type.sign == 's') {
                                shift = 64 - ch->scan_type.realbits;
-                               t += sprintf(buf + t, "%lld ",
-                                            (s64)(data << shift) >> shift);
+                               t += snprintf(buf + t, PAGE_SIZE - t, "%lld ",
+                                             (s64)(data << shift) >> shift);
                        } else {
-                               t += sprintf(buf + t, "%llu ", data);
+                               t += snprintf(buf + t, PAGE_SIZE - t, "%llu ",
+                                             data);
                        }
                } else {
-                       t += sprintf(buf + t, "ERR ");
+                       t += snprintf(buf + t, PAGE_SIZE - t, "ERR ");
                }
        }
-       t += sprintf(buf + t, "ts=%lld  ts_diff=%lld\n", st->ts, st->ts_diff);
+       t += snprintf(buf + t, PAGE_SIZE - t, "ts=%lld  ts_diff=%lld\n",
+                     st->ts, st->ts_diff);
        return t;
 }
 
@@ -721,47 +735,50 @@ static ssize_t nvs_attr_show(struct device *dev,
        case NVS_ATTR_ENABLE:
                if (st->fn_dev->enable != nvs_fn_dev_enable) {
                        mutex_lock(&indio_dev->mlock);
-                       ret = sprintf(buf, "%x\n",
-                                     st->fn_dev->enable(st->client,
+                       ret = snprintf(buf, PAGE_SIZE, "%x\n",
+                                      st->fn_dev->enable(st->client,
                                                        st->cfg->snsr_id, -1));
                        mutex_unlock(&indio_dev->mlock);
                }
                return ret;
 
        case NVS_ATTR_PART:
-               return sprintf(buf, "%s %s\n", st->cfg->part, st->cfg->name);
+               return snprintf(buf, PAGE_SIZE, "%s %s\n",
+                               st->cfg->part, st->cfg->name);
 
        case NVS_ATTR_VENDOR:
-               return sprintf(buf, "%s\n", st->cfg->vendor);
+               return snprintf(buf, PAGE_SIZE, "%s\n", st->cfg->vendor);
 
        case NVS_ATTR_VERSION:
-               return sprintf(buf, "%d\n", st->cfg->version);
+               return snprintf(buf, PAGE_SIZE, "%d\n", st->cfg->version);
 
        case NVS_ATTR_MILLIAMP:
-               return sprintf(buf, "%d.%06u\n",
-                              st->cfg->milliamp.ival,
-                              st->cfg->milliamp.fval);
+               return snprintf(buf, PAGE_SIZE, "%d.%06u\n",
+                               st->cfg->milliamp.ival,
+                               st->cfg->milliamp.fval);
 
        case NVS_ATTR_FIFO_RSRV_EVNT_CNT:
-               return sprintf(buf, "%u\n", st->cfg->fifo_rsrv_evnt_cnt);
+               return snprintf(buf, PAGE_SIZE, "%u\n",
+                               st->cfg->fifo_rsrv_evnt_cnt);
 
        case NVS_ATTR_FIFO_MAX_EVNT_CNT:
-               return sprintf(buf, "%u\n", st->cfg->fifo_max_evnt_cnt);
+               return snprintf(buf, PAGE_SIZE, "%u\n",
+                               st->cfg->fifo_max_evnt_cnt);
 
        case NVS_ATTR_FLAGS:
-               return sprintf(buf, "%u\n", st->cfg->flags);
+               return snprintf(buf, PAGE_SIZE, "%u\n", st->cfg->flags);
 
        case NVS_ATTR_MATRIX:
-               return sprintf(buf, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
-                              st->cfg->matrix[0],
-                              st->cfg->matrix[1],
-                              st->cfg->matrix[2],
-                              st->cfg->matrix[3],
-                              st->cfg->matrix[4],
-                              st->cfg->matrix[5],
-                              st->cfg->matrix[6],
-                              st->cfg->matrix[7],
-                              st->cfg->matrix[8]);
+               return snprintf(buf, PAGE_SIZE, "%d,%d,%d,%d,%d,%d,%d,%d,%d\n",
+                               st->cfg->matrix[0],
+                               st->cfg->matrix[1],
+                               st->cfg->matrix[2],
+                               st->cfg->matrix[3],
+                               st->cfg->matrix[4],
+                               st->cfg->matrix[5],
+                               st->cfg->matrix[6],
+                               st->cfg->matrix[7],
+                               st->cfg->matrix[8]);
 
        case NVS_ATTR_SELF_TEST:
                if (st->fn_dev->self_test) {
@@ -860,12 +877,13 @@ static ssize_t nvs_info_show(struct device *dev,
                return nvs_dbg_data(indio_dev, buf);
 
        case NVS_INFO_VER:
-               return sprintf(buf, "version=%u\n", NVS_IIO_DRIVER_VERSION);
+               return snprintf(buf, PAGE_SIZE, "version=%u\n",
+                               NVS_IIO_DRIVER_VERSION);
 
        case NVS_INFO_ERRS:
                i = *st->fn_dev->errs;
                *st->fn_dev->errs = 0;
-               return sprintf(buf, "error count=%u\n", i);
+               return snprintf(buf, PAGE_SIZE, "error count=%u\n", i);
 
        case NVS_INFO_RESET:
                if (st->fn_dev->reset) {
@@ -874,9 +892,9 @@ static ssize_t nvs_info_show(struct device *dev,
                        mutex_unlock(&indio_dev->mlock);
                }
                if (ret)
-                       return sprintf(buf, "reset ERR\n");
+                       return snprintf(buf, PAGE_SIZE, "reset ERR\n");
                else
-                       return sprintf(buf, "reset done\n");
+                       return snprintf(buf, PAGE_SIZE, "reset done\n");
 
        case NVS_INFO_REGS:
                if (st->fn_dev->regs)
@@ -888,23 +906,23 @@ static ssize_t nvs_info_show(struct device *dev,
                return nvs_dbg_cfg(indio_dev, buf);
 
        case NVS_INFO_DBG_KEY:
-               return sprintf(buf, "%s\n", NVS_DBG_KEY);
+               return snprintf(buf, PAGE_SIZE, "%s\n", NVS_DBG_KEY);
 
        case NVS_INFO_DBG:
-               return sprintf(buf, "DBG spew=%x\n",
-                              !!(*st->fn_dev->sts & NVS_STS_SPEW_MSG));
+               return snprintf(buf, PAGE_SIZE, "DBG spew=%x\n",
+                               !!(*st->fn_dev->sts & NVS_STS_SPEW_MSG));
 
        case NVS_INFO_DBG_DATA:
-               return sprintf(buf, "DATA spew=%x\n",
-                              !!(*st->fn_dev->sts & NVS_STS_SPEW_DATA));
+               return snprintf(buf, PAGE_SIZE, "DATA spew=%x\n",
+                               !!(*st->fn_dev->sts & NVS_STS_SPEW_DATA));
 
        case NVS_INFO_DBG_BUF:
-               return sprintf(buf, "BUF spew=%x\n",
-                              !!(*st->fn_dev->sts & NVS_STS_SPEW_BUF));
+               return snprintf(buf, PAGE_SIZE, "BUF spew=%x\n",
+                               !!(*st->fn_dev->sts & NVS_STS_SPEW_BUF));
 
        case NVS_INFO_DBG_IRQ:
-               return sprintf(buf, "IRQ spew=%x\n",
-                              !!(*st->fn_dev->sts & NVS_STS_SPEW_IRQ));
+               return snprintf(buf, PAGE_SIZE, "IRQ spew=%x\n",
+                               !!(*st->fn_dev->sts & NVS_STS_SPEW_IRQ));
 
        default:
                if (dbg < NVS_INFO_LIMIT_MAX)
@@ -1117,10 +1135,14 @@ static int nvs_write_raw(struct iio_dev *indio_dev,
        case IIO_CHAN_INFO_BATCH_PERIOD:
                msg = "IIO_CHAN_INFO_BATCH_PERIOD";
                old = st->batch_period_us;
-               if (val < st->cfg->delay_us_min)
-                       val = st->cfg->delay_us_min;
-               if (st->cfg->delay_us_max && val > st->cfg->delay_us_max)
-                       val = st->cfg->delay_us_max;
+               if (SENSOR_FLAG_ONE_SHOT_MODE != (st->cfg->flags &
+                                                 REPORTING_MODE_MASK)) {
+                       if (val < st->cfg->delay_us_min)
+                               val = st->cfg->delay_us_min;
+                       if (st->cfg->delay_us_max && (val >
+                                                     st->cfg->delay_us_max))
+                               val = st->cfg->delay_us_max;
+               }
                if (st->fn_dev->batch) {
                        ret = st->fn_dev->batch(st->client, st->cfg->snsr_id,
                                                st->batch_flags,
index c5ed766..30cb11a 100644 (file)
@@ -29,7 +29,7 @@
 
 #include "nvi.h"
 
-#define NVI_DRIVER_VERSION             (333)
+#define NVI_DRIVER_VERSION             (334)
 #define NVI_VENDOR                     "Invensense"
 #define NVI_NAME                       "mpu6xxx"
 #define NVI_NAME_MPU6050               "mpu6050"
@@ -1407,10 +1407,10 @@ static int nvi_period_src(struct nvi_state *st, int src)
        }
 
        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_min)
+                       period_us = st->src[src].period_us_min;
+               if (period_us > st->src[src].period_us_max)
+                       period_us = st->src[src].period_us_max;
                if (period_us != st->src[src].period_us_req) {
                        st->src[src].period_us_req = period_us;
                        return 1;
@@ -1976,7 +1976,7 @@ static int nvi_aux_dev_valid(struct nvi_state *st,
        /* enable it at fastest speed */
        st->aux.port[AUX_PORT_IO].nmp.delay_ms = 0;
        st->aux.port[AUX_PORT_IO].period_us =
-                       st->hal->src[st->hal->dev[DEV_AUX]->src].period_us_min;
+                            st->src[st->hal->dev[DEV_AUX]->src].period_us_min;
        ret = nvi_user_ctrl_en(st, __func__, false, false, false, false);
        ret |= nvi_aux_port_enable(st, 1 << AUX_PORT_IO, true);
        ret |= nvi_user_ctrl_en(st, __func__, false, false, true, false);
@@ -3123,7 +3123,20 @@ static int nvi_batch(void *client, int snsr_id, int flags,
                     unsigned int period, unsigned int timeout)
 {
        struct nvi_state *st = (struct nvi_state *)client;
-       int ret;
+       int ret = 0;
+
+       /* We use batch to set parameters in realtime for one-shot sensors
+        * (that normally doesn't use batch)
+        */
+       if (SENSOR_FLAG_ONE_SHOT_MODE == (st->snsr[snsr_id].cfg.flags &
+                                         REPORTING_MODE_MASK)) {
+               st->snsr[snsr_id].cfg.delay_us_min = period;
+               st->snsr[snsr_id].cfg.delay_us_max = timeout;
+               st->snsr[snsr_id].cfg.report_n = flags;
+               if (st->en_msk & (1 << DEV_DMP))
+                       ret = st->hal->dmp->fn_dev_init(st, snsr_id);
+               return ret;
+       }
 
        if (timeout && !st->snsr[snsr_id].cfg.fifo_max_evnt_cnt)
                return -EINVAL;
@@ -3848,23 +3861,6 @@ static int nvi_id_dev(struct nvi_state *st,
                                              st->hal->dev[dev]->milliamp.fval;
        }
 
-#define SRM                            (SENSOR_FLAG_SPECIAL_REPORTING_MODE)
-#define OSM                            (SENSOR_FLAG_ONE_SHOT_MODE)
-       BUG_ON(SRC_N < st->hal->src_n);
-       for (dev = 0; dev < DEV_N; dev++) {
-               src = st->hal->dev[dev]->src;
-               if (src < 0)
-                       continue;
-
-               BUG_ON(src >= st->hal->src_n);
-               if ((st->snsr[dev].cfg.flags & SRM) != OSM) {
-                       st->snsr[dev].cfg.delay_us_min =
-                                              st->hal->src[src].period_us_min;
-                       st->snsr[dev].cfg.delay_us_max =
-                                              st->hal->src[src].period_us_max;
-               }
-       }
-
        ret = nvs_vregs_sts(st->vreg, ARRAY_SIZE(nvi_vregs));
        if (ret < 0)
                /* regulators aren't supported so manually do master reset */
@@ -3875,10 +3871,27 @@ static int nvi_id_dev(struct nvi_state *st,
                st->dev_offset[DEV_ACC][i] = 0;
                st->dev_offset[DEV_GYR][i] = 0;
        }
+
+       BUG_ON(SRC_N < st->hal->src_n);
        if (st->hal->fn->init)
                ret = st->hal->fn->init(st);
        else
                ret = 0;
+       /* set sensor period limits after st->hal->fn->init executes */
+       for (dev = 0; dev < DEV_N; dev++) {
+               src = st->hal->dev[dev]->src;
+               if (src < 0)
+                       continue;
+
+               if (SENSOR_FLAG_ONE_SHOT_MODE == (st->snsr[dev].cfg.flags &
+                                                 REPORTING_MODE_MASK))
+                       continue;
+
+               BUG_ON(src >= st->hal->src_n);
+               st->snsr[dev].cfg.delay_us_min = st->src[src].period_us_min;
+               st->snsr[dev].cfg.delay_us_max = st->src[src].period_us_max;
+       }
+
        if (hw_id == NVI_HW_ID_AUTO)
                dev_info(&st->i2c->dev, "%s: USING DEVICE TREE: %s\n",
                         __func__, i2c_dev_id->name);
@@ -4033,6 +4046,17 @@ static void nvi_of_dt_post(struct nvi_state *st, struct device_node *dn)
        for (i = 0; i < DEV_N; i++)
                nvs_of_dt(dn, &st->snsr[i].cfg, NULL);
 
+       /* nvs_of_dt doesn't allow REPORTING_MODE_MASK bits to change so we
+        * allow for it here so that we can configure whether batch sysfs nodes
+        * are populated to be used to configure significant motion parameters
+        * in realtime.
+        */
+       if (!of_property_read_u32(dn, "significant_motion_flags", &tmp)) {
+               msk = SENSOR_FLAG_READONLY_MASK & ~REPORTING_MODE_MASK;
+               tmp &= ~msk;
+               st->snsr[DEV_SM].cfg.flags &= msk;
+               st->snsr[DEV_SM].cfg.flags |= tmp;
+       }
        for (i = 0; i < DEV_N; i++) {
                tmp = 0;
                for (j = 0; j < 9; j++)
@@ -4160,6 +4184,11 @@ static int nvi_init(struct nvi_state *st,
        if (!n)
                return -ENODEV;
 
+       /* restore SENSOR_FLAG_ONE_SHOT_MODE for significant motion in case it
+        * was cleared to allow realtime calibration with batch.
+        */
+       st->snsr[DEV_SM].cfg.flags &= ~REPORTING_MODE_MASK;
+       st->snsr[DEV_SM].cfg.flags |= SENSOR_FLAG_ONE_SHOT_MODE;
        ret = request_threaded_irq(st->i2c->irq, nvi_handler, nvi_thread,
                                   IRQF_TRIGGER_RISING, NVI_NAME, st);
        if (ret) {
index 4d6d6ab..f81be14 100644 (file)
@@ -307,10 +307,11 @@ struct nvi_mc_icm {
        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 smd_timer_thld;
+       u32 smd_thld;
+       u32 smd_thld_n;
+       u32 smd_delay;
+       u32 smd_delay2;
+       u32 smd_reset;
        u32 wom_enable;
        u32 wtf_8a;
 };
@@ -320,6 +321,8 @@ struct nvi_mc_mpu {
        u32 d_smd_mot_thld;
        u32 d_smd_delay_thld;
        u32 d_smd_delay2_thld;
+       u32 d_smd_exe_state;
+       u32 d_smd_delay_cntr;
 };
 
 struct nvi_mc {
index b7684f9..90647ed 100644 (file)
@@ -359,22 +359,38 @@ static int nvi_dmp_sm_init(struct nvi_state *st, u32 *out_ctl,
 
        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);
+                              &st->mc.icm.smd_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);
+                               st->snsr[DEV_SM].cfg.delay_us_min,
+                               &st->mc.icm.smd_delay);
        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);
+                               &st->mc.icm.smd_delay2);
        return ret;
 #elif ICM_DMP_FW_VER == 1
-       return nvi_mem_wr_be_mc(st, SMD_TIMER_THLD, 4,
-                               st->snsr[DEV_SM].cfg.thresh_hi,
-                               &st->mc.icm.smd_timer_thld);
+       /* no documentation from INV here.
+        * It can be assumed that SMD is broken with DMP FW version 1.
+        */
+       return 0;
 #elif ICM_DMP_FW_VER == 2
-       return nvi_mem_wr_be_mc(st, SMD_CNTR_TH, 4,
+       int ret;
+
+       ret = nvi_mem_wr_be_mc(st, SMD_E1_THLD, 4,
+                              st->snsr[DEV_SM].cfg.thresh_lo,
+                              &st->mc.icm.smd_thld);
+       ret |= nvi_mem_wr_be_mc(st, SMD_E1_COUNTER_TH, 4,
                                st->snsr[DEV_SM].cfg.thresh_hi,
-                               &st->mc.icm.smd_timer_thld);
+                               &st->mc.icm.smd_thld_n);
+       ret |= nvi_mem_wr_be_mc(st, SMD_CNTR_LO_TH, 4,
+                               st->snsr[DEV_SM].cfg.delay_us_min,
+                               &st->mc.icm.smd_delay);
+       ret |= nvi_mem_wr_be_mc(st, SMD_CNTR_TH, 4,
+                               st->snsr[DEV_SM].cfg.delay_us_max,
+                               &st->mc.icm.smd_delay2);
+       ret |= nvi_mem_wr_be_mc(st, SMD_LOW_ENERGY_TIMER_TH, 4,
+                               st->snsr[DEV_SM].cfg.report_n,
+                               &st->mc.icm.smd_reset);
+       return ret;
 #else
        return 0;
 #endif /* ICM_DMP_FW_VER */
@@ -805,7 +821,7 @@ static int nvi_dmp_period(struct nvi_state *st, u32 *out_ctl,
         */
        /* initialize source period */
        for (src = 0; src < st->hal->src_n; src++)
-               period_us_req[src] = st->hal->src[src].period_us_max;
+               period_us_req[src] = st->src[src].period_us_max;
 
        /* set source's period_us_req[] to fastest enabled sensor */
        for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
@@ -856,6 +872,8 @@ static int nvi_dmp_period(struct nvi_state *st, u32 *out_ctl,
                }
 
                period_us_req[SRC_AUX] = period_us;
+       } else {
+               period_us_req[SRC_AUX] = st->src[SRC_AUX].period_us_req;
        }
 
        period_us_req[SRC_GYR] = period_us;
@@ -865,62 +883,17 @@ static int nvi_dmp_period(struct nvi_state *st, u32 *out_ctl,
        /* program the sources */
        for (src = 0; src < st->hal->src_n; src++) {
                dev_msk = st->hal->src[src].dev_msk;
+#if 0 /* WAR: program all clock sources regardless if used */
                if (dev_msk & (1 << DEV_AUX))
                        dev_msk |= MSK_EN_AUX_PORTS;
                if (!(dev_msk & en_msk))
                        /* no active sensors use this source */
                        continue;
-
+#endif /* WAR: end */
                if (period_us_req[src] < st->src[src].period_us_min)
                        period_us_req[src] = st->src[src].period_us_min;
-               if (period_us_req[src] > st->src[src].period_us_max)
-                       period_us_req[src] = st->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--;
-/* WAR: start
- * It appears that the latest INV ICM DMP FW runs everything off of the SRC_GYR
- */
-#if 0 /* WAR: must program both SRC_ACC & SRC_GYR regardless if used*/
-                       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;
-#endif /* WAR */
-                       ret = nvi_i2c_write_rc(st,
-                                              &st->hal->reg->smplrt[SRC_GYR],
-                                              i, __func__,
-                                         (u8 *)&st->rc.smplrt[SRC_GYR], true);
-                       if (ret)
-                               ret_t |= ret;
-                       else
-                               st->src[SRC_GYR].period_us_src = ((i + 1) *
-                                              st->src[SRC_GYR].base_t) / 1000;
-                       ret = nvi_i2c_write_rc(st,
-                                              &st->hal->reg->smplrt[SRC_ACC],
-                                              i, __func__,
-                                         (u8 *)&st->rc.smplrt[SRC_ACC], true);
-                       if (ret)
-                               ret_t |= ret;
-                       else
-                               st->src[SRC_ACC].period_us_src = ((i + 1) *
-                                              st->src[SRC_ACC].base_t) / 1000;
-/* WAR: end */
-                       break;
-
-               case SRC_AUX:
-                       ret_t |= st->hal->src[SRC_AUX].fn_period(st);
-                       break;
-               }
+               ret_t |= st->hal->src[src].fn_period(st);
        }
 
        /* now set each DMP device's ODR based on their period */
@@ -1167,11 +1140,8 @@ static int nvi_dd_able(struct nvi_state *st,
        int ret;
 
        st->en_msk &= ~MSK_DEV_SNSR;
-       for (i = 0; i < st->hal->src_n; i++) {
-               st->src[i].period_us_min = st->hal->src[i].period_us_min;
-               st->src[i].period_us_max = st->hal->src[i].period_us_max;
-       }
-
+       st->src[SRC_AUX].period_us_min = st->hal->src[SRC_AUX].period_us_min;
+       st->src[SRC_AUX].period_us_max = st->hal->src[SRC_AUX].period_us_max;
        for (i = 0; i < ARRAY_SIZE(nvi_dmp_devs); i++) {
                dd = &nvi_dmp_devs[i];
                if (dd->dev > DEV_AUX)
@@ -1304,6 +1274,9 @@ static int nvi_dmp_en(struct nvi_state *st)
                st->mc_dis = false; /* enable cache */
        }
        ret |= nvi_dd_able(st, en_msk, irq_msk);
+       /* restore any modified period limits */
+       st->src[SRC_AUX].period_us_min = st->hal->src[SRC_AUX].period_us_min;
+       st->src[SRC_AUX].period_us_max = st->hal->src[SRC_AUX].period_us_max;
        if (!ret) {
                st->en_msk |= (1 << DEV_DMP);
                ret = nvi_i2c_wr(st, &st->hal->reg->pm2, 0, __func__);
index 0dc0329..e448786 100644 (file)
 #if ICM_DMP_FW_VER == 0
 #define ICM_DMP_FW_CRC32               (0x12F362A6)
 #define ICM_DMP_FIFO_MODE              (0)
-#define ICM_SMD_TIMER_THLD_INIT                (0x00000258)
+#define ICM_SMD_THLD_INIT              (0x00000BB8)
+#define ICM_SMD_THLD_N_INIT            (0)
+#define ICM_SMD_TIMER_INIT             (0x00000258)
+#define ICM_SMD_TIMER2_INIT            (0x000000C8)
+#define ICM_SMD_RESET_INIT             (0)
 #elif ICM_DMP_FW_VER == 1
 #define ICM_DMP_FW_CRC32               (0xFEF1270D)
 #define ICM_DMP_FIFO_MODE              (0)
-#define ICM_SMD_TIMER_THLD_INIT                (0x0000015E)
+#define ICM_SMD_THLD_INIT              (0x00000BB8)
+#define ICM_SMD_THLD_N_INIT            (0)
+#define ICM_SMD_TIMER_INIT             (0x00000258)
+#define ICM_SMD_TIMER2_INIT            (0x000000C8)
+#define ICM_SMD_RESET_INIT             (0)
 #else /* ICM_DMP_FW_VER == 2 */
 #define ICM_DMP_FW_CRC32               (0xDA126847)
 #define ICM_DMP_FIFO_MODE              (-1)
-#define ICM_SMD_TIMER_THLD_INIT                (0x0000015E)
+#define ICM_SMD_THLD_INIT              (0x00133333)
+#define ICM_SMD_THLD_N_INIT            (0x00000002)
+#define ICM_SMD_TIMER_INIT             (0x00000028)
+#define ICM_SMD_TIMER2_INIT            (0x0000005A)
+#define ICM_SMD_RESET_INIT             (0x00000032)
 #endif /* ICM_DMP_FW_VER */
 
 #define ICM_DMP_FREQ                   (102)
index 80d5733..ba18cde 100644 (file)
@@ -78,12 +78,18 @@ static int nvi_dmp_sm_init(struct nvi_state *st, unsigned int *en_msk)
        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,
+       ret |= nvi_mem_wr_be_mc(st, D_SMD_DELAY_CNTR, 4,
                                st->snsr[DEV_SM].cfg.thresh_hi,
+                               &st->mc.mpu.d_smd_delay_cntr);
+       ret |= nvi_mem_wr_be_mc(st, D_SMD_DELAY_THLD, 4,
+                               st->snsr[DEV_SM].cfg.delay_us_min,
                                &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);
+       ret |= nvi_mem_wr_be_mc(st, D_SMD_EXE_STATE, 4,
+                               st->snsr[DEV_SM].cfg.report_n,
+                               &st->mc.mpu.d_smd_exe_state);
        return ret;
 }
 
index 91b1d9a..3d60b78 100644 (file)
 #ifndef _NVI_DMP_MPU_H_
 #define _NVI_DMP_MPU_H_
 
+#define MPU_SMD_THLD_INIT              (0x000005DC)
+#define MPU_SMD_DELAY_N_INIT           (0x0000006C)
+#define MPU_SMD_TIMER_INIT             (0x00000258)
+#define MPU_SMD_TIMER2_INIT            (0x000000C8)
+#define MPU_SMD_EXE_STATE_INIT         (0x00000001)
+
 #define FCFG_1                         (1078)          /* 0x0436 */
 #define FCFG_3                         (1103)          /* 0x044F */
 #define FCFG_2                         (1082)          /* 0x043A */
index 044db1b..eb2c4a5 100644 (file)
@@ -628,7 +628,11 @@ static int nvi_init_icm(struct nvi_state *st)
        int ret;
 
        st->snsr[DEV_ACC].cfg.thresh_hi = 0; /* no ACC LP on ICM */
-       st->snsr[DEV_SM].cfg.thresh_hi = ICM_SMD_TIMER_THLD_INIT;
+       st->snsr[DEV_SM].cfg.thresh_lo = ICM_SMD_THLD_INIT;
+       st->snsr[DEV_SM].cfg.thresh_hi = ICM_SMD_THLD_N_INIT;
+       st->snsr[DEV_SM].cfg.delay_us_min = ICM_SMD_TIMER_INIT;
+       st->snsr[DEV_SM].cfg.delay_us_max = ICM_SMD_TIMER2_INIT;
+       st->snsr[DEV_SM].cfg.report_n = ICM_SMD_RESET_INIT;
        ret = nvi_i2c_rd(st, &st->hal->reg->tbc_pll, &val);
        if (ret)
                return ret;
@@ -643,6 +647,15 @@ static int nvi_init_icm(struct nvi_state *st)
        for (src = 0; src < st->hal->src_n; src++)
                st->src[src].base_t /= ICM_BASE_SAMPLE_RATE;
 
+       /* calculate the period_us_max */
+       st->src[SRC_GYR].period_us_max = (st->src[SRC_GYR].base_t *
+                                         0xFF) / 1000;
+       /* WAR: limit SRC_ACC to SRC_GYR since same period required by WAR */
+       st->src[SRC_ACC].period_us_max = st->src[SRC_GYR].period_us_max;
+       st->src[SRC_AUX].period_us_max = st->hal->src[SRC_AUX].period_us_max;
+       for (src = 0; src < st->hal->src_n; src++)
+               st->src[src].period_us_min = st->hal->src[src].period_us_min;
+
        nvi_en_gyr_icm(st);
        nvi_en_acc_icm(st);
        return 0;
@@ -663,13 +676,14 @@ static int nvi_src(struct nvi_state *st, unsigned int src)
        unsigned int rate;
        int ret;
 
-       rate = st->src[src].period_us_req / ICM_BASE_SAMPLE_RATE;
+       rate = (st->src[src].period_us_req * 1000) / st->src[src].base_t;
        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;
+               st->src[src].period_us_src = ((rate + 1) *
+                                             st->src[src].base_t) / 1000;
        if (st->sts & (NVS_STS_SPEW_MSG | NVI_DBG_SPEW_MSG))
                dev_info(&st->i2c->dev,
                         "%s src[%u] period_req=%u period_src=%u err=%d\n",
@@ -732,13 +746,13 @@ static const struct nvi_hal_src src[] = {
        [SRC_GYR]                       {
                .dev_msk                = (1 << DEV_GYR) | (1 << DEV_GYU),
                .period_us_min          = 10000,
-               .period_us_max          = 256000,
+               .period_us_max          = 200000,
                .fn_period              = nvi_src_gyr,
        },
        [SRC_ACC]                       {
                .dev_msk                = (1 << DEV_ACC),
                .period_us_min          = 10000,
-               .period_us_max          = 256000,
+               .period_us_max          = 200000,
                .fn_period              = nvi_src_acc,
        },
        [SRC_AUX]                       {
index 79d2ad2..1f59f6a 100644 (file)
@@ -13,6 +13,7 @@
 #include <linux/kernel.h>
 #include <linux/delay.h>
 #include "nvi.h"
+#include "nvi_dmp_mpu.h"
 
 enum inv_filter_e {
        INV_FILTER_256HZ_NOLPF2 = 0,
@@ -265,6 +266,7 @@ static short index_of_key(u16 key)
 
 int inv_init_6050(struct nvi_state *st)
 {
+       unsigned int i;
        int ret;
        u8 prod_ver = 0x00, prod_rev = 0x00;
        struct prod_rev_map_t *p_rev;
@@ -280,6 +282,12 @@ int inv_init_6050(struct nvi_state *st)
                st->en_msk |= (1 << EN_LP);
        else
                st->en_msk &= ~(1 << EN_LP);
+       for (i = 0; i < st->hal->src_n; i++) {
+               st->src[i].period_us_min = st->hal->src[i].period_us_min;
+               st->src[i].period_us_max = st->hal->src[i].period_us_max;
+       }
+
+       /* INV crap starts here */
        ret = nvi_i2c_r(st, 0, REG_PRODUCT_ID, 1, &prod_ver);
        if (ret)
                return ret;
@@ -343,10 +351,24 @@ int inv_init_6050(struct nvi_state *st)
 
 static int nvi_init_6500(struct nvi_state *st)
 {
+       unsigned int i;
+
        if (st->snsr[DEV_ACC].cfg.thresh_hi > 0)
                st->en_msk |= (1 << EN_LP);
        else
                st->en_msk &= ~(1 << EN_LP);
+       for (i = 0; i < st->hal->src_n; i++) {
+               st->src[i].period_us_min = st->hal->src[i].period_us_min;
+               st->src[i].period_us_max = st->hal->src[i].period_us_max;
+       }
+       st->snsr[DEV_SM].cfg.thresh_lo = MPU_SMD_THLD_INIT;
+       st->snsr[DEV_SM].cfg.thresh_hi = MPU_SMD_DELAY_N_INIT;
+       /* delay_us_min/max is ignored by NVS since this is a one-shot
+        * sensor so we use them to store parameters.
+        */
+       st->snsr[DEV_SM].cfg.delay_us_min = MPU_SMD_TIMER_INIT;
+       st->snsr[DEV_SM].cfg.delay_us_max = MPU_SMD_TIMER2_INIT;
+       st->snsr[DEV_SM].cfg.report_n = MPU_SMD_EXE_STATE_INIT;
        return 0;
 }
 
index b1ed437..c8d64a8 100644 (file)
 #define SENSOR_FLAG_ON_CHANGE_MODE             (0x2)
 #define SENSOR_FLAG_ONE_SHOT_MODE              (0x4)
 #define SENSOR_FLAG_SPECIAL_REPORTING_MODE     (0x6)
+#define REPORTING_MODE_MASK                    (0xE)
+#define REPORTING_MODE_SHIFT                   (1)
 /* end AOS sensors.h */
-#define SENSOR_FLAG_READONLY_MASK      (0xE) /* unconfigurable flags */
+/* unconfigurable flags */
+#define SENSOR_FLAG_READONLY_MASK      (REPORTING_MODE_MASK)
 
 enum nvs_float_significance {
        NVS_FLOAT_MICRO                 = 0, /* IIO_VAL_INT_PLUS_MICRO */