Revert "motion: remove load_fifo"

This reverts commit 1d89369b51135d979c5adcd8cc7b9c6d5853a467.

A few EC CLs made some bad assumption and caused CTS to fail (without
realizing it in time). This is a series to revert those bad CLs and other
CLs that depend on it.

They might reland back soon.

BUG=b/111220617
TEST=With series, CTS passes once again

Change-Id: I0b549d036a1bfd2126b50afe0da73c21188745e8
Signed-off-by: Alexandru M Stan <amstan@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1134057
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Commit-Queue: Furquan Shaikh <furquan@chromium.org>
Tested-by: Furquan Shaikh <furquan@chromium.org>

6 files changed, 227 insertions, 234 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 317734d878..7fd71b875e 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -187,22 +187,6 @@ static void motion_sense_get_fifo_info(
 }
 #endif
 
-static inline int motion_sensor_in_forced_mode(
-		const struct motion_sensor_t *sensor)
-{
-#ifdef CONFIG_ACCEL_FORCE_MODE_MASK
-	/* Sensor not in force mode, its irq_handler is getting data. */
-	if (!(CONFIG_ACCEL_FORCE_MODE_MASK & (1 << (sensor - motion_sensors))))
-		return 0;
-	else
-		return 1;
-#else
-	return 0;
-#endif
-}
-
-
-
 /* Minimal amount of time since last collection before triggering a new one */
 static inline int motion_sensor_time_to_read(const timestamp_t *ts,
 		const struct motion_sensor_t *sensor)
@@ -303,12 +287,14 @@ static int motion_sense_set_ec_rate_from_ap(
 
 	if (new_rate_us == 0)
 		return 0;
-	if (motion_sensor_in_forced_mode(sensor))
+#ifdef CONFIG_ACCEL_FORCE_MODE_MASK
+	if (CONFIG_ACCEL_FORCE_MODE_MASK & (1 << (sensor - motion_sensors)))
 		/*
 		 * AP EC sampling rate does not matter: we will collect at the
 		 * requested sensor frequency.
 		 */
 		goto end_set_ec_rate_from_ap;
+#endif
 	if (odr_mhz == 0)
 		goto end_set_ec_rate_from_ap;
 
@@ -350,16 +336,18 @@ static int motion_sense_select_ec_rate(
 		enum sensor_config config_id,
 		int interrupt)
 {
-	if (interrupt == 0 && motion_sensor_in_forced_mode(sensor)) {
+#ifdef CONFIG_ACCEL_FORCE_MODE_MASK
+	if (interrupt == 0 &&
+	    (CONFIG_ACCEL_FORCE_MODE_MASK & (1 << (sensor - motion_sensors)))) {
 		int rate_mhz = BASE_ODR(sensor->config[config_id].odr);
 		/* we have to run ec at the sensor frequency rate.*/
 		if (rate_mhz > 0)
 			return SECOND * 1000 / rate_mhz;
 		else
 			return 0;
-	} else {
-		return sensor->config[config_id].ec_rate;
-	}
+	} else
+#endif
+	return sensor->config[config_id].ec_rate;
 }
 
 /* motion_sense_ec_rate
@@ -716,28 +704,28 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 	}
 #endif
 #ifdef CONFIG_ACCEL_FIFO
-	if (motion_sensor_in_forced_mode(sensor)) {
-		if (motion_sensor_time_to_read(ts, sensor)) {
-			struct ec_response_motion_sensor_data vector;
-			int *v = sensor->raw_xyz;
-
-			ret = motion_sense_read(sensor);
-			if (ret == EC_SUCCESS) {
-				vector.flags = 0;
-				vector.sensor_num = sensor - motion_sensors;
+	if (sensor->drv->load_fifo != NULL) {
+		/* Load fifo is filling raw_xyz sensor vector */
+		sensor->drv->load_fifo(sensor);
+	} else if (motion_sensor_time_to_read(ts, sensor)) {
+		struct ec_response_motion_sensor_data vector;
+		int *v = sensor->raw_xyz;
+		ret = motion_sense_read(sensor);
+		if (ret == EC_SUCCESS) {
+			vector.flags = 0;
+			vector.sensor_num = sensor - motion_sensors;
 #ifdef CONFIG_ACCEL_SPOOF_MODE
-				if (sensor->in_spoof_mode)
-					v = sensor->spoof_xyz;
+			if (sensor->in_spoof_mode)
+				v = sensor->spoof_xyz;
 #endif /* defined(CONFIG_ACCEL_SPOOF_MODE) */
-				vector.data[X] = v[X];
-				vector.data[Y] = v[Y];
-				vector.data[Z] = v[Z];
-				motion_sense_fifo_add_unit(&vector, sensor, 3);
-			}
-			sensor->last_collection = ts->le.lo;
-		} else {
-			ret = EC_ERROR_BUSY;
+			vector.data[X] = v[X];
+			vector.data[Y] = v[Y];
+			vector.data[Z] = v[Z];
+			motion_sense_fifo_add_unit(&vector, sensor, 3);
 		}
