iio: imu: Invensense: Update to official 5.3.0.K-52 code base.

Bug 1767467

Change-Id: If4b18b80eb30e252d0649ff760ce2bf7d02d4268
Signed-off-by: Robert Collins <rcollins@nvidia.com>
Reviewed-on: http://git-master/r/1177673
(cherry picked from commit a400dd588884892eed50f7af2426829265e8b573)
Reviewed-on: http://git-master/r/1189578
Reviewed-by: Akhilesh Khumbum <akhumbum@nvidia.com>
Reviewed-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
Tested-by: Erik Lilliebjerg <elilliebjerg@nvidia.com>
GVS: Gerrit_Virtual_Submit

diff --git a/drivers/iio/imu/inv_mpu/inv530/dmp3Default.h b/drivers/iio/imu/inv_mpu/inv530/dmp3Default.h
index 0a781cd4528a14ed4e9e9895a3da945780df432d..62fe704cfebd4b9ffc1fe33395c1a50b658d29c1 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/dmp3Default.h
+++ b/drivers/iio/imu/inv_mpu/inv530/dmp3Default.h
@@ -78,6 +78,7 @@
 #define ACCEL_COVARIANCE               (101 * 16 + 8)
 #define ACCEL_ALPHA_VAR                        (91 * 16)
 #define ACCEL_A_VAR                            (92 * 16)
+#define ACCEL_CAL_INIT                 (94 * 16 + 2)
 
 #define CPASS_BIAS_X            (126 * 16 +  4)
 #define CPASS_BIAS_Y            (126 * 16 +  8)
@@ -88,11 +89,24 @@
 #define CPASS_COVARIANCE               (115 * 16)
 #define CPASS_COVARIANCE_CUR   (118 * 16 +  8)
 #define CPASS_REF_MAG_3D               (122 * 16)
+#define CPASS_CAL_INIT                 (114 * 16)
+#define CPASS_EST_FIRST_BIAS   (113 * 16)
+#define MAG_DISTURB_STATE              (113 * 16 + 2)
+#define CPASS_VAR_COUNT                        (112 * 16 + 6)
+#define CPASS_COUNT_7                  ( 87 * 16 + 2)
+#define CPASS_MAX_INNO                 (124 * 16)
+#define CPASS_BIAS_OFFSET              (113 * 16 + 4)
+#define CPASS_CUR_BIAS_OFFSET  (114 * 16 + 4)
+#define CPASS_PRE_SENSOR_DATA  ( 87 * 16 + 4)
 #define CPASS_TIME_BUFFER              (112 * 16 + 14)
 #define CPASS_RADIUS_3D_THRESH_ANOMALY (112 * 16 + 8)
 
 #define CPASS_STATUS_CHK               (25 * 16 + 12)
 
+#define MAGN_THR_9X                            (35 * 16 +  8)
+#define MAGN_LPF_THR_9X                        (32 * 16 + 12)
+#define QFB_THR_9X                             (70 * 16 + 12)
+
 #define DMPRATE_CNTR                   (18 * 16 + 4)
 
 #define PEDSTD_BP_B                            (49 * 16 + 12)
@@ -112,10 +126,14 @@
 #define PEDSTD_DECI                            (58 * 16)
 #define PEDSTD_SB2                             (60 * 16 + 14)
 #define STPDET_TIMESTAMP               (18 * 16 +  8)
-#define PEDSTD_DRIVE_STATE             (43 * 16 + 10)
+#define PEDSTD_DRIVE_STATE             (18 * 16)
 #define PED_RATE                               (58 * 16 +  4)
 
-#define SMD_TIMER_THLD                 (26 * 16)
+#define SMD_E1_THLD                            (75 * 16 +  8)
+#define SMD_CNTR_TH             (74 * 16 +  8)
+#define SMD_CNTR_LO_TH          (74 * 16 + 12)
+#define SMD_LOW_ENERGY_TIMER_TH (76 * 16 +  8)
+#define SMD_E1_COUNTER_TH       (76 * 16 + 12)
 
 
 // Wake on Motion
