Reland "common: Refactor motion_sense_fifo"

This is a reland of 2f6bf961911c87ce7d2007b6695f3885155ad431

Original change's description:
> common: Refactor motion_sense_fifo
> 
> This change refactors the motion_sense_fifo to uniformly prefix
> all the functions to avoid collisions. It also adds several unit
> tests and fixes a few bugs with the fifo logic.
> 
> BUG=b:137758297
> BRANCH=kukui
> TEST=buildall
> TEST=run CTS on arcada, kohaku, and kukui
> TEST=boot kohaku (verify tablet mode works as expected)
> 
> Change-Id: I6e8492ae5fa474d0aa870088ab56f76b220a73e3
> Signed-off-by: Yuval Peress <peress@chromium.org>
> Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1835221
> Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
> (cherry picked from commit 083ced83d71a6ab60204c7ada6c22d9c82731713)
> Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1910376
> Reviewed-by: Eric Yilun Lin <yllin@chromium.org>
> Commit-Queue: Eric Yilun Lin <yllin@chromium.org>
> Tested-by: Eric Yilun Lin <yllin@chromium.org>

Bug: b:137758297
Change-Id: I5d16133deb13e9433ad3868e9bce5655fd8d61b6
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1928412
Reviewed-by: Eric Yilun Lin <yllin@chromium.org>
Commit-Queue: Eric Yilun Lin <yllin@chromium.org>
Tested-by: Eric Yilun Lin <yllin@chromium.org>

8 files changed, 691 insertions, 177 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 96c557abd5..ce8c3588ca 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -78,6 +78,9 @@ 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;
 
+/* Whether or not the FIFO interrupt should be enabled (set from the AP). */
+__maybe_unused static int fifo_int_enabled;
+
 static inline int motion_sensor_in_forced_mode(
 		const struct motion_sensor_t *sensor)
 {
@@ -680,7 +683,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		int flush_pending = atomic_read_clear(&sensor->flush_pending);
 
 		for (; flush_pending > 0; flush_pending--) {
-			motion_sense_insert_async_event(
+			motion_sense_fifo_insert_async_event(
 				sensor, ASYNC_EVENT_FLUSH);
 		}
 	}
@@ -690,7 +693,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		motion_sense_set_data_rate(sensor);
 		motion_sense_set_motion_intervals();
 		if (IS_ENABLED(CONFIG_ACCEL_FIFO))
-			motion_sense_insert_async_event(
+			motion_sense_fifo_insert_async_event(
 				sensor, ASYNC_EVENT_ODR);
 	}
 	return ret;
@@ -905,25 +908,18 @@ void motion_sense_task(void *u)
 		 * - we haven't done it for a while.
 		 */
 		if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
-		    (motion_sense_fifo_is_wake_up_needed() ||
+		    (motion_sense_fifo_wake_up_needed() ||
 		     event & (TASK_EVENT_MOTION_ODR_CHANGE |
 			      TASK_EVENT_MOTION_FLUSH_PENDING) ||
+		     motion_sense_fifo_over_thres() ||
 		     (ap_event_interval > 0 &&
 		      time_after(ts_begin_task.le.lo,
 				 ts_last_int.le.lo + ap_event_interval)))) {
 			if ((event & TASK_EVENT_MOTION_FLUSH_PENDING) == 0) {
-				motion_sense_fifo_stage_timestamp(
+				motion_sense_fifo_add_timestamp(
 					__hw_clock_source_read());
-				motion_sense_fifo_commit_data();
 			}
 			ts_last_int = ts_begin_task;
-			/*
-			 * Count the number of event the AP is allowed to
-			 * collect.
-			 */
-			mutex_lock(&g_sensor_mutex);
-			fifo_queue_count = queue_count(&motion_sense_fifo);
-			mutex_unlock(&g_sensor_mutex);
 #ifdef CONFIG_MKBP_EVENT
 			/*
 			 * Send an event if we know we are in S0 and the kernel
@@ -933,9 +929,9 @@ void motion_sense_task(void *u)
 			 */
 			if ((fifo_int_enabled &&
 			     sensor_active == SENSOR_ACTIVE_S0) ||
-			    wake_up_needed) {
+			    motion_sense_fifo_wake_up_needed()) {
 				mkbp_send_event(EC_MKBP_EVENT_SENSOR_FIFO);
-				wake_up_needed = 0;
+				motion_sense_fifo_reset_wake_up_needed();
 			}
 #endif /* CONFIG_MKBP_EVENT */
 		}
@@ -1270,12 +1266,11 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			args->response_size = sizeof(out->fifo_info);
 			break;
 		}
-		motion_sense_get_fifo_info(&out->fifo_info);
+		motion_sense_fifo_get_info(&out->fifo_info, 1);
 		for (i = 0; i < motion_sensor_count; i++) {
 			out->fifo_info.lost[i] = motion_sensors[i].lost;
 			motion_sensors[i].lost = 0;
 		}
-		motion_sense_fifo_lost = 0;
 		args->response_size = sizeof(out->fifo_info) +
 			sizeof(uint16_t) * motion_sensor_count;
 		break;
@@ -1283,17 +1278,12 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	case MOTIONSENSE_CMD_FIFO_READ:
 		if (!IS_ENABLED(CONFIG_ACCEL_FIFO))
 			return EC_RES_INVALID_PARAM;
-		mutex_lock(&g_sensor_mutex);
-		reported = MIN((args->response_max - sizeof(out->fifo_read)) /
-			       motion_sense_fifo.unit_bytes,
-			       MIN(queue_count(&motion_sense_fifo),
-				   in->fifo_read.max_data_vector));
-		reported = queue_remove_units(&motion_sense_fifo,
-				out->fifo_read.data, reported);
-		mutex_unlock(&g_sensor_mutex);
-		out->fifo_read.number_data = reported;
-		args->response_size = sizeof(out->fifo_read) + reported *
-			motion_sense_fifo.unit_bytes;
+		out->fifo_read.number_data = motion_sense_fifo_read(
+			args->response_max - sizeof(out->fifo_read),
+			in->fifo_read.max_data_vector,
+			out->fifo_read.data,
+			&(args->response_size));
+		args->response_size += sizeof(out->fifo_read);
 		break;
 	case MOTIONSENSE_CMD_FIFO_INT_ENABLE:
 		if (!IS_ENABLED(CONFIG_ACCEL_FIFO))
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index ec4c63b6de..bdf39a857c 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -7,30 +7,14 @@
 #include "hwtimer.h"
 #include "mkbp_event.h"
 #include "motion_sense_fifo.h"
-#include "queue.h"
 #include "tablet_mode.h"
+#include "task.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_SIZE, struct ec_response_motion_sensor_data);
-int motion_sense_fifo_lost;
-
 /**
- * Staged metadata for the motion_sense_fifo.
+ * Staged metadata for the fifo queue.
  * @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
@@ -46,15 +30,61 @@ struct fifo_staged {
 	uint8_t sample_count[SENSOR_COUNT];
 	uint8_t requires_spreading;
 };
+
+/**
+ * Timestamp state metadata for maintaining spreading between commits.
+ * @prev: The previous timestamp that was added to the FIFO
+ * @next: The predicted next timestamp that will be added to the FIFO
+ */
+struct timestamp_state {
+	uint32_t prev;
+	uint32_t next;
+};
+
+/** Queue to hold the data to be sent to the AP. */
+static struct queue fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO_SIZE,
+				      struct ec_response_motion_sensor_data);
+/** Count of the number of entries lost due to a small queue. */
+static int fifo_lost;
+/** Metadata for the fifo, used for staging and spreading data. */
 static struct fifo_staged fifo_staged;
 
