motion_sense_fifo: Reset timestamp only when ODR changes

Timestamp spreading is reset after each fifo commit.
It prevents the fifo logic to operate when the EC takes
more than a period to collect the samples.

Reset timestamp spreading only when ODR changes.
Move needed fields inside motion_sense_fifo to handle
virtual sensors.

Fixes commit bc9660a4b3e8 ("common: motionsense fifo: Reset the initialized bits after commit")

BUG=b:168335284,b:237305991,b:217580259
BRANCH=all
TEST=make run-motion_sense_fifo

Conflicts:
	common/motion_sense_fifo.c: Clang changes not in.

(cherry picked from commit 1404036c889324442a9559812f5c4ed6143506f3)

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

4 files changed, 39 insertions, 46 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index afffb02e78..428963c569 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -195,6 +195,10 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 	sensor->collection_rate = odr > 0 ? SECOND * 1000 / odr : 0;
 	sensor->next_collection = ts.le.lo + sensor->collection_rate;
 	sensor->oversampling = 0;
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
+		motion_sense_set_data_period(sensor - motion_sensors,
+					     sensor->collection_rate);
+	}
 	mutex_unlock(&g_sensor_mutex);
 	if (IS_ENABLED(CONFIG_BODY_DETECTION) &&
 	    (sensor - motion_sensors == CONFIG_BODY_DETECTION_SENSOR))
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index 3e31400cba..d0bbfcd342 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -60,6 +60,18 @@ static struct fifo_staged fifo_staged;
 static struct timestamp_state next_timestamp[MAX_MOTION_SENSORS];
 
 /**
+ * Expected data periods:
+ * copy of collection rate, updated when ODR changes.
+ */
+static uint32_t expected_data_periods[MAX_MOTION_SENSORS];
+
+/**
+ * Calculated data periods:
+ * can be different from collection rate when spreading.
+ */
+static uint32_t data_periods[MAX_MOTION_SENSORS];
+
+/**
  * Bitmap telling which sensors have valid entries in the next_timestamp array.
  */
 static uint32_t next_timestamp_initialized;
@@ -426,8 +438,6 @@ void motion_sense_fifo_stage_data(
 
 void motion_sense_fifo_commit_data(void)
 {
-	/* Cached data periods, static to store off stack. */
-	static uint32_t data_periods[MAX_MOTION_SENSORS];
 	struct ec_response_motion_sensor_data *data;
 	int i, window, sensor_num;
 
@@ -468,7 +478,7 @@ void motion_sense_fifo_commit_data(void)
 		if (!fifo_staged.sample_count[i])
 			continue;
 
-		period = motion_sensors[i].collection_rate;
+		period = expected_data_periods[i];
 		/*
 		 * Clamp the sample period to the MIN of collection_rate and the
 		 * window length / (sample count - 1).
@@ -531,9 +541,9 @@ commit_data_end:
 		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;
+			fifo_staged.requires_spreading ?
+				data_periods[sensor_num] :
+				expected_data_periods[sensor_num];
 
 		/* Update online calibration if enabled. */
 		data = peek_fifo_staged(i);
@@ -549,12 +559,6 @@ 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);
 }
 
@@ -621,6 +625,12 @@ void motion_sense_fifo_reset(void)
 	queue_init(&fifo);
 }
 
+void motion_sense_set_data_period(int sensor_num, uint32_t data_period)
+{
+	expected_data_periods[sensor_num] = data_period;
+	next_timestamp_initialized &= ~BIT(sensor_num);
+}
+
 #ifdef CONFIG_CMD_ACCEL_FIFO
 static int motion_sense_read_fifo(int argc, char **argv)
 {
diff --git a/include/motion_sense_fifo.h b/include/motion_sense_fifo.h
index 90d3f78879..e55aac6c3b 100644
--- a/include/motion_sense_fifo.h
+++ b/include/motion_sense_fifo.h
@@ -22,6 +22,15 @@ enum motion_sense_async_event {
 void motion_sense_fifo_init(void);
 
 /**
+ * Set the expected period between samples. Must be call under
+ * g_mutex_lock each time the sensor ODR changes.
+ *
+ * @param sensor_num Affected sensor
+ * @param data_period expected milliseconds between samples.
+ */
+void motion_sense_set_data_period(int sensor_num, uint32_t data_period);
+
+/**
  * Whether or not we need to bypass the FIFO to send an important message.
  *
  * @return Non zero when a bypass is needed.
diff --git a/test/motion_sense_fifo.c b/test/motion_sense_fifo.c
index ad333caa28..a2c94516c0 100644
--- a/test/motion_sense_fifo.c
+++ b/test/motion_sense_fifo.c
@@ -270,7 +270,7 @@ static int test_spread_data_in_window(void)
 	int read_count;
 
 	motion_sensors[0].oversampling_ratio = 1;
-	motion_sensors[0].collection_rate = 20; /* us */
+	motion_sense_set_data_period(0, 20 /* us */);
 	now = __hw_clock_source_read();
 
 	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 18);
@@ -297,9 +297,9 @@ static int test_spread_data_on_overflow(void)
 	int i, read_count;
 
 	/* Set up the sensors */
-	motion_sensors[0].collection_rate = 20; /* us */
 	motion_sensors[0].oversampling_ratio = 1;
 	motion_sensors[1].oversampling_ratio = 1;
+	motion_sense_set_data_period(0, 20 /* us */);
 
 	/* Add 1 sample for sensor [1]. This will be evicted. */
 	data->sensor_num = 1;
@@ -348,7 +348,7 @@ static int test_spread_data_by_collection_rate(void)
 	int read_count;
 
 	motion_sensors[0].oversampling_ratio = 1;
-	motion_sensors[0].collection_rate = 20; /* us */
+	motion_sense_set_data_period(0, 20 /* us */);
 	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 25);
 	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 25);
 	motion_sense_fifo_commit_data();
@@ -363,42 +363,13 @@ static int test_spread_data_by_collection_rate(void)
 	return EC_SUCCESS;
 }
 
-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 = 20; /* us */
-	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 25);
-	motion_sense_fifo_commit_data();
-	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 25);
-	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 - 25, "%u");
-	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
-	TEST_EQ(data[2].timestamp, now - 25, "%u");
-
-	return EC_SUCCESS;
-}
-
 static int test_commit_non_data_or_timestamp_entries(void)
 {
 	const uint32_t now = __hw_clock_source_read();
 	int read_count;
 
 	motion_sensors[0].oversampling_ratio = 1;
-	motion_sensors[0].collection_rate = 20; /* us */
+	motion_sense_set_data_period(0, 20 /* us */);
 
 	/* Insert non-data entry */
 	data[0].flags = MOTIONSENSE_SENSOR_FLAG_ODR;
@@ -462,7 +433,6 @@ void run_test(int argc, char **argv)
 	RUN_TEST(test_spread_data_in_window);
 	RUN_TEST(test_spread_data_on_overflow);
 	RUN_TEST(test_spread_data_by_collection_rate);
-	RUN_TEST(test_spread_double_commit_same_timestamp);
 	RUN_TEST(test_commit_non_data_or_timestamp_entries);
 	RUN_TEST(test_get_info_size);