common: motion_sense: Spread timestamps in motion sense fifo

This changes moves the specialized logic for timestamp spreading
away from the accelgyro_lsm6dsm and into the main motion_sense
loop. The motion_sense_fifo_add_data function was replaced by a
stage equivalent, and a commit function was added. Similarly,
internal static functions for motion_sense.c were renamed to
use the stage terminology. The idea is:

When a sensor is read, it might provide more than one measurement
though the only known timestamp is the one that caused the interrupt.
Staging this data allows us to use the same fifo queue space that the
entries would consume eventually anyway without making the entries
readable. Upon commit, the timestamp entries are spread if needed.

Note that if tight timestamps are disabled, the commit becomes a
simple tail move.

BUG=chromium:966506
BRANCH=None
TEST=Ran CTS on arcada.

Change-Id: Ib7d0a75c9c56fc4e275aed794058a5eca58ff47f
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1637416
Reviewed-by: Jack Rosenthal <jrosenth@chromium.org>
Reviewed-by: Jett Rink <jettrink@chromium.org>

1 files changed, 305 insertions, 35 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index c2df8f080e..3e9bd4b550 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -84,6 +84,12 @@ static void print_spoof_mode_status(int id);
 static uint32_t odr_event_required;
 
 #ifdef CONFIG_ACCEL_FIFO
+
+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 */
 static int wake_up_needed;
 
@@ -95,24 +101,138 @@ struct queue motion_sense_fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO,
 		struct ec_response_motion_sensor_data);
 static int motion_sense_fifo_lost;
 
+/**
+ * Staged metadata for the motion_sense_fifo.
+ * @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
+ *	staged.
+ * @sample_count: The total number of sensor readings per sensor that are
+ *	currently staged.
+ * @requires_spreading: Flag used to shortcut the commit process. This should be
+ *	true iff at least one of sample_count[] > 1
+ */
+struct fifo_staged {
+	uint32_t read_ts;
+	uint8_t count;
+	uint8_t sample_count[SENSOR_COUNT];
+	uint8_t requires_spreading;
+};
+static struct fifo_staged fifo_staged;
+
+static inline struct ec_response_motion_sensor_data *
+get_motion_sense_fifo_head(void)
+{
+	return ((struct ec_response_motion_sensor_data *)
+		motion_sense_fifo.buffer) +
+	       (motion_sense_fifo.state->head &
+		motion_sense_fifo.unit_bytes);
+}
+
+/**
+ * Pop one entry from the motion sense fifo. Poping will give priority to
+ * committed data (data residing between the head and tail of the queue). If no
+ * committed data is available (all the data is staged), then this function will
+ * remove the oldest staged data by moving both the head and tail.
+ *
+ * As a side-effect of this function, it'll updated any appropriate lost and
+ * count variables.
+ *
+ * WARNING: This function MUST be called from within a locked context of
+ * g_sensor_mutex.
+ */
+static void motion_sense_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);
+
+	/* Check that we have something to pop. */
+	if (!initial_count && !fifo_staged.count)
+		return;
+
+	/*
+	 * If all the data is staged (nothing in the committed queue), we'll
+	 * need to move the head and the tail over to simulate poping from the
+	 * staged data.
+	 */
+	if (!initial_count)
+		queue_advance_tail(&motion_sense_fifo, 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++;
+
+	/* Increment lost counter if we have valid data. */
+	if (!is_timestamp(head))
+		motion_sensors[head->sensor_num].lost++;
+
+	/* Only continue if we removed from staged. */
+	if (!initial_count)
+		return;
+
+	fifo_staged.count--;
+
+	/* If we removed a timestamp there's nothing else for us to do. */
+	if (is_timestamp(head))
+		return;
+
+	/*
+	 * Decrement sample count, if the count was 2 before, we might not need
+	 * to spread anymore. Loop through and check.
+	 */
+	if (--fifo_staged.sample_count[head->sensor_num] < 2) {
+		int i;
+
+		fifo_staged.requires_spreading = 0;
+		for (i = 0; i < SENSOR_COUNT; i++)
+			if (fifo_staged.sample_count[i] > 1) {
+				fifo_staged.requires_spreading = 1;
+				break;
+			}
+	}
+}
+
+static void motion_sense_fifo_ensure_space(void)
+{
+	/* If we already have space just bail. */
+	if (queue_space(&motion_sense_fifo) > fifo_staged.count)
+		return;
+
+	/*
+	 * Pop at least 1 spot, but if all the following conditions are met we
+	 * will continue to pop:
+	 * 1. We're operating with tight timestamps.
+	 * 2. The new head isn't a timestamp.
+	 * 3. We have data that we can possibly pop.
+	 *
+	 * Removing more than one entry is needed because if we are using tight
+	 * timestamps and we pop a timestamp, then the next head is data, the AP
+	 * would assign a bad timestamp to it.
+	 */
+	do {
+		motion_sense_fifo_pop();
+	} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
+		 !is_timestamp(get_motion_sense_fifo_head()) &&
+		 queue_count(&motion_sense_fifo) + fifo_staged.count);
+}
+
 /*
  * Do not use this function directly if you just want to add sensor data, use
- * motion_sense_fifo_add_data instead to get a proper timestamp too.
+ * motion_sense_fifo_stage_data instead to get a proper timestamp too.
  */
