common: Move fifo logic out of motion_sense.c

This change is needed to allow better testing of the fifo
behavior. Additionally, motion_sense_fifo.c will only be compiled
if CONFIG_ACCEL_FIFO is defined. This behaviour requires a few
small changes to several boards and baseboards to make
sure that we only define CONFIG_ACCEL_FIFO when the MOTIONSENSE
task is present (some times that may be only in one section RW or
RO).

BUG=b:137758297
BRANCH=None
TEST=buildall and ran CTS on arcada

Change-Id: I2f7e4e436ba9568a35b7a0b2c8d53a73f198ba73
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1704163
Reviewed-by: Alexandru M Stan <amstan@chromium.org>
Commit-Queue: Alexandru M Stan <amstan@chromium.org>

19 files changed, 512 insertions, 436 deletions

diff --git a/baseboard/grunt/baseboard.h b/baseboard/grunt/baseboard.h
index ceff4d6e71..27d87091b2 100644
--- a/baseboard/grunt/baseboard.h
+++ b/baseboard/grunt/baseboard.h
@@ -185,11 +185,13 @@
 /* Thermal */
 #define CONFIG_TEMP_SENSOR_SB_TSI
 
+#ifndef VARIANT_GRUNT_NO_SENSORS
 /* FIFO size is a power of 2. */
 #define CONFIG_ACCEL_FIFO 256
 
 /* Depends on how fast the AP boots and typical ODRs. */
 #define CONFIG_ACCEL_FIFO_THRES (CONFIG_ACCEL_FIFO / 3)
+#endif /* VARIANT_GRUNT_NO_SENSORS */
 
 #define USB_PD_PORT_ANX74XX	0
 #define USB_PD_PORT_PS8751	1
diff --git a/baseboard/kukui/baseboard.h b/baseboard/kukui/baseboard.h
index 1c1b8a980c..317c51be51 100644
--- a/baseboard/kukui/baseboard.h
+++ b/baseboard/kukui/baseboard.h
@@ -140,8 +140,10 @@
 #define GPIO_LID_OPEN GPIO_HALL_INT_L
 
 /* FIFO size is in power of 2. */
+#ifndef VARIANT_KUKUI_NO_SENSORS
 #define CONFIG_ACCEL_FIFO 256
 #define CONFIG_ACCEL_FIFO_THRES (CONFIG_ACCEL_FIFO / 3)
+#endif /* VARIANT_KUKUI_NO_SENSORS */
 
 /* USB PD config */
 #define CONFIG_CHARGE_MANAGER
diff --git a/board/careena/board.h b/board/careena/board.h
index cfe0abd479..f8b225713a 100644
--- a/board/careena/board.h
+++ b/board/careena/board.h
@@ -9,6 +9,7 @@
 #define __CROS_EC_BOARD_H
 
 #define VARIANT_GRUNT_TCPC_0_ANX3429
+#define VARIANT_GRUNT_NO_SENSORS
 
 #include "baseboard.h"
 
diff --git a/board/flapjack/board.h b/board/flapjack/board.h
index 15b52782b0..a7420a55eb 100644
--- a/board/flapjack/board.h
+++ b/board/flapjack/board.h
@@ -114,9 +114,11 @@
 #define CONFIG_TABLET_MODE
 #define CONFIG_TABLET_MODE_SWITCH
 
+#ifdef SECTION_IS_RW
 /* FIFO size is in power of 2. */
 #define CONFIG_ACCEL_FIFO 256
 #define CONFIG_ACCEL_FIFO_THRES (CONFIG_ACCEL_FIFO / 3)
+#endif /* SECTION_IS_RW */
 
 /* Wireless Power Charger Config */
 #ifdef SECTION_IS_RW
diff --git a/board/jacuzzi/board.h b/board/jacuzzi/board.h
index bad56feb97..08a7974032 100644
--- a/board/jacuzzi/board.h
+++ b/board/jacuzzi/board.h
@@ -11,6 +11,10 @@
 #define VARIANT_KUKUI_BATTERY_SMART
 #define VARIANT_KUKUI_CHARGER_ISL9238
 
+#ifndef SECTION_IS_RW
+#define VARIANT_KUKUI_NO_SENSORS
+#endif /* SECTION_IS_RW */
+
 #include "baseboard.h"
 
 /* TODO(b:135086465) led implementation */
diff --git a/board/kodama/board.h b/board/kodama/board.h
index d865edefcd..44b1cae9ca 100644
--- a/board/kodama/board.h
+++ b/board/kodama/board.h
@@ -12,6 +12,10 @@
 #define VARIANT_KUKUI_CHARGER_MT6370
 #define VARIANT_KUKUI_DP_MUX_GPIO
 
+#ifndef SECTION_IS_RW
+#define VARIANT_KUKUI_NO_SENSORS
+#endif /* SECTION_IS_RW */
+
 #include "baseboard.h"
 
 #define CONFIG_VOLUME_BUTTONS
diff --git a/board/kukui/board.c b/board/kukui/board.c
index 3d7b514a71..8f54d78f3b 100644
--- a/board/kukui/board.c
+++ b/board/kukui/board.c
@@ -306,7 +306,7 @@ DECLARE_HOOK(HOOK_INIT, board_rev_init, HOOK_PRIO_INIT_ADC + 1);
 
 /* Motion sensors */
 /* Mutexes */
