LSM6: Follow ODR change procedure from application notes

There are a few ways the current driver differs from the application
notes for the LSM6DS3, notably:
- FIFO disable is done before any ODR registers are set
- FIFO ODR is set before any FIFO decimations

BUG=b:122912601
BRANCH=octopus
TEST=used "ectool" to change ODR values on phaser360, ensured
"accelinfo on" output looked sane with the exception of points which
will be discarded next CL

Change-Id: I39fdcc67d0b34382a348dc2442785a847a4bc68d
Signed-off-by: Diana Z <dzigterman@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1419123
Reviewed-by: Jett Rink <jettrink@chromium.org>

3 files changed, 52 insertions, 31 deletions

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 30650eebe7..cb36de8900 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -71,7 +71,7 @@ static int config_interrupt(const struct motion_sensor_t *s)
  * @s: Motion sensor pointer: must be MOTIONSENSE_TYPE_ACCEL.
  * @fmode: BYPASS or CONTINUOUS
  */
-static int fifo_disable(const struct motion_sensor_t *s)
+int accelgyro_fifo_disable(const struct motion_sensor_t *s)
 {
 	return st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, 0x00);
 }
@@ -93,13 +93,14 @@ static void fifo_reset_pattern(struct lsm6dsm_data *private)
  * Configure FIFO decimator to have every time the right pattern
  * with acc/gyro
  */
-static int fifo_enable(const struct motion_sensor_t *accel)
+int accelgyro_fifo_enable(const struct motion_sensor_t *s)
 {
 	int err, i, rate;
 	uint8_t decimator[FIFO_DEV_NUM] = { 0 };
 	unsigned int min_odr = LSM6DSM_ODR_MAX_VAL;
 	unsigned int max_odr = 0;
-	struct lsm6dsm_data *private = accel->drv_data;
+	uint8_t odr_reg_val;
+	struct lsm6dsm_data *private = s->drv_data;
 	/* In FIFO sensors are mapped in a different way. */
 	uint8_t agm_maps[] = {
 		MOTIONSENSE_TYPE_GYRO,
@@ -111,7 +112,7 @@ static int fifo_enable(const struct motion_sensor_t *accel)
 	/* Search for min and max odr values for acc, gyro. */
 	for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) {
 		/* Check if sensor enabled with ODR. */
-		rate = st_get_data_rate(accel + agm_maps[i]);
+		rate = st_get_data_rate(s + agm_maps[i]);
 		if (rate > 0) {
 			min_odr = MIN(min_odr, rate);
 			max_odr = MAX(max_odr, rate);
@@ -123,10 +124,16 @@ static int fifo_enable(const struct motion_sensor_t *accel)
 		return EC_SUCCESS;
 	}
 
+	/* FIFO ODR must be set before the decimation factors */
+	odr_reg_val = LSM6DSM_ODR_TO_REG(max_odr) <<
+					LSM6DSM_FIFO_CTRL5_ODR_OFF;
+	err = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR,
+			    odr_reg_val);
+
 	/* Scan all sensors configuration to calculate FIFO decimator. */
 	private->config.total_samples_in_pattern = 0;
 	for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) {
-		rate = st_get_data_rate(accel + agm_maps[i]);
+		rate = st_get_data_rate(s + agm_maps[i]);
 		if (rate > 0) {
 			private->config.samples_in_pattern[i] = rate / min_odr;
 			decimator[i] = LSM6DSM_FIFO_DECIMATOR(max_odr / rate);
@@ -137,17 +144,19 @@ static int fifo_enable(const struct motion_sensor_t *accel)
 			private->config.samples_in_pattern[i] = 0;
 		}
 	}
-	st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL3_ADDR,
+	st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL3_ADDR,
 			(decimator[FIFO_DEV_GYRO] << LSM6DSM_FIFO_DEC_G_OFF) |
 			(decimator[FIFO_DEV_ACCEL] << LSM6DSM_FIFO_DEC_XL_OFF));
 #ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
-	st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL4_ADDR,
+	st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL4_ADDR,
 			decimator[FIFO_DEV_MAG]);
 #endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */
-	err = st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL5_ADDR,
-			 (LSM6DSM_ODR_TO_REG(max_odr) <<
-			  LSM6DSM_FIFO_CTRL5_ODR_OFF) |
-			 LSM6DSM_FIFO_MODE_CONTINUOUS_VAL);
+	/*
+	 * After ODR and decimation values are set, continuous mode can be
+	 * enabled
+	 */
+	err = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR,
+			    odr_reg_val | LSM6DSM_FIFO_MODE_CONTINUOUS_VAL);
 	if (err != EC_SUCCESS)
 		return err;
 	fifo_reset_pattern(private);