-static void motion_sense_fifo_add_unit(
-				struct ec_response_motion_sensor_data *data,
-				struct motion_sensor_t *sensor,
-				int valid_data)
+static void motion_sense_fifo_stage_unit(
+	struct ec_response_motion_sensor_data *data,
+	struct motion_sensor_t *sensor,
+	int valid_data)
 {
-	struct ec_response_motion_sensor_data vector;
+	struct queue_chunk chunk;
 	int i;
 
 	mutex_lock(&g_sensor_mutex);
-	if (queue_space(&motion_sense_fifo) == 0) {
-		queue_remove_unit(&motion_sense_fifo, &vector);
-		motion_sense_fifo_lost++;
-		motion_sensors[vector.sensor_num].lost++;
-	}
+
 	for (i = 0; i < valid_data; i++)
 		sensor->xyz[i] = data->data[i];
 
@@ -131,17 +251,47 @@ static void motion_sense_fifo_add_unit(
 			return;
 		}
 	}
+
+	/* Make sure we have room for the data */
+	motion_sense_fifo_ensure_space();
 	mutex_unlock(&g_sensor_mutex);
-	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP) {
+
+	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
 		wake_up_needed = 1;
-	}
 #ifdef CONFIG_TABLET_MODE
 	data->flags |= (tablet_get_mode() ?
 			MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
 #endif
-	mutex_lock(&g_sensor_mutex);
-	queue_add_unit(&motion_sense_fifo, data);
-	mutex_unlock(&g_sensor_mutex);
+
+	/*
+	 * 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);
+
+	/*
+	 * Save the data to the writable block and increment count. This data
+	 * will now reside AFTER the tail of the queue and will not be visible
+	 * to the AP until the motion_sense_fifo_commit_data() function is
+	 * called. Because count is incremented, the following staged data will
+	 * be written to the next available block and this one will remain
+	 * staged.
+	 */
+	memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
+	fifo_staged.count++;
+	/*
+	 * If we're using tight timestamps, and the current entry isn't a
+	 * timestamp we'll increment the sample_count for the given sensor.
+	 * If the new per-sensor sample count is greater than 1, we'll need to
+	 * spread.
+	 */
+	if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
+			!is_timestamp(data) &&
+			++fifo_staged.sample_count[data->sensor_num] > 1) {
+		fifo_staged.requires_spreading = 1;
+	}
 }
 
 enum motion_sense_async_event {
@@ -155,30 +305,144 @@ static void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
 					    enum motion_sense_async_event evt)
 {
 	struct ec_response_motion_sensor_data vector;
+
 	vector.flags = evt;
 	vector.timestamp = __hw_clock_source_read();
 	vector.sensor_num = sensor - motion_sensors;
 
-	motion_sense_fifo_add_unit(&vector, sensor, 0);
+	motion_sense_fifo_stage_unit(&vector, sensor, 0);
+	motion_sense_fifo_commit_data();
 }
 
-static void motion_sense_insert_timestamp(uint32_t timestamp)
+static void motion_sense_fifo_stage_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_add_unit(&vector, NULL, 0);
+	motion_sense_fifo_stage_unit(&vector, NULL, 0);
 }
 