-#ifdef SECTION_IS_RW
+#ifndef VARIANT_KUKUI_NO_SENSORS
 static struct mutex g_lid_mutex;
 
 static struct bmi160_drv_data_t g_bmi160_data;
@@ -486,7 +486,7 @@ const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
 const struct motion_sensor_t *motion_als_sensors[] = {
 	&motion_sensors[CLEAR_ALS],
 };
-#endif /* SECTION_IS_RW */
+#endif /* VARIANT_KUKUI_NO_SENSORS */
 
 void usb_charger_set_switches(int port, enum usb_switch setting)
 {
diff --git a/board/kukui/board.h b/board/kukui/board.h
index c017dc6c8c..4fd1a76a50 100644
--- a/board/kukui/board.h
+++ b/board/kukui/board.h
@@ -17,6 +17,10 @@
 #define VARIANT_KUKUI_CHARGER_MT6370
 #define VARIANT_KUKUI_DP_MUX_GPIO
 
+#ifndef SECTION_IS_RW
+#define VARIANT_KUKUI_NO_SENSORS
+#endif /* SECTION_IS_RW */
+
 #include "baseboard.h"
 
 #define CONFIG_VOLUME_BUTTONS
diff --git a/board/liara/board.h b/board/liara/board.h
index e5260ee06e..b869f9f8c1 100644
--- a/board/liara/board.h
+++ b/board/liara/board.h
@@ -9,6 +9,7 @@
 #define __CROS_EC_BOARD_H
 
 #define VARIANT_GRUNT_TCPC_0_ANX3429
+#define VARIANT_GRUNT_NO_SENSORS
 
 #include "baseboard.h"
 
diff --git a/common/build.mk b/common/build.mk
index 6b999e1fe9..e38f1bdeb8 100644
--- a/common/build.mk
+++ b/common/build.mk
@@ -17,6 +17,7 @@ common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DS0)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DSM)+=math_util.o
 common-$(CONFIG_ACCELGYRO_LSM6DSO)+=math_util.o
+common-$(CONFIG_ACCEL_FIFO)+=motion_sense_fifo.o
 common-$(CONFIG_ACCEL_LIS2DW12)+=math_util.o
 common-$(CONFIG_ACCEL_LIS2DH)+=math_util.o
 common-$(CONFIG_ACCEL_KXCJ9)+=math_util.o
@@ -261,4 +262,4 @@ include $(_common_dir)fpsensor/build.mk
 include $(_common_dir)usbc/build.mk
 
 include $(_common_dir)mock/build.mk
-common-y+=$(foreach m,$(mock-y),mock/$(m))
\ No newline at end of file
+common-y+=$(foreach m,$(mock-y),mock/$(m))
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 17806aabfe..424da27011 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -19,6 +19,7 @@
 #include "math_util.h"
 #include "mkbp_event.h"
 #include "motion_sense.h"
+#include "motion_sense_fifo.h"
 #include "motion_lid.h"
 #include "power.h"
 #include "queue.h"
@@ -63,13 +64,7 @@ static int accel_disp;
  */
 #define MOTION_SENSOR_INT_ADJUSTMENT_US 10
 
-/*
- * Mutex to protect sensor values between host command task and
- * motion sense task:
- * When we process CMD_DUMP, we want to be sure the motion sense
- * task is not updating the sensor values at the same time.
- */
-static struct mutex g_sensor_mutex;
+struct mutex g_sensor_mutex;
 
 /*
  * Current power level (S0, S3, S5, ...)
@@ -83,394 +78,6 @@ static void print_spoof_mode_status(int id);
 /* Flags to control whether to send an ODR change event for a sensor */
 static uint32_t odr_event_required;
 