@@ -127,7 +145,9 @@
 #define TILT_ENABLE             (68 * 16 + 12)
 #define BAC_STATE               (147 * 16)
 
-#define ACC_SCALE                              (30 * 16)
+// Accel FSR
+#define ACC_SCALE               (30 * 16 + 0)
+#define ACC_SCALE2              (79 * 16 + 4)
 
 #define ACCEL_MASK             0x80
 #define GYRO_MASK              0x40
@@ -136,6 +156,7 @@
 #define QUAT6_MASK             0x08
 #define QUAT9_MASK             0x04
 #define PQUAT6_MASK            0x02
+#define FOOTER_MASK            0x01
 #define PRESSURE_MASK  0x80
 #define GYRO_CALIBR_MASK       0x40
 #define CPASS_CALIBR_MASK      0x20
@@ -150,6 +171,7 @@
 #define QUAT6_SET              0x0800
 #define QUAT9_SET              0x0400
 #define PQUAT6_SET             0x0200
+#define FOOTER_SET             0x0100
 #define PRESSURE_SET   0x0080
 #define GYRO_CALIBR_SET        0x0040
 #define CPASS_CALIBR_SET 0x0020
@@ -180,7 +202,7 @@
 #define NINE_AXIS_EN        0x40
 
 #define HEADER_SZ              2
-#define ACCEL_DATA_SZ  12
+#define ACCEL_DATA_SZ  6
 #define GYRO_DATA_SZ   6
 #define CPASS_DATA_SZ  6
 #define ALS_DATA_SZ            8
@@ -191,6 +213,7 @@
 #define GYRO_CALIBR_DATA_SZ            12
 #define CPASS_CALIBR_DATA_SZ   12
 #define PED_STEPDET_TIMESTAMP_SZ       4
+#define FOOTER_SZ              2
 
 #define HEADER2_SZ                     2
 #define ACCEL_ACCURACY_SZ      2

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_core.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_core.c
index 99fdcb0fdd84755bb8e7978416703fac9b4064d5..0948da6dd4e3cb85cab54fe10b81ffd67d01bce0 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_core.c
@@ -132,7 +132,6 @@ static int inv_set_gyro_sf(struct inv_mpu_state *st)
 static int inv_set_accel_sf(struct inv_mpu_state *st)
 {
        int result;
-       int scale[] = {33554432, 67108864, 134217728, 268435456};
 
        result = inv_set_bank(st, BANK_SEL_2);
        if (result)
@@ -146,8 +145,12 @@ static int inv_set_accel_sf(struct inv_mpu_state *st)
        if (result)
                return result;
 
-       result = write_be32_to_mem(st,
-                       scale[st->chip_config.accel_fs], ACC_SCALE);
+       result = inv_set_accel_fsr_V3(st);
+       if (result)
+               return result;
+       result = inv_set_accel_scale2_V3(st);
+       if (result)
+               return result;
 
        return result;
 }
