motion_sense: Allow multiple IRQ based sensors

Add a mask of custom events reserved for IRQ based sensors.
Copy data from raw_xzy to xyz while filling the FIFO
when FIFO is enabled.

BRANCH=smaug
TEST=Test with si1141 driver, check irq works for both driver.
BUG=chrome-os-partner:32829

Change-Id: I5e106df0c121e3bf1385f635195717395235ccc3
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/291334
Reviewed-by: Sheng-liang Song <ssl@chromium.org>

5 files changed, 52 insertions, 26 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index cad99aa633..4855a68d93 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -69,9 +69,12 @@ struct queue motion_sense_fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO,
 static int motion_sense_fifo_lost;
 
 void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
-				const struct motion_sensor_t *sensor)
+				struct motion_sensor_t *sensor,
+				int valid_data)
 {
 	struct ec_response_motion_sensor_data vector;
+	int i;
+
 	data->sensor_num = (sensor - motion_sensors);
 	mutex_lock(&g_sensor_mutex);
 	if (queue_space(&motion_sense_fifo) == 0) {
@@ -81,18 +84,19 @@ void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
 		if (vector.flags & MOTIONSENSE_SENSOR_FLAG_FLUSH)
 			CPRINTS("Lost flush for sensor %d", vector.sensor_num);
 	}
+	for (i = 0; i < valid_data; i++)
+		sensor->xyz[i] = data->data[i];
 	mutex_unlock(&g_sensor_mutex);
 	queue_add_unit(&motion_sense_fifo, data);
 }
 
-static inline void motion_sense_insert_flush(
-		const struct motion_sensor_t *sensor)
+static inline void motion_sense_insert_flush(struct motion_sensor_t *sensor)
 {
 	struct ec_response_motion_sensor_data vector;
 	vector.flags = MOTIONSENSE_SENSOR_FLAG_FLUSH |
 		       MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
 	vector.timestamp = __hw_clock_source_read();
-	motion_sense_fifo_add_unit(&vector, sensor);
+	motion_sense_fifo_add_unit(&vector, sensor, 0);
 }
 
 static inline void motion_sense_insert_timestamp(void)
@@ -100,7 +104,7 @@ static inline void motion_sense_insert_timestamp(void)
 	struct ec_response_motion_sensor_data vector;
 	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
 	vector.timestamp = __hw_clock_source_read();
-	motion_sense_fifo_add_unit(&vector, motion_sensors);
+	motion_sense_fifo_add_unit(&vector, motion_sensors, 0);
 }
 
 static void motion_sense_get_fifo_info(
@@ -357,9 +361,9 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 	int ret = EC_SUCCESS;
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
-	if ((event & TASK_EVENT_MOTION_INTERRUPT) &&
+	if ((event & TASK_EVENT_MOTION_INTERRUPT_MASK) &&
 	    (sensor->drv->irq_handler != NULL))
-		sensor->drv->irq_handler(sensor);
+		sensor->drv->irq_handler(sensor, event);
 #endif
 #ifdef CONFIG_ACCEL_FIFO
 	if (sensor->drv->load_fifo != NULL) {
@@ -370,11 +374,13 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		struct ec_response_motion_sensor_data vector;
 		sensor->last_collection = ts->val;
 		ret = motion_sense_read(sensor);
-		vector.flags = 0;
-		vector.data[X] = sensor->raw_xyz[X];
-		vector.data[Y] = sensor->raw_xyz[Y];
-		vector.data[Z] = sensor->raw_xyz[Z];
-		motion_sense_fifo_add_unit(&vector, sensor);
+		if (ret == EC_SUCCESS) {
+			vector.flags = 0;
+			vector.data[X] = sensor->raw_xyz[X];
+			vector.data[Y] = sensor->raw_xyz[Y];
+			vector.data[Z] = sensor->raw_xyz[Z];
+			motion_sense_fifo_add_unit(&vector, sensor, 3);
+		}
 	} else {
 		ret = EC_ERROR_BUSY;
 	}
@@ -395,6 +401,12 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 	} 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);
+	}
+
 #endif
 	return ret;
 }
@@ -455,10 +467,6 @@ void motion_sense_task(void)
 				if (ret != EC_SUCCESS)
 					continue;
 				ready_status |= (1 << i);
-				mutex_lock(&g_sensor_mutex);
-				memcpy(sensor->xyz, sensor->raw_xyz,
-					sizeof(sensor->xyz));
-				mutex_unlock(&g_sensor_mutex);
 			}
 		}
 