-#ifdef CONFIG_ACCEL_FIFO
-
-static inline int is_timestamp(struct ec_response_motion_sensor_data *data)
-{
-	return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
-}
-
-/* Need to wake up the AP */
-static int wake_up_needed;
-
-/* Number of element the AP should collect */
-static int fifo_queue_count;
-static int fifo_int_enabled;
-
-struct queue motion_sense_fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO,
-		struct ec_response_motion_sensor_data);
-static int motion_sense_fifo_lost;
-
-/**
- * Staged metadata for the motion_sense_fifo.
- * @read_ts: The timestamp at which the staged data was read. This value will
- *	serve as the upper bound for spreading
- * @count: The total number of motion_sense_fifo entries that are currently
- *	staged.
- * @sample_count: The total number of sensor readings per sensor that are
- *	currently staged.
- * @requires_spreading: Flag used to shortcut the commit process. This should be
- *	true iff at least one of sample_count[] > 1
- */
-struct fifo_staged {
-	uint32_t read_ts;
-	uint16_t count;
-	uint8_t sample_count[SENSOR_COUNT];
-	uint8_t requires_spreading;
-};
-static struct fifo_staged fifo_staged;
-
-static inline struct ec_response_motion_sensor_data *
-get_motion_sense_fifo_head(void)
-{
-	return ((struct ec_response_motion_sensor_data *)
-		motion_sense_fifo.buffer) +
-	       (motion_sense_fifo.state->head &
-		motion_sense_fifo.unit_bytes);
-}
-
-/**
- * Pop one entry from the motion sense fifo. Poping will give priority to
- * committed data (data residing between the head and tail of the queue). If no
- * committed data is available (all the data is staged), then this function will
- * remove the oldest staged data by moving both the head and tail.
- *
- * As a side-effect of this function, it'll updated any appropriate lost and
- * count variables.
- *
- * WARNING: This function MUST be called from within a locked context of
- * g_sensor_mutex.
- */
-static void motion_sense_fifo_pop(void)
-{
-	struct ec_response_motion_sensor_data *head =
-		get_motion_sense_fifo_head();
-	const size_t initial_count = queue_count(&motion_sense_fifo);
-
-	/* Check that we have something to pop. */
-	if (!initial_count && !fifo_staged.count)
-		return;
-
-	/*
-	 * If all the data is staged (nothing in the committed queue), we'll
-	 * need to move the head and the tail over to simulate poping from the
-	 * staged data.
-	 */
-	if (!initial_count)
-		queue_advance_tail(&motion_sense_fifo, 1);
-
-	/*
-	 * By not using queue_remove_unit we're avoiding an un-necessary memcpy.
-	 */
-	queue_advance_head(&motion_sense_fifo, 1);
-	motion_sense_fifo_lost++;
-
-	/* Increment lost counter if we have valid data. */
-	if (!is_timestamp(head))
-		motion_sensors[head->sensor_num].lost++;
-
-	/*
-	 * We're done if the initial count was non-zero and we only advanced the
-	 * head. Else, decrement the staged count and update staged metadata.
-	 */
-	if (initial_count)
-		return;
-
-	fifo_staged.count--;
-
-	/* If we removed a timestamp there's nothing else for us to do. */
-	if (is_timestamp(head))
-		return;
-
-	/*
-	 * Decrement sample count, if the count was 2 before, we might not need
-	 * to spread anymore. Loop through and check.
-	 */
-	if (--fifo_staged.sample_count[head->sensor_num] < 2) {
-		int i;
-
-		fifo_staged.requires_spreading = 0;
-		for (i = 0; i < SENSOR_COUNT; i++)
-			if (fifo_staged.sample_count[i] > 1) {
-				fifo_staged.requires_spreading = 1;
-				break;
-			}
-	}
-}
-
-static void motion_sense_fifo_ensure_space(void)
-{
-	/* If we already have space just bail. */
-	if (queue_space(&motion_sense_fifo) > fifo_staged.count)
-		return;
-
-	/*
-	 * Pop at least 1 spot, but if all the following conditions are met we
-	 * will continue to pop:
-	 * 1. We're operating with tight timestamps.
-	 * 2. The new head isn't a timestamp.
-	 * 3. We have data that we can possibly pop.
-	 *
-	 * Removing more than one entry is needed because if we are using tight
-	 * timestamps and we pop a timestamp, then the next head is data, the AP
-	 * would assign a bad timestamp to it.
-	 */
-	do {
-		motion_sense_fifo_pop();
-	} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
-		 !is_timestamp(get_motion_sense_fifo_head()) &&
-		 queue_count(&motion_sense_fifo) + fifo_staged.count);
-}
-
-/*
- * Do not use this function directly if you just want to add sensor data, use
- * motion_sense_fifo_stage_data instead to get a proper timestamp too.
- */
-static void motion_sense_fifo_stage_unit(
-	struct ec_response_motion_sensor_data *data,
-	struct motion_sensor_t *sensor,
-	int valid_data)
-{
-	struct queue_chunk chunk;
-	int i;
-
-	mutex_lock(&g_sensor_mutex);
-
-	for (i = 0; i < valid_data; i++)
-		sensor->xyz[i] = data->data[i];
-
-	/* For valid sensors, check if AP really needs this data */
-	if (valid_data) {
-		int removed;
-
-		if (sensor->oversampling_ratio == 0) {
-			mutex_unlock(&g_sensor_mutex);
-			return;
-		}
-		removed = sensor->oversampling++;
-		sensor->oversampling %= sensor->oversampling_ratio;
-		if (removed != 0) {
-			mutex_unlock(&g_sensor_mutex);
-			return;
-		}
-	}
-
-	/* Make sure we have room for the data */
-	motion_sense_fifo_ensure_space();
-	mutex_unlock(&g_sensor_mutex);
-
-	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
-		wake_up_needed = 1;
-#ifdef CONFIG_TABLET_MODE
-	data->flags |= (tablet_get_mode() ?
-			MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
-#endif
-
-	/*
-	 * Get the next writable block in the fifo. We don't need to lock this
-	 * because it will always be past the tail and thus the AP will never
-	 * read this until motion_sense_fifo_commit_data() is called.
-	 */
-	chunk = queue_get_write_chunk(
-			&motion_sense_fifo, fifo_staged.count);
-
-	if (!chunk.buffer) {
-		/*
-		 * This should never happen since we already ensured there was
-		 * space, but if there was a bug, we don't want to write to
-		 * address 0. Just don't add any data to the queue instead.
-		 */
-		CPRINTS("Failed to get write chunk for new fifo data!");
-		return;
-	}
-
-	/*
-	 * Save the data to the writable block and increment count. This data
-	 * will now reside AFTER the tail of the queue and will not be visible
-	 * to the AP until the motion_sense_fifo_commit_data() function is
-	 * called. Because count is incremented, the following staged data will
-	 * be written to the next available block and this one will remain
-	 * staged.
-	 */
-	memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
-	fifo_staged.count++;
-
-	/*
-	 * If we're using tight timestamps, and the current entry isn't a
-	 * timestamp we'll increment the sample_count for the given sensor.
-	 * If the new per-sensor sample count is greater than 1, we'll need to
-	 * spread.
-	 */
-	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
-			!is_timestamp(data) &&
-			++fifo_staged.sample_count[data->sensor_num] > 1) {
-		fifo_staged.requires_spreading = 1;
-	}
-}
-
-enum motion_sense_async_event {
-	ASYNC_EVENT_FLUSH = MOTIONSENSE_SENSOR_FLAG_FLUSH |
-			    MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
-	ASYNC_EVENT_ODR =   MOTIONSENSE_SENSOR_FLAG_ODR |
-			    MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
-};
-
-static void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
-					    enum motion_sense_async_event evt)
-{
-	struct ec_response_motion_sensor_data vector;
-
-	vector.flags = evt;
-	vector.timestamp = __hw_clock_source_read();
-	vector.sensor_num = sensor - motion_sensors;
-
-	motion_sense_fifo_stage_unit(&vector, sensor, 0);
-	motion_sense_fifo_commit_data();
-}
-
-static void motion_sense_fifo_stage_timestamp(uint32_t timestamp)
-{
-	struct ec_response_motion_sensor_data vector;
-
-	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
-	vector.timestamp = timestamp;
-	vector.sensor_num = 0;
-	motion_sense_fifo_stage_unit(&vector, NULL, 0);
-}
-
-void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
-				  struct motion_sensor_t *sensor,
-				  int valid_data,
-				  uint32_t time)
-{
-	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS)) {
-		/* First entry, save the time for spreading later. */
-		if (!fifo_staged.count)
-			fifo_staged.read_ts = __hw_clock_source_read();
-		motion_sense_fifo_stage_timestamp(time);
-	}
-	motion_sense_fifo_stage_unit(data, sensor, valid_data);
-}
-
-/**
- * Peek into the staged data at a given offset. This function performs no bound
- * checking and is purely for convenience.
- */
-static inline struct ec_response_motion_sensor_data *
-motion_sense_peek_fifo_staged(size_t offset)
-{
-	return (struct ec_response_motion_sensor_data *)
-	       queue_get_write_chunk(&motion_sense_fifo, offset).buffer;
-}
-
-void motion_sense_fifo_commit_data(void)
-{
-	/*
-	 * Static data to use off stack. Note that next_timestamp should persist
-	 * and is only updated if the timestamp from the sensor is greater.
-	 */
-	static uint32_t data_periods[SENSOR_COUNT];
-	static uint32_t next_timestamp[SENSOR_COUNT];
-	struct ec_response_motion_sensor_data *data;
-	int i, window, sensor_num = 0;
-
-	/* Nothing staged, no work to do. */
-	if (!fifo_staged.count)
-		return;
-
-	/*
-	 * If per-sensor event counts are never more than 1, no spreading is
-	 * needed. This will also catch cases where tight timestamps aren't
-	 * used.
-	 */
-	if (!fifo_staged.requires_spreading)
-		goto flush_data_end;
-
-	data = motion_sense_peek_fifo_staged(0);
-
-	/*
-	 * Spreading only makes sense if tight timestamps are used. In such case
-	 * entries are expected to be ordered: timestamp then data. If the first
-	 * entry isn't a timestamp we must have gotten out of sync. Just commit
-	 * all the data and skip the spreading.
-	 */
-	if (!is_timestamp(data)) {
-		CPRINTS("Spreading skipped, first entry is not a timestamp");
-		goto flush_data_end;
-	}
-
-	window = time_until(data->timestamp, fifo_staged.read_ts);
-
-	/* Update the data_periods as needed for this flush. */
-	for (i = 0; i < SENSOR_COUNT; i++) {
-		int period;
-
-		/* Skip empty sensors. */
-		if (!fifo_staged.sample_count[i])
-			continue;
-
-		period = motion_sensors[i].collection_rate;
-		/*
-		 * Clamp the sample period to the MIN of collection_rate and the
-		 * window length / sample counts.
-		 */
-		if (window)
-			period = MIN(period,
-				     window / fifo_staged.sample_count[i]);
-		data_periods[i] = period;
-	}
-
-	/*
-	 * Spread the timestamps.
-	 *
-	 * If we got this far that means that the tight timestamps config is
-	 * enabled. This means that we can expect the staged entries to have 1
-	 * or more timestamps followed by exactly 1 data entry. We'll loop
-	 * through the timestamps until we get to data. We only need to update
-	 * the timestamp right before it to keep things correct.
-	 */
-	for (i = 0; i < fifo_staged.count; i++) {
-		data = motion_sense_peek_fifo_staged(i);
-
-		/* Skip timestamp, we don't know the sensor number yet. */
-		if (is_timestamp(data))
-			continue;
-
-		/* Get the sensor number and point to the timestamp entry. */
-		sensor_num = data->sensor_num;
-		data = motion_sense_peek_fifo_staged(i - 1);
-
-		/* If the timestamp is after our computed next, skip ahead. */
-		if (time_after(data->timestamp, next_timestamp[sensor_num]))
-			next_timestamp[sensor_num] = data->timestamp;
-
-		/* Spread the timestamp and compute the expected next. */
-		data->timestamp = next_timestamp[sensor_num];
-		next_timestamp[sensor_num] += data_periods[sensor_num];
-	}
-
-flush_data_end:
-	/* Advance the tail and clear the staged metadata. */
-	mutex_lock(&g_sensor_mutex);
-	queue_advance_tail(&motion_sense_fifo, fifo_staged.count);
-	mutex_unlock(&g_sensor_mutex);
-
-	/* Reset metadata for next staging cycle. */
-	memset(&fifo_staged, 0, sizeof(fifo_staged));
-}
-
-static void motion_sense_get_fifo_info(
-		struct ec_response_motion_sense_fifo_info *fifo_info)
-{
-	fifo_info->size = motion_sense_fifo.buffer_units;
-	mutex_lock(&g_sensor_mutex);
-	fifo_info->count = fifo_queue_count;
-	fifo_info->total_lost = motion_sense_fifo_lost;
-	mutex_unlock(&g_sensor_mutex);
-	fifo_info->timestamp = mkbp_last_event_time;
-}
-#endif
-
 static inline int motion_sensor_in_forced_mode(
 		const struct motion_sensor_t *sensor)
 {
@@ -1369,18 +976,6 @@ void motion_sense_task(void *u)
 	}
 }
 
