common: Refactor motion_sense_fifo

This change refactors the motion_sense_fifo to uniformly prefix
all the functions to avoid collisions. It also adds several unit
tests and fixes a few bugs with the fifo logic.

BUG=b:137758297
BRANCH=None
TEST=buildall & run CTS on arcada

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

1 files changed, 62 insertions, 76 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 650f2a7617..c74971a14b 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -78,6 +78,9 @@ static void print_spoof_mode_status(int id);
 /* Flags to control whether to send an ODR change event for a sensor */
 static uint32_t odr_event_required;
 
+/* Number of element the AP should collect */
+__maybe_unused static int fifo_int_enabled;
+
 static inline int motion_sensor_in_forced_mode(
 		const struct motion_sensor_t *sensor)
 {
@@ -610,6 +613,36 @@ static inline void increment_sensor_collection(struct motion_sensor_t *sensor,
 	}
 }
 
+/**
+ * Commit the data in a sensor's raw_xyz vector. This operation might have
+ * different meanings depending on the CONFIG_ACCEL_FIFO flag.
+ *
+ * @param s Pointer to the sensor.
+ */
+static void motion_sense_push_raw_xyz(struct motion_sensor_t *s)
+{
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
+		struct ec_response_motion_sensor_data vector;
+		int *v = s->raw_xyz;
+
+		vector.flags = 0;
+		vector.sensor_num = s - motion_sensors;
+		if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE) &&
+		    s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE)
+			v = s->spoof_xyz;
+		mutex_lock(&g_sensor_mutex);
+		memcpy(vector.data, v, sizeof(v));
+		mutex_unlock(&g_sensor_mutex);
+		motion_sense_fifo_stage_data(&vector, s, 3,
+					     __hw_clock_source_read());
+		motion_sense_fifo_commit_data();
+	} else {
+		mutex_lock(&g_sensor_mutex);
+		memcpy(s->xyz, s->raw_xyz, sizeof(s->xyz));
+		mutex_unlock(&g_sensor_mutex);
+	}
+}
+
 static int motion_sense_process(struct motion_sensor_t *sensor,
 				uint32_t *event,
 				const timestamp_t *ts)
@@ -632,58 +665,24 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		ret = sensor->drv->irq_handler(sensor, event);
 	}
 #endif
-	if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
-		if (motion_sensor_in_forced_mode(sensor)) {
-			if (motion_sensor_time_to_read(ts, sensor)) {
-				struct ec_response_motion_sensor_data vector;
-				int *v = sensor->raw_xyz;
-
-				ret = motion_sense_read(sensor);
-				if (ret == EC_SUCCESS) {
-					vector.flags = 0;
-					vector.sensor_num = sensor -
-						motion_sensors;
-					if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE)
-					    && sensor->flags &
-					    MOTIONSENSE_FLAG_IN_SPOOF_MODE)
-						v = sensor->spoof_xyz;
-					vector.data[X] = v[X];
-					vector.data[Y] = v[Y];
-					vector.data[Z] = v[Z];
-					motion_sense_fifo_stage_data(
-						&vector, sensor, 3,
-						__hw_clock_source_read());
-					motion_sense_fifo_commit_data();
-				}
-				increment_sensor_collection(sensor, ts);
-			} else {
-				ret = EC_ERROR_BUSY;
-			}
+	if (motion_sensor_in_forced_mode(sensor)) {
+		if (motion_sensor_time_to_read(ts, sensor)) {
+			ret = motion_sense_read(sensor);
+			increment_sensor_collection(sensor, ts);
+		} else {
+			ret = EC_ERROR_BUSY;
 		}
-		if (*event & TASK_EVENT_MOTION_FLUSH_PENDING) {
-			int flush_pending = atomic_read_clear(
-				&sensor->flush_pending);
 
-			for (; flush_pending > 0; flush_pending--) {
-				motion_sense_insert_async_event(sensor,
-					ASYNC_EVENT_FLUSH);
-			}
-		}
-	} else {
-		if (motion_sensor_in_forced_mode(sensor)) {
-			if (motion_sensor_time_to_read(ts, sensor)) {
-				/* Get latest data for local calculation */
-				ret = motion_sense_read(sensor);
-				increment_sensor_collection(sensor, ts);
-			} else {
-				ret = EC_ERROR_BUSY;
-			}
-			if (ret == EC_SUCCESS) {
-				mutex_lock(&g_sensor_mutex);
-				memcpy(sensor->xyz, sensor->raw_xyz,
-				       sizeof(sensor->xyz));
-				mutex_unlock(&g_sensor_mutex);
-			}
+		if (ret == EC_SUCCESS)
+			motion_sense_push_raw_xyz(sensor);
+	}
+	if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
+	    *event & TASK_EVENT_MOTION_FLUSH_PENDING) {
+		int flush_pending = atomic_read_clear(&sensor->flush_pending);
+
+		for (; flush_pending > 0; flush_pending--) {
+			motion_sense_fifo_insert_async_event(
+				sensor, ASYNC_EVENT_FLUSH);
 		}
 	}
 