-static inline struct ec_response_motion_sensor_data *
-get_motion_sense_fifo_head(void)
+/**
+ * Cached expected timestamp per sensor. If a sensor's timestamp pre-dates this
+ * timestamp it will be fast forwarded.
+ */
+static struct timestamp_state next_timestamp[SENSOR_COUNT];
+
+/**
+ * Bitmap telling which sensors have valid entries in the next_timestamp array.
+ */
+static uint32_t next_timestamp_initialized;
+
+/** Need to wake up the AP. */
+static int wake_up_needed;
+
+/**
+ * Check whether or not a give sensor data entry is a timestamp or not.
+ *
+ * @param data The data entry to check.
+ * @return 1 if the entry is a timestamp, 0 otherwise.
+ */
+static inline int is_timestamp(
+	const struct ec_response_motion_sensor_data *data)
+{
+	return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+}
+
+/**
+ * Convenience function to get the head of the fifo. This function makes no
+ * guarantee on whether or not the entry is valid.
+ *
+ * @return Pointer to the head of the fifo.
+ */
+static inline struct ec_response_motion_sensor_data *get_fifo_head(void)
 {
-	return ((struct ec_response_motion_sensor_data *)
-		motion_sense_fifo.buffer) +
-	       (motion_sense_fifo.state->head &
-		motion_sense_fifo.unit_bytes);
+	return ((struct ec_response_motion_sensor_data *) fifo.buffer) +
+		(fifo.state->head & fifo.buffer_units_mask);
 }
 
 /**
@@ -69,11 +99,10 @@ get_motion_sense_fifo_head(void)
  * WARNING: This function MUST be called from within a locked context of
  * g_sensor_mutex.
  */
-static void motion_sense_fifo_pop(void)
+static void 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);
+	struct ec_response_motion_sensor_data *head = get_fifo_head();
+	const size_t initial_count = queue_count(&fifo);
 
 	/* Check that we have something to pop. */
 	if (!initial_count && !fifo_staged.count)
@@ -85,13 +114,19 @@ static void motion_sense_fifo_pop(void)
 	 * staged data.
 	 */
 	if (!initial_count)
