Revert "common: Refactor motion_sense_fifo"

This reverts commit 36b47ab3c06e477f5e95d6d9e84a5220248784e6.

With this CL the lid angle calculation on hatch devices reports 500
which is what's used for can't caluclate a meaninfgul value.

BUG=b:141840539
BRANCH=None
TEST=On helios tested with this CL and saw that lid angle calculations
returned 500 for lid angle. Then reverted the CL and verifed that lid
angle calculations were correct. Used 'accelinfo on 10000' to check
the EC reported lid angle.

Change-Id: Id4e36219792d00357d2885e9944c58fe0e15c5ca
Signed-off-by: Scott Collyer <scollyer@google.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1834705
Tested-by: Scott Collyer <scollyer@chromium.org>
Reviewed-by: Yuval Peress <peress@chromium.org>
Reviewed-by: Furquan Shaikh <furquan@chromium.org>
Commit-Queue: Scott Collyer <scollyer@chromium.org>

1 files changed, 97 insertions, 161 deletions

diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index 938a4d142c..ec4c63b6de 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -7,14 +7,30 @@
 #include "hwtimer.h"
 #include "mkbp_event.h"
 #include "motion_sense_fifo.h"
+#include "queue.h"
 #include "tablet_mode.h"
-#include "task.h"
 #include "util.h"
 
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)
 
+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 */
+int wake_up_needed;
+
+/* Number of element the AP should collect */
+int fifo_queue_count;
+int fifo_int_enabled;
+
+struct queue motion_sense_fifo = QUEUE_NULL(
+	CONFIG_ACCEL_FIFO_SIZE, struct ec_response_motion_sensor_data);
+int motion_sense_fifo_lost;
+
 /**
- * Staged metadata for the fifo queue.
+ * 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
@@ -30,39 +46,15 @@ struct fifo_staged {
 	uint8_t sample_count[SENSOR_COUNT];
 	uint8_t requires_spreading;
 };
-
-/** Queue to hold the data to be sent to the AP. */
-struct queue fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO_SIZE,
-			       struct ec_response_motion_sensor_data);
-/** Count of the number of entries lost due to a small queue. */
-static int fifo_lost;
-/** Metadata for the fifo, used for staging and spreading data. */
 static struct fifo_staged fifo_staged;
 
-/** Need to wake up the AP. */
-int motion_sense_fifo_wake_up_needed;
-
-/**
- * Check whether or not a give sensor data entry is a timestamp or not.
- *
- * @param data The data entry to check.
- * @return 1 if the entry is a timestamp, 0 otherwise.
- */
-static inline int is_timestamp(struct ec_response_motion_sensor_data *data)
-{
-	return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
-}
-
-/**
- * Convinience function to get the head of the fifo. This function makes no
- * guarantee on whether or not the entry is valid.
- *
- * @return Pointer to the head of the fifo.
- */
-static inline struct ec_response_motion_sensor_data *get_fifo_head(void)
+static inline struct ec_response_motion_sensor_data *
+get_motion_sense_fifo_head(void)
 {
-	return ((struct ec_response_motion_sensor_data *) fifo.buffer) +
-		(fifo.state->head & fifo.buffer_units_mask);
+	return ((struct ec_response_motion_sensor_data *)
+		motion_sense_fifo.buffer) +
+	       (motion_sense_fifo.state->head &
+		motion_sense_fifo.unit_bytes);
 }
 
 /**
@@ -77,10 +69,11 @@ static inline struct ec_response_motion_sensor_data *get_fifo_head(void)
  * WARNING: This function MUST be called from within a locked context of
  * g_sensor_mutex.
  */
-static void fifo_pop(void)
+static void motion_sense_fifo_pop(void)
 {
-	struct ec_response_motion_sensor_data *head = get_fifo_head();
-	const size_t initial_count = queue_count(&fifo);
+	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)
@@ -92,19 +85,13 @@ static void fifo_pop(void)
 	 * staged data.
 	 */
 	if (!initial_count)
-		queue_advance_tail(&fifo, 1);
+		queue_advance_tail(&motion_sense_fifo, 1);
 
 	/*
-	 * If we're about to pop a wakeup flag, we should remember it as though
-	 * it was committed.
-	 */
-	if (head->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
-		motion_sense_fifo_wake_up_needed = 1;
-	/*
 	 * By not using queue_remove_unit we're avoiding an un-necessary memcpy.
 	 */
-	queue_advance_head(&fifo, 1);
-	fifo_lost++;
+	queue_advance_head(&motion_sense_fifo, 1);
+	motion_sense_fifo_lost++;
 
 	/* Increment lost counter if we have valid data. */
 	if (!is_timestamp(head))
@@ -140,13 +127,10 @@ static void fifo_pop(void)
 	}
 }
 
