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>

5 files changed, 16 insertions, 66 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 933838251f..d06d3f7d99 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -842,7 +842,7 @@ static int bmi160_decode_header(struct motion_sensor_t *accel,
 				vector.data[Y] = v[Y];
 				vector.data[Z] = v[Z];
 				vector.sensor_num = s - motion_sensors;
-				motion_sense_fifo_add_data(&vector, s, 3,
+				motion_sense_fifo_stage_data(&vector, s, 3,
 						last_ts);
 				*bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
 			}
@@ -994,6 +994,7 @@ static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 			state = FIFO_HEADER;
 		}
 	}
+	motion_sense_fifo_commit_data();
 	return EC_SUCCESS;
 }
 #endif  /* CONFIG_ACCEL_FIFO */
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 8f7a9a26ae..e5166eccc9 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -29,18 +29,6 @@
 static volatile uint32_t last_interrupt_timestamp;
 
 /**
- * A queue for holding data from the FIFO as we read it. This will be used to
- * spread the timestamps if more than one entry is available. The allocated size
- * is calculated by assuming that the motion sense read loop will never exceed
- * 20ms. During this time, sensors running full tile (210Hz) will sample ~5
- * times. At most, this driver supports 3 sensors, leaving us with 15 samples.
- * Since the queue size needs to be a power of 2, and 16 is too close, 32 was
- * chosen.
- */
-static struct queue single_fifo_read_queue = QUEUE_NULL(32,
-		struct ec_response_motion_sensor_data);
-
-/**
  * Resets the lsm6dsm load fifo sensor states to the given timestamp. This
  * should be called at the start of the fifo read sequence.
  *
@@ -318,7 +306,8 @@ static int fifo_next(struct lsm6dsm_data *private)
  * push_fifo_data - Scan data pattern and push upside
  */
 static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
-			   uint16_t flen)
+			   uint16_t flen,
+			   uint32_t timestamp)
 {
 	struct motion_sensor_t *s;
 	struct lsm6dsm_data *private = LSM6DSM_GET_DATA(accel);
@@ -362,16 +351,7 @@ static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
 
 			vect.flags = 0;
 			vect.sensor_num = s - motion_sensors;
-			/*
-			 * queue_add_units will override old values in case of
-			 * an overflow. The sample count should still be
-			 * incremented as it might affect the computed sample
-			 * rate later on.
-			 */
-			queue_add_units(&single_fifo_read_queue, &vect, 1);
-			private->load_fifo_sensor_state[next_fifo]
-					.sample_count++;
-
+			motion_sense_fifo_stage_data(&vect, s, 3, timestamp);
 		}
 
 		fifo += OUT_XYZ_SIZE;
@@ -383,12 +363,8 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts,
 		     uint32_t *last_fifo_read_ts)
 {
 	uint32_t interrupt_timestamp = last_interrupt_timestamp;
-	uint32_t fifo_read_start = *last_fifo_read_ts;
 	int err, left, length;
 	uint8_t fifo[FIFO_READ_LEN];
-	struct ec_response_motion_sensor_data data;
-	struct load_fifo_sensor_state_t *load_fifo_sensor_state =
-			LSM6DSM_GET_DATA(s)->load_fifo_sensor_state;
 
 	/* Reset the load_fifo_sensor_state so we can start a new read. */
 	reset_load_fifo_sensor_state(s, interrupt_timestamp);
@@ -431,42 +407,11 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts,
 		 * reading the last sample and pushing it into the FIFO.
 		 */
 
-		push_fifo_data(s, fifo, length);
+		push_fifo_data(s, fifo, length, interrupt_timestamp);
 		left -= length;
 	} while (left > 0);
 