-void motion_sense_fifo_add_data(struct ec_response_motion_sensor_data *data,
-				struct motion_sensor_t *sensor,
-				int valid_data,
-				uint32_t time) {
-#ifdef CONFIG_SENSOR_TIGHT_TIMESTAMPS
-	motion_sense_insert_timestamp(time);
-#endif
-	motion_sense_fifo_add_unit(data, sensor, valid_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)
+{
+	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);
+	}
+	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;
+}
+
+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.
+	 */
+	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 = 0;
+
+	/* Nothing staged, no work to do. */
+	if (!fifo_staged.count)
+		return;
+
+	/*
+	 * 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;
+
+	data = motion_sense_peek_fifo_staged(0);
+
+	/*
+	 * Spreading only makes sense if tight timestamps are used. In such case
+	 * entries are expected to be ordered: timestamp then data. If the first
+	 * entry isn't a timestamp we must have gotten out of sync. Just commit
+	 * all the data and skip the spreading.
+	 */
+	if (!is_timestamp(data)) {
+		CPRINTS("Spreading skipped, first entry is not a timestamp");
+		goto flush_data_end;
+	}
+
+	window = time_until(data->timestamp, fifo_staged.read_ts);
+
+	/* Update the data_periods as needed for this flush. */
+	for (i = 0; i < SENSOR_COUNT; i++) {
+		int period;
+
+		/* Skip empty sensors. */
+		if (!fifo_staged.sample_count[i])
+			continue;
+
+		period = motion_sensors[i].collection_rate;
+		/*
+		 * Clamp the sample period to the MIN of collection_rate and the
+		 * window length / sample counts.
+		 */
+		if (window)
+			period = MIN(period,
+				     window / fifo_staged.sample_count[i]);
+		data_periods[i] = period;
+	}
+
+	/*
+	 * 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
+	 * or more timestamps followed by exactly 1 data entry. We'll loop
+	 * through the timestamps until we get to data. We only need to update
+	 * the timestamp right before it to keep things correct.
+	 */
+	for (i = 0; i < fifo_staged.count; i++) {
+		data = motion_sense_peek_fifo_staged(i);
+
+		/* Skip timestamp, we don't know the sensor number yet. */
+		if (is_timestamp(data))
+			continue;
+
+		/* Get the sensor number and point to the timestamp entry. */
+		sensor_num = data->sensor_num;
+		data = motion_sense_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;
+
+		/* Spread the timestamp and compute the expected next. */
+		data->timestamp = next_timestamp[sensor_num];
+		next_timestamp[sensor_num] += data_periods[sensor_num];
+	}
+
+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);
+
+	/* Reset metadata for next staging cycle. */
+	memset(&fifo_staged, 0, sizeof(fifo_staged));
 }
 
 static void motion_sense_get_fifo_info(
@@ -762,8 +1026,10 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 				vector.data[X] = v[X];
 				vector.data[Y] = v[Y];
 				vector.data[Z] = v[Z];
-				motion_sense_fifo_add_data(&vector, sensor, 3,
-						   __hw_clock_source_read());
+				motion_sense_fifo_stage_data(
+					&vector, sensor, 3,
+					__hw_clock_source_read());
+				motion_sense_fifo_commit_data();
 			}
 			increment_sensor_collection(sensor, ts);
 		} else {
@@ -854,8 +1120,9 @@ static void check_and_queue_gestures(uint32_t *event)
 		vector.activity = MOTIONSENSE_ACTIVITY_DOUBLE_TAP;
 		vector.state = 1; /* triggered */
 		vector.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID;
-		motion_sense_fifo_add_data(&vector, NULL, 0,
-					   __hw_clock_source_read());
+		motion_sense_fifo_stage_data(&vector, NULL, 0,
+					     __hw_clock_source_read());
+		motion_sense_fifo_commit_data();
 #endif
 		/* Call board specific function to process tap */
 		sensor_board_proc_double_tap();
@@ -873,8 +1140,9 @@ static void check_and_queue_gestures(uint32_t *event)
 		vector.activity = MOTIONSENSE_ACTIVITY_SIG_MOTION;
 		vector.state = 1; /* triggered */
 		vector.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID;
-		motion_sense_fifo_add_data(&vector, NULL, 0,
-					   __hw_clock_source_read());
+		motion_sense_fifo_stage_data(&vector, NULL, 0,
+				__hw_clock_source_read());
+		motion_sense_fifo_commit_data();
 #endif
 		/* Disable further detection */
 		activity_sensor = &motion_sensors[CONFIG_GESTURE_SIGMO];
@@ -1019,9 +1287,11 @@ void motion_sense_task(void *u)
 		    (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_insert_timestamp(
+			if ((event & TASK_EVENT_MOTION_FLUSH_PENDING) == 0) {
+				motion_sense_fifo_stage_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