-/**
- * Make sure that the fifo has at least 1 empty spot to stage data into.
- */
-static void fifo_ensure_space(void)
+static void motion_sense_fifo_ensure_space(void)
 {
 	/* If we already have space just bail. */
-	if (queue_space(&fifo) > fifo_staged.count)
+	if (queue_space(&motion_sense_fifo) > fifo_staged.count)
 		return;
 
 	/*
@@ -161,21 +145,17 @@ static void fifo_ensure_space(void)
 	 * would assign a bad timestamp to it.
 	 */
 	do {
-		fifo_pop();
+		motion_sense_fifo_pop();
 	} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
-		 !is_timestamp(get_fifo_head()) &&
-		 queue_count(&fifo) + fifo_staged.count);
+		 !is_timestamp(get_motion_sense_fifo_head()) &&
+		 queue_count(&motion_sense_fifo) + fifo_staged.count);
 }
 
-/**
- * Stage a single data unit to the motion sense fifo. Note that for the AP to
- * see this data, it must be committed.
- *
- * @param data The data to stage.
- * @param sensor The sensor that generated the data
- * @param valid_data The number of readable data entries in the data.
+/*
+ * Do not use this function directly if you just want to add sensor data, use
+ * motion_sense_fifo_stage_data instead to get a proper timestamp too.
  */
-void fifo_stage_unit(
+static void motion_sense_fifo_stage_unit(
 	struct ec_response_motion_sensor_data *data,
 	struct motion_sensor_t *sensor,
 	int valid_data)
@@ -198,26 +178,29 @@ void fifo_stage_unit(
 		}
 		removed = sensor->oversampling++;
 		sensor->oversampling %= sensor->oversampling_ratio;
-		if (removed) {
+		if (removed != 0) {
 			mutex_unlock(&g_sensor_mutex);
 			return;
 		}
 	}
 
 	/* Make sure we have room for the data */
-	fifo_ensure_space();
+	motion_sense_fifo_ensure_space();
 	mutex_unlock(&g_sensor_mutex);
 
+	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
+		wake_up_needed = 1;
 	if (IS_ENABLED(CONFIG_TABLET_MODE))
 		data->flags |= (tablet_get_mode() ?
-				MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
+			MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
 
 	/*
 	 * 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(&fifo, fifo_staged.count);
+	chunk = queue_get_write_chunk(
+		&motion_sense_fifo, fifo_staged.count);
 
 	if (!chunk.buffer) {
 		/*
@@ -237,7 +220,7 @@ void fifo_stage_unit(
 	 * be written to the next available block and this one will remain
 	 * staged.
 	 */
-	memcpy(chunk.buffer, data, fifo.unit_bytes);
+	memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
 	fifo_staged.count++;
 
 	/*
@@ -252,68 +235,52 @@ void fifo_stage_unit(
 		fifo_staged.requires_spreading = 1;
 }
 
-/**
- * Stage an entry representing a single timestamp.
- *
- * @param timestamp The timestamp to add to the fifo.
- */
-static void fifo_stage_timestamp(uint32_t timestamp)
+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 = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
-	vector.timestamp = timestamp;
-	vector.sensor_num = 0;
-	fifo_stage_unit(&vector, NULL, 0);
-}
-
-/**
- * Peek into the staged data at a given offset. This function performs no bound
- * checking and is purely for confinience.
- *
- * @param offset The offset into the staged data to peek into.
- * @return Pointer to the entry at the given offset.
- */
-static inline struct ec_response_motion_sensor_data *
-peek_fifo_staged(size_t offset)
-{
-	return (struct ec_response_motion_sensor_data *)
-		queue_get_write_chunk(&fifo, offset).buffer;
-}
-
-void motion_sense_fifo_insert_async_event(
-	struct motion_sensor_t *sensor,
-	enum motion_sense_async_event event)
-{
-	struct ec_response_motion_sensor_data vector;
-
-	vector.flags = event;
+	vector.flags = evt;
 	vector.timestamp = __hw_clock_source_read();
 	vector.sensor_num = sensor - motion_sensors;
 
-	fifo_stage_unit(&vector, sensor, 0);
+	motion_sense_fifo_stage_unit(&vector, sensor, 0);
 	motion_sense_fifo_commit_data();
 }
 
-inline void motion_sense_fifo_add_timestamp(uint32_t timestamp)
+void motion_sense_fifo_stage_timestamp(uint32_t timestamp)
 {
-	fifo_stage_timestamp(timestamp);
-	motion_sense_fifo_commit_data();
+	struct ec_response_motion_sensor_data vector;
+
+	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+	vector.timestamp = timestamp;
+	vector.sensor_num = 0;
+	motion_sense_fifo_stage_unit(&vector, NULL, 0);
 }
 
-void motion_sense_fifo_stage_data(
-	struct ec_response_motion_sensor_data *data,
-	struct motion_sensor_t *sensor,
-	int valid_data,
-	uint32_t time)
+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();
-		fifo_stage_timestamp(time);
+		motion_sense_fifo_stage_timestamp(time);
 	}
-	fifo_stage_unit(data, sensor, valid_data);
+	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)
@@ -324,7 +291,6 @@ void motion_sense_fifo_commit_data(void)
 	 */
 	static uint32_t data_periods[SENSOR_COUNT];
 	static uint32_t next_timestamp[SENSOR_COUNT];