-#ifdef CONFIG_ACCEL_FIFO
-static int motion_sense_get_next_event(uint8_t *out)
-{
-	union ec_response_get_next_data *data =
-		(union ec_response_get_next_data *)out;
-	/* out is not padded. It has one byte for the event type */
-	motion_sense_get_fifo_info(&data->sensor_fifo.info);
-	return sizeof(data->sensor_fifo);
-}
-
-DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event);
-#endif
 /*****************************************************************************/
 /* Host commands */
 
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
new file mode 100644
index 0000000000..00c2f5a8cc
--- /dev/null
+++ b/common/motion_sense_fifo.c
@@ -0,0 +1,403 @@
+/* Copyright 2019 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "console.h"
+#include "hwtimer.h"
+#include "mkbp_event.h"
+#include "motion_sense_fifo.h"
+#include "queue.h"
+#include "tablet_mode.h"
+#include "util.h"
+
+#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)
+
+static inline int is_timestamp(struct ec_response_motion_sensor_data *data)
+{
+	return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+}
+
+/* Need to wake up the AP */
+int wake_up_needed;
+
+/* Number of element the AP should collect */
+int fifo_queue_count;
+int fifo_int_enabled;
+
+struct queue motion_sense_fifo = QUEUE_NULL(
+	CONFIG_ACCEL_FIFO, struct ec_response_motion_sensor_data);
+int motion_sense_fifo_lost;
+
+/**
+ * Staged metadata for the motion_sense_fifo.
+ * @read_ts: The timestamp at which the staged data was read. This value will
+ *	serve as the upper bound for spreading
+ * @count: The total number of motion_sense_fifo entries that are currently
+ *	staged.
+ * @sample_count: The total number of sensor readings per sensor that are
+ *	currently staged.
+ * @requires_spreading: Flag used to shortcut the commit process. This should be
+ *	true iff at least one of sample_count[] > 1
+ */
+struct fifo_staged {
+	uint32_t read_ts;
+	uint16_t count;
+	uint8_t sample_count[SENSOR_COUNT];
+	uint8_t requires_spreading;
+};
+static struct fifo_staged fifo_staged;
+
+static inline struct ec_response_motion_sensor_data *
+get_motion_sense_fifo_head(void)
+{
+	return ((struct ec_response_motion_sensor_data *)
+		motion_sense_fifo.buffer) +
+	       (motion_sense_fifo.state->head &
+		motion_sense_fifo.unit_bytes);
+}
+
+/**
+ * Pop one entry from the motion sense fifo. Poping will give priority to
+ * committed data (data residing between the head and tail of the queue). If no
+ * committed data is available (all the data is staged), then this function will
+ * remove the oldest staged data by moving both the head and tail.
+ *
+ * As a side-effect of this function, it'll updated any appropriate lost and
+ * count variables.
+ *
+ * WARNING: This function MUST be called from within a locked context of
+ * g_sensor_mutex.
+ */
+static void motion_sense_fifo_pop(void)
+{
+	struct ec_response_motion_sensor_data *head =
+		get_motion_sense_fifo_head();
+	const size_t initial_count = queue_count(&motion_sense_fifo);
+
+	/* Check that we have something to pop. */
+	if (!initial_count && !fifo_staged.count)
+		return;
+
+	/*
+	 * If all the data is staged (nothing in the committed queue), we'll
+	 * need to move the head and the tail over to simulate poping from the
+	 * staged data.
+	 */
+	if (!initial_count)
+		queue_advance_tail(&motion_sense_fifo, 1);
+
+	/*
+	 * By not using queue_remove_unit we're avoiding an un-necessary memcpy.
+	 */
+	queue_advance_head(&motion_sense_fifo, 1);
+	motion_sense_fifo_lost++;
+
+	/* Increment lost counter if we have valid data. */
+	if (!is_timestamp(head))
+		motion_sensors[head->sensor_num].lost++;
+
+	/*
+	 * We're done if the initial count was non-zero and we only advanced the
+	 * head. Else, decrement the staged count and update staged metadata.
+	 */
+	if (initial_count)
+		return;
+
+	fifo_staged.count--;
+
+	/* If we removed a timestamp there's nothing else for us to do. */
+	if (is_timestamp(head))
+		return;
+
+	/*
+	 * Decrement sample count, if the count was 2 before, we might not need
+	 * to spread anymore. Loop through and check.
+	 */
+	if (--fifo_staged.sample_count[head->sensor_num] < 2) {
+		int i;
+
+		fifo_staged.requires_spreading = 0;
+		for (i = 0; i < SENSOR_COUNT; i++) {
+			if (fifo_staged.sample_count[i] > 1) {
+				fifo_staged.requires_spreading = 1;
+				break;
+			}
+		}
+	}
+}
+
+static void motion_sense_fifo_ensure_space(void)
+{
+	/* If we already have space just bail. */
+	if (queue_space(&motion_sense_fifo) > fifo_staged.count)
+		return;
+
+	/*
+	 * Pop at least 1 spot, but if all the following conditions are met we
+	 * will continue to pop:
+	 * 1. We're operating with tight timestamps.
+	 * 2. The new head isn't a timestamp.
+	 * 3. We have data that we can possibly pop.
+	 *
+	 * Removing more than one entry is needed because if we are using tight
+	 * timestamps and we pop a timestamp, then the next head is data, the AP
+	 * would assign a bad timestamp to it.
+	 */
+	do {
+		motion_sense_fifo_pop();
+	} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
+		 !is_timestamp(get_motion_sense_fifo_head()) &&
+		 queue_count(&motion_sense_fifo) + fifo_staged.count);
+}
+
+/*
+ * Do not use this function directly if you just want to add sensor data, use
+ * motion_sense_fifo_stage_data instead to get a proper timestamp too.
+ */
+static void motion_sense_fifo_stage_unit(
+	struct ec_response_motion_sensor_data *data,
+	struct motion_sensor_t *sensor,
+	int valid_data)
+{
+	struct queue_chunk chunk;
+	int i;
+
+	mutex_lock(&g_sensor_mutex);
+
+	for (i = 0; i < valid_data; i++)
+		sensor->xyz[i] = data->data[i];
+
+	/* For valid sensors, check if AP really needs this data */
+	if (valid_data) {
+		int removed;
+
+		if (sensor->oversampling_ratio == 0) {
+			mutex_unlock(&g_sensor_mutex);
+			return;
+		}
+		removed = sensor->oversampling++;
+		sensor->oversampling %= sensor->oversampling_ratio;
+		if (removed != 0) {
+			mutex_unlock(&g_sensor_mutex);
+			return;
+		}
+	}
+
+	/* Make sure we have room for the data */
+	motion_sense_fifo_ensure_space();
+	mutex_unlock(&g_sensor_mutex);
+
+	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
+		wake_up_needed = 1;
+	if (IS_ENABLED(CONFIG_TABLET_MODE))
+		data->flags |= (tablet_get_mode() ?
+			MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
+
+	/*
+	 * Get the next writable block in the fifo. We don't need to lock this
+	 * because it will always be past the tail and thus the AP will never
+	 * read this until motion_sense_fifo_commit_data() is called.
+	 */
+	chunk = queue_get_write_chunk(
+		&motion_sense_fifo, fifo_staged.count);
+
+	if (!chunk.buffer) {
+		/*
+		 * This should never happen since we already ensured there was
+		 * space, but if there was a bug, we don't want to write to
+		 * address 0. Just don't add any data to the queue instead.
+		 */
+		CPRINTS("Failed to get write chunk for new fifo data!");
+		return;
+	}
+
+	/*
+	 * Save the data to the writable block and increment count. This data
+	 * will now reside AFTER the tail of the queue and will not be visible
+	 * to the AP until the motion_sense_fifo_commit_data() function is
+	 * called. Because count is incremented, the following staged data will
+	 * be written to the next available block and this one will remain
+	 * staged.
+	 */
+	memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
+	fifo_staged.count++;
+
+	/*
+	 * If we're using tight timestamps, and the current entry isn't a
+	 * timestamp we'll increment the sample_count for the given sensor.
+	 * If the new per-sensor sample count is greater than 1, we'll need to
+	 * spread.
+	 */
+	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
+	    !is_timestamp(data) &&
+	    ++fifo_staged.sample_count[data->sensor_num] > 1)
+		fifo_staged.requires_spreading = 1;
+}
+
+void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
+				     enum motion_sense_async_event evt)
+{
+	struct ec_response_motion_sensor_data vector;
+
+	vector.flags = evt;
+	vector.timestamp = __hw_clock_source_read();
+	vector.sensor_num = sensor - motion_sensors;
+
+	motion_sense_fifo_stage_unit(&vector, sensor, 0);
+	motion_sense_fifo_commit_data();
+}
+
+void motion_sense_fifo_stage_timestamp(uint32_t timestamp)
+{
+	struct ec_response_motion_sensor_data vector;
+
+	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+	vector.timestamp = timestamp;
+	vector.sensor_num = 0;
+	motion_sense_fifo_stage_unit(&vector, NULL, 0);
+}
+
+void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
+				  struct motion_sensor_t *sensor,
+				  int valid_data,
+				  uint32_t time)
+{
+	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS)) {
+		/* First entry, save the time for spreading later. */
+		if (!fifo_staged.count)
+			fifo_staged.read_ts = __hw_clock_source_read();
+		motion_sense_fifo_stage_timestamp(time);
+	}
+	motion_sense_fifo_stage_unit(data, sensor, valid_data);
+}
+
+/**
+ * Peek into the staged data at a given offset. This function performs no bound
+ * checking and is purely for convenience.
+ */
+static inline struct ec_response_motion_sensor_data *
+motion_sense_peek_fifo_staged(size_t offset)
+{
+	return (struct ec_response_motion_sensor_data *)
+	       queue_get_write_chunk(&motion_sense_fifo, offset).buffer;
+}
+
+void motion_sense_fifo_commit_data(void)
+{
+	/*
+	 * Static data to use off stack. Note that next_timestamp should persist
+	 * and is only updated if the timestamp from the sensor is greater.
+	 */
+	static uint32_t data_periods[SENSOR_COUNT];
+	static uint32_t next_timestamp[SENSOR_COUNT];
+	struct ec_response_motion_sensor_data *data;
+	int i, window, sensor_num;
+
+	/* Nothing staged, no work to do. */
+	if (!fifo_staged.count)
+		return;
+
+	/*
+	 * If per-sensor event counts are never more than 1, no spreading is
+	 * needed. This will also catch cases where tight timestamps aren't
+	 * used.
+	 */
+	if (!fifo_staged.requires_spreading)
+		goto flush_data_end;
+
+	data = motion_sense_peek_fifo_staged(0);
+
+	/*
+	 * Spreading only makes sense if tight timestamps are used. In such case
+	 * entries are expected to be ordered: timestamp then data. If the first
+	 * entry isn't a timestamp we must have gotten out of sync. Just commit
+	 * all the data and skip the spreading.
+	 */
+	if (!is_timestamp(data)) {
+		CPRINTS("Spreading skipped, first entry is not a timestamp");
+		goto flush_data_end;
+	}
+
+	window = time_until(data->timestamp, fifo_staged.read_ts);
+
+	/* Update the data_periods as needed for this flush. */
+	for (i = 0; i < SENSOR_COUNT; i++) {
+		int period;
+
+		/* Skip empty sensors. */
+		if (!fifo_staged.sample_count[i])
+			continue;
+
+		period = motion_sensors[i].collection_rate;
+		/*
+		 * Clamp the sample period to the MIN of collection_rate and the
+		 * window length / sample counts.
+		 */
+		if (window)
+			period = MIN(period,
+				     window / fifo_staged.sample_count[i]);
+		data_periods[i] = period;
+	}
+
+	/*
+	 * Spread the timestamps.
+	 *
+	 * If we got this far that means that the tight timestamps config is
+	 * enabled. This means that we can expect the staged entries to have 1
+	 * or more timestamps followed by exactly 1 data entry. We'll loop
+	 * through the timestamps until we get to data. We only need to update
+	 * the timestamp right before it to keep things correct.
+	 */
+	for (i = 0; i < fifo_staged.count; i++) {
+		data = motion_sense_peek_fifo_staged(i);
+
+		/* Skip timestamp, we don't know the sensor number yet. */
+		if (is_timestamp(data))
+			continue;
+
+		/* Get the sensor number and point to the timestamp entry. */
+		sensor_num = data->sensor_num;
+		data = motion_sense_peek_fifo_staged(i - 1);
+
+		/* If the timestamp is after our computed next, skip ahead. */
+		if (time_after(data->timestamp, next_timestamp[sensor_num]))
+			next_timestamp[sensor_num] = data->timestamp;
+
+		/* Spread the timestamp and compute the expected next. */
+		data->timestamp = next_timestamp[sensor_num];
+		next_timestamp[sensor_num] += data_periods[sensor_num];
+	}
+
+flush_data_end:
+	/* Advance the tail and clear the staged metadata. */
+	mutex_lock(&g_sensor_mutex);
+	queue_advance_tail(&motion_sense_fifo, fifo_staged.count);
+	mutex_unlock(&g_sensor_mutex);
+
+	/* Reset metadata for next staging cycle. */
+	memset(&fifo_staged, 0, sizeof(fifo_staged));
+}
+
+void motion_sense_get_fifo_info(
+	struct ec_response_motion_sense_fifo_info *fifo_info)
+{
+	fifo_info->size = motion_sense_fifo.buffer_units;
+	mutex_lock(&g_sensor_mutex);
+	fifo_info->count = fifo_queue_count;
+	fifo_info->total_lost = motion_sense_fifo_lost;
+	mutex_unlock(&g_sensor_mutex);
+	fifo_info->timestamp = mkbp_last_event_time;
+}
+
+static int motion_sense_get_next_event(uint8_t *out)
+{
+	union ec_response_get_next_data *data =
+		(union ec_response_get_next_data *)out;
+	/* out is not padded. It has one byte for the event type */
+	motion_sense_get_fifo_info(&data->sensor_fifo.info);
+	return sizeof(data->sensor_fifo);
+}
+
+DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event);
+
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 5fb79194d1..6408bfcc79 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -17,6 +17,7 @@
 #include "hwtimer.h"
 #include "i2c.h"
 #include "math_util.h"
