common: motionsense fifo: Reset the initialized bits after commit

Previously once we had one sample from a sensor, we considered the
fifo initialized. Meaning we would use our computed next timestamp
to spread the timestamps. This was causing issues with some devices
that ended up causing the timestamps to run ahead.

Reset next_timestamp_initialized back to 0 after each commit. This
will allow repeated timestamps but only if a driver stages/commits
the same timestamp twice. Staging the same timestamp with only a
single commit will still work as before (which is the expected path).

BRANCH=none
BUG=b:168335284
TEST=make run-motion_sense_fifo

Signed-off-by: Yuval Peress <peress@chromium.org>
Change-Id: Ib7c566f69d7c1e4e898050b67105555dd05376e5
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2871055
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

2 files changed, 13 insertions, 2 deletions

diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index 2b697e1f4c..244032d10a 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -549,6 +549,12 @@ commit_data_end:
 	/* Reset metadata for next staging cycle. */
 	memset(&fifo_staged, 0, sizeof(fifo_staged));
 
+	/*
+	 * Reset the initialized bits. This will allow new timestamps to be
+	 * considered as the new "source of truth".
+	 */
+	next_timestamp_initialized = 0;
+
 	mutex_unlock(&g_sensor_mutex);
 }
 
diff --git a/test/motion_sense_fifo.c b/test/motion_sense_fifo.c
index 28e7a3019a..c75cbe3c35 100644
--- a/test/motion_sense_fifo.c
+++ b/test/motion_sense_fifo.c
@@ -295,6 +295,12 @@ static int test_spread_double_commit_same_timestamp(void)
 	const uint32_t now = __hw_clock_source_read();
 	int read_count;
 
+	/*
+	 * Stage and commit the same sample. This is not expected to happen
+	 * since batches of sensor samples should be staged together and only
+	 * commit once. We assume that the driver did this on purpose and will
+	 * allow the same timestamp to be sent.
+	 */
 	motion_sensors[0].oversampling_ratio = 1;
 	motion_sensors[0].collection_rate = 20000; /* ns */
 	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 20500);
@@ -308,8 +314,7 @@ static int test_spread_double_commit_same_timestamp(void)
 	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");
+	TEST_EQ(data[2].timestamp, now - 20500, "%u");
 
 	return EC_SUCCESS;
 }