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

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>

2 files changed, 24 insertions, 10 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 5a1f3a805c..2cd1f2fe5f 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -196,6 +196,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 ccd1ffaa97..43de7a1e6d 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -66,6 +66,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;
@@ -429,8 +441,6 @@ void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *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;
 
@@ -471,7 +481,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).
@@ -536,7 +546,7 @@ commit_data_end:
 		next_timestamp[sensor_num].next +=
 			fifo_staged.requires_spreading ?
 				data_periods[sensor_num] :
-				motion_sensors[sensor_num].collection_rate;
+				expected_data_periods[sensor_num];
 
 		/* Update online calibration if enabled. */
 		data = peek_fifo_staged(i);
@@ -552,12 +562,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);
 }
 
@@ -633,6 +637,12 @@ void motion_sense_fifo_reset(void)
 	motion_sense_fifo_get_info(&fifo_info, /*reset=*/true);
 }
 
+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)
 {