-		queue_advance_tail(&motion_sense_fifo, 1);
+		queue_advance_tail(&fifo, 1);
 
 	/*
+	 * If we're about to pop a wakeup flag, we should remember it as though
+	 * it was committed.
+	 */
+	if (head->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
+		wake_up_needed = 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++;
+	queue_advance_head(&fifo, 1);
+	fifo_lost++;
 
 	/* Increment lost counter if we have valid data. */
 	if (!is_timestamp(head))
@@ -127,10 +162,13 @@ static void motion_sense_fifo_pop(void)
 	}
 }
 
-static void motion_sense_fifo_ensure_space(void)
+/**
+ * Make sure that the fifo has at least 1 empty spot to stage data into.
+ */
+static void fifo_ensure_space(void)
 {
 	/* If we already have space just bail. */
-	if (queue_space(&motion_sense_fifo) > fifo_staged.count)
+	if (queue_space(&fifo) > fifo_staged.count)
 		return;
 
 	/*
@@ -145,17 +183,21 @@ static void motion_sense_fifo_ensure_space(void)
 	 * would assign a bad timestamp to it.
 	 */
 	do {
-		motion_sense_fifo_pop();
+		fifo_pop();
 	} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
-		 !is_timestamp(get_motion_sense_fifo_head()) &&
-		 queue_count(&motion_sense_fifo) + fifo_staged.count);
+		 !is_timestamp(get_fifo_head()) &&
+		 queue_count(&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.
+/**
+ * Stage a single data unit to the motion sense fifo. Note that for the AP to
+ * see this data, it must be committed.
+ *
+ * @param data The data to stage.
+ * @param sensor The sensor that generated the data
+ * @param valid_data The number of readable data entries in the data.
  */
-static void motion_sense_fifo_stage_unit(
+static void fifo_stage_unit(
 	struct ec_response_motion_sensor_data *data,
 	struct motion_sensor_t *sensor,
 	int valid_data)
@@ -172,35 +214,27 @@ static void motion_sense_fifo_stage_unit(
 	if (valid_data) {
 		int removed;
 
-		if (sensor->oversampling_ratio == 0) {
-			mutex_unlock(&g_sensor_mutex);
-			return;
-		}
+		if (sensor->oversampling_ratio == 0)
+			goto stage_unit_end;
 		removed = sensor->oversampling++;
 		sensor->oversampling %= sensor->oversampling_ratio;
-		if (removed != 0) {
-			mutex_unlock(&g_sensor_mutex);
-			return;
-		}
+		if (removed)
+			goto stage_unit_end;
 	}
 
 	/* Make sure we have room for the data */
-	motion_sense_fifo_ensure_space();
-	mutex_unlock(&g_sensor_mutex);
+	fifo_ensure_space();
 
-	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);
+				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);
+	chunk = queue_get_write_chunk(&fifo, fifo_staged.count);
 
 	if (!chunk.buffer) {
 		/*
@@ -209,7 +243,7 @@ static void motion_sense_fifo_stage_unit(
 		 * address 0. Just don't add any data to the queue instead.
 		 */
 		CPRINTS("Failed to get write chunk for new fifo data!");
-		return;
+		goto stage_unit_end;
 	}
 
 	/*
@@ -220,7 +254,7 @@ static void motion_sense_fifo_stage_unit(
 	 * be written to the next available block and this one will remain
 	 * staged.
 	 */
-	memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
+	memcpy(chunk.buffer, data, fifo.unit_bytes);
 	fifo_staged.count++;
 
 	/*
@@ -233,64 +267,96 @@ static void motion_sense_fifo_stage_unit(
 	    !is_timestamp(data) &&
 	    ++fifo_staged.sample_count[data->sensor_num] > 1)
 		fifo_staged.requires_spreading = 1;
+
+stage_unit_end:
+	mutex_unlock(&g_sensor_mutex);
 }
 
-void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
-				     enum motion_sense_async_event evt)
+/**
+ * Stage an entry representing a single timestamp.
+ *
+ * @param timestamp The timestamp to add to the fifo.
+ */
+static void fifo_stage_timestamp(uint32_t timestamp)
 {
 	struct ec_response_motion_sensor_data vector;
 
-	vector.flags = evt;
+	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+	vector.timestamp = timestamp;
+	vector.sensor_num = 0;
+	fifo_stage_unit(&vector, NULL, 0);
+}
+
+/**
+ * Peek into the staged data at a given offset. This function performs no bound
+ * checking and is purely for confinience.
+ *
+ * @param offset The offset into the staged data to peek into.
+ * @return Pointer to the entry at the given offset.
+ */
+static inline struct ec_response_motion_sensor_data *
+peek_fifo_staged(size_t offset)
+{
+	return (struct ec_response_motion_sensor_data *)
+		queue_get_write_chunk(&fifo, offset).buffer;
+}
+
+int motion_sense_fifo_wake_up_needed(void)
+{
+	int res;
+
+	mutex_lock(&g_sensor_mutex);
+	res = wake_up_needed;
+	mutex_unlock(&g_sensor_mutex);
+	return res;
+}
+
+void motion_sense_fifo_reset_wake_up_needed(void)
+{
+	mutex_lock(&g_sensor_mutex);
+	wake_up_needed = 0;
+	mutex_unlock(&g_sensor_mutex);
+}
+
+void motion_sense_fifo_insert_async_event(
+	struct motion_sensor_t *sensor,
+	enum motion_sense_async_event event)
+{
+	struct ec_response_motion_sensor_data vector;
+
+	vector.flags = event;
 	vector.timestamp = __hw_clock_source_read();
 	vector.sensor_num = sensor - motion_sensors;
 
-	motion_sense_fifo_stage_unit(&vector, sensor, 0);
+	fifo_stage_unit(&vector, sensor, 0);
 	motion_sense_fifo_commit_data();
 }
 
-void motion_sense_fifo_stage_timestamp(uint32_t timestamp)
+inline void motion_sense_fifo_add_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);
+	fifo_stage_timestamp(timestamp);
+	motion_sense_fifo_commit_data();
 }
 
