motion_sense: Fix null pointer reference in gesture detection

The motion_sense_fifo_stage_data with NULL were removed in CL:4499747.
But we need to stage and commit the motion state changed to activity
sensor for body detection on the ap side. Revert the change and Add
null pointer check in motion_sense_fifo_stage_data and fifo_stage_unit
to fix it.

BUG=b:272655176
TEST=./twister -v -i -p native_posix -p unit_testing -s
drivers/drivers.body_detection
TEST=received User proximity: Near/Far event from powerd log

Change-Id: I1ef882f25e07fa17d879d8b0a4b9ffe211b071e7
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/4520740
Tested-by: Ruihai Zhou <zhouruihai@huaqin.corp-partner.google.com>
Commit-Queue: Tim Van Patten <timvp@google.com>
Reviewed-by: Sung-Chi Li <lschyi@chromium.org>
Reviewed-by: Tim Van Patten <timvp@google.com>

3 files changed, 57 insertions, 2 deletions

diff --git a/common/body_detection.c b/common/body_detection.c
index d16e83df00..646bc6d740 100644
--- a/common/body_detection.c
+++ b/common/body_detection.c
@@ -102,6 +102,19 @@ void body_detect_change_state(enum body_detect_states state, bool spoof)
 {
 	if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE) && spoof_enable && !spoof)
 		return;
+	if (IS_ENABLED(CONFIG_GESTURE_HOST_DETECTION)) {
+		struct ec_response_motion_sensor_data vector = {
+			.flags = MOTIONSENSE_SENSOR_FLAG_BYPASS_FIFO,
+			.activity_data = {
+				.activity = MOTIONSENSE_ACTIVITY_BODY_DETECTION,
+				.state = state,
+			},
+			.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID,
+		};
+		motion_sense_fifo_stage_data(&vector, NULL, 0,
+					     __hw_clock_source_read());
+		motion_sense_fifo_commit_data();
+	}
 	/* change the motion state */
 	motion_state = state;
 	if (state == BODY_DETECTION_ON_BODY) {
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 16a37d37a5..546034c7e2 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -698,6 +698,26 @@ static void check_and_queue_gestures(uint32_t *event)
 	if (IS_ENABLED(CONFIG_GESTURE_SENSOR_DOUBLE_TAP) &&
 	    (*event & TASK_EVENT_MOTION_ACTIVITY_INTERRUPT(
 			      MOTIONSENSE_ACTIVITY_DOUBLE_TAP))) {
+		if (IS_ENABLED(CONFIG_GESTURE_HOST_DETECTION)) {
+			struct ec_response_motion_sensor_data vector;
+
+			vector.flags = MOTIONSENSE_SENSOR_FLAG_BYPASS_FIFO;
+			/*
+			 * Send events to the FIFO
+			 * AP is ignoring double tap event, do no wake up and no
+			 * automatic disable.
+			 */
+			if (IS_ENABLED(
+				    CONFIG_GESTURE_SENSOR_DOUBLE_TAP_FOR_HOST))
+				vector.flags |= MOTIONSENSE_SENSOR_FLAG_WAKEUP;
+			vector.activity_data.activity =
+				MOTIONSENSE_ACTIVITY_DOUBLE_TAP;
+			vector.activity_data.state = 1 /* triggered */;
+			vector.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID;
+			motion_sense_fifo_stage_data(&vector, NULL, 0,
+						     __hw_clock_source_read());
+			motion_sense_fifo_commit_data();
+		}
 		/* Call board specific function to process tap */
 		sensor_board_proc_double_tap();
 	}
@@ -705,7 +725,20 @@ static void check_and_queue_gestures(uint32_t *event)
 	    (*event & TASK_EVENT_MOTION_ACTIVITY_INTERRUPT(
 			      MOTIONSENSE_ACTIVITY_SIG_MOTION))) {
 		struct motion_sensor_t *activity_sensor;
-
+		if (IS_ENABLED(CONFIG_GESTURE_HOST_DETECTION)) {
+			struct ec_response_motion_sensor_data vector;
+
+			/* Send events to the FIFO */
+			vector.flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP |
+				       MOTIONSENSE_SENSOR_FLAG_BYPASS_FIFO;
+			vector.activity_data.activity =
+				MOTIONSENSE_ACTIVITY_SIG_MOTION;
+			vector.activity_data.state = 1 /* triggered */;
+			vector.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID;
+			motion_sense_fifo_stage_data(&vector, NULL, 0,
+						     __hw_clock_source_read());
+			motion_sense_fifo_commit_data();
+		}
 		/* Disable further detection */
 		activity_sensor = &motion_sensors[CONFIG_GESTURE_SIGMO_SENSOR];
 		activity_sensor->drv->manage_activity(
@@ -730,6 +763,12 @@ static void check_and_queue_gestures(uint32_t *event)
 			    (*motion_orientation_ptr(sensor) !=
 			     MOTIONSENSE_ORIENTATION_UNKNOWN)) {
 				motion_orientation_update(sensor);
+				vector.activity_data.state =
+					*motion_orientation_ptr(sensor);
+				motion_sense_fifo_stage_data(
+					&vector, NULL, 0,
+					__hw_clock_source_read());
+				motion_sense_fifo_commit_data();
 				if (IS_ENABLED(CONFIG_DEBUG_ORIENTATION)) {
 					static const char *const mode[] = {
 						"Landscape", "Portrait",
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index fd1bfda88c..1a7303bc79 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -271,6 +271,9 @@ static void fifo_stage_unit(struct ec_response_motion_sensor_data *data,
 	struct queue_chunk chunk;
 	int i;
 
+	if (valid_data > 0 && !sensor)
+		return;
+
 	mutex_lock(&g_sensor_mutex);
 
 	for (i = 0; i < valid_data; i++)
@@ -460,7 +463,7 @@ void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
 			fifo_staged.read_ts = __hw_clock_source_read();
 		fifo_stage_timestamp(time, data->sensor_num);
 	}
-	if (sensor->config[SENSOR_CONFIG_AP].ec_rate > 0 &&
+	if (sensor && sensor->config[SENSOR_CONFIG_AP].ec_rate > 0 &&
 	    time_after(time, ts_last_int[id] +
 				     sensor->config[SENSOR_CONFIG_AP].ec_rate -
 				     MOTION_SENSOR_INT_ADJUSTMENT_US)) {