driver: bmi160: Add FIFO and interrupt support

Add FIFO support, where bmi160 hardware FIFO is copied in local fifo.
Add rudimentary support for single/double tap and lift detection.

BUG=chrome-os-partner:39900
BRANCH=smaug
TEST=Check on F411 that FIFO data is retrieved and correct.
Check on Smaug as well, with proper kernel the collect the FIFO:
- check that increasing sampling_frequency we are collecting
the FIFO less often
- check no frames are lost.
- check tap/lift interrupts are working
- if latency is less than 100ms, check we are collecting much faster.

Change-Id: Ic2317c27fad0ef31dacd6e18cd5f71ccd2cec807
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/274227

9 files changed, 731 insertions, 120 deletions

diff --git a/board/ryu/board.c b/board/ryu/board.c
index 11f7753ccd..11ece706c7 100644
--- a/board/ryu/board.c
+++ b/board/ryu/board.c
@@ -275,9 +275,6 @@ const unsigned int i2c_ports_used = ARRAY_SIZE(i2c_ports);
 /* Sensor mutex */
 static struct mutex g_mutex;
 
-/*  local sensor data (per-sensor) */
-struct bmi160_drv_data_t g_bmi160_data;
-
 struct motion_sensor_t motion_sensors[] = {
 
 	/*
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 8a34d1a613..153198e930 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -6,17 +6,21 @@
 /* Motion sense module to read from various motion sensors. */
 
 #include "accelgyro.h"
+#include "atomic.h"
 #include "chipset.h"
 #include "common.h"
 #include "console.h"
 #include "gesture.h"
 #include "hooks.h"
 #include "host_command.h"
+#include "hwtimer.h"
 #include "lid_angle.h"
 #include "math_util.h"
+#include "mkbp_event.h"
 #include "motion_sense.h"
 #include "motion_lid.h"
 #include "power.h"
+#include "queue.h"
 #include "timer.h"
 #include "task.h"
 #include "util.h"
@@ -34,23 +38,35 @@
 
 
 /* Bounds for setting the sensor polling interval. */
-#define MIN_POLLING_INTERVAL_MS 5
-#define MAX_POLLING_INTERVAL_MS 1000
+#define MIN_POLLING_INTERVAL_MS 1
 
 /* Define sensor sampling interval in suspend. */
 #ifdef CONFIG_GESTURE_DETECTION
-#define SUSPEND_SAMPLING_INTERVAL CONFIG_GESTURE_SAMPLING_INTERVAL_MS
+#define SUSPEND_SAMPLING_INTERVAL (CONFIG_GESTURE_SAMPLING_INTERVAL_MS * MSEC)
+#elif defined(CONFIG_ACCEL_FIFO)
+#define SUSPEND_SAMPLING_INTERVAL (1000 * MSEC)
 #else
-#define SUSPEND_SAMPLING_INTERVAL 100
+#define SUSPEND_SAMPLING_INTERVAL (100 * MSEC)
 #endif
 
 /* Accelerometer polling intervals based on chipset state. */
-static int accel_interval_ap_on_ms = 10;
+#ifdef CONFIG_ACCEL_FIFO
+/*
+ * TODO(crbug.com/498352):
+ * For now, we collect the data every accel_interval.
+ * For non-FIFO sensor, we should set accel_interval at a lower value.
+ * But if we have a mix a FIFO and non FIFO sensors, setting it
+ * to low will increase the number of interrupts.
+ */
+static int accel_interval_ap_on = 1000 * MSEC;
+#else
+static int accel_interval_ap_on = 10 * MSEC;
+#endif
 /*
  * Sampling interval for measuring acceleration and calculating lid angle.
- * Set to accel_interval_ap_on_ms when ap is on.
+ * Set to accel_interval_ap_on when ap is on.
  */
-static int accel_interval_ms;
+static int accel_interval;
 
 #ifdef CONFIG_CMD_ACCEL_INFO
 static int accel_disp;
@@ -64,6 +80,61 @@ static int accel_disp;
  */
 static struct mutex g_sensor_mutex;
 
+#ifdef CONFIG_ACCEL_FIFO
+struct queue motion_sense_fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO,
+		struct ec_response_motion_sensor_data);
+int motion_sense_fifo_lost;
+
+static void *nullcpy(void *dest, const void *src, size_t n)
+{
+	return dest;
+}
+
+void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
+				const struct motion_sensor_t *sensor)
+{
+	data->sensor_num = (sensor - motion_sensors);
+	mutex_lock(&g_sensor_mutex);
+	if (queue_space(&motion_sense_fifo) == 0) {
+		motion_sense_fifo_lost++;
+		queue_remove_memcpy(&motion_sense_fifo, NULL, 1,
+				nullcpy);
+	}
+	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)
+{
+	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);
+}
+
+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);
+}
+
+static void motion_sense_get_fifo_info(
+		struct ec_response_motion_sense_fifo_info *fifo_info)
+{
+	fifo_info->size = motion_sense_fifo.buffer_units;
+	mutex_lock(&g_sensor_mutex);
+	fifo_info->count = queue_count(&motion_sense_fifo);
+	fifo_info->lost = motion_sense_fifo_lost;
+	motion_sense_fifo_lost = 0;
+	mutex_unlock(&g_sensor_mutex);
+	fifo_info->timestamp = __hw_clock_source_read();
+}
+#endif
+
 static void motion_sense_shutdown(void)
 {
 	int i;
@@ -89,7 +160,7 @@ static void motion_sense_suspend(void)
 	int i;
 	struct motion_sensor_t *sensor;
 
-	accel_interval_ms = SUSPEND_SAMPLING_INTERVAL;
+	accel_interval = SUSPEND_SAMPLING_INTERVAL;
 
 	for (i = 0; i < motion_sensor_count; i++) {
 		sensor = &motion_sensors[i];
@@ -116,7 +187,7 @@ static void motion_sense_resume(void)
 	int i;
 	struct motion_sensor_t *sensor;
 
-	accel_interval_ms = accel_interval_ap_on_ms;
+	accel_interval = accel_interval_ap_on;
 
 	for (i = 0; i < motion_sensor_count; i++) {
 		sensor = &motion_sensors[i];
@@ -205,6 +276,44 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
 	return sensor->drv->read(sensor, sensor->raw_xyz);
 }
 
+static int motion_sense_process(struct motion_sensor_t *sensor,
+				uint32_t event,
+				int *flush_needed)
+{
+	int ret = EC_SUCCESS;
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	if ((event & TASK_EVENT_MOTION_INTERRUPT) &&
+	    (sensor->drv->irq_handler != NULL))
+		sensor->drv->irq_handler(sensor);
+#endif
+#ifdef CONFIG_ACCEL_FIFO
+	if (sensor->drv->load_fifo != NULL) {
+		/* Load fifo is filling raw_xyz sensor vector */
+		sensor->drv->load_fifo(sensor);
+	} else {
+		ret = motion_sense_read(sensor);
+		/* Put data in fifo.
+		 * Depending on the frequency on that particular sensor,
+		 * we may not do it all the time.
+		 * TODO(chromium:498352)
+		 */
+	}
+	if (event & TASK_EVENT_MOTION_FLUSH_PENDING) {
+		int flush_pending;
+		flush_pending = atomic_read_clear(&sensor->flush_pending);
+		for (; flush_pending > 0; flush_pending--) {
+			*flush_needed = 1;
+			motion_sense_insert_flush(sensor);
+		}
+	}
+#else
+	/* Get latest data for local calculation */
+	ret = motion_sense_read(sensor);
+#endif
+	return ret;
+}
+
 /*
  * Motion Sense Task
  * Requirement: motion_sensors[] are defined in board.c file.
@@ -214,14 +323,17 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
  */
 void motion_sense_task(void)
 {
-	int i;
-	int wait_us;
-	static timestamp_t ts0, ts1;
+	int i, ret, wait_us, fifo_flush_needed = 0;
+	static timestamp_t ts_begin_task, ts_end_task;
 	uint8_t *lpc_status;
 	uint16_t *lpc_data;
+	uint32_t event;
 	int sample_id = 0;
 	int rd_cnt;
 	struct motion_sensor_t *sensor;
+#ifdef CONFIG_ACCEL_FIFO
+	static timestamp_t ts_last_int;
+#endif
 
 	lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
 	lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA);
@@ -243,14 +355,18 @@ void motion_sense_task(void)
 			sensor->active = SENSOR_ACTIVE_S0;
 		}
 
-		accel_interval_ms = accel_interval_ap_on_ms;
+		accel_interval = accel_interval_ap_on;
 	} else {
 		/* sensor->active already initializes to SENSOR_ACTIVE_S5 */
-		accel_interval_ms = SUSPEND_SAMPLING_INTERVAL;
+		accel_interval = SUSPEND_SAMPLING_INTERVAL;
 	}
 
