From 2a929afefbc0ec387545646b107309fea2fd82bb Mon Sep 17 00:00:00 2001
From: Gwendal Grignou <gwendal@chromium.org>
Date: Sun, 12 Jul 2015 22:26:06 -0700
Subject: [PATCH] motion: Fix for FIFO support and BMI150

- Compass was not set properly if default config is set to
disable it (frequency == 0). We were trying to set it up
while stuck in debug mode.
- BMI150 FIFO collects sensor info even when suspended.
Ask FIFO to ommit suspended sensors.
- FIx compliation issue on nucleo-f411 board, where
MKBP is not enabled.
- Fix location of __packed arguement.

BRANCH=smaug
BUG=none
TEST=Check the compass is back with accelinfo,
FIFO is not filled with garbage with fiforead.
Check by echoing in in_accel_z_calibbias that the format
of MOTIONSENSE_CMD_SENSOR_OFFSET has not changed.

Change-Id: I7ebec12a14a74b8385b9f9532562a1fd0213f4d7
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/284929
Reviewed-by: Alec Berg <alecaberg@chromium.org>
---
 common/motion_sense.c     |  2 ++
 driver/accelgyro_bmi160.c | 30 ++++++++++++++++++++++--------
 include/ec_commands.h     |  4 ++--
 3 files changed, 26 insertions(+), 10 deletions(-)

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 0fc71d1677..57a4bd26bb 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -467,7 +467,9 @@ void motion_sense_task(void)
 				motion_sense_insert_timestamp();
 			fifo_flush_needed = 0;
 			ts_last_int = ts_end_task;
+#ifdef CONFIG_MKBP_EVENT
 			mkbp_send_event(EC_MKBP_EVENT_SENSOR_FIFO);
+#endif
 		}
 #endif
 		/* Delay appropriately to keep sampling time consistent. */
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 19dc062ac1..b07d4f3851 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -259,6 +259,12 @@ static int set_data_rate(const struct motion_sensor_t *s,
 	struct motion_data_t *data = BMI160_GET_SAVED_DATA(s);
 
 	if (rate == 0) {
+#ifdef CONFIG_ACCEL_FIFO
+		/* FIFO stop collecting events */
+		ret = raw_read8(s->i2c_addr, BMI160_FIFO_CONFIG_1, &val);
+		val &= ~BMI160_FIFO_SENSOR_EN(s->type);
+		ret = raw_write8(s->i2c_addr, BMI160_FIFO_CONFIG_1, val);
+#endif
 		/* go to suspend mode */
 		ret = raw_write8(s->i2c_addr, BMI160_CMD_REG,
 				 BMI160_CMD_MODE_SUSPEND(s->type));
@@ -324,10 +330,18 @@ static int set_data_rate(const struct motion_sensor_t *s,
 
 	val = (val & ~BMI160_ODR_MASK) | reg_val;
 	ret = raw_write8(s->i2c_addr, ctrl_reg, val);
+	if (ret != EC_SUCCESS)
+		goto accel_cleanup;
 
 	/* Now that we have set the odr, update the driver's value. */
-	if (ret == EC_SUCCESS)
-		data->odr = normalized_rate;
+	data->odr = normalized_rate;
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* FIFO start collecting events */
+	ret = raw_read8(s->i2c_addr, BMI160_FIFO_CONFIG_1, &val);
+	val |= BMI160_FIFO_SENSOR_EN(s->type);
+	ret = raw_write8(s->i2c_addr, BMI160_FIFO_CONFIG_1, val);
+#endif
 
 accel_cleanup:
 	mutex_unlock(s->mutex);
@@ -812,9 +826,6 @@ static int init(const struct motion_sensor_t *s)
 			BMI160_CMD_MODE_NORMAL(s->type));
 	msleep(30);
 
-	set_range(s, s->runtime_config.range, 0);
-	set_data_rate(s, s->runtime_config.odr, 0);
-
 #ifdef CONFIG_MAG_BMI160_BMM150
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
 		struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
@@ -881,15 +892,15 @@ static int init(const struct motion_sensor_t *s)
 		if (tmp != BMM150_CHIP_ID_MAJOR)
 			return EC_ERROR_ACCESS_DENIED;
 
-		/* Leave the address for reading the data */
-		raw_write8(s->i2c_addr, BMI160_MAG_I2C_READ_ADDR,
-				BMM150_BASE_DATA);
 		/*
 		 * Set the compass forced mode, to sleep after each measure.
 		 */
 		ret = raw_mag_write8(s->i2c_addr, BMM150_OP_CTRL,
 			BMM150_OP_MODE_FORCED << BMM150_OP_MODE_OFFSET);
 
+		/* Leave the address for reading the data */
+		raw_write8(s->i2c_addr, BMI160_MAG_I2C_READ_ADDR,
+				BMM150_BASE_DATA);
 		/*
 		 * Put back the secondary interface in normal mode.
 		 * BMI160 will poll based on the configure ODR.
@@ -897,6 +908,9 @@ static int init(const struct motion_sensor_t *s)
 		bmm150_mag_access_ctrl(s->i2c_addr, 0);
 	}
 #endif
+	set_range(s, s->runtime_config.range, 0);
+	set_data_rate(s, s->runtime_config.odr, 0);
+
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	ret = config_interrupt(s);
 #endif
diff --git a/include/ec_commands.h b/include/ec_commands.h
index e528067089..255e6f0e18 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1825,7 +1825,7 @@ struct ec_params_motion_sense {
 		} ec_rate, sensor_odr, sensor_range;
 
 		/* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */
-		struct __attribute__((__packed__)) {
+		struct {
 			uint8_t sensor_num;
 
 			/*
@@ -1850,7 +1850,7 @@ struct ec_params_motion_sense {
 			 * Gyro:          1/1024 deg/s
 			 */
 			int16_t offset[3];
-		} sensor_offset;
+		} __packed sensor_offset;
 
 		/* Used for MOTIONSENSE_CMD_FIFO_INFO */
 		struct {