common: motion_sense_fifo: fix iteration bug

Fix potential bug when iterating over FIFO entries for commit.
This bug can manifest when non-timestamp/non-data entries are
inserted into the FIFO.

BUG=b:138303429,chromium:1023858
TEST=Added unit tests
BRANCH=None

Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2012846
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
(cherry picked from commit e8b5147f37bdb347ba25780bc4b969b37fd6b239)
Change-Id: I64855db2141f7ad6aafb3f3a84632a1dabef11f4
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2492531

2 files changed, 54 insertions, 2 deletions

diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index bdf39a857c..aa51d2ccab 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -10,6 +10,7 @@
 #include "tablet_mode.h"
 #include "task.h"
 #include "util.h"
+#include "stdbool.h"
 
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)
 
@@ -76,6 +77,18 @@ static inline int is_timestamp(
 }
 
 /**
+ * Check whether or not a given sensor data entry contains sensor data or not.
+ *
+ * @param data The data entry to check.
+ * @return True if the entry contains data, false otherwise.
+ */
+static inline bool is_data(const struct ec_response_motion_sensor_data *data)
+{
+	return (data->flags & (MOTIONSENSE_SENSOR_FLAG_TIMESTAMP |
+			       MOTIONSENSE_SENSOR_FLAG_ODR)) == 0;
+}
+
+/**
  * Convenience function to get the head of the fifo. This function makes no
  * guarantee on whether or not the entry is valid.
  *
@@ -424,14 +437,23 @@ commit_data_end:
 		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))
+		/*
+		 * Skip non-data entries, we don't know the sensor number yet.
+		 */
+		if (!is_data(data))
 			continue;
 
 		/* Get the sensor number and point to the timestamp entry. */
 		sensor_num = data->sensor_num;
 		data = peek_fifo_staged(i - 1);
 
+		/* Verify we're pointing at a timestamp. */
+		if (!is_timestamp(data)) {
+			CPRINTS("FIFO entries out of order,"
+				" expected timestamp");
+			continue;
+		}
+
 		/*
 		 * If this is the first time we're seeing a timestamp for this
 		 * sensor or the timestamp is after our computed next, skip
diff --git a/test/motion_sense_fifo.c b/test/motion_sense_fifo.c
index f18ef1de72..b16afc3479 100644
--- a/test/motion_sense_fifo.c
+++ b/test/motion_sense_fifo.c
@@ -329,6 +329,35 @@ static int test_spread_double_commit_same_timestamp(void)
 	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 = 20000; /* ns */
+
+	/* Insert non-data entry */
+	data[0].flags = MOTIONSENSE_SENSOR_FLAG_ODR;
+	motion_sense_fifo_stage_data(data, motion_sensors, 3, now - 20500);
+
+	/* Insert data entry */
+	data[0].flags = 0;
+	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[1].flags, MOTIONSENSE_SENSOR_FLAG_ODR);
+	TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+	TEST_EQ(data[2].timestamp, now - 20500, "%u");
+
+	return EC_SUCCESS;
+}
+
 void before_test(void)
 {
 	motion_sense_fifo_commit_data();
@@ -356,6 +385,7 @@ void run_test(void)
 	RUN_TEST(test_spread_data_in_window);
 	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);
 
 	test_print_result();
 }