-void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
-				  struct motion_sensor_t *sensor,
-				  int valid_data,
-				  uint32_t time)
+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);
+		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;
+	fifo_stage_unit(data, sensor, valid_data);
 }
 
 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.
-	 */
+	/* Cached data periods, static to store off stack. */
 	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;
 
@@ -298,15 +364,16 @@ void motion_sense_fifo_commit_data(void)
 	if (!fifo_staged.count)
 		return;
 
+	mutex_lock(&g_sensor_mutex);
 	/*
 	 * 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;
+		goto commit_data_end;
 
-	data = motion_sense_peek_fifo_staged(0);
+	data = peek_fifo_staged(0);
 
 	/*
 	 * Spreading only makes sense if tight timestamps are used. In such case
@@ -316,7 +383,8 @@ void motion_sense_fifo_commit_data(void)
 	 */
 	if (!is_timestamp(data)) {
 		CPRINTS("Spreading skipped, first entry is not a timestamp");
-		goto flush_data_end;
+		fifo_staged.requires_spreading = 0;
+		goto commit_data_end;
 	}
 
 	window = time_until(data->timestamp, fifo_staged.read_ts);
@@ -332,16 +400,18 @@ void motion_sense_fifo_commit_data(void)
 		period = motion_sensors[i].collection_rate;
 		/*
 		 * Clamp the sample period to the MIN of collection_rate and the
-		 * window length / sample counts.
+		 * window length / (sample count - 1).
 		 */
-		if (window)
-			period = MIN(period,
-				     window / fifo_staged.sample_count[i]);
+		if (window && fifo_staged.sample_count[i] > 1)
+			period = MIN(
+				period,
+				window / (fifo_staged.sample_count[i] - 1));
 		data_periods[i] = period;
 	}
 
+commit_data_end:
 	/*
-	 * Spread the timestamps.
+	 * Conditionally 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
@@ -350,7 +420,9 @@ void motion_sense_fifo_commit_data(void)
 	 * the timestamp right before it to keep things correct.
 	 */
 	for (i = 0; i < fifo_staged.count; i++) {
-		data = motion_sense_peek_fifo_staged(i);
+		data = peek_fifo_staged(i);
+		if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
+			wake_up_needed = 1;
 
 		/* Skip timestamp, we don't know the sensor number yet. */
 		if (is_timestamp(data))
@@ -358,36 +430,52 @@ void motion_sense_fifo_commit_data(void)
 
 		/* Get the sensor number and point to the timestamp entry. */
 		sensor_num = data->sensor_num;
-		data = motion_sense_peek_fifo_staged(i - 1);
+		data = 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;
+		/*
+		 * If this is the first time we're seeing a timestamp for this
+		 * sensor or the timestamp is after our computed next, skip
+		 * ahead.
+		 */
+		if (!(next_timestamp_initialized & BIT(sensor_num)) ||
+		    time_after(data->timestamp,
+			       next_timestamp[sensor_num].prev)) {
+			next_timestamp[sensor_num].next = data->timestamp;
+			next_timestamp_initialized |= BIT(sensor_num);
+		}
 
 		/* Spread the timestamp and compute the expected next. */
-		data->timestamp = next_timestamp[sensor_num];
-		next_timestamp[sensor_num] += data_periods[sensor_num];
+		data->timestamp = next_timestamp[sensor_num].next;
+		next_timestamp[sensor_num].prev =
+			next_timestamp[sensor_num].next;
+		next_timestamp[sensor_num].next +=
+			fifo_staged.requires_spreading
+			? data_periods[sensor_num]
+			: motion_sensors[sensor_num].collection_rate;
 	}
 
-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);
+	queue_advance_tail(&fifo, fifo_staged.count);
 
 	/* Reset metadata for next staging cycle. */
 	memset(&fifo_staged, 0, sizeof(fifo_staged));
