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, 76 insertions, 62 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 77f007e556..7f34af1fc7 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -78,9 +78,6 @@ 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)
 {
@@ -613,36 +610,6 @@ 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)
@@ -665,24 +632,58 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		ret = sensor->drv->irq_handler(sensor, event);
 	}
 #endif
-	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 (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 (*event & TASK_EVENT_MOTION_FLUSH_PENDING) {
+			int flush_pending = atomic_read_clear(
+				&sensor->flush_pending);
 
-		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);
+			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);
+			}
 		}
 	}
 
@@ -691,7 +692,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_fifo_insert_async_event(
+			motion_sense_insert_async_event(
 				sensor, ASYNC_EVENT_ODR);
 	}
 	return ret;
@@ -901,18 +902,25 @@ void motion_sense_task(void *u)
 		 * - we haven't done it for a while.
 		 */
 		if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
-		    (motion_sense_fifo_wake_up_needed ||
+		    (motion_sense_fifo_is_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_add_timestamp(
+				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
+			 * 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
@@ -922,9 +930,9 @@ void motion_sense_task(void *u)
 			 */
 			if ((fifo_int_enabled &&
 			     sensor_active == SENSOR_ACTIVE_S0) ||
-			    motion_sense_fifo_wake_up_needed) {
+			    wake_up_needed) {
 				mkbp_send_event(EC_MKBP_EVENT_SENSOR_FIFO);
-				motion_sense_fifo_wake_up_needed = 0;
+				wake_up_needed = 0;
 			}
 #endif /* CONFIG_MKBP_EVENT */
 		}
@@ -1259,11 +1267,12 @@ static enum ec_status host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			args->response_size = sizeof(out->fifo_info);
 			break;
 		}
-		motion_sense_fifo_get_info(&out->fifo_info, 1);
+		motion_sense_get_fifo_info(&out->fifo_info);
 		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;
@@ -1271,12 +1280,17 @@ static enum ec_status 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;
-		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);
+		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;
 		break;
 	case MOTIONSENSE_CMD_FIFO_INT_ENABLE:
 		if (!IS_ENABLED(CONFIG_ACCEL_FIFO))