@@ -252,6 +261,16 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts)
 	left *= sizeof(uint16_t);
 	left = (left / OUT_XYZ_SIZE) * OUT_XYZ_SIZE;
 
+	/*
+	 * TODO(b/122912601): phaser360: Investigate Standard Deviation error
+	 *				 during CtsSensorTests
+	 * - track number of samples to throw out after ODR changes
+	 * Accel discard: "should" be 0 for freq <= 26, 1 until 1666 Hz (table
+	 * 17)
+	 * Gyro discard: 12.5 Hz - 2, 26-833 Hz - 3 (table 19)
+	 * - check "pattern" register versus where code thinks it is parsing
+	 */
+
 	/* Push all data on upper side. */
 	do {
 		/* Fit len to pre-allocated static buffer. */
@@ -279,22 +298,6 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts)
 
 	return EC_SUCCESS;
 }
-
-/**
- * accelgyro_config_fifo - update mode and ODR for FIFO decimator
- */
-int accelgyro_config_fifo(const struct motion_sensor_t *accel)
-{
-	int err;
-
-	/* Changing in ODR must stop FIFO. */
-	err = fifo_disable(accel);
-	if (err != EC_SUCCESS)
-		return err;
-
-
-	return fifo_enable(accel);
-}
 #endif /* CONFIG_ACCEL_FIFO */
 
 /**
@@ -437,6 +440,14 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 	struct stprivate_data *data = s->drv_data;
 	uint8_t ctrl_reg, reg_val = 0;
 
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO must be disabled before setting any ODR values */
+	ret = accelgyro_fifo_disable(LSM6DSM_MAIN_SENSOR(s));
+	if (ret != EC_SUCCESS) {
+		CPRINTS("Failed to disable FIFO. Error: %d", ret);
+		return ret;
+	}
+#endif
 	ctrl_reg = LSM6DSM_ODR_REG(s->type);
 	if (rate > 0) {
 		reg_val = LSM6DSM_ODR_TO_REG(rate);
@@ -457,7 +468,9 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 	if (ret == EC_SUCCESS) {
 		data->base.odr = normalized_rate;
 #ifdef CONFIG_ACCEL_FIFO
-		accelgyro_config_fifo(LSM6DSM_MAIN_SENSOR(s));
+		ret = accelgyro_fifo_enable(LSM6DSM_MAIN_SENSOR(s));
+		if (ret != EC_SUCCESS)
+			CPRINTS("Failed to enable FIFO. Error: %d", ret);
 #endif
 	}
 
@@ -569,7 +582,7 @@ static int init(const struct motion_sensor_t *s)
 			goto err_unlock;
 
 #ifdef CONFIG_ACCEL_FIFO
-		ret = fifo_disable(s);
+		ret = accelgyro_fifo_disable(s);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 #endif /* CONFIG_ACCEL_FIFO */
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 0be055271e..3336326b90 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -273,7 +273,8 @@ struct lsm6dsm_data {
 #define LSM6DSM_MAIN_SENSOR(_s) ((_s) - (_s)->type)
 
 #ifdef CONFIG_ACCEL_FIFO
-int accelgyro_config_fifo(const struct motion_sensor_t *accel);
+int accelgyro_fifo_disable(const struct motion_sensor_t *s);
+int accelgyro_fifo_enable(const struct motion_sensor_t *s);
 #endif /* CONFIG_ACCEL_FIFO */
 
 #endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */
diff --git a/driver/sensorhub_lsm6dsm.c b/driver/sensorhub_lsm6dsm.c
index 04776b0557..c9989b8c0b 100644
--- a/driver/sensorhub_lsm6dsm.c
+++ b/driver/sensorhub_lsm6dsm.c
@@ -200,7 +200,14 @@ int sensorhub_set_ext_data_rate(const struct motion_sensor_t *s,
 	xl_rate = st_get_data_rate(s);
 	*ret_rate = MIN(rate, xl_rate);
 #ifdef CONFIG_ACCEL_FIFO
-	ret = accelgyro_config_fifo(s);
+	ret = accelgyro_fifo_disable(s);
+	if (ret != EC_SUCCESS)
+		return ret;
+	/*
+	 * NOTE: the ODR on the pass-through sensor does not currently change.
+	 * If it does, that step needs to be performed here
+	 */
+	ret = accelgyro_fifo_enable(s);
 #endif
 	return ret;
 }