@@ -477,7 +485,7 @@ void motion_sense_task(void)
 #endif
 #ifdef CONFIG_CMD_ACCEL_INFO
 		if (accel_disp) {
-			CPRINTF("[%T ");
+			CPRINTF("[%T event 0x%08x ", event);
 			for (i = 0; i < motion_sensor_count; ++i) {
 				sensor = &motion_sensors[i];
 				CPRINTF("%s=%-5d, %-5d, %-5d ", sensor->name,
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index b3d32ec880..a6c29bfbb2 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -625,7 +625,8 @@ void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
  */
 void bmi160_interrupt(enum gpio_signal signal)
 {
-	task_set_event(TASK_ID_MOTIONSENSE, TASK_EVENT_MOTION_INTERRUPT, 0);
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCELGYRO_BMI160_INT_EVENT, 0);
 }
 
 
@@ -690,11 +691,12 @@ static int config_interrupt(const struct motion_sensor_t *s)
  * For now, we just print out. We should set a bitmask motion sense code will
  * act upon.
  */
-int irq_handler(const struct motion_sensor_t *s)
+static int irq_handler(struct motion_sensor_t *s, uint32_t event)
 {
 	int interrupt;
 
-	if (s->type != MOTIONSENSE_TYPE_ACCEL)
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(event & CONFIG_ACCELGYRO_BMI160_INT_EVENT)))
 		return EC_SUCCESS;
 
 	raw_read32(s->addr, BMI160_INT_STATUS_0, &interrupt);
@@ -770,7 +772,7 @@ static int bmi160_decode_header(struct motion_sensor_t *s,
 				vector.data[X] = v[X];
 				vector.data[Y] = v[Y];
 				vector.data[Z] = v[Z];
-				motion_sense_fifo_add_unit(&vector, s + i);
+				motion_sense_fifo_add_unit(&vector, s + i, 3);
 				*bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
 			}
 		}
diff --git a/include/accelgyro.h b/include/accelgyro.h
index af15c0cd58..f4cb542dc6 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -107,8 +107,9 @@ struct accelgyro_drv {
 	 * handler for interrupts triggered by the sensor: it runs in task and
 	 * process the events that triggered an interrupt.
 	 * @s Pointer to sensor data.
+	 * @event Event to process.
 	 */
-	int (*irq_handler)(const struct motion_sensor_t *s);
+	int (*irq_handler)(struct motion_sensor_t *s, uint32_t event);
 #endif
 #ifdef CONFIG_ACCEL_FIFO
 	/**
diff --git a/include/config.h b/include/config.h
index 4685c89f59..b4eb21df59 100644
--- a/include/config.h
+++ b/include/config.h
@@ -49,6 +49,12 @@
 #undef CONFIG_ACCELGYRO_LSM6DS0
 #undef CONFIG_ACCELGYRO_BMI160
 
+/*
+ * Define the event to raise when BMI160 interrupt.
+ * Must be within TASK_EVENT_MOTION_INTERRUPT_MASK.
+ */
+#undef CONFIG_ACCELGYRO_BMI160_INT_EVENT
+
 /* Compile chip support for analog-to-digital convertor */
 #undef CONFIG_ADC
 
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 7f0ac57edd..7acd22225d 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -29,9 +29,10 @@ enum sensor_state {
 #define SENSOR_ACTIVE_S0_S3_S5 (SENSOR_ACTIVE_S0_S3 | SENSOR_ACTIVE_S5)
 
 /* Events the motion sense task may have to process.*/
-#define TASK_EVENT_MOTION_FLUSH_PENDING TASK_EVENT_CUSTOM(1)
-#define TASK_EVENT_MOTION_INTERRUPT     TASK_EVENT_CUSTOM(2)
-#define TASK_EVENT_MOTION_ODR_CHANGE    TASK_EVENT_CUSTOM(4)
+#define TASK_EVENT_MOTION_FLUSH_PENDING     TASK_EVENT_CUSTOM(1)
+#define TASK_EVENT_MOTION_ODR_CHANGE        TASK_EVENT_CUSTOM(2)
+/* Next 8 events for sensor interrupt lines */
+#define TASK_EVENT_MOTION_INTERRUPT_MASK    (0xff << 2)
 
 /* Define sensor sampling interval in suspend. */
 #ifdef CONFIG_GESTURE_DETECTION
@@ -138,8 +139,16 @@ void accel_int_base(enum gpio_signal signal);
 #ifdef CONFIG_ACCEL_FIFO
 extern struct queue motion_sense_fifo;
 
+/**
+ * Interrupt function for lid accelerometer.
+ *
+ * @param data data to insert in the FIFO
+ * @param sensor sensor the data comes from
+ * @valid_data data should be copied into the public sensor vector
+ */
 void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
-				const struct motion_sensor_t *sensor);
+				struct motion_sensor_t *sensor,
+				int valid_data);
 
 #endif
 #endif /* __CROS_EC_MOTION_SENSE_H */