+
+	mutex_unlock(&g_sensor_mutex);
 }
 
-void motion_sense_get_fifo_info(
-	struct ec_response_motion_sense_fifo_info *fifo_info)
+void motion_sense_fifo_get_info(
+	struct ec_response_motion_sense_fifo_info *fifo_info,
+	int reset)
 {
-	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;
+	fifo_info->size = fifo.buffer_units;
+	fifo_info->count = queue_count(&fifo);
+	fifo_info->total_lost = fifo_lost;
 	mutex_unlock(&g_sensor_mutex);
 	fifo_info->timestamp = mkbp_last_event_time;
+
+	if (reset)
+		fifo_lost = 0;
 }
 
 static int motion_sense_get_next_event(uint8_t *out)
@@ -395,14 +483,41 @@ 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);
+	motion_sense_fifo_get_info(&data->sensor_fifo.info, 0);
 	return sizeof(data->sensor_fifo);
 }
 
 DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event);
 
-inline int motion_sense_fifo_is_wake_up_needed(void)
+inline int motion_sense_fifo_over_thres(void)
 {
-	return queue_space(&motion_sense_fifo) < CONFIG_ACCEL_FIFO_THRES ||
-		wake_up_needed;
+	int result;
+
+	mutex_lock(&g_sensor_mutex);
+	result = queue_space(&fifo) < CONFIG_ACCEL_FIFO_THRES;
+	mutex_unlock(&g_sensor_mutex);
+
+	return result;
+}
+
+int motion_sense_fifo_read(int capacity_bytes, int max_count, void *out,
+			   uint16_t *out_size)
+{
+	int count;
+
+	mutex_lock(&g_sensor_mutex);
+	count = MIN(capacity_bytes / fifo.unit_bytes,
+		    MIN(queue_count(&fifo), max_count));
+	count = queue_remove_units(&fifo, out, count);
+	mutex_unlock(&g_sensor_mutex);
+	*out_size = count * fifo.unit_bytes;
+
+	return count;
+}
+
+void motion_sense_fifo_reset(void)
+{
+	next_timestamp_initialized = 0;
+	memset(&fifo_staged, 0, sizeof(fifo_staged));
+	queue_init(&fifo);
 }
diff --git a/include/motion_sense_fifo.h b/include/motion_sense_fifo.h
index 6b306b5f29..e5d8566ed1 100644
--- a/include/motion_sense_fifo.h
+++ b/include/motion_sense_fifo.h
@@ -7,14 +7,8 @@
 #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;
 