-	/* Compute the window length (ns) between the interrupt and the read. */
-	length = time_until(interrupt_timestamp, fifo_read_start);
-
-	/* Get the event count. */
-	left = queue_count(&single_fifo_read_queue);
-
-	/*
-	 * Spread timestamps if we have more than one reading for a given
-	 * sensor.
-	 */
-	while (left-- > 0) {
-		struct motion_sensor_t *data_sensor;
-		struct load_fifo_sensor_state_t *state;
-
-		/*
-		 * Pop an entry off our read queue and get the pointers for the
-		 * sensor and the read state.
-		 */
-		queue_remove_unit(&single_fifo_read_queue, &data);
-		data_sensor = &motion_sensors[data.sensor_num];
-		state = &load_fifo_sensor_state[get_fifo_type(data_sensor)];
-
-		/*
-		 * Push the data to the motion sense fifo and increment the
-		 * timestamp in case we have another reading for this sensor
-		 * (spreading).
-		 */
-		motion_sense_fifo_add_data(&data, data_sensor, 3,
-				state->int_timestamp);
-		state->int_timestamp += MIN(state->sample_rate,
-				length / state->sample_count);
-	}
+	motion_sense_fifo_commit_data();
 
 	return EC_SUCCESS;
 }
diff --git a/driver/als_si114x.c b/driver/als_si114x.c
index be3f090db3..b51cf8cfc6 100644
--- a/driver/als_si114x.c
+++ b/driver/als_si114x.c
@@ -151,8 +151,9 @@ static int si114x_read_results(struct motion_sensor_t *s, int nb)
 	for (i = nb; i < 3; i++)
 		vector.data[i] = 0;
 	vector.sensor_num = s - motion_sensors;
-	motion_sense_fifo_add_data(&vector, s, nb,
+	motion_sense_fifo_stage_data(&vector, s, nb,
 				   __hw_clock_source_read());
+	motion_sense_fifo_commit_data();
 	/*
 	 * TODO: get time at a more accurate spot.
 	 * Like in si114x_interrupt
diff --git a/driver/als_tcs3400.c b/driver/als_tcs3400.c
index 9e7251f711..e08d385a95 100644
--- a/driver/als_tcs3400.c
+++ b/driver/als_tcs3400.c
@@ -134,7 +134,7 @@ skip_clear_vector_load:
 
 #ifdef CONFIG_ACCEL_FIFO
 		vector.sensor_num = s - motion_sensors;
-		motion_sense_fifo_add_data(&vector, s, 3, last_ts);
+		motion_sense_fifo_stage_data(&vector, s, 3, last_ts);
 #endif
 	}
 
@@ -192,8 +192,9 @@ skip_rgb_load:
 skip_vector_load:
 #endif
 		vector.sensor_num = rgb_s - motion_sensors;
-		motion_sense_fifo_add_data(&vector, rgb_s, 3, last_ts);
+		motion_sense_fifo_stage_data(&vector, rgb_s, 3, last_ts);
 	}
+	motion_sense_fifo_commit_data();
 	return EC_SUCCESS;
 }
 
@@ -209,7 +210,7 @@ void tcs3400_interrupt(enum gpio_signal signal)
 /*
  * tcs3400_irq_handler - bottom half of the interrupt stack.
  * Ran from the motion_sense task, finds the events that raised the interrupt,
- * and posts those events via motion_sense_fifo_add_data()..
+ * and posts those events via motion_sense_fifo_stage_data()..
  */
 static int tcs3400_irq_handler(struct motion_sensor_t *s, uint32_t *event)
 {
diff --git a/driver/sync.c b/driver/sync.c
index 6f994eabb2..34df824090 100644
--- a/driver/sync.c
+++ b/driver/sync.c
@@ -88,8 +88,10 @@ static int motion_irq_handler(struct motion_sensor_t *s, uint32_t *event)
 
 	while (queue_remove_unit(&sync_event_queue, &sync_event)) {
 		vector.data[X] = sync_event.counter;
-		motion_sense_fifo_add_data(&vector, s, 1, sync_event.timestamp);
+		motion_sense_fifo_stage_data(
+			&vector, s, 1, sync_event.timestamp);
 	}
+	motion_sense_fifo_commit_data();
 
 	return EC_SUCCESS;
 }