@@ -615,14 +618,6 @@ static ssize_t _misc_attr_store(struct device *dev,
                        return result;
                st->ped.int_thresh = data;
 
-               return 0;
-       case ATTR_DMP_SMD_TIMER_THLD:
-               if (data < 0)
-                       return -EINVAL;
-               result = write_be32_to_mem(st, data, SMD_TIMER_THLD);
-               if (result)
-                       return result;
-               st->smd.timer_thresh = data;
                return 0;
        default:
                return -EINVAL;
@@ -1090,8 +1085,6 @@ static ssize_t inv_attr_show(struct device *dev,
                return sprintf(buf, "%d\n", st->ped.int_thresh);
        case ATTR_DMP_SMD_ENABLE:
                return sprintf(buf, "%d\n", st->smd.on);
-       case ATTR_DMP_SMD_TIMER_THLD:
-               return sprintf(buf, "%d\n", st->smd.timer_thresh);
        case ATTR_DMP_LOW_POWER_GYRO_ON:
                return sprintf(buf, "%d\n", st->chip_config.low_power_gyro_on);
        case ATTR_DEBUG_DATA_COLLECTION_MODE:
@@ -1625,8 +1618,6 @@ static IIO_DEVICE_ATTR(in_step_indicator_enable, S_IRUGO | S_IWUGO,
 
 static IIO_DEVICE_ATTR(event_smd_enable, S_IRUGO | S_IWUGO,
        inv_attr_show, inv_basic_attr_store, ATTR_DMP_SMD_ENABLE);
-static IIO_DEVICE_ATTR(params_smd_timer_thresh, S_IRUGO | S_IWUGO,
-       inv_attr_show, inv_misc_attr_store, ATTR_DMP_SMD_TIMER_THLD);
 
 static IIO_DEVICE_ATTR(params_pedometer_int_on, S_IRUGO | S_IWUGO,
        inv_attr_show, inv_misc_attr_store, ATTR_DMP_PED_INT_ON);
@@ -1742,7 +1733,6 @@ static const struct attribute *inv_pedometer_attributes[] = {
 static const struct attribute *inv_smd_attributes[] = {
        &dev_attr_poll_smd.attr,
        &iio_dev_attr_event_smd_enable.dev_attr.attr,
-       &iio_dev_attr_params_smd_timer_thresh.dev_attr.attr,
 };
 
 static const struct attribute *inv_pressure_attributes[] = {

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_dts.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_dts.c
index f3bddd5709acc88c21f0bde29bdbb1970ba6b7a9..27046e2c98fa498f1f04fdfce72610fceb45e3b8 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_dts.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_dts.c
@@ -278,7 +278,7 @@ int inv_parse_readonly_secondary(struct device *dev, struct mpu_platform_data *p
 int invensense_mpu_parse_dt(struct device *dev, struct mpu_platform_data *pdata)
 {
        int rc;
-       pr_info("%s: 15066 47", __func__);
+       pr_info("%s: 15667 52", __func__);
        pr_info("%s: Invensense MPU parse_dt started.\n", __func__);
 
        rc = inv_parse_orientation_matrix(dev, pdata->orientation);

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_i2c.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_i2c.c
index 078c181aec059d9ee1c2d75518ee5e30ed5c300f..bd76271ff24f2af0e4df38c4660b3b0b2937ae2b 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_i2c.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_i2c.c
@@ -437,9 +437,10 @@ static int inv_mpu_resume(struct device *dev)
        } else {
                inv_switch_power_in_lp(st, true);
        }
+       st->suspend_state = false;
        mutex_unlock(&indio_dev->mlock);
+       inv_read_fifo(0, (void *)st);
        /* add code according to different request End */
-       mutex_unlock(&st->suspend_resume_lock);
 
        return 0;
 }
@@ -460,6 +461,8 @@ static int inv_mpu_suspend(struct device *dev)
 
        /* add code according to different request Start */
        pr_debug("%s inv_mpu_suspend\n", st->hw->name);
+       mutex_lock(&indio_dev->mlock);
+       st->suspend_state = true;
 
        if (st->chip_config.dmp_on) {
                if (st->batch.on) {
@@ -472,11 +475,9 @@ static int inv_mpu_suspend(struct device *dev)
                /* in non DMP case, just turn off the power */
                inv_set_power(st, false);
        }
+       mutex_unlock(&indio_dev->mlock);
+
        /* add code according to different request End */
-       st->suspend_state = true;
-       msleep(100);
-       mutex_lock(&st->suspend_resume_lock);
-       st->suspend_state = false;
 
        return 0;
 }

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_iio.h b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_iio.h
index 428659a8735f06c6f1c346cdd2bd384f4988af37..89863d206482f44f0273a1d9afde3c8289f81bc3 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_iio.h
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_iio.h
@@ -19,6 +19,7 @@
 #include <linux/miscdevice.h>
 #include <linux/spinlock.h>
 #include <linux/mpu.h>
+#include <linux/interrupt.h>
 
 #include "iio.h"
 #include "buffer.h"
@@ -242,6 +243,7 @@
 #define DMP_OFFSET               0x90
 #define DMP_IMAGE_SIZE           (7021 + DMP_OFFSET)
 #define MIN_MST_ODR_CONFIG       4
+#define MAX_MST_NON_COMPASS_ODR_CONFIG 7
 #define THREE_AXES               3
 #define NINE_ELEM                (THREE_AXES * THREE_AXES)
 #define MPU_TEMP_SHIFT           16
@@ -824,7 +826,6 @@ enum MPU_IIO_ATTR_ADDR {
        ATTR_DMP_PED_INT_THRESH,
        ATTR_DMP_PED_ON,
        ATTR_DMP_SMD_ENABLE,
-       ATTR_DMP_SMD_TIMER_THLD,
        ATTR_DMP_PEDOMETER_STEPS,
        ATTR_DMP_PEDOMETER_TIME,
        ATTR_DMP_PEDOMETER_COUNTER,
@@ -970,6 +971,10 @@ int inv_plat_read(struct inv_mpu_state *st, u8 reg, int len, u8 *data);
 int inv_set_dmp(struct inv_mpu_state *st);
 int inv_set_secondary(struct inv_mpu_state *st);
 
+irqreturn_t inv_read_fifo(int irq, void *dev_id);
+int inv_set_accel_fsr_V3(struct inv_mpu_state *st);
+int inv_set_accel_scale2_V3(struct inv_mpu_state *st);
+
 #define mem_w(a, b, c) \
        mpu_memory_write(st, st->i2c_addr, a, b, c)
 #define mem_r(a, b, c) \

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_misc.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_misc.c
index 99435dd7c5d0bcd937cb1e3bca87cb738894f545..fb81c0257742603b381d4c15eb0c7e1bfd07d2f0 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_misc.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_misc.c
@@ -31,7 +31,7 @@
 #include "inv_test/inv_counters.h"
 
 /* DMP defines */
-#define FIRMWARE_CRC           0xfef1270d
+#define FIRMWARE_CRC           0xda126847
 #define FIRMWARE_CRC_1         0x24e5ed7e
 
 int inv_get_pedometer_steps(struct inv_mpu_state *st, int *ped)
@@ -72,6 +72,67 @@ int inv_read_pedometer_counter(struct inv_mpu_state *st)
        return 0;
 }
 
+
+/*
+input param: fsr for accel parts
+1: 1g. 2: 2g. 4: 4g. 8: 8g. 16: 16g. 32: 32g.
+
+The goal is to set 1g data to 2^25, 2g data to 2^26, etc.
+
+For 2g parts, raw accel data is 1g = 2^14, 2g = 2^15.
+DMP takes raw accel data and shifts by 16 bits, so this scale means to shift by -5 bits.
+In Q-30 math, >> 5 equals multiply by 2^25 = 33554432.
+
+For 8g parts, raw accel data is 4g = 2^14, 8g = 2^15.
+DMP takes raw accel data and shifts by 16 bits, so this scale means to shift by -3 bits.
+In Q-30 math, >> 3 equals multiply by 2^27 = 134217728.
+*/
+int inv_set_accel_fsr_V3(struct inv_mpu_state *st)
+{
+       u32 scale;
+
+       switch (st->chip_config.accel_fs) {
+               case 0: //2g
+                       scale =  33554432;  // 2^25
+                       break;
+               case 1: //4g
+                       scale =  67108864;  // 2^26
+                       break;
+               case 2: //8g
+                       scale = 134217728;  // 2^27
+                       break;
+               case 3: //16g
+                       scale = 268435456;  // 2^28
+                       break;
+               default:
+                       return -EINVAL;
+       }
+       return write_be32_to_mem(st, scale, ACC_SCALE); 
+}
+
+int inv_set_accel_scale2_V3(struct inv_mpu_state *st)
+{
+       u32 scale;
+
+       switch (st->chip_config.accel_fs) {
+               case 0: //2g
+                       scale =  524288;  // 2^19
+                       break;
+               case 1: //4g
+                       scale =  262144;  // 2^18
+                       break;
+               case 2: //8g
+                       scale = 131072;  // 2^17
+                       break;
+               case 3: //16g
+                       scale = 65536;  // 2^16
+                       break;
+               default:
+                       return -EINVAL;
+       }
+       return write_be32_to_mem(st, scale, ACC_SCALE2); 
+}
+
 static int inv_load_firmware(struct inv_mpu_state *st)
 {
        int bank, write_size;
@@ -236,6 +297,14 @@ static int inv_setup_dmp_firmware(struct inv_mpu_state *st)
                pr_err("dmp loading eror:inv_write_gyro_sf\n");
                return result;
        }
+
+       result = inv_set_accel_fsr_V3(st);
+       if (result)
+               return result;
+       result = inv_set_accel_scale2_V3(st);
+       if (result)
+               return result;
+
        if (st->chip_config.has_compass) {
                result = inv_compass_dmp_cal(st);
                if (result)

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_ring.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_ring.c
index 9b9c2603451484a27cab9f0f52603222c3a85ae9..eee0ccbff18c5104772545904614e20c450ac951 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_ring.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_ring.c
@@ -245,7 +245,7 @@ static int inv_push_sensor(struct inv_mpu_state *st, int ind, u64 t, u8 *d)
 
        switch (ind) {
        case SENSOR_ACCEL:
-               inv_convert_and_push_16bytes(st, hdr, d, t, iden);
+               inv_convert_and_push_8bytes(st, hdr, d, t, iden);
                break;
        case SENSOR_GYRO:
                inv_convert_and_push_8bytes(st, hdr, d, t, iden);
@@ -945,10 +945,11 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id)
 
 #define NON_DMP_MIN_RUN_TIME (10 * NSEC_PER_MSEC)
 
-       if (st->suspend_state)
-               return IRQ_HANDLED;
-       mutex_lock(&st->suspend_resume_lock);
        mutex_lock(&indio_dev->mlock);
+       if (st->suspend_state) {
+               mutex_unlock(&indio_dev->mlock);
+               return IRQ_HANDLED;
+       }
 
        if (st->chip_config.dmp_on) {
                st->last_run_time = get_time_ns();
@@ -980,7 +981,6 @@ irqreturn_t inv_read_fifo(int irq, void *dev_id)
 end_read_fifo:
        inv_switch_power_in_lp(st, false);
        mutex_unlock(&indio_dev->mlock);
-       mutex_unlock(&st->suspend_resume_lock);
 
        return IRQ_HANDLED;
 
@@ -989,7 +989,6 @@ err_reset_fifo:
        inv_reset_fifo(indio_dev, true);
        inv_switch_power_in_lp(st, false);
        mutex_unlock(&indio_dev->mlock);
-       mutex_unlock(&st->suspend_resume_lock);
 
        return IRQ_HANDLED;
 

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_selftest.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_selftest.c
index 643ac7a467c13f61a61eceb0479b036afee3e55a..2478ce5d6a2800ca5d79574111aacdbf086cc4b4 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_selftest.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_selftest.c
@@ -67,6 +67,7 @@
 struct recover_regs {
        /* Bank#0 */
        u8 fifo_cfg;                    /* REG_FIFO_CFG */
+       u8 fifo_size_0;                 /* REG_FIFO_SIZE_0 */
        u8 user_ctrl;                   /* REG_USER_CTRL */
        u8 lp_config;                   /* REG_LP_CONFIG */
        u8 int_enable;                  /* REG_INT_ENABLE */
@@ -126,6 +127,7 @@ static const u16 mpu_st_tb[256] = {
 static void inv_show_saved_setting(struct inv_mpu_state *st)
 {
        pr_debug(" REG_FIFO_CFG : 0x%02X\n", saved_regs.fifo_cfg);
+       pr_debug(" REG_FIFO_SIZE_0 : 0x%02X\n", saved_regs.fifo_size_0);
        pr_debug(" REG_USER_CTRL : 0x%02X\n", saved_regs.user_ctrl);
        pr_debug(" REG_LP_CONFIG : 0x%02X\n", saved_regs.lp_config);
        pr_debug(" REG_INT_ENABLE : 0x%02X\n", saved_regs.int_enable);
@@ -153,6 +155,9 @@ static int inv_save_setting(struct inv_mpu_state *st)
                return result;
 
        result = inv_plat_read(st, REG_FIFO_CFG, 1, &saved_regs.fifo_cfg);
+       if (result)
+               return result;
+       result = inv_plat_read(st, REG_FIFO_SIZE_0, 1, &saved_regs.fifo_size_0);
        if (result)
                return result;
        result = inv_plat_read(st, REG_USER_CTRL, 1, &saved_regs.user_ctrl);

diff --git a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_setup.c b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_setup.c
index 3392ecd2ec3a648c739df966012c0ec0d04780d5..4eebd71030b99d34f2bed79051d50fbf9be97dd8 100644 (file)
--- a/drivers/iio/imu/inv_mpu/inv530/inv_mpu_setup.c
+++ b/drivers/iio/imu/inv_mpu/inv530/inv_mpu_setup.c
@@ -35,6 +35,7 @@ struct inv_local_store {
        u8 reg_pwr_mgmt_2;
        u8 reg_lp_config;
        u8 reg_fifo_cfg;
+       u8 reg_fifo_size_0;
        u8 reg_delay_enable;
        u8 reg_delay_time;
        u8 reg_gyro_smplrt;
@@ -135,6 +136,10 @@ static struct inv_accel_cal_params accel_cal_para[] = {
        },
 };
 
+static int accel_gyro_rate[] = {5, 6, 7, 8, 9, 10, 11, 12, 13,
+       14, 15, 17, 18, 22, 25, 28, 32, 37, 45,
+       51, 75, 102, 225};
+
 static int inv_out_data_cntl(struct inv_mpu_state *st, u16 wd, bool en)
 {
        return inv_write_cntl(st, wd, en, DATA_OUT_CTL1);
@@ -171,13 +176,12 @@ static int inv_calc_engine_dur(struct inv_engine_info *ei)
 static int inv_batchmode_calc(struct inv_mpu_state *st)
 {
        int b, timeout;
-       int i, bps, max_rate, max_ind;
+       int i, bps, max_rate;
        enum INV_ENGINE eng;
        int ps, real_rate;
        int en_num, en_sen, count;
 
        max_rate = 0;
-       max_ind = -1;
        bps = 0;
        ps = 0;
        en_num = 0;
@@ -187,7 +191,6 @@ static int inv_batchmode_calc(struct inv_mpu_state *st)
                if (st->sensor[i].on) {
                        if (max_rate < st->sensor[i].rate) {
                                max_rate = st->sensor[i].rate;
-                               max_ind = i;
                        }
                        /* get actual rate */
                        real_rate = (MSEC_PER_SEC * NSEC_PER_MSEC) /
@@ -217,7 +220,12 @@ static int inv_batchmode_calc(struct inv_mpu_state *st)
                } else
                        timeout = st->batch.timeout;
 
-               eng = st->sensor[max_ind].engine_base;
+               if (st->chip_config.gyro_enable)
+                       eng = ENGINE_GYRO;
+               else if (st->chip_config.accel_enable)
+                       eng = ENGINE_ACCEL;
+               else
+                       eng = ENGINE_I2C;
        } else {
                if (st->chip_config.step_detector_on ||
                                        st->chip_config.step_indicator_on) {
@@ -437,7 +445,7 @@ int inv_reset_fifo(struct iio_dev *indio_dev, bool turn_off)
 
 static int inv_turn_on_engine(struct inv_mpu_state *st)
 {
-       u8 w, v;
+       u8 w;
        int r;
 
        r = 0;
@@ -471,22 +479,10 @@ static int inv_turn_on_engine(struct inv_mpu_state *st)
                                        BIT_PWR_PRESSURE_STBY);
                }
        }
-       inv_plat_read(st, REG_PWR_MGMT_2, 1, &v);
-       if ((BIT_PWR_ALL_OFF == v) &&
-               (BIT_PWR_ALL_OFF != w) &&
-               (!st->chip_config.slave_enable)) {
-               r = inv_plat_single_write(st, REG_PWR_MGMT_2,
-                                               BIT_PWR_GYRO_STBY |
-                                               BIT_PWR_PRESSURE_STBY);
-               if (r)
-                       return r;
-       }
-       if (!st->chip_config.dmp_on) {
-               r = inv_plat_single_write(st, REG_PWR_MGMT_2,
-                                               BIT_PWR_PRESSURE_STBY);
-               if (r)
-                       return r;
-       }
+       r = inv_plat_single_write(st, REG_PWR_MGMT_2, w);
+       if (r)
+               return r;
+
        if (st->chip_config.gyro_enable)
                msleep(GYRO_ENGINE_UP_TIME);
 
@@ -727,13 +723,12 @@ static int inv_set_rate(struct inv_mpu_state *st)
 static int inv_set_fifo_size(struct inv_mpu_state *st)
 {
        int result;
-       u8 size, cfg, ind;
+       u8 cfg, ind;
 
        result = 0;
        if (st->chip_config.dmp_on) {
                /* use one FIFO in DMP mode */
-               cfg = BIT_MULTI_FIFO_CFG;
-               size = BIT_GYRO_FIFO_SIZE_1024;
+               cfg = BIT_SINGLE_FIFO_CFG;
        } else {
                ind = 0;
                if (st->sensor[SENSOR_GYRO].on)
@@ -742,24 +737,16 @@ static int inv_set_fifo_size(struct inv_mpu_state *st)
                        ind++;
                if (st->sensor[SENSOR_COMPASS].on)
                        ind++;
-               if (ind > 1) {
+               if (ind > 1)
                        cfg = BIT_MULTI_FIFO_CFG;
-                       size = (BIT_GYRO_FIFO_SIZE_1024 |
-                                       BIT_ACCEL_FIFO_SIZE_1024
-                                       | BIT_FIFO_3_SIZE_64);
-               } else {
+               else
                        cfg = BIT_SINGLE_FIFO_CFG;
-                       size = BIT_FIFO_SIZE_1024;
-               }
-       }
-       if (cfg != local.reg_fifo_cfg) {
-               result = inv_plat_single_write(st, REG_FIFO_CFG, cfg);
-               if (result)
-                       return result;
-               local.reg_fifo_cfg = cfg;
        }
+       result = inv_plat_single_write(st, REG_FIFO_CFG, cfg);
+       if (result)
+               return result;
 
-       return result;
+       return 0;
 }
 
 /*
@@ -895,10 +882,17 @@ static int inv_set_ICM20628_secondary(struct inv_mpu_state *st)
        }
        if (mst_odr_config < MIN_MST_ODR_CONFIG)
                mst_odr_config = MIN_MST_ODR_CONFIG;
+       if (compass_rate) {
+               if (mst_odr_config > MAX_MST_NON_COMPASS_ODR_CONFIG)
+                       mst_odr_config = MAX_MST_NON_COMPASS_ODR_CONFIG;
+       }
 
        base = BASE_SAMPLE_RATE / (1 << mst_odr_config);
-       st->eng_info[ENGINE_I2C].running_rate = base;
-       st->eng_info[ENGINE_I2C].divider = (1 << mst_odr_config);
+       if ((!st->chip_config.gyro_enable) &&
+                       (!st->chip_config.accel_enable)) {
+               st->eng_info[ENGINE_I2C].running_rate = base;
+               st->eng_info[ENGINE_I2C].divider = (1 << mst_odr_config);
+       }
        inv_calc_engine_dur(&st->eng_info[ENGINE_I2C]);
 
        d = 0;
@@ -977,6 +971,10 @@ static int inv_set_wom(struct inv_mpu_state *st)
                local.wom_on = st->chip_config.wom_on;
        }
 
+       inv_write_2bytes(st, 0x8a, st->chip_config.gyro_enable |
+                       (st->chip_config.accel_enable << 1) |
+                       (st->chip_config.slave_enable << 3));
+
        return 0;
 }
 
@@ -1138,11 +1136,25 @@ static int inv_setup_dmp(struct inv_mpu_state *st)
 
        return result;
 }
+
+static int inv_get_accel_gyro_rate(int compass_rate)
+{
+       int i;
+
+       i = 0;
+       while ((i < ARRAY_SIZE(accel_gyro_rate)) &&
+                       compass_rate > accel_gyro_rate[i])
+               i++;
+
+       return accel_gyro_rate[i];
+}
+
 static int inv_determine_engine(struct inv_mpu_state *st)
 {
        int i;
        bool a_en, g_en, c_en, p_en, data_on, ped_on;
        int compass_rate, pressure_rate, nineq_rate, accel_rate, gyro_rate;
+       u32 base_time;
 
 #define NINEQ_MIN_COMPASS_RATE 35
 #define GEOMAG_MIN_COMPASS_RATE    70
@@ -1268,7 +1280,7 @@ static int inv_determine_engine(struct inv_mpu_state *st)
                        accel_rate = st->sensor[SENSOR_ACCEL].rate;
                }
        }
-       if (a_en) {
+       if (a_en | g_en) {
                gyro_rate = max(gyro_rate, PEDOMETER_FREQ);
         accel_rate = max(accel_rate, PEDOMETER_FREQ);
     }
@@ -1277,6 +1289,26 @@ static int inv_determine_engine(struct inv_mpu_state *st)
        if (st->sensor[SENSOR_CALIB_GYRO].on)
                gyro_rate = max(gyro_rate, st->sensor[SENSOR_CALIB_GYRO].rate);
 
+       if (g_en) {
+               if (a_en)
+                       gyro_rate = max(gyro_rate, accel_rate);
+               if (c_en || p_en) {
+                       if (gyro_rate < compass_rate)
+                               gyro_rate =
+                                       inv_get_accel_gyro_rate(compass_rate);
+               }
+               accel_rate = gyro_rate;
+               compass_rate = gyro_rate;
+       } else if (a_en) {
+               if (c_en || p_en) {
+                       if (accel_rate < compass_rate)
+                               accel_rate =
+                                       inv_get_accel_gyro_rate(compass_rate);
+               }
+               compass_rate = accel_rate;
+               gyro_rate = accel_rate;
+       }
+
        st->eng_info[ENGINE_GYRO].running_rate = gyro_rate;
        st->eng_info[ENGINE_ACCEL].running_rate = accel_rate;
        st->eng_info[ENGINE_PRESSURE].running_rate = MPU_DEFAULT_DMP_FREQ;
@@ -1290,6 +1322,16 @@ static int inv_determine_engine(struct inv_mpu_state *st)
                                (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) *
                                (MPU_DEFAULT_DMP_FREQ /
                                st->eng_info[ENGINE_ACCEL].running_rate);
+       st->eng_info[ENGINE_I2C].divider =
+                               (BASE_SAMPLE_RATE / MPU_DEFAULT_DMP_FREQ) *
+                               (MPU_DEFAULT_DMP_FREQ /
+                               st->eng_info[ENGINE_ACCEL].running_rate);
+
+       base_time = NSEC_PER_SEC;
+
+       st->eng_info[ENGINE_GYRO].base_time = base_time;
+       st->eng_info[ENGINE_ACCEL].base_time = base_time;
+       st->eng_info[ENGINE_I2C].base_time = base_time;
 
        inv_calc_engine_dur(&st->eng_info[ENGINE_GYRO]);
        inv_calc_engine_dur(&st->eng_info[ENGINE_ACCEL]);
@@ -1312,14 +1354,16 @@ static int inv_determine_engine(struct inv_mpu_state *st)
                st->chip_config.slave_enable = 1;
        else
                st->chip_config.slave_enable = 0;
-
        if (a_en) {
-               st->gyro_cal_enable = 1;
                st->accel_cal_enable = 1;
        } else {
-               st->gyro_cal_enable = 0;
                st->accel_cal_enable = 0;
        }
+       if (g_en) {
+               st->gyro_cal_enable = 1;
+       } else {
+               st->gyro_cal_enable = 0;
+       }
        if (c_en)
                st->compass_cal_enable = 1;
        else