-	while (1) {
-		ts0 = get_time();
+	wait_us = accel_interval;
+#ifdef CONFIG_ACCEL_FIFO
+	ts_last_int = get_time();
+#endif
+	while ((event = task_wait_event(wait_us))) {
+		ts_begin_task = get_time();
 		rd_cnt = 0;
 		for (i = 0; i < motion_sensor_count; ++i) {
 
@@ -258,13 +374,13 @@ void motion_sense_task(void)
 
 			/* if the sensor is active in the current power state */
 			if (sensor->active & sensor->active_mask) {
-
 				if (sensor->state == SENSOR_NOT_INITIALIZED)
 					motion_sense_init(sensor);
 
-				if (EC_SUCCESS != motion_sense_read(sensor))
+				ret = motion_sense_process(sensor, event,
+						&fifo_flush_needed);
+				if (ret != EC_SUCCESS)
 					continue;
-
 				rd_cnt++;
 				/*
 				 * Rotate the accel vector so the reference for
@@ -314,9 +430,34 @@ void motion_sense_task(void)
 #endif
 		update_sense_data(lpc_status, lpc_data, &sample_id);
 
+		ts_end_task = get_time();
+#ifdef CONFIG_ACCEL_FIFO
+		/*
+		 * If ODR of any sensor changed, insert a timestamp to be ease
+		 * calculation of each events.
+		 */
+		if (event & TASK_EVENT_MOTION_ODR_CHANGE)
+			motion_sense_insert_timestamp();
+
+		/*
+		 * Ask the host to flush the queue if
+		 * - a flush event has been queued.
+		 * - the queue is almost full,
+		 * - we haven't done it for a while.
+		 */
+		if (fifo_flush_needed ||
+		    queue_space(&motion_sense_fifo) < CONFIG_ACCEL_FIFO_THRES ||
+		    (ts_end_task.val - ts_last_int.val) > accel_interval) {
+			if (!fifo_flush_needed)
+				motion_sense_insert_timestamp();
+			fifo_flush_needed = 0;
+			ts_last_int = ts_end_task;
+			mkbp_send_event(EC_MKBP_EVENT_SENSOR_FIFO);
+		}
+#endif
 		/* Delay appropriately to keep sampling time consistent. */
-		ts1 = get_time();
-		wait_us = accel_interval_ms * MSEC - (ts1.val-ts0.val);
+		wait_us = accel_interval -
+			(ts_end_task.val - ts_begin_task.val);
 
 		/*
 		 * Guarantee some minimum delay to allow other lower priority
@@ -325,10 +466,21 @@ void motion_sense_task(void)
 		if (wait_us < MIN_MOTION_SENSE_WAIT_TIME)
 			wait_us = MIN_MOTION_SENSE_WAIT_TIME;
 
-		task_wait_event(wait_us);
 	}
 }
 
+#ifdef CONFIG_ACCEL_FIFO
+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_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);
+#endif
 /*****************************************************************************/
 /* Host commands */
 
@@ -385,7 +537,6 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	case MOTIONSENSE_CMD_DATA:
 		sensor = host_sensor_id_to_motion_sensor(
 				in->sensor_odr.sensor_num);
-
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
 
@@ -402,7 +553,6 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 	case MOTIONSENSE_CMD_INFO:
 		sensor = host_sensor_id_to_motion_sensor(
 				in->sensor_odr.sensor_num);
-
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
 
@@ -423,14 +573,12 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			data = in->ec_rate.data;
 			if (data < MIN_POLLING_INTERVAL_MS)
 				data = MIN_POLLING_INTERVAL_MS;
-			if (data > MAX_POLLING_INTERVAL_MS)
-				data = MAX_POLLING_INTERVAL_MS;
 
-			accel_interval_ap_on_ms = data;
-			accel_interval_ms = data;
+			accel_interval_ap_on = data * MSEC;
+			accel_interval = data * MSEC;
 		}
 
-		out->ec_rate.ret = accel_interval_ap_on_ms;
+		out->ec_rate.ret = accel_interval_ap_on / MSEC;
 
 		args->response_size = sizeof(out->ec_rate);
 		break;
@@ -454,6 +602,12 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			}
 		}
 
+		/* To be sure timestamps are calculated properly,
+		 * Send an event to have a timestamp inserted in the FIFO.
+		 */
+		task_set_event(TASK_ID_MOTIONSENSE,
+			       TASK_EVENT_MOTION_ODR_CHANGE, 0);
+
 		sensor->drv->get_data_rate(sensor, &data);
 
 		/* Save configuration parameter: ODR */
@@ -490,6 +644,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 		out->sensor_range.ret = data;
 		args->response_size = sizeof(out->sensor_range);
 		break;
+
 	case MOTIONSENSE_CMD_SENSOR_OFFSET:
 		/* Verify sensor number is valid. */
 		sensor = host_sensor_id_to_motion_sensor(
@@ -512,6 +667,43 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 				&out->sensor_offset.temp);
 		args->response_size = sizeof(out->sensor_offset);
 		break;
+
+#ifdef CONFIG_ACCEL_FIFO
+	case MOTIONSENSE_CMD_FIFO_FLUSH:
+		sensor = host_sensor_id_to_motion_sensor(
+				in->sensor_odr.sensor_num);
+		if (sensor == NULL)
+			return EC_RES_INVALID_PARAM;
+		atomic_add(&sensor->flush_pending, 1);
+
+		task_set_event(TASK_ID_MOTIONSENSE,
+			       TASK_EVENT_MOTION_FLUSH_PENDING, 0);
+		/* passthrough */
+	case MOTIONSENSE_CMD_FIFO_INFO:
+		motion_sense_get_fifo_info(&out->fifo_info);
+		args->response_size = sizeof(out->fifo_info);
+		break;
+
+	case MOTIONSENSE_CMD_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;
+#else
+	case MOTIONSENSE_CMD_FIFO_INFO:
+		/* Only support the INFO command, to tell there is no FIFO. */
+		memset(&out->fifo_info, 0, sizeof(out->fifo_info));
+		args->response_size = sizeof(out->fifo_info);
+		break;
+#endif
 	default:
 		/* Call other users of the motion task */
 #ifdef CONFIG_LID_ANGLE
@@ -678,7 +870,7 @@ DECLARE_CONSOLE_COMMAND(accelrate, command_accel_data_rate,
 static int command_accel_read_xyz(int argc, char **argv)
 {
 	char *e;
-	int id, n = 1;
+	int id, n = 1, ret;
 	struct motion_sensor_t *sensor;
 	vector_3_t v;
 
@@ -697,9 +889,12 @@ static int command_accel_read_xyz(int argc, char **argv)
 	sensor = &motion_sensors[id];
 
 	while ((n == -1) || (n-- > 0)) {
-		sensor->drv->read(sensor, v);
-		ccprintf("Current raw data %d: %-5d %-5d %-5d\n",
-			 id, v[X], v[Y], v[Z]);
+		ret = sensor->drv->read(sensor, v);
+		if (ret == 0)
+			ccprintf("Current raw data %d: %-5d %-5d %-5d\n",
+				 id, v[X], v[Y], v[Z]);
+		else
+			ccprintf("vector not ready\n");
 		ccprintf("Last calib. data %d: %-5d %-5d %-5d\n",
 			 id, sensor->xyz[X], sensor->xyz[Y], sensor->xyz[Z]);
 		task_wait_event(MIN_MOTION_SENSE_WAIT_TIME);
@@ -763,7 +958,7 @@ static int command_display_accel_info(int argc, char **argv)
 		if (*e)
 			return EC_ERROR_PARAM2;
 
-		accel_interval_ms = val;
+		accel_interval = val * MSEC;
 	}
 
 	return EC_SUCCESS;
@@ -775,7 +970,7 @@ DECLARE_CONSOLE_COMMAND(accelinfo, command_display_accel_info,
 #endif /* CONFIG_CMD_ACCEL_INFO */
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
-/* TODO(crosbug.com/p/426659): this code is broken, does not compile. */
+/* TODO(crosbug.com/p/426659): this code is broken, does not with ST sensors. */
 void accel_int_lid(enum gpio_signal signal)
 {
 	/*
@@ -824,4 +1019,37 @@ DECLARE_CONSOLE_COMMAND(accelint, command_accelerometer_interrupt,
 	"Write interrupt threshold", NULL);
 #endif /* CONFIG_ACCEL_INTERRUPTS */
 
+#ifdef CONFIG_ACCEL_FIFO
+static int motion_sense_read_fifo(int argc, char **argv)
+{
+	int count, i;
+	struct ec_response_motion_sensor_data v;
+
+	if (argc < 1)
+		return EC_ERROR_PARAM_COUNT;
+
+	/* Limit the amount of data to avoid saturating the UART buffer */
+	count = MIN(queue_count(&motion_sense_fifo), 16);
+	for (i = 0; i < count; i++) {
+		queue_peek_units(&motion_sense_fifo, &v, i, 1);
+		if (v.flags & (MOTIONSENSE_SENSOR_FLAG_TIMESTAMP |
+			       MOTIONSENSE_SENSOR_FLAG_FLUSH)) {
+			uint64_t timestamp;
+			memcpy(&timestamp, v.data, sizeof(v.data));
+			ccprintf("Timestamp: 0x%016lx%s\n", timestamp,
+				 (v.flags & MOTIONSENSE_SENSOR_FLAG_FLUSH ?
+				  " - Flush" : ""));
+		} else {
+			ccprintf("%d %d: %-5d %-5d %-5d\n", i, v.sensor_num,
+				 v.data[X], v.data[Y], v.data[Z]);
+		}
+	}
+	return EC_SUCCESS;
+}
+
+DECLARE_CONSOLE_COMMAND(fiforead, motion_sense_read_fifo,
+	"id",
+	"Read Fifo sensor", NULL);
+#endif
+
 #endif /* CONFIG_CMD_ACCELS */
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 48190a963d..9e8682d41d 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -21,6 +21,7 @@
 
 #define CPUTS(outstr) cputs(CC_ACCEL, outstr)
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
+#define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
 
 /*
  * Struct for pairing an engineering value with the register value for a
@@ -117,7 +118,7 @@ static int get_engineering_val(const int reg_val,
 }
 
 /**
- * Read register from accelerometer.
+ * Read 8bit register from accelerometer.
  */
 static inline int raw_read8(const int addr, const int reg, int *data_ptr)
 {
@@ -125,13 +126,29 @@ static inline int raw_read8(const int addr, const int reg, int *data_ptr)
 }
 
 /**
- * Write register from accelerometer.
+ * Write 8bit register from accelerometer.
  */
 static inline int raw_write8(const int addr, const int reg, int data)
 {
 	return i2c_write8(I2C_PORT_ACCEL, addr, reg, data);
 }
 
+/**
+ * Read 16bit register from accelerometer.
+ */
+static inline int raw_read16(const int addr, const int reg, int *data_ptr)
+{
+	return i2c_read16(I2C_PORT_ACCEL, addr, reg, data_ptr);
+}
+
+/**
+ * Read 32bit register from accelerometer.
+ */
+static inline int raw_read32(const int addr, const int reg, int *data_ptr)
+{
+	return i2c_read32(I2C_PORT_ACCEL, addr, reg, data_ptr);
+}
+
 #ifdef CONFIG_MAG_BMI160_BMM150
 /**
  * Control access to the compass on the secondary i2c interface:
@@ -184,8 +201,7 @@ static int set_range(const struct motion_sensor_t *s,
 	int ret, range_tbl_size;
 	uint8_t reg_val, ctrl_reg;
 	const struct accel_param_pair *ranges;
-	struct motion_data_t *data =
-		&((struct bmi160_drv_data_t *)s->drv_data)->saved_data[s->type];
+	struct motion_data_t *data = BMI160_GET_SAVED_DATA(s);
 
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
 		data->range = range;
@@ -207,8 +223,7 @@ static int set_range(const struct motion_sensor_t *s,
 static int get_range(const struct motion_sensor_t *s,
 				int *range)
 {
-	struct motion_data_t *data =
-		&((struct bmi160_drv_data_t *)s->drv_data)->saved_data[s->type];
+	struct motion_data_t *data = BMI160_GET_SAVED_DATA(s);
 
 	*range = data->range;
 	return EC_SUCCESS;
@@ -235,15 +250,20 @@ static int set_data_rate(const struct motion_sensor_t *s,
 {
 	int ret, val, normalized_rate;
 	uint8_t ctrl_reg, reg_val;
-	struct motion_data_t *data =
-		&((struct bmi160_drv_data_t *)s->drv_data)->saved_data[s->type];
+	struct motion_data_t *data = BMI160_GET_SAVED_DATA(s);
 
 	if (rate == 0) {
-		/* suspend */
+		/* go to suspend mode */
 		ret = raw_write8(s->i2c_addr, BMI160_CMD_REG,
 				 BMI160_CMD_MODE_SUSPEND(s->type));
-		msleep(30);
+		msleep(3);
+		data->odr = 0;
 		return ret;
+	} else if (data->odr == 0) {
+		/* back from suspend mode */
+		ret = raw_write8(s->i2c_addr, BMI160_CMD_REG,
+				 BMI160_CMD_MODE_NORMAL(s->type));
+		msleep(3);
 	}
 	ctrl_reg = BMI160_CONF_REG(s->type);
 	reg_val = BMI160_ODR_TO_REG(rate);
@@ -311,8 +331,7 @@ accel_cleanup:
 static int get_data_rate(const struct motion_sensor_t *s,
 				int *rate)
 {
-	struct motion_data_t *data =
-		&((struct bmi160_drv_data_t *)s->drv_data)->saved_data[s->type];
+	struct motion_data_t *data = BMI160_GET_SAVED_DATA(s);
 
 	*rate = data->odr;
 	return EC_SUCCESS;
@@ -350,13 +369,249 @@ void normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
 }
 
 #ifdef CONFIG_ACCEL_INTERRUPTS
+/**
+ * bmi160_interrupt - called when the sensor activate the interrupt line.
+ *
+ * This is a "top half" interrupt handler, it just asks motion sense ask
+ * to schedule the "bottom half", ->irq_handler().
+ */
+void bmi160_interrupt(enum gpio_signal signal)
+{
+	task_set_event(TASK_ID_MOTIONSENSE, TASK_EVENT_MOTION_INTERRUPT, 0);
+}
+
+
 static int set_interrupt(const struct motion_sensor_t *s,
 			       unsigned int threshold)
 {
-	/* Currently unsupported. */
-	return EC_ERROR_UNKNOWN;
+	int ret, tmp;
+	if (s->type != MOTIONSENSE_TYPE_ACCEL)
+		return EC_SUCCESS;
+
+	mutex_lock(s->mutex);
+	raw_write8(s->i2c_addr, BMI160_CMD_REG, BMI160_CMD_FIFO_FLUSH);
+	msleep(30);
+	raw_write8(s->i2c_addr, BMI160_CMD_REG, BMI160_CMD_INT_RESET);
+
+	/* Latch until interupts */
+	/* configure int2 as an external input */
+	tmp = BMI160_INT2_INPUT_EN | BMI160_LATCH_FOREVER;
+	ret = raw_write8(s->i2c_addr, BMI160_INT_LATCH, tmp);
+
+	/* configure int1 as an interupt */
+	ret = raw_write8(s->i2c_addr, BMI160_INT_OUT_CTRL,
+		BMI160_INT_CTRL(1, OUTPUT_EN));
+
+	/* Map Simple/Double Tap to int 1
+	 * Map Flat interrupt to int 1
+	 */
+	ret = raw_write8(s->i2c_addr, BMI160_INT_MAP_REG(1),
+		BMI160_INT_FLAT | BMI160_INT_D_TAP | BMI160_INT_S_TAP);
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* map fifo water mark to int 1 */
+	ret = raw_write8(s->i2c_addr, BMI160_INT_FIFO_MAP,
+			BMI160_INT_MAP(1, FWM));
+
+	/* configure fifo watermark at 50% */
+	ret = raw_write8(s->i2c_addr, BMI160_FIFO_CONFIG_0,
+			512 / sizeof(uint32_t));
+	ret = raw_write8(s->i2c_addr, BMI160_FIFO_CONFIG_1,
+			BMI160_FIFO_TAG_INT1_EN |
+			BMI160_FIFO_TAG_INT2_EN |
+			BMI160_FIFO_HEADER_EN |
+			BMI160_FIFO_MAG_EN |
+			BMI160_FIFO_ACC_EN |
+			BMI160_FIFO_GYR_EN);
+#endif
+
+	/* Set double tap interrupt and fifo*/
+	ret = raw_read8(s->i2c_addr, BMI160_INT_EN_0, &tmp);
+	tmp |= BMI160_INT_FLAT_EN | BMI160_INT_D_TAP_EN | BMI160_INT_S_TAP_EN;
+	ret = raw_write8(s->i2c_addr, BMI160_INT_EN_0, tmp);
+
+#ifdef CONFIG_ACCEL_FIFO
+	ret = raw_read8(s->i2c_addr, BMI160_INT_EN_1, &tmp);
+	tmp |= BMI160_INT_FWM_EN;
+	ret = raw_write8(s->i2c_addr, BMI160_INT_EN_1, tmp);
+#endif
+
+	mutex_unlock(s->mutex);
+	return ret;
 }
+
+/**
+ * irq_handler - bottom half of the interrupt stack.
+ * Ran from the motion_sense task, finds the events that raised the interrupt.
+ *
+ * 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)
+{
+	int interrupt;
+
+	raw_read32(s->i2c_addr, BMI160_INT_STATUS_0, &interrupt);
+	raw_write8(s->i2c_addr, BMI160_CMD_REG, BMI160_CMD_INT_RESET);
+
+	if (interrupt & BMI160_S_TAP_INT)
+		CPRINTS("single tap: %08x", interrupt);
+	if (interrupt & BMI160_D_TAP_INT)
+		CPRINTS("double tap: %08x", interrupt);
+	if (interrupt & BMI160_FLAT_INT)
+		CPRINTS("flat: %08x", interrupt);
+	/*
+	 * No need to read the FIFO here, motion sense task is
+	 * doing it on every interrupt.
+	 */
+	return EC_SUCCESS;
+}
+
+#endif  /* CONFIG_ACCEL_INTERRUPTS */
+
+#ifdef CONFIG_ACCEL_FIFO
+enum fifo_state {
+	FIFO_HEADER,
+	FIFO_DATA_SKIP,
+	FIFO_DATA_TIME,
+	FIFO_DATA_CONFIG,
+};
+
+
+#define BMI160_FIFO_BUFFER 64
+static uint8_t bmi160_buffer[BMI160_FIFO_BUFFER];
+#define BUFFER_END(_buffer) ((_buffer) + sizeof(_buffer))
+/*
+ * Decode the header from the fifo.
+ * Return 0 if we need further processing.
+ * Sensor mutex must be held during processing, to protect the fifos.
+ *
+ * @s: base sensor
+ * @hdr: the header to decode
+ * @bp: current pointer in the buffer, updated when processing the header.
+ */
+static int bmi160_decode_header(struct motion_sensor_t *s,
+		enum fifo_header hdr, uint8_t **bp)
+{
+	if ((hdr & BMI160_FH_MODE_MASK) == BMI160_EMPTY &&
+			(hdr & BMI160_FH_PARM_MASK) != 0) {
+		int i, size = 0;
+		/* Check if there is enough space for the data frame */
+		for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL;
+		     i--) {
+			if (hdr & (1 << (i + BMI160_FH_PARM_OFFSET)))
+				size += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
+		}
+		if (*bp + size > BUFFER_END(bmi160_buffer)) {
+			/* frame is not complete, it
+			 * will be retransmitted.
+			 */
+			*bp = BUFFER_END(bmi160_buffer);
+			return 1;
+		}
+		for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL;
+		     i--) {
+			if (hdr & (1 << (i + BMI160_FH_PARM_OFFSET))) {
+				struct ec_response_motion_sensor_data vector;
+				int *v = (s + i)->raw_xyz;
+				vector.flags = 0;
+				normalize(s + i, v, *bp);
+				vector.data[X] = v[X];
+				vector.data[Y] = v[Y];
+				vector.data[Z] = v[Z];
+				motion_sense_fifo_add_unit(&vector, s + i);
+				*bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
+			}
+		}
+#if 0
+		if (hdr & BMI160_FH_EXT_MASK)
+			CPRINTF("%s%s\n",
+				(hdr & 0x1 ? "INT1" : ""),
+				(hdr & 0x2 ? "INT2" : ""));
 #endif
+		return 1;
+	} else {
+		return 0;
+	}
+}
+
+static int load_fifo(struct motion_sensor_t *s)
+{
+	int done = 0;
+	int fifo_length;
+
+	if (s->type != MOTIONSENSE_TYPE_ACCEL)
+		return EC_SUCCESS;
+
+	/* Read fifo length */
+	raw_read16(s->i2c_addr, BMI160_FIFO_LENGTH_0, &fifo_length);
+	fifo_length &= BMI160_FIFO_LENGTH_MASK;
+	if (fifo_length == 0)
+		return EC_SUCCESS;
+	do {
+		enum fifo_state state = FIFO_HEADER;
+		uint8_t fifo_reg = BMI160_FIFO_DATA;
+		uint8_t *bp = bmi160_buffer;
+		i2c_lock(I2C_PORT_ACCEL, 1);
+		i2c_xfer(I2C_PORT_ACCEL, s->i2c_addr,
+				&fifo_reg, 1, bmi160_buffer,
+				sizeof(bmi160_buffer), I2C_XFER_SINGLE);
+		i2c_lock(I2C_PORT_ACCEL, 0);
+		while (!done && bp != BUFFER_END(bmi160_buffer)) {
+			switch (state) {
+			case FIFO_HEADER: {
+				enum fifo_header hdr = *bp++;
+				if (bmi160_decode_header(s, hdr, &bp))
+					continue;
+				/* Other cases */
+				hdr &= 0xdc;
+				switch (hdr) {
+				case BMI160_EMPTY:
+					done = 1;
+					break;
+				case BMI160_SKIP:
+					state = FIFO_DATA_SKIP;
+					break;
+				case BMI160_TIME:
+					state = FIFO_DATA_TIME;
+					break;
+				case BMI160_CONFIG:
+					state = FIFO_DATA_CONFIG;
+					break;
+				default:
+					CPRINTS("Unknown header: 0x%02x", hdr);
+				}
+				break;
+			}
+			case FIFO_DATA_SKIP:
+				CPRINTF("skipped %d frames\n", *bp++);
+				state = FIFO_HEADER;
+				break;
+			case FIFO_DATA_CONFIG:
+				CPRINTF("config change: 0x%02x\n", *bp++);
+				state = FIFO_HEADER;
+				break;
+			case FIFO_DATA_TIME:
+				if (bp + 3 > BUFFER_END(bmi160_buffer)) {
+					bp = BUFFER_END(bmi160_buffer);
+					continue;
+				}
+				/* We are not requesting timestamp */
+				CPRINTF("timestamp %d\n", (bp[2] << 16) |
+					(bp[1] << 8) | bp[0]);
+				state = FIFO_HEADER;
+				bp += 3;
+				break;
+			default:
+				CPRINTS("Unknown data: 0x%02x\n", *bp++);
+				state = FIFO_HEADER;
+			}
+		}
+	} while (!done);
+	return EC_SUCCESS;
+}
+#endif  /* CONFIG_ACCEL_FIFO */
+
 
 static int is_data_ready(const struct motion_sensor_t *s, int *ready)
 {
@@ -425,8 +680,7 @@ static int init(const struct motion_sensor_t *s)
 
 
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
-		struct bmi160_drv_data_t *data =
-			(struct bmi160_drv_data_t *)s->drv_data;
+		struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
 
 		/* Reset the chip to be in a good state */
 		raw_write8(s->i2c_addr, BMI160_CMD_REG,
@@ -446,8 +700,7 @@ static int init(const struct motion_sensor_t *s)
 
 #ifdef CONFIG_MAG_BMI160_BMM150
 	if (s->type == MOTIONSENSE_TYPE_MAG) {
-		struct bmi160_drv_data_t *data =
-			(struct bmi160_drv_data_t *)s->drv_data;
+		struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
 		if ((data->flags & BMI160_FLAG_SEC_I2C_ENABLED) == 0) {
 			int ext_page_reg, pullup_reg;
 			/* Enable secondary interface */
@@ -527,7 +780,9 @@ static int init(const struct motion_sensor_t *s)
 		bmm150_mag_access_ctrl(s->i2c_addr, 0);
 	}
 #endif
-
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	ret = s->drv->set_interrupt(s, 0);
+#endif
 	/* Fifo setup is done elsewhere */
 	CPRINTF("[%T %s: MS Done Init type:0x%X range:%d odr:%d]\n",
 			s->name, s->type, s->runtime_config.range,
@@ -546,5 +801,13 @@ const struct accelgyro_drv bmi160_drv = {
 	.get_data_rate = get_data_rate,
 #ifdef CONFIG_ACCEL_INTERRUPTS
 	.set_interrupt = set_interrupt,
+	.irq_handler = irq_handler,
+#endif
+#ifdef CONFIG_ACCEL_FIFO
+	.load_fifo = load_fifo,
 #endif
 };
+
+struct bmi160_drv_data_t g_bmi160_data = {
+	.flags = 0,
+};
diff --git a/driver/accelgyro_bmi160.h b/driver/accelgyro_bmi160.h
index b73b28bbb9..d8b12a0bc8 100644
--- a/driver/accelgyro_bmi160.h
+++ b/driver/accelgyro_bmi160.h
@@ -49,39 +49,82 @@
 #define BMI160_SENSORTIME_2    0x1a
 
 #define BMI160_STATUS          0x1b
-#define BMI160_DRDY_ACC        0x80
-#define BMI160_DRDY_GYR        0x40
-#define BMI160_DRDY_MAG        0x20
+#define BMI160_POR_DETECTED        (1 << 0)
+#define BMI160_GYR_SLF_TST         (1 << 1)
+#define BMI160_MAG_MAN_OP          (1 << 2)
+#define BMI160_FOC_RDY             (1 << 3)
+#define BMI160_NVM_RDY             (1 << 4)
+#define BMI160_DRDY_MAG            (1 << 5)
+#define BMI160_DRDY_GYR            (1 << 6)
+#define BMI160_DRDY_ACC            (1 << 7)
 #define BMI160_DRDY_OFF(_sensor)   (7 - (_sensor))
 #define BMI160_DRDY_MASK(_sensor)  (1 << BMI160_DRDY_OFF(_sensor))
-#define BMI160_NVM_RDY         0x10
-#define BMI160_FOC_RDY         0x08
-#define BMI160_MAG_MAN_OP      0x04
-#define BMI160_GYR_SLF_TST     0x02
-#define BMI160_POR_DETECTED    0x01
-
 
+/* first 2 bytes are the interrupt reasons, next 2 some qualifier */
 #define BMI160_INT_STATUS_0    0x1c
+#define BMI160_STEP_INT            (1 << 0)
+#define BMI160_SIGMOT_INT          (1 << 1)
+#define BMI160_ANYM_INT            (1 << 2)
+#define BMI160_PMU_TRIGGER_INT     (1 << 3)
+#define BMI160_D_TAP_INT           (1 << 4)
+#define BMI160_S_TAP_INT           (1 << 5)
+#define BMI160_ORIENT_INT          (1 << 6)
+#define BMI160_FLAT_INT            (1 << 7)
+
 #define BMI160_INT_STATUS_1    0x1d
+#define BMI160_HIGHG_INT           (1 << (2 + 8))
+#define BMI160_LOWG_INT            (1 << (3 + 8))
+#define BMI160_DRDY_INT            (1 << (4 + 8))
+#define BMI160_FFULL_INT           (1 << (5 + 8))
+#define BMI160_FWM_INT             (1 << (6 + 8))
+#define BMI160_NOMO_INT            (1 << (7 + 8))
+
+#define BMI160_INT_MASK            0xFFFF
+
 #define BMI160_INT_STATUS_2    0x1e
 #define BMI160_INT_STATUS_3    0x1f
+#define BMI160_FIRST_X             (1 << (0 + 16))
+#define BMI160_FIRST_Y             (1 << (1 + 16))
+#define BMI160_FIRST_Z             (1 << (2 + 16))
+#define BMI160_SIGN                (1 << (3 + 16))
+#define BMI160_ANYM_OFFSET         0
+#define BMI160_TAP_OFFSET          4
+#define BMI160_HIGH_OFFSET         8
+#define BMI160_INT_INFO(_type, _data) \
+(CONCAT2(BMI160_, _data) << CONCAT3(BMI160_, _type, _OFFSET))
+
+#define BMI160_ORIENT_Z            (1 << (6 + 24))
+#define BMI160_FLAT                (1 << (7 + 24))
 
 #define BMI160_TEMPERATURE_0   0x20
 #define BMI160_TEMPERATURE_1   0x21
 
 #define BMI160_FIFO_LENGTH_0   0x22
 #define BMI160_FIFO_LENGTH_1   0x23
+#define BMI160_FIFO_LENGTH_MASK    ((1 << 11) - 1)
 #define BMI160_FIFO_DATA       0x24
+enum fifo_header {
+	BMI160_EMPTY = 0x80,
+	BMI160_SKIP = 0x40,
+	BMI160_TIME = 0x44,
+	BMI160_CONFIG = 0x48
+};
+
+#define BMI160_FH_MODE_MASK    0xc0
+#define BMI160_FH_PARM_OFFSET    2
+#define BMI160_FH_PARM_MASK    (0x7 << BMI160_FH_PARM_OFFSET)
+#define BMI160_FH_EXT_MASK     0x03
+
 
 #define BMI160_ACC_CONF        0x40
-#define BMI160_GSEL_2G         0X03
-#define BMI160_GSEL_4G         0X05
-#define BMI160_GSEL_8G         0X08
-#define BMI160_GSEL_16G        0X0C
+#define BMI160_GSEL_2G         0x03
+#define BMI160_GSEL_4G         0x05
+#define BMI160_GSEL_8G         0x08
+#define BMI160_GSEL_16G        0x0c
 
 #define BMI160_ODR_MASK                 0x0F
 
-#define BMI160_ACC_BW_OFFSET   4
+#define BMI160_ACC_BW_OFFSET            4
 #define BMI160_ACC_BW_MASK     (0x7 << BMI160_ACC_BW_OFFSET)
 #define BMI160_ACC_RANGE       0x41
 
@@ -119,6 +162,18 @@
 #define BMI160_FIFO_DOWNS      0x45
 #define BMI160_FIFO_CONFIG_0   0x46
 #define BMI160_FIFO_CONFIG_1   0x47
+#define BMI160_FIFO_TAG_TIME_EN    (1 << 1)
+#define BMI160_FIFO_TAG_INT2_EN    (1 << 2)
+#define BMI160_FIFO_TAG_INT1_EN    (1 << 3)
+#define BMI160_FIFO_HEADER_EN      (1 << 4)
+#define BMI160_FIFO_MAG_EN         (1 << 5)
+#define BMI160_FIFO_ACC_EN         (1 << 6)
+#define BMI160_FIFO_GYR_EN         (1 << 7)
+#define BMI160_FIFO_TARG_INT(_i)  CONCAT3(BMI160_FIFO_TAG_INT, _i, _EN)
+#define BMI160_FIFO_SENSOR_EN(_sensor) \
+	((_sensor) == MOTIONSENSE_TYPE_ACCEL ? BMI160_FIFO_ACC_EN : \
+	  ((_sensor) == MOTIONSENSE_TYPE_GYRO ? BMI160_FIFO_GYR_EN : \
+	   BMI160_FIFO_MAG_EN))
 
 #define BMI160_MAG_IF_0        0x4b
 #define BMI160_MAG_I2C_ADDRESS BMI160_MAG_IF_0
@@ -142,16 +197,71 @@
 #define BMI160_MAG_I2C_READ_DATA    BMI160_MAG_X_L_G
 
 #define BMI160_INT_EN_0        0x50
+#define BMI160_INT_ANYMO_X_EN      (1 << 0)
+#define BMI160_INT_ANYMO_Y_EN      (1 << 1)
+#define BMI160_INT_ANYMO_Z_EN      (1 << 2)
+#define BMI160_INT_D_TAP_EN        (1 << 4)
+#define BMI160_INT_S_TAP_EN        (1 << 5)
+#define BMI160_INT_ORIENT_EN       (1 << 6)
+#define BMI160_INT_FLAT_EN         (1 << 7)
 #define BMI160_INT_EN_1        0x51
+#define BMI160_INT_HIGHG_X_EN      (1 << 0)
+#define BMI160_INT_HIGHG_Y_EN      (1 << 1)
+#define BMI160_INT_HIGHG_Z_EN      (1 << 2)
+#define BMI160_INT_LOW_EN          (1 << 3)
+#define BMI160_INT_DRDY_EN         (1 << 4)
+#define BMI160_INT_FFUL_EN         (1 << 5)
+#define BMI160_INT_FWM_EN          (1 << 6)
 #define BMI160_INT_EN_2        0x52
+#define BMI160_INT_NOMOX_EN        (1 << 0)
+#define BMI160_INT_NOMOY_EN        (1 << 1)
+#define BMI160_INT_NOMOZ_EN        (1 << 2)
+#define BMI160_INT_STEP_DET_EN     (1 << 3)
 
 #define BMI160_INT_OUT_CTRL    0x53
+#define BMI160_INT_EDGE_CTRL       (1 << 0)
+#define BMI160_INT_LVL_CTRL        (1 << 1)
+#define BMI160_INT_OD              (1 << 2)
+#define BMI160_INT_OUTPUT_EN       (1 << 3)
+#define BMI160_INT1_CTRL_OFFSET     0
+#define BMI160_INT2_CTRL_OFFSET     4
+#define BMI160_INT_CTRL(_i, _bit) \
+(CONCAT2(BMI160_INT_, _bit) << CONCAT3(BMI160_INT, _i, _CTRL_OFFSET))
+
 #define BMI160_INT_LATCH       0x54
+#define BMI160_INT1_INPUT_EN       (1 << 4)
+#define BMI160_INT2_INPUT_EN       (1 << 5)
+#define BMI160_LATCH_MASK          0xf
+#define BMI160_LATCH_NONE          0
+#define BMI160_LATCH_FOREVER       0xf
 
 #define BMI160_INT_MAP_0       0x55
+#define BMI160_INT_LOWG_STEP       (1 << 0)
+#define BMI160_INT_HIGHG           (1 << 1)
+#define BMI160_INT_ANYMOTION       (1 << 2)
+#define BMI160_INT_NOMOTION        (1 << 3)
+#define BMI160_INT_D_TAP           (1 << 4)
+#define BMI160_INT_S_TAP           (1 << 5)
+#define BMI160_INT_ORIENT          (1 << 6)
+#define BMI160_INT_FLAT            (1 << 7)
+
 #define BMI160_INT_MAP_1       0x56
+#define BMI160_INT_PMU_TRIG        (1 << 0)
+#define BMI160_INT_FFULL           (1 << 1)
+#define BMI160_INT_FWM             (1 << 2)
+#define BMI160_INT_DRDY            (1 << 3)
+#define BMI160_INT1_MAP_OFFSET      4
+#define BMI160_INT2_MAP_OFFSET      0
+#define BMI160_INT_MAP(_i, _bit) \
+(CONCAT2(BMI160_INT_, _bit) << CONCAT3(BMI160_INT, _i, _MAP_OFFSET))
+#define BMI160_INT_FIFO_MAP    BMI160_INT_MAP_1
+
 #define BMI160_INT_MAP_2       0x57
 
+#define BMI160_INT_MAP_INT_1   BMI160_INT_MAP_0
+#define BMI160_INT_MAP_INT_2   BMI160_INT_MAP_2
+#define BMI160_INT_MAP_REG(_i)  CONCAT2(BMI160_INT_MAP_INT_, _i)
+
 #define BMI160_INT_DATA_0      0x58
 #define BMI160_INT_DATA_1      0x59
 
@@ -193,8 +303,10 @@
 #define BMI160_CMD_MAG_MODE_SUSP   0x18
 #define BMI160_CMD_MAG_MODE_NORMAL 0x19
 #define BMI160_CMD_MAG_MODE_LOWPOWER 0x1a
-#define BMI160_CMD_MODE_NORMAL(_sensor) (0x11 + 4 * (_sensor))
-#define BMI160_CMD_MODE_SUSPEND(_sensor) (0x10 + 4 * (_sensor))
+#define BMI160_CMD_MODE_SUSPEND(_sensor) \
+	(BMI160_CMD_ACC_MODE_SUSP + 4 * (_sensor))
+#define BMI160_CMD_MODE_NORMAL(_sensor) \
+	(BMI160_CMD_ACC_MODE_NORMAL + 4 * (_sensor))
 
 #define BMI160_CMD_FIFO_FLUSH      0xb0
 #define BMI160_CMD_INT_RESET       0xb1
@@ -221,52 +333,6 @@
 #define BMI160_FF_DATA_LEN_GYR          6
 #define BMI160_FF_DATA_LEN_MAG          8
 
-#if 0
-#define BMI160_DPS_SEL_245     (0 << 3)
-#define BMI160_DPS_SEL_500     (1 << 3)
-#define BMI160_DPS_SEL_1000    (2 << 3)
-#define BMI160_DPS_SEL_2000    (3 << 3)
-#define BMI160_GSEL_2G         (0 << 3)
-#define BMI160_GSEL_4G         (2 << 3)
-#define BMI160_GSEL_8G         (3 << 3)
-
-#define BMI160_RANGE_MASK      (3 << 3)
-
-#define BMI160_ODR_PD          (0 << 5)
-#define BMI160_ODR_10HZ        (1 << 5)
-#define BMI160_ODR_15HZ        (1 << 5)
-#define BMI160_ODR_50HZ        (2 << 5)
-#define BMI160_ODR_59HZ        (2 << 5)
-#define BMI160_ODR_119HZ       (3 << 5)
-#define BMI160_ODR_238HZ       (4 << 5)
-#define BMI160_ODR_476HZ       (5 << 5)
-#define BMI160_ODR_952HZ       (6 << 5)
-
-#define BMI160_ODR_MASK        (7 << 5)
-
-/*
- * Register      : STATUS_REG
- * Address       : 0X27
- */
-enum bmi160_status {
-	BMI160_STS_DOWN                = 0x00,
-	BMI160_STS_XLDA_UP             = 0x01,
-	BMI160_STS_GDA_UP              = 0x02,
-};
-#define         BMI160_STS_XLDA_MASK       0x01
-#define         BMI160_STS_GDA_MASK        0x02
-
-/*
- * Register      : CTRL_REG8
- * Address       : 0X22
- * Bit Group Name: BDU
- */
-enum bmi160_bdu {
-	BMI160_BDU_DISABLE              = 0x00,
-	BMI160_BDU_ENABLE               = 0x40,
-};
-#endif
-
 /* Sensor resolution in number of bits. This sensor has fixed resolution. */
 #define BMI160_RESOLUTION      16
 
@@ -285,8 +351,19 @@ enum bmi160_running_mode {
 };
 
 #define BMI160_FLAG_SEC_I2C_ENABLED    (1 << 0)
+
 struct bmi160_drv_data_t {
 	struct motion_data_t saved_data[3];
 	uint8_t              flags;
 };
+
+#define BMI160_GET_DATA(_s) \
+	((struct bmi160_drv_data_t *)(_s)->drv_data)
+#define BMI160_GET_SAVED_DATA(_s) \
+	(&BMI160_GET_DATA(_s)->saved_data[(_s)->type])
+
+extern struct bmi160_drv_data_t g_bmi160_data;
+
+void bmi160_interrupt(enum gpio_signal signal);
+
 #endif /* __CROS_EC_ACCELGYRO_BMI160_H */
diff --git a/include/accelgyro.h b/include/accelgyro.h
index 723efa569c..1c1c92566d 100644
--- a/include/accelgyro.h
+++ b/include/accelgyro.h
@@ -101,6 +101,23 @@ struct accelgyro_drv {
 	 */
 	int (*set_interrupt)(const struct motion_sensor_t *s,
 			     unsigned int threshold);
+
+	/**
+	 * handler for interrupts triggered by the sensor: it runs in task and
+	 * process the events that triggered an interrupt.
+	 * @s Pointer to sensor data.
+	 */
+	int (*irq_handler)(const struct motion_sensor_t *s);
+#endif
+#ifdef CONFIG_ACCEL_FIFO
+	/**
+	 * Retrieve hardware FIFO from sensor,
+	 * - put data in Sensor Hub fifo.
+	 * - update sensor raw_xyz vector with the last information.
+	 * We put raw data in hub fifo and process data from theres.
+	 * @s Pointer to sensor data.
+	 */
+	int (*load_fifo)(struct motion_sensor_t *s);
 #endif
 };
 
diff --git a/include/config.h b/include/config.h
index 2f80a9aa2b..a913009aeb 100644
--- a/include/config.h
+++ b/include/config.h
@@ -38,6 +38,11 @@
 
 /* Enable accelerometer interrupts. */
 #undef CONFIG_ACCEL_INTERRUPTS
+/* Add support for sensor FIFO:
+ * define the size of the global fifo, must be a power of 2. */
+#undef CONFIG_ACCEL_FIFO
+/* The amount of free entries that trigger an interrupt to the AP. */
+#undef CONFIG_ACCEL_FIFO_THRES
 
 /* Specify type of accelerometers attached. */
 #undef CONFIG_ACCEL_KXCJ9
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 0e0275ec23..1c95051ee2 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1721,7 +1721,13 @@ struct ec_response_motion_sensor_data {
 	/* sensor number the data comes from */
 	uint8_t sensor_num;
 	/* Each sensor is up to 3-axis. */
-	int16_t data[3];
+	union {
+		int16_t             data[3];
+		struct {
+			uint16_t    rsvd;
+			uint32_t    timestamp;
+		} __packed;
+	};
 } __packed;
 
 struct ec_response_motion_sense_fifo_info {
diff --git a/include/motion_sense.h b/include/motion_sense.h
index bdbd7b238c..dc6c51bac9 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -13,6 +13,8 @@
 #include "ec_commands.h"
 #include "gpio.h"
 #include "math_util.h"
+#include "queue.h"
+#include "timer.h"
 
 enum sensor_state {
 	SENSOR_NOT_INITIALIZED = 0,
@@ -55,6 +57,7 @@ struct motion_sensor_t {
 	enum chipset_state_mask active;
 	vector_3_t raw_xyz;
 	vector_3_t xyz;
+	uint32_t flush_pending;
 };
 
 /* Defined at board level. */
@@ -83,4 +86,17 @@ void accel_int_lid(enum gpio_signal signal);
 void accel_int_base(enum gpio_signal signal);
 #endif
 
+#ifdef CONFIG_ACCEL_FIFO
+extern struct queue motion_sense_fifo;
+
+void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
+				const struct motion_sensor_t *sensor);
+
+#endif
+
+/* 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)
+
 #endif /* __CROS_EC_MOTION_SENSE_H */
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 62b7cbf3eb..84a539e226 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -158,6 +158,8 @@ static int test_lid_angle(void)
 	lid->xyz[Z] = 1000;
 	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
 	task_wake(TASK_ID_MOTIONSENSE);
+	msleep(5);
+	task_wake(TASK_ID_MOTIONSENSE);
 	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
 		msleep(5);
 	TEST_ASSERT(motion_lid_get_angle() == 0);