+#include "motion_sense_fifo.h"
 #include "spi.h"
 #include "task.h"
 #include "timer.h"
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index e2bba975d6..1a098f3b1e 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -15,6 +15,7 @@
 #include "hwtimer.h"
 #include "mag_cal.h"
 #include "math_util.h"
+#include "motion_sense_fifo.h"
 #include "queue.h"
 #include "task.h"
 #include "timer.h"
diff --git a/driver/als_si114x.c b/driver/als_si114x.c
index 2ac8f1ccbd..4db3fcc9d7 100644
--- a/driver/als_si114x.c
+++ b/driver/als_si114x.c
@@ -14,6 +14,7 @@
 #include "hwtimer.h"
 #include "i2c.h"
 #include "math_util.h"
+#include "motion_sense_fifo.h"
 #include "task.h"
 #include "timer.h"
 #include "util.h"
diff --git a/driver/als_tcs3400.c b/driver/als_tcs3400.c
index 6b58735ad2..774efd863c 100644
--- a/driver/als_tcs3400.c
+++ b/driver/als_tcs3400.c
@@ -12,6 +12,7 @@
 #include "hwtimer.h"
 #include "i2c.h"
 #include "math_util.h"
+#include "motion_sense_fifo.h"
 #include "task.h"
 #include "util.h"
 