+		sensor->last_collection = ts->le.lo;
+	} else {
+		ret = EC_ERROR_BUSY;
 	}
 	if (*event & TASK_EVENT_MOTION_FLUSH_PENDING) {
 		int flush_pending;
@@ -748,21 +736,19 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		}
 	}
 #else
-	if (motion_sensor_in_forced_mode(sensor)) {
-		if (motion_sensor_time_to_read(ts, sensor)) {
-			/* Get latest data for local calculation */
-			ret = motion_sense_read(sensor);
-			sensor->last_collection = ts->le.lo;
-		} else {
-			ret = EC_ERROR_BUSY;
-		}
-		if (ret == EC_SUCCESS) {
-			mutex_lock(&g_sensor_mutex);
-			memcpy(sensor->xyz, sensor->raw_xyz,
-			       sizeof(sensor->xyz));
-			mutex_unlock(&g_sensor_mutex);
-		}
+	if (motion_sensor_time_to_read(ts, sensor)) {
+		/* Get latest data for local calculation */
+		ret = motion_sense_read(sensor);
+		sensor->last_collection = ts->le.lo;
+	} else {
+		ret = EC_ERROR_BUSY;
 	}
+	if (ret == EC_SUCCESS) {
+		mutex_lock(&g_sensor_mutex);
+		memcpy(sensor->xyz, sensor->raw_xyz, sizeof(sensor->xyz));
+		mutex_unlock(&g_sensor_mutex);
+	}
+
 #endif
 	return ret;
 }
diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c
index 77379eb52b..f7d897757b 100644
--- a/driver/accel_lis2dh.c
+++ b/driver/accel_lis2dh.c
@@ -396,6 +396,9 @@ const struct accelgyro_drv lis2dh_drv = {
 	.set_offset = st_set_offset,
 	.get_offset = st_get_offset,
 	.perform_calib = NULL,
+#ifdef CONFIG_ACCEL_FIFO
+	.load_fifo = load_fifo,
+#endif /* CONFIG_ACCEL_FIFO */
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.irq_handler = irq_handler,
 #endif /* CONFIG_ACCEL_INTERRUPTS */
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index cad62df246..96eea09844 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -759,6 +759,169 @@ int list_activities(const struct motion_sensor_t *s,
 #endif
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
+/**
+ * bmi160_interrupt - called when the sensor activates the interrupt line.
+ *
+ * This is a "top half" interrupt handler, it just asks motion sense ask
+ * to schedule the "bottom half", ->irq_handler().
+ */
+void bmi160_interrupt(enum gpio_signal signal)
+{
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCELGYRO_BMI160_INT_EVENT, 0);
+}
+
+
+static int config_interrupt(const struct motion_sensor_t *s)
+{
+	int ret, tmp;
+
+	if (s->type != MOTIONSENSE_TYPE_ACCEL)
+		return EC_SUCCESS;
+
+	mutex_lock(s->mutex);
+	raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_FIFO_FLUSH);
+	raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_INT_RESET);
+
+#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
+	raw_write8(s->port, s->addr, BMI160_INT_TAP_0,
+		BMI160_TAP_DUR(s, CONFIG_GESTURE_TAP_MAX_INTERSTICE_T));
+	ret = raw_write8(s->port, s->addr, BMI160_INT_TAP_1,
+		BMI160_TAP_TH(s, CONFIG_GESTURE_TAP_THRES_MG));
+#endif
+#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
+	/* only use orientation sensor on the lid sensor */
+	if (s->location == MOTIONSENSE_LOC_LID) {
+		ret = raw_write8(s->port, s->addr, BMI160_INT_ORIENT_0,
+			BMI160_INT_ORIENT_0_INIT_VAL);
+		ret = raw_write8(s->port, s->addr, BMI160_INT_ORIENT_1,
+			BMI160_INT_ORIENT_1_INIT_VAL);
+	}
+#endif
+
+	/*
+	 * Set a 5ms latch to be sure the EC can read the interrupt register
+	 * properly, even when it is running more slowly.
+	 */
+#ifdef CONFIG_ACCELGYRO_BMI160_INT2_OUTPUT
+	ret = raw_write8(s->port, s->addr, BMI160_INT_LATCH, BMI160_LATCH_5MS);
+#else
+	/* Also, configure int2 as an external input. */
+	ret = raw_write8(s->port, s->addr, BMI160_INT_LATCH,
+		BMI160_INT2_INPUT_EN | BMI160_LATCH_5MS);
+#endif
+
+	/* configure int1 as an interrupt */
+	ret = raw_write8(s->port, s->addr, BMI160_INT_OUT_CTRL,
+		BMI160_INT_CTRL(1, OUTPUT_EN));
+
+	/* Map activity interrupt to int 1 */
+	tmp = 0;
+#ifdef CONFIG_GESTURE_SIGMO
+	tmp |= BMI160_INT_ANYMOTION;
+#endif
+#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
+	tmp |= BMI160_INT_D_TAP;
+#endif
+#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
+	/* enable orientation interrupt for lid sensor only */
+	if (s->location == MOTIONSENSE_LOC_LID)
+		tmp |= BMI160_INT_ORIENT;
+#endif
+	ret = raw_write8(s->port, s->addr, BMI160_INT_MAP_REG(1), tmp);
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* map fifo water mark to int 1 */
+	ret = raw_write8(s->port, s->addr, BMI160_INT_FIFO_MAP,
+			BMI160_INT_MAP(1, FWM) |
+			BMI160_INT_MAP(1, FFULL));
+
+	/* configure fifo watermark to int whenever there's any data in there */
+	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_0, 1);
+#ifdef CONFIG_ACCELGYRO_BMI160_INT2_OUTPUT
+	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
+			BMI160_FIFO_HEADER_EN);
+#else
+	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
+			BMI160_FIFO_TAG_INT2_EN |
+			BMI160_FIFO_HEADER_EN);
+#endif
+
+	/* Set fifo*/
+	ret = raw_read8(s->port, s->addr, BMI160_INT_EN_1, &tmp);
+	tmp |= BMI160_INT_FWM_EN | BMI160_INT_FFUL_EN;
+	ret = raw_write8(s->port, s->addr, BMI160_INT_EN_1, tmp);
+#endif
+	mutex_unlock(s->mutex);
+	return ret;
+}
+
+/**
+ * irq_handler - bottom half of the interrupt stack.
+ * Ran from the motion_sense task, finds the events that raised the interrupt.
+ *
+ * For now, we just print out. We should set a bitmask motion sense code will
+ * act upon.
+ */
+static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int interrupt;
+#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
+	int shifted_masked_orientation;
+#endif
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(*event & CONFIG_ACCELGYRO_BMI160_INT_EVENT)))
+		return EC_ERROR_NOT_HANDLED;
+
+	raw_read32(s->port, s->addr, BMI160_INT_STATUS_0, &interrupt);
+
+#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
+	if (interrupt & BMI160_D_TAP_INT)
+		*event |= CONFIG_GESTURE_TAP_EVENT;
+#endif
+#ifdef CONFIG_GESTURE_SIGMO
+	if (interrupt & BMI160_SIGMOT_INT)
+		*event |= CONFIG_GESTURE_SIGMO_EVENT;
+#endif
+#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
+	shifted_masked_orientation = (interrupt >> 24) & BMI160_ORIENT_XY_MASK;
+	if (BMI160_GET_DATA(s)->raw_orientation != shifted_masked_orientation) {
+		enum motionsensor_orientation orientation =
+				MOTIONSENSE_ORIENTATION_UNKNOWN;
+
+		BMI160_GET_DATA(s)->raw_orientation =
+				shifted_masked_orientation;
+
+		switch (shifted_masked_orientation) {
+		case BMI160_ORIENT_PORTRAIT:
+			orientation = MOTIONSENSE_ORIENTATION_PORTRAIT;
+			break;
+		case BMI160_ORIENT_PORTRAIT_INVERT:
+			orientation =
+				MOTIONSENSE_ORIENTATION_UPSIDE_DOWN_PORTRAIT;
+			break;
+		case BMI160_ORIENT_LANDSCAPE:
+			orientation = MOTIONSENSE_ORIENTATION_LANDSCAPE;
+			break;
+		case BMI160_ORIENT_LANDSCAPE_INVERT:
+			orientation =
+				MOTIONSENSE_ORIENTATION_UPSIDE_DOWN_LANDSCAPE;
+			break;
+		default:
+			break;
+		}
+		orientation = motion_sense_remap_orientation(s, orientation);
+		SET_ORIENTATION(s, orientation);
+	}
+#endif
+	/*
+	 * No need to read the FIFO here, motion sense task is
+	 * doing it on every interrupt.
+	 */
+	return EC_SUCCESS;
+}
+#endif  /* CONFIG_ACCEL_INTERRUPTS */
 
 #ifdef CONFIG_ACCEL_FIFO
 enum fifo_state {
@@ -823,17 +986,6 @@ static int bmi160_decode_header(struct motion_sensor_t *s,
 	}
 }
 