+/** Allowed async events. */
 enum motion_sense_async_event {
 	ASYNC_EVENT_FLUSH = MOTIONSENSE_SENSOR_FLAG_FLUSH |
 			    MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
@@ -23,6 +17,35 @@ enum motion_sense_async_event {
 };
 
 /**
+ * Whether or not we need to wake up the AP.
+ *
+ * @return Non zero when a wake-up is needed.
+ */
+int motion_sense_fifo_wake_up_needed(void);
+
+/**
+ * Resets the flag for wake up needed.
+ */
+void motion_sense_fifo_reset_wake_up_needed(void);
+
+/**
+ * Insert an async event into the fifo.
+ *
+ * @param sensor The sensor that generated the async event.
+ * @param event The event to insert.
+ */
+void motion_sense_fifo_insert_async_event(
+	struct motion_sensor_t *sensor,
+	enum motion_sense_async_event event);
+
+/**
+ * Insert a timestamp into the fifo.
+ *
+ * @param timestamp The timestamp to insert.
+ */
+void motion_sense_fifo_add_timestamp(uint32_t 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.
  *
@@ -32,46 +55,51 @@ enum motion_sense_async_event {
  * @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);
+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.
+ * Commit all the currently staged data to the fifo. Doing so makes it readable
+ * to the AP.
  */
 void motion_sense_fifo_commit_data(void);
 
 /**
- * Insert an async event into the fifo.
+ * Get information about the fifo.
  *
- * @param sensor Pointer to the sensor generating the event.
- * @param evt The event to insert.
+ * @param fifo_info The struct to modify with the current information about the
+ *	  fifo.
+ * @param reset Whether or not to reset statistics after reading them.
  */
-void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
-				     enum motion_sense_async_event evt);
+void motion_sense_fifo_get_info(
+	struct ec_response_motion_sense_fifo_info *fifo_info,
+	int reset);
 
 /**
- * Stage a timestamp into the fifo.
+ * Check whether or not the fifo has gone over its threshold.
  *
- * @param timestamp The timestamp to stage.
+ * @return 1 if yes, 0 for no.
  */
-void motion_sense_fifo_stage_timestamp(uint32_t timestamp);
+int motion_sense_fifo_over_thres(void);
 
 /**
- * Get information about the fifo.
+ * Read available committed entries from the fifo.
  *
- * @param fifo_info The struct to store the info.
+ * @param capacity_bytes The number of bytes available to be written to `out`.
+ * @param max_count The maximum number of entries to be placed in `out`.
+ * @param out The target to copy the data into.
+ * @param out_size The number of bytes written to `out`.
+ * @return The number of entries written to `out`.
  */
-void motion_sense_get_fifo_info(
-	struct ec_response_motion_sense_fifo_info *fifo_info);
+int motion_sense_fifo_read(int capacity_bytes, int max_count, void *out,
+			   uint16_t *out_size);
 
 /**
- * Checks if either the AP should be woken up due to the fifo.
- *
- * @return 1 if the AP should be woken up, 0 otherwise.
+ * Reset the internal data structures of the motion sense fifo.
  */
-int motion_sense_fifo_is_wake_up_needed(void);
+__test_only void motion_sense_fifo_reset(void);
 
-#endif /* __CROS_EC_MOTION_SENSE_FIFO_H */
+#endif /*__CROS_EC_MOTION_SENSE_FIFO_H */
diff --git a/include/test_util.h b/include/test_util.h
index 124ef77aa4..e35fc6baae 100644
--- a/include/test_util.h
+++ b/include/test_util.h
@@ -54,6 +54,8 @@
 #define TEST_NE(a, b, fmt) TEST_OPERATOR(a, b, !=, fmt)
 #define TEST_BITS_SET(a, bits) TEST_OPERATOR(a & (int)bits, (int)bits, ==, "%u")
 #define TEST_BITS_CLEARED(a, bits) TEST_OPERATOR(a & (int)bits, 0, ==, "%u")
+#define TEST_NEAR(a, b, epsilon, fmt) \
+	TEST_OPERATOR(ABS((a) - (b)), epsilon, <, fmt)
 
 #define __ABS(n) ((n) > 0 ? (n) : -(n))
 
diff --git a/test/build.mk b/test/build.mk
index 65a28fc3f6..58ff833719 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -46,6 +46,7 @@ test-list-host += math_util
 test-list-host += motion_angle
 test-list-host += motion_angle_tablet
 test-list-host += motion_lid
+test-list-host += motion_sense_fifo
 test-list-host += mutex
 test-list-host += nvmem
 test-list-host += pingpong
@@ -85,7 +86,6 @@ test-list-host += vboot
 test-list-host += x25519
 endif
 
-
 aes-y=aes.o
 base32-y=base32.o
 battery_get_params_smart-y=battery_get_params_smart.o
@@ -119,6 +119,7 @@ math_util-y=math_util.o
 motion_angle-y=motion_angle.o motion_angle_data_literals.o motion_common.o
 motion_angle_tablet-y=motion_angle_tablet.o motion_angle_data_literals_tablet.o motion_common.o
 motion_lid-y=motion_lid.o
+motion_sense_fifo-y=motion_sense_fifo.o
 mutex-y=mutex.o
 nvmem-y=nvmem.o nvmem_tpm2_mock.o
 pingpong-y=pingpong.o
diff --git a/test/motion_sense_fifo.c b/test/motion_sense_fifo.c
new file mode 100644
index 0000000000..f18ef1de72
--- /dev/null
+++ b/test/motion_sense_fifo.c
@@ -0,0 +1,362 @@
+/* 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.
+ *
+ * Test motion_sense_fifo.
+ */
+
+#include "stdio.h"
+#include "motion_sense_fifo.h"
+#include "test_util.h"
+#include "util.h"
+#include "hwtimer.h"
+
+struct motion_sensor_t motion_sensors[] = {
+	[BASE] = {},
+	[LID] = {},
+};
+
+const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
+
+uint32_t mkbp_last_event_time;
+
+static struct ec_response_motion_sensor_data data[CONFIG_ACCEL_FIFO_SIZE];
+static uint16_t data_bytes_read;
+
+static int test_insert_async_event(void)
+{
+	int read_count;
+
+	motion_sense_fifo_insert_async_event(motion_sensors, ASYNC_EVENT_FLUSH);
+	motion_sense_fifo_insert_async_event(motion_sensors + 1,
+					     ASYNC_EVENT_ODR);
+
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 2, "%d");
+	TEST_EQ(data_bytes_read,
+		(int) (2 * sizeof(struct ec_response_motion_sensor_data)),
+		"%d");
+
+	TEST_BITS_SET(data[0].flags, ASYNC_EVENT_FLUSH);
+	TEST_BITS_CLEARED(data[0].flags, MOTIONSENSE_SENSOR_FLAG_ODR);
+	TEST_EQ(data[0].sensor_num, 0, "%d");
+
+	TEST_BITS_SET(data[1].flags, ASYNC_EVENT_ODR);
+	TEST_BITS_CLEARED(data[1].flags, MOTIONSENSE_SENSOR_FLAG_FLUSH);
+	TEST_EQ(data[1].sensor_num, 1, "%d");
+
+	return EC_SUCCESS;
+}
+
+static int test_wake_up_needed(void)
+{
+	data[0].flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP;
+
+	motion_sense_fifo_stage_data(data, motion_sensors, 0, 100);
+	TEST_EQ(motion_sense_fifo_wake_up_needed(), 0, "%d");
+
+	motion_sense_fifo_commit_data();
+	TEST_EQ(motion_sense_fifo_wake_up_needed(), 1, "%d");
+
+	return EC_SUCCESS;
+}
+
+static int test_wake_up_needed_overflow(void)
+{
+	int i;
+
+	data[0].flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP;
+	motion_sense_fifo_stage_data(data, motion_sensors, 0, 100);
+
+	data[0].flags = 0;
+	/*
+	 * Using CONFIG_ACCEL_FIFO_SIZE / 2 since 2 entries are inserted per
+	 * 'data':
+	 * - a timestamp
+	 * - the data
+	 */
+	for (i = 0; i < (CONFIG_ACCEL_FIFO_SIZE / 2); i++)
+		motion_sense_fifo_stage_data(data, motion_sensors, 0, 101 + i);
+
+	TEST_EQ(motion_sense_fifo_wake_up_needed(), 1, "%d");
+	return EC_SUCCESS;
+}
+
+static int test_adding_timestamp(void)
+{
+	int read_count;
+
+	motion_sense_fifo_add_timestamp(100);
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+
+	TEST_EQ(read_count, 1, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, 100, "%u");
+	return EC_SUCCESS;
+}
+
+static int test_stage_data_sets_xyz(void)
+{
+	motion_sensors->oversampling_ratio = 1;
+	motion_sensors->oversampling = 0;
+	data->data[0] = 1;
+	data->data[1] = 2;
+	data->data[2] = 3;
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+
+	TEST_EQ(motion_sensors->xyz[0], 1, "%d");
+	TEST_EQ(motion_sensors->xyz[1], 2, "%d");
+	TEST_EQ(motion_sensors->xyz[2], 3, "%d");
+
+	return EC_SUCCESS;
+}
+
+static int test_stage_data_removed_oversample(void)
+{
+	int read_count;
+
+	motion_sensors->oversampling_ratio = 2;
+	motion_sensors->oversampling = 0;
+	data->data[0] = 1;
+	data->data[1] = 2;
+	data->data[2] = 3;
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+
+	data->data[0] = 4;
+	data->data[1] = 5;
+	data->data[2] = 6;
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 110);
+	motion_sense_fifo_commit_data();
+
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 3, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, 100, "%u");
+	TEST_BITS_CLEARED(data[1].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[1].data[0], 1, "%d");
+	TEST_EQ(data[1].data[1], 2, "%d");
+	TEST_EQ(data[1].data[2], 3, "%d");
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[2].timestamp, 110, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_stage_data_remove_all_oversampling(void)
+{
+	int read_count;
+
+	motion_sensors->oversampling_ratio = 0;
+	motion_sensors->oversampling = 0;
+	data->data[0] = 1;
+	data->data[1] = 2;
+	data->data[2] = 3;
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+
+	data->data[0] = 4;
+	data->data[1] = 5;
+	data->data[2] = 6;
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 110);
+	motion_sense_fifo_commit_data();
+
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 2, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, 100, "%u");
+	TEST_BITS_SET(data[1].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[1].timestamp, 110, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_stage_data_evicts_data_with_timestamp(void)
+{
+	int i, read_count;
+
+	/* Fill the fifo */
+	motion_sensors->oversampling_ratio = 1;
+	for (i = 0; i < CONFIG_ACCEL_FIFO_SIZE / 2; i++)
+		motion_sense_fifo_stage_data(data, motion_sensors,
+					     3, i * 100);
+
+	/* Add a single entry (should evict 2) */
+	motion_sense_fifo_add_timestamp(CONFIG_ACCEL_FIFO_SIZE * 100);
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, CONFIG_ACCEL_FIFO_SIZE - 1, "%d");
+	TEST_BITS_SET(data->flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data->timestamp, 100, "%u");
+	TEST_BITS_SET(data[CONFIG_ACCEL_FIFO_SIZE - 2].flags,
+		      MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[CONFIG_ACCEL_FIFO_SIZE - 2].timestamp,
+		CONFIG_ACCEL_FIFO_SIZE * 100, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_add_data_no_spreading_when_different_sensors(void)
+{
+	int read_count;
+	uint32_t now = __hw_clock_source_read();
+
+	motion_sensors[0].oversampling_ratio = 1;
+	motion_sensors[1].oversampling_ratio = 1;
+
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, now);
+	motion_sense_fifo_stage_data(data, motion_sensors + 1, 3, now);
+	motion_sense_fifo_commit_data();
+
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 4, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, now, "%u");
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[2].timestamp, now, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_add_data_no_spreading_different_timestamps(void)
+{
+	int read_count;
+
+	motion_sensors[0].oversampling_ratio = 1;
+
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, 120);
+	motion_sense_fifo_commit_data();
+
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 4, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, 100, "%u");
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[2].timestamp, 120, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_spread_data_in_window(void)
+{
+	uint32_t now;
+	int read_count;
+
+	motion_sensors[0].oversampling_ratio = 1;
+	motion_sensors[0].collection_rate = 20000; /* ns */
+	now = __hw_clock_source_read();
+
+	motion_sense_fifo_stage_data(data, motion_sensors, 3,
+				     now - 18000);
+	motion_sense_fifo_stage_data(data, motion_sensors, 3,
+				     now - 18000);
+	motion_sense_fifo_commit_data();
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 4, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, now - 18000, "%u");
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	/* TODO(b/142892004): mock __hw_clock_source_read so we can check for
+	 * exact TS.
+	 */
+	TEST_NEAR(data[2].timestamp, now, 2, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_spread_data_by_collection_rate(void)
+{
+	const uint32_t now = __hw_clock_source_read();
+	int read_count;
+
+	motion_sensors[0].oversampling_ratio = 1;
+	motion_sensors[0].collection_rate = 20000; /* ns */
+	motion_sense_fifo_stage_data(data, motion_sensors, 3,
+				     now - 20500);
+	motion_sense_fifo_stage_data(data, motion_sensors, 3,
+				     now - 20500);
+	motion_sense_fifo_commit_data();
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 4, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, now - 20500, "%u");
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[2].timestamp, now - 500, "%u");
+
+	return EC_SUCCESS;
+}
+
+static int test_spread_double_commit_same_timestamp(void)
+{
+	const uint32_t now = __hw_clock_source_read();
+	int read_count;
+
+	motion_sensors[0].oversampling_ratio = 1;
+	motion_sensors[0].collection_rate = 20000; /* ns */
+	motion_sense_fifo_stage_data(data, motion_sensors, 3,
+				     now - 20500);
+	motion_sense_fifo_commit_data();
+	motion_sense_fifo_stage_data(data, motion_sensors, 3,
+				     now - 20500);
+	motion_sense_fifo_commit_data();
+
+	read_count = motion_sense_fifo_read(
+		sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+		data, &data_bytes_read);
+	TEST_EQ(read_count, 4, "%d");
+	TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[0].timestamp, now - 20500, "%u");
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_GT(time_until(now - 20500, data[2].timestamp), 10000, "%u");
+	TEST_LE(time_until(now - 20500, data[2].timestamp), 20000, "%u");
+
+	return EC_SUCCESS;
+}
+
+void before_test(void)
+{
+	motion_sense_fifo_commit_data();
+	motion_sense_fifo_read(sizeof(data), CONFIG_ACCEL_FIFO_SIZE, &data,
+			       &data_bytes_read);
+	motion_sense_fifo_reset_wake_up_needed();
+	memset(data, 0, sizeof(data));
+	motion_sense_fifo_reset();
+}
+
+void run_test(void)
+{
+	test_reset();
+
+	RUN_TEST(test_insert_async_event);
+	RUN_TEST(test_wake_up_needed);
+	RUN_TEST(test_wake_up_needed_overflow);
+	RUN_TEST(test_adding_timestamp);
+	RUN_TEST(test_stage_data_sets_xyz);
+	RUN_TEST(test_stage_data_removed_oversample);
+	RUN_TEST(test_stage_data_remove_all_oversampling);
+	RUN_TEST(test_stage_data_evicts_data_with_timestamp);
+	RUN_TEST(test_add_data_no_spreading_when_different_sensors);
+	RUN_TEST(test_add_data_no_spreading_different_timestamps);
+	RUN_TEST(test_spread_data_in_window);
+	RUN_TEST(test_spread_data_by_collection_rate);
+	RUN_TEST(test_spread_double_commit_same_timestamp);
+
+	test_print_result();
+}
+
diff --git a/test/motion_sense_fifo.tasklist b/test/motion_sense_fifo.tasklist
new file mode 100644
index 0000000000..0e3696c3f0
--- /dev/null
+++ b/test/motion_sense_fifo.tasklist
@@ -0,0 +1,10 @@
+/* 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.
+ */
+
+/**
+ * See CONFIG_TASK_LIST in config.h for details.
+ */
+#define CONFIG_TEST_TASK_LIST \
+	TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE)
diff --git a/test/test_config.h b/test/test_config.h
index c422143112..acf890beb8 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -82,8 +82,14 @@
 #define CONFIG_SHA256
 #endif
 
+#ifdef TEST_MOTION_SENSE_FIFO
+#define CONFIG_ACCEL_FIFO
+#define CONFIG_ACCEL_FIFO_SIZE 256
+#define CONFIG_ACCEL_FIFO_THRES 10
+#endif
+
 #if defined(TEST_MOTION_LID) || defined(TEST_MOTION_ANGLE) || \
-	defined(TEST_MOTION_ANGLE_TABLET)
+	defined(TEST_MOTION_ANGLE_TABLET) || defined(TEST_MOTION_SENSE_FIFO)
 enum sensor_id {
 	BASE,
 	LID,