diff --git a/driver/sync.c b/driver/sync.c
index 34df824090..91d161765a 100644
--- a/driver/sync.c
+++ b/driver/sync.c
@@ -12,6 +12,7 @@
 #include "console.h"
 #include "driver/sync.h"
 #include "hwtimer.h"
+#include "motion_sense_fifo.h"
 #include "queue.h"
 #include "task.h"
 #include "util.h"
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 8c3f0cdbde..b0e5244e3b 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -227,6 +227,14 @@ struct motion_sensor_t {
 	uint32_t max_frequency;
 };
 
+/*
+ * Mutex to protect sensor values between host command task and
+ * motion sense task:
+ * When we process CMD_DUMP, we want to be sure the motion sense
+ * task is not updating the sensor values at the same time.
+ */
+extern struct mutex g_sensor_mutex;
+
 /* Defined at board level. */
 extern struct motion_sensor_t motion_sensors[];
 
@@ -249,32 +257,6 @@ extern unsigned int motion_min_interval;
  */
 #define MOTION_SENSE_HOOK_PRIO (HOOK_PRIO_DEFAULT)
 
-#ifdef CONFIG_ACCEL_FIFO
-extern struct queue motion_sense_fifo;
-
-/**
- * Stage data to the fifo, including a timestamp. This data will not be
- * available to the AP until motion_sense_fifo_commit_data is called.
- *
- * @param data data to insert in the FIFO
- * @param sensor sensor the data comes from
- * @param valid_data data should be copied into the public sensor vector
- * @param time accurate time (ideally measured in an interrupt) the sample
- *             was taken at
- */
-void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
-				  struct motion_sensor_t *sensor,
-				  int valid_data,
-				  uint32_t time);
-
-/**
- * Commits all staged data to the fifo. If multiple readings were placed using
- * the same timestamps, they will be spread out.
- */
-void motion_sense_fifo_commit_data(void);
-
-#endif
-
 /**
  * Take actions at end of sensor initialization:
  * - print init done status to console,
diff --git a/include/motion_sense_fifo.h b/include/motion_sense_fifo.h
new file mode 100644
index 0000000000..f0d5f853dd
--- /dev/null
+++ b/include/motion_sense_fifo.h
@@ -0,0 +1,70 @@
+/* Copyright 2019 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef __CROS_EC_MOTION_SENSE_FIFO_H
+#define __CROS_EC_MOTION_SENSE_FIFO_H
+
+#include "motion_sense.h"
+#include "task.h"
+
+extern struct queue motion_sense_fifo;
+extern int wake_up_needed;
+extern int fifo_int_enabled;
+extern int fifo_queue_count;
+extern int motion_sense_fifo_lost;
+
+enum motion_sense_async_event {
+	ASYNC_EVENT_FLUSH = MOTIONSENSE_SENSOR_FLAG_FLUSH |
+			    MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
+	ASYNC_EVENT_ODR =   MOTIONSENSE_SENSOR_FLAG_ODR |
+			    MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
+};
+
+/**
+ * Stage data to the fifo, including a timestamp. This data will not be
+ * available to the AP until motion_sense_fifo_commit_data is called.
+ *
+ * @param data data to insert in the FIFO
+ * @param sensor sensor the data comes from
+ * @param valid_data data should be copied into the public sensor vector
+ * @param time accurate time (ideally measured in an interrupt) the sample
+ *             was taken at
+ */
+void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
+				  struct motion_sensor_t *sensor,
+				  int valid_data,
+				  uint32_t time);
+
+/**
+ * Commits all staged data to the fifo. If multiple readings were placed using
+ * the same timestamps, they will be spread out.
+ */
+void motion_sense_fifo_commit_data(void);
+
+/**
+ * Insert an async event into the fifo.
+ *
+ * @param sensor Pointer to the sensor generating the event.
+ * @param evt The event to insert.
+ */
+void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
+				     enum motion_sense_async_event evt);
+
+/**
+ * Stage a timestamp into the fifo.
+ *
+ * @param timestamp The timestamp to stage.
+ */
+void motion_sense_fifo_stage_timestamp(uint32_t timestamp);
+
+/**
+ * Get information about the fifo.
+ *
+ * @param fifo_info The struct to store the info.
+ */
+void motion_sense_get_fifo_info(
+	struct ec_response_motion_sense_fifo_info *fifo_info);
+
+#endif /* __CROS_EC_MOTION_SENSE_FIFO_H */