-	static uint16_t next_timestamp_initialized;
 	struct ec_response_motion_sensor_data *data;
 	int i, window, sensor_num;
 
@@ -340,7 +306,7 @@ void motion_sense_fifo_commit_data(void)
 	if (!fifo_staged.requires_spreading)
 		goto flush_data_end;
 
-	data = peek_fifo_staged(0);
+	data = motion_sense_peek_fifo_staged(0);
 
 	/*
 	 * Spreading only makes sense if tight timestamps are used. In such case
@@ -350,7 +316,6 @@ void motion_sense_fifo_commit_data(void)
 	 */
 	if (!is_timestamp(data)) {
 		CPRINTS("Spreading skipped, first entry is not a timestamp");
-		fifo_staged.requires_spreading = 0;
 		goto flush_data_end;
 	}
 
@@ -369,16 +334,14 @@ void motion_sense_fifo_commit_data(void)
 		 * Clamp the sample period to the MIN of collection_rate and the
 		 * window length / sample counts.
 		 */
-		if (window && fifo_staged.sample_count[i] > 1)
-			period = MIN(
-				period,
-				window / (fifo_staged.sample_count[i] - 1));
+		if (window)
+			period = MIN(period,
+				     window / fifo_staged.sample_count[i]);
 		data_periods[i] = period;
 	}
 
-flush_data_end:
 	/*
-	 * Conditionally spread the timestamps.
+	 * 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
@@ -386,58 +349,45 @@ flush_data_end:
 	 * through the timestamps until we get to data. We only need to update
 	 * the timestamp right before it to keep things correct.
 	 */
-	next_timestamp_initialized = 0;
 	for (i = 0; i < fifo_staged.count; i++) {
-		data = peek_fifo_staged(i);
-		if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
-			motion_sense_fifo_wake_up_needed = 1;
+		data = motion_sense_peek_fifo_staged(i);
 
 		/* Skip timestamp, we don't know the sensor number yet. */
-		if (is_timestamp(data) || !fifo_staged.requires_spreading)
+		if (is_timestamp(data))
 			continue;
 
 		/* Get the sensor number and point to the timestamp entry. */
 		sensor_num = data->sensor_num;
-		data = peek_fifo_staged(i - 1);
+		data = motion_sense_peek_fifo_staged(i - 1);
 
-		/*
-		 * If this is the first time we're seeing a timestamp for this
-		 * sensor or the timestamp is after our computed next, skip
-		 * ahead.
-		 */
-		if (!(next_timestamp_initialized & BIT(sensor_num)) ||
-		    time_after(data->timestamp, next_timestamp[sensor_num])) {
+		/* 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;
-			next_timestamp_initialized |= BIT(sensor_num);
-		}
 
 		/* 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(&fifo, fifo_staged.count);
+	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));
 }
 
-void motion_sense_fifo_get_info(
-	struct ec_response_motion_sense_fifo_info *fifo_info,
-	int reset)
+void motion_sense_get_fifo_info(
+	struct ec_response_motion_sense_fifo_info *fifo_info)
 {
-	fifo_info->size = fifo.buffer_units;
+	fifo_info->size = motion_sense_fifo.buffer_units;
 	mutex_lock(&g_sensor_mutex);
-	fifo_info->count = queue_count(&fifo);
-	fifo_info->total_lost = fifo_lost;
+	fifo_info->count = fifo_queue_count;
+	fifo_info->total_lost = motion_sense_fifo_lost;
 	mutex_unlock(&g_sensor_mutex);
 	fifo_info->timestamp = mkbp_last_event_time;
-
-	if (reset)
-		fifo_lost = 0;
 }
 
 static int motion_sense_get_next_event(uint8_t *out)
@@ -445,28 +395,14 @@ static int motion_sense_get_next_event(uint8_t *out)
 	union ec_response_get_next_data *data =
 		(union ec_response_get_next_data *)out;
 	/* out is not padded. It has one byte for the event type */
-	motion_sense_fifo_get_info(&data->sensor_fifo.info, 0);
+	motion_sense_get_fifo_info(&data->sensor_fifo.info);
 	return sizeof(data->sensor_fifo);
 }
 
 DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event);
 
-inline int motion_sense_fifo_over_thres(void)
-{
-	return queue_space(&fifo) < CONFIG_ACCEL_FIFO_THRES;
-}
-
-int motion_sense_fifo_read(int capacity_bytes, int max_count, void *out,
-			   uint16_t *out_size)
+inline int motion_sense_fifo_is_wake_up_needed(void)
 {
-	int count;
-
-	mutex_lock(&g_sensor_mutex);
-	count = MIN(capacity_bytes / fifo.unit_bytes,
-		    MIN(queue_count(&fifo), max_count));
-	count = queue_remove_units(&fifo, out, count);
-	mutex_unlock(&g_sensor_mutex);
-	*out_size = count * fifo.unit_bytes;
-
-	return count;
+	return queue_space(&motion_sense_fifo) < CONFIG_ACCEL_FIFO_THRES ||
+		wake_up_needed;
 }