@@ -692,7 +691,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		motion_sense_set_data_rate(sensor);
 		motion_sense_set_motion_intervals();
 		if (IS_ENABLED(CONFIG_ACCEL_FIFO))
-			motion_sense_insert_async_event(
+			motion_sense_fifo_insert_async_event(
 				sensor, ASYNC_EVENT_ODR);
 	}
 	return ret;
@@ -902,25 +901,18 @@ void motion_sense_task(void *u)
 		 * - we haven't done it for a while.
 		 */
 		if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
-		    (motion_sense_fifo_is_wake_up_needed() ||
+		    (motion_sense_fifo_wake_up_needed ||
 		     event & (TASK_EVENT_MOTION_ODR_CHANGE |
 			      TASK_EVENT_MOTION_FLUSH_PENDING) ||
+		     motion_sense_fifo_over_thres() ||
 		     (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_fifo_stage_timestamp(
+				motion_sense_fifo_add_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
-			 * collect.
-			 */
-			mutex_lock(&g_sensor_mutex);
-			fifo_queue_count = queue_count(&motion_sense_fifo);
-			mutex_unlock(&g_sensor_mutex);
 #ifdef CONFIG_MKBP_EVENT
 			/*
 			 * Send an event if we know we are in S0 and the kernel
@@ -930,9 +922,9 @@ void motion_sense_task(void *u)
 			 */
 			if ((fifo_int_enabled &&
 			     sensor_active == SENSOR_ACTIVE_S0) ||
-			    wake_up_needed) {
+			    motion_sense_fifo_wake_up_needed) {
 				mkbp_send_event(EC_MKBP_EVENT_SENSOR_FIFO);
-				wake_up_needed = 0;
+				motion_sense_fifo_wake_up_needed = 0;
 			}
 #endif /* CONFIG_MKBP_EVENT */
 		}
@@ -1267,12 +1259,11 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			args->response_size = sizeof(out->fifo_info);
 			break;
 		}
-		motion_sense_get_fifo_info(&out->fifo_info);
+		motion_sense_fifo_get_info(&out->fifo_info, 1);
 		for (i = 0; i < motion_sensor_count; i++) {
 			out->fifo_info.lost[i] = motion_sensors[i].lost;
 			motion_sensors[i].lost = 0;
 		}
-		motion_sense_fifo_lost = 0;
 		args->response_size = sizeof(out->fifo_info) +
 			sizeof(uint16_t) * motion_sensor_count;
 		break;
@@ -1280,17 +1271,12 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	case MOTIONSENSE_CMD_FIFO_READ:
 		if (!IS_ENABLED(CONFIG_ACCEL_FIFO))
 			return EC_RES_INVALID_PARAM;
-		mutex_lock(&g_sensor_mutex);
-		reported = MIN((args->response_max - sizeof(out->fifo_read)) /
-			       motion_sense_fifo.unit_bytes,
-			       MIN(queue_count(&motion_sense_fifo),
-				   in->fifo_read.max_data_vector));
-		reported = queue_remove_units(&motion_sense_fifo,
-				out->fifo_read.data, reported);
-		mutex_unlock(&g_sensor_mutex);
-		out->fifo_read.number_data = reported;
-		args->response_size = sizeof(out->fifo_read) + reported *
-			motion_sense_fifo.unit_bytes;
+		out->fifo_read.number_data = motion_sense_fifo_read(
+			args->response_max - sizeof(out->fifo_read),
+			in->fifo_read.max_data_vector,
+			out->fifo_read.data,
+			&(args->response_size));
+		args->response_size += sizeof(out->fifo_read);
 		break;
 	case MOTIONSENSE_CMD_FIFO_INT_ENABLE:
 		if (!IS_ENABLED(CONFIG_ACCEL_FIFO))