-/**
- * Retrieve hardware FIFO from sensor,
- * - put data in Sensor Hub fifo.
- * - update sensor raw_xyz vector with the last information.
- * We put raw data in hub fifo and process data from there.
- * @s Pointer to sensor data.
- *
- * NOTE: If a new driver supports this function, be sure to add a check
- * for spoof_mode in order to load the sensor stack with the spoofed
- * data.  See accelgyro_bmi160.c::load_fifo for an example.
- */
 static int load_fifo(struct motion_sensor_t *s)
 {
 	int done = 0;
@@ -940,174 +1092,6 @@ static int load_fifo(struct motion_sensor_t *s)
 }
 #endif  /* CONFIG_ACCEL_FIFO */
 
-/**
- * bmi160_interrupt - called when the sensor activates the interrupt line.
- *
- * This is a "top half" interrupt handler, it just asks motion sense ask
- * to schedule the "bottom half", ->irq_handler().
- */
-void bmi160_interrupt(enum gpio_signal signal)
-{
-	task_set_event(TASK_ID_MOTIONSENSE,
-		       CONFIG_ACCELGYRO_BMI160_INT_EVENT, 0);
-}
-
-
-static int config_interrupt(const struct motion_sensor_t *s)
-{
-	int ret, tmp;
-
-	if (s->type != MOTIONSENSE_TYPE_ACCEL)
-		return EC_SUCCESS;
-
-	mutex_lock(s->mutex);
-	raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_FIFO_FLUSH);
-	raw_write8(s->port, s->addr, BMI160_CMD_REG, BMI160_CMD_INT_RESET);
-
-#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
-	raw_write8(s->port, s->addr, BMI160_INT_TAP_0,
-		BMI160_TAP_DUR(s, CONFIG_GESTURE_TAP_MAX_INTERSTICE_T));
-	ret = raw_write8(s->port, s->addr, BMI160_INT_TAP_1,
-		BMI160_TAP_TH(s, CONFIG_GESTURE_TAP_THRES_MG));
-#endif
-#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
-	/* only use orientation sensor on the lid sensor */
-	if (s->location == MOTIONSENSE_LOC_LID) {
-		ret = raw_write8(s->port, s->addr, BMI160_INT_ORIENT_0,
-			BMI160_INT_ORIENT_0_INIT_VAL);
-		ret = raw_write8(s->port, s->addr, BMI160_INT_ORIENT_1,
-			BMI160_INT_ORIENT_1_INIT_VAL);
-	}
-#endif
-
-	/*
-	 * Set a 5ms latch to be sure the EC can read the interrupt register
-	 * properly, even when it is running more slowly.
-	 */
-#ifdef CONFIG_ACCELGYRO_BMI160_INT2_OUTPUT
-	ret = raw_write8(s->port, s->addr, BMI160_INT_LATCH, BMI160_LATCH_5MS);
-#else
-	/* Also, configure int2 as an external input. */
-	ret = raw_write8(s->port, s->addr, BMI160_INT_LATCH,
-		BMI160_INT2_INPUT_EN | BMI160_LATCH_5MS);
-#endif
-
-	/* configure int1 as an interrupt */
-	ret = raw_write8(s->port, s->addr, BMI160_INT_OUT_CTRL,
-		BMI160_INT_CTRL(1, OUTPUT_EN));
-
-	/* Map activity interrupt to int 1 */
-	tmp = 0;
-#ifdef CONFIG_GESTURE_SIGMO
-	tmp |= BMI160_INT_ANYMOTION;
-#endif
-#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
-	tmp |= BMI160_INT_D_TAP;
-#endif
-#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
-	/* enable orientation interrupt for lid sensor only */
-	if (s->location == MOTIONSENSE_LOC_LID)
-		tmp |= BMI160_INT_ORIENT;
-#endif
-	ret = raw_write8(s->port, s->addr, BMI160_INT_MAP_REG(1), tmp);
-
-#ifdef CONFIG_ACCEL_FIFO
-	/* map fifo water mark to int 1 */
-	ret = raw_write8(s->port, s->addr, BMI160_INT_FIFO_MAP,
-			BMI160_INT_MAP(1, FWM) |
-			BMI160_INT_MAP(1, FFULL));
-
-	/* configure fifo watermark to int whenever there's any data in there */
-	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_0, 1);
-#ifdef CONFIG_ACCELGYRO_BMI160_INT2_OUTPUT
-	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
-			BMI160_FIFO_HEADER_EN);
-#else
-	ret = raw_write8(s->port, s->addr, BMI160_FIFO_CONFIG_1,
-			BMI160_FIFO_TAG_INT2_EN |
-			BMI160_FIFO_HEADER_EN);
-#endif
-
-	/* Set fifo*/
-	ret = raw_read8(s->port, s->addr, BMI160_INT_EN_1, &tmp);
-	tmp |= BMI160_INT_FWM_EN | BMI160_INT_FFUL_EN;
-	ret = raw_write8(s->port, s->addr, BMI160_INT_EN_1, tmp);
-#endif
-	mutex_unlock(s->mutex);
-	return ret;
-}
-
-/**
- * irq_handler - bottom half of the interrupt stack.
- * Ran from the motion_sense task, finds the events that raised the interrupt.
- *
- * For now, we just print out. We should set a bitmask motion sense code will
- * act upon.
- */
-static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
-{
-	int interrupt;
-#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
-	int shifted_masked_orientation;
-#endif
-
-	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
-			(!(*event & CONFIG_ACCELGYRO_BMI160_INT_EVENT)))
-		return EC_ERROR_NOT_HANDLED;
-
-	raw_read32(s->port, s->addr, BMI160_INT_STATUS_0, &interrupt);
-
-#ifdef CONFIG_GESTURE_SENSOR_BATTERY_TAP
-	if (interrupt & BMI160_D_TAP_INT)
-		*event |= CONFIG_GESTURE_TAP_EVENT;
-#endif
-#ifdef CONFIG_GESTURE_SIGMO
-	if (interrupt & BMI160_SIGMOT_INT)
-		*event |= CONFIG_GESTURE_SIGMO_EVENT;
-#endif
-#ifdef CONFIG_ACCEL_FIFO
-	if (interrupt & (BMI160_FWM_INT | BMI160_FFULL_INT))
-		load_fifo(s);
-#endif
-#ifdef CONFIG_BMI160_ORIENTATION_SENSOR
-	shifted_masked_orientation = (interrupt >> 24) & BMI160_ORIENT_XY_MASK;
-	if (BMI160_GET_DATA(s)->raw_orientation != shifted_masked_orientation) {
-		enum motionsensor_orientation orientation =
-				MOTIONSENSE_ORIENTATION_UNKNOWN;
-
-		BMI160_GET_DATA(s)->raw_orientation =
-				shifted_masked_orientation;
-
-		switch (shifted_masked_orientation) {
-		case BMI160_ORIENT_PORTRAIT:
-			orientation = MOTIONSENSE_ORIENTATION_PORTRAIT;
-			break;
-		case BMI160_ORIENT_PORTRAIT_INVERT:
-			orientation =
-				MOTIONSENSE_ORIENTATION_UPSIDE_DOWN_PORTRAIT;
-			break;
-		case BMI160_ORIENT_LANDSCAPE:
-			orientation = MOTIONSENSE_ORIENTATION_LANDSCAPE;
-			break;
-		case BMI160_ORIENT_LANDSCAPE_INVERT:
-			orientation =
-				MOTIONSENSE_ORIENTATION_UPSIDE_DOWN_LANDSCAPE;
-			break;
-		default:
-			break;
-		}
-		orientation = motion_sense_remap_orientation(s, orientation);
-		SET_ORIENTATION(s, orientation);
-	}
-#endif
-	/*
-	 * No need to read the FIFO here, motion sense task is
-	 * doing it on every interrupt.
-	 */
-	return EC_SUCCESS;
-}
-#endif  /* CONFIG_ACCEL_INTERRUPTS */
-
 
 static int read(const struct motion_sensor_t *s, vector_3_t v)
 {
@@ -1301,6 +1285,9 @@ const struct accelgyro_drv bmi160_drv = {
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.irq_handler = irq_handler,
 #endif
+#ifdef CONFIG_ACCEL_FIFO
+	.load_fifo = load_fifo,
+#endif
 #ifdef CONFIG_GESTURE_HOST_DETECTION
 	.manage_activity = manage_activity,
 	.list_activities = list_activities,
diff --git a/driver/accelgyro_lsm6ds0.c b/driver/accelgyro_lsm6ds0.c
index cf2eb78ff4..05556e4226 100644
--- a/driver/accelgyro_lsm6ds0.c
+++ b/driver/accelgyro_lsm6ds0.c
@@ -355,7 +355,7 @@ static int read(const struct motion_sensor_t *s, vector_3_t v)
 	}
 
 	for (i = X; i <= Z; i++)
-		v[i] = (int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2]);
+		v[i] = ((int16_t)((raw[i * 2 + 1] << 8) | raw[i * 2]));
 
 	rotate(v, *s->rot_standard_ref, v);
 
diff --git a/driver/als_si114x.c b/driver/als_si114x.c
index 5d9e595f07..3b4cf39daa 100644
--- a/driver/als_si114x.c
+++ b/driver/als_si114x.c
@@ -581,4 +581,7 @@ const struct accelgyro_drv si114x_drv = {
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.irq_handler = irq_handler,
 #endif
+#ifdef CONFIG_ACCEL_FIFO
+	.load_fifo = NULL,
+#endif
 };
diff --git a/include/accelgyro.h b/include/accelgyro.h
index ea7b3efa6c..85e4bcc04a 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -101,6 +101,20 @@ struct accelgyro_drv {
 	 */
 	int (*irq_handler)(struct motion_sensor_t *s, uint32_t *event);
 #endif
+#ifdef CONFIG_ACCEL_FIFO
+	/**
+	 * Retrieve hardware FIFO from sensor,
+	 * - put data in Sensor Hub fifo.
+	 * - update sensor raw_xyz vector with the last information.
+	 * We put raw data in hub fifo and process data from there.
+	 * @s Pointer to sensor data.
+	 *
+	 * NOTE: If a new driver supports this function, be sure to add a check
+	 * for spoof_mode in order to load the sensor stack with the spoofed
+	 * data.  See accelgyro_bmi160.c::load_fifo for an example.
+	 */
+	int (*load_fifo)(struct motion_sensor_t *s);
+#endif
 #ifdef CONFIG_GESTURE_DETECTION
 	/**
 	 * handler for setting/getting activity information.