From 87e769a87e43c17eea69cbe6d04c3dc072f79a91 Mon Sep 17 00:00:00 2001
From: Mario Tesi <mario.tesi@st.com>
Date: Tue, 4 Apr 2017 11:00:55 +0200
Subject: [PATCH] driver: lsm6dsm/lsm6dsl: Add FIFO support

Added FIFO support to Acc and Gyro with
watermark interrupt management.
Watermark is configurable setting macro
CONFIG_ACCEL_FIFO_THRES properly (board.h)

BUG=b:73546254
BRANCH=master
TEST=Pass CTS test cheets_CTS_N.7.1_r15.x86.CtsHardwareTestCases
on meowth.
TEST=Tested on discovery (target stmems) BOARD with LSM6DSM
connected to EC i2c master bus.

Using motion sense console commands is possible to:
 - enable sensor (accelinit 0, accelinit 1 for acc and gyro)
 - set ODR (accelrate 0 x, accelrate 1 y where x, y are mHz)
 - show sensor data (accelinfo on <time> where time is in ms)

Using this procedure is possible to see Green Led of Discovery Board
Blinking each time an interrupt from FIFO arrives. To be sure to
generate interrupt is better to use high ODR and low time in accelinfo.

Change-Id: Icf95b0e889dc806206b8ca50e74636e6a2441a18
Signed-off-by: Mario Tesi <mario.tesi@st.com>
Reviewed-on: https://chromium-review.googlesource.com/467326
Commit-Ready: Gwendal Grignou <gwendal@chromium.org>
Tested-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
---
 driver/accelgyro_lsm6dsm.c | 382 ++++++++++++++++++++++++++++++++-----
 driver/accelgyro_lsm6dsm.h | 122 ++++++++++--
 driver/stm_mems_common.c   |   5 -
 3 files changed, 445 insertions(+), 64 deletions(-)

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 9eff6fecc6..eaccda78ca 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -11,13 +11,19 @@
 
 #include "driver/accelgyro_lsm6dsm.h"
 #include "hooks.h"
+#include "hwtimer.h"
 #include "math_util.h"
 #include "task.h"
+#include "timer.h"
 
 #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)
 
+#ifdef CONFIG_ACCEL_FIFO
+static uint32_t last_interrupt_timestamp;
+#endif
+
 /**
  * @return output base register for sensor
  */
@@ -27,6 +33,298 @@ static inline int get_xyz_reg(enum motionsensor_type type)
 		(LSM6DSM_ACCEL_OUT_X_L_ADDR - LSM6DSM_GYRO_OUT_X_L_ADDR) * type;
 }
 
+#ifdef CONFIG_ACCEL_INTERRUPTS
+/**
+ * Configure interrupt int 1 to fire handler for:
+ *
+ * FIFO threshold on watermark
+ *
+ * @s: Motion sensor pointer
+ */
+static int config_interrupt(const struct motion_sensor_t *s)
+{
+	int ret = EC_SUCCESS;
+	int int1_ctrl_val;
+
+	ret = raw_read8(s->port, s->addr, LSM6DSM_INT1_CTRL, &int1_ctrl_val);
+	if (ret != EC_SUCCESS)
+		return ret;
+
+#ifdef CONFIG_ACCEL_FIFO
+	/* As soon as one sample is ready, trigger an interrupt. */
+	ret = raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL1_ADDR,
+			 OUT_XYZ_SIZE / sizeof(uint16_t));
+	if (ret != EC_SUCCESS)
+		return ret;
+	int1_ctrl_val |= LSM6DSM_INT_FIFO_TH | LSM6DSM_INT_FIFO_OVR |
+		LSM6DSM_INT_FIFO_FULL;
+#endif /* CONFIG_ACCEL_FIFO */
+
+	return raw_write8(s->port, s->addr, LSM6DSM_INT1_CTRL, int1_ctrl_val);
+}
+
+
+#ifdef CONFIG_ACCEL_FIFO
+/**
+ * fifo_disable - set fifo mode
+ * @s: Motion sensor pointer: must be MOTIONSENSE_TYPE_ACCEL.
+ * @fmode: BYPASS or CONTINUOUS
+ */
+static int fifo_disable(const struct motion_sensor_t *s)
+{
+	return raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, 0x00);
+}
+
+/**
+ * fifo_reset_pattern: called at each new FIFO pattern.
+ */
+static void fifo_reset_pattern(struct lsm6dsm_data *private)
+{
+	/* The fifo is ready to run. */
+	memcpy(&private->current, &private->config,
+	       sizeof(struct lsm6dsm_fifo_data));
+	private->next_in_patten = FIFO_DEV_INVALID;
+}
+
+/**
+ * set_fifo_params - Configure internal FIFO parameters
+ *
+ * Configure FIFO decimator to have every time the right pattern
+ * with acc/gyro
+ */
+static int fifo_enable(const struct motion_sensor_t *accel)
+{
+	int err, i, rate;
+	uint8_t decimator[FIFO_DEV_NUM] = { 0 };
+	unsigned int min_odr = LSM6DSM_ODR_MAX_VAL;
+	unsigned int max_odr = 0;
+	struct lsm6dsm_data *private = accel->drv_data;
+	/* In FIFO sensors are mapped in a different way. */
+	uint8_t agm_maps[] = {
+		MOTIONSENSE_TYPE_GYRO,
+		MOTIONSENSE_TYPE_ACCEL,
+		MOTIONSENSE_TYPE_MAG,
+	};
+
+
+	/* Search for min and max odr values for acc, gyro. */
+	for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) {
+		/* Check if sensor enabled with ODR. */
+		rate = st_get_data_rate(accel + agm_maps[i]);
+		if (rate > 0) {
+			min_odr = MIN(min_odr, rate);
+			max_odr = MAX(max_odr, rate);
+		}
+	}
+
+	if (max_odr == 0) {
+		/* Leave FIFO disabled. */
+		return EC_SUCCESS;
+	}
+
+	/* Scan all sensors configuration to calculate FIFO decimator. */
+	private->config.total_samples_in_pattern = 0;
+	for (i = FIFO_DEV_GYRO; i < FIFO_DEV_NUM; i++) {
+		rate = st_get_data_rate(accel + agm_maps[i]);
+		if (rate > 0) {
+			private->config.samples_in_pattern[i] = rate / min_odr;
+			decimator[i] = LSM6DSM_FIFO_DECIMATOR(max_odr / rate);
+			private->config.total_samples_in_pattern +=
+				private->config.samples_in_pattern[i];
+		} else {
+			/* Not in FIFO if sensor disabled. */
+			private->config.samples_in_pattern[i] = 0;
+		}
+	}
+	raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL3_ADDR,
+			(decimator[FIFO_DEV_GYRO] << LSM6DSM_FIFO_DEC_G_OFF) |
+			(decimator[FIFO_DEV_ACCEL] << LSM6DSM_FIFO_DEC_XL_OFF));
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+	raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL4_ADDR,
+			decimator[FIFO_DEV_MAG]);
+#endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */
+	err = raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL5_ADDR,
+			 (LSM6DSM_ODR_TO_REG(max_odr) <<
+			  LSM6DSM_FIFO_CTRL5_ODR_OFF) |
+			 LSM6DSM_FIFO_MODE_CONTINUOUS_VAL);
+	if (err != EC_SUCCESS)
+		return err;
+	fifo_reset_pattern(private);
+	return EC_SUCCESS;
+}
+
+/*
+ * Must order FIFO read based on ODR:
+ * For example Acc @ 52 Hz, Gyro @ 26 Hz Mag @ 13 Hz in FIFO we have
+ * for each pattern this data samples:
+ *  ________ _______ _______ _______ ________ _______ _______
+ * | Gyro_0 | Acc_0 | Mag_0 | Acc_1 | Gyro_1 | Acc_2 | Acc_3 |
+ * |________|_______|_______|_______|________|_______|_______|
+ *
+ * Total samples for each pattern: 2 Gyro, 4 Acc, 1 Mag.
+ */
+static int fifo_next(struct lsm6dsm_data *private)
+{
+	int next_id;
+
+	if (private->current.total_samples_in_pattern == 0)
+		fifo_reset_pattern(private);
+
+	if (private->current.total_samples_in_pattern == 0) {
+		/*
+		 * Not expected we are supposed to be called to process FIFO
+		 * data.
+		 */
+		CPRINTF("[%T FIFO empty pattern]\n");
+		return FIFO_DEV_INVALID;
+	}
+
+	for (next_id = private->next_in_patten + 1; 1; next_id++) {
+		if (next_id == FIFO_DEV_NUM)
+			next_id = FIFO_DEV_GYRO;
+		if (private->current.samples_in_pattern[next_id] != 0) {
+			private->current.samples_in_pattern[next_id]--;
+			private->current.total_samples_in_pattern--;
+			private->next_in_patten = next_id;
+			return next_id;
+		}
+	}
+	/* Will never happen. */
+	return FIFO_DEV_INVALID;
+}
+
+/**
+ * push_fifo_data - Scan data pattern and push upside
+ */
+static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
+			   uint16_t flen)
+{
+	struct lsm6dsm_data *private = accel->drv_data;
+	/* In FIFO sensors are mapped in a different way. */
+	uint8_t agm_maps[] = {
+		MOTIONSENSE_TYPE_GYRO,
+		MOTIONSENSE_TYPE_ACCEL,
+		MOTIONSENSE_TYPE_MAG,
+	};
+
+	while (flen > 0) {
+		struct ec_response_motion_sensor_data vect;
+		int id = agm_maps[fifo_next(private)];
+		int *axis = (accel + id)->raw_xyz;
+
+		/* Apply precision, sensitivity and rotation. */
+		st_normalize(accel + id, axis, fifo);
+		vect.data[X] = axis[X];
+		vect.data[Y] = axis[Y];
+		vect.data[Z] = axis[Z];
+
+		vect.flags = 0;
+		vect.sensor_num = accel - motion_sensors + id;
+		motion_sense_fifo_add_data(&vect, accel + id, 3,
+					   last_interrupt_timestamp);
+
+		fifo += OUT_XYZ_SIZE;
+		flen -= OUT_XYZ_SIZE;
+	}
+}
+
+static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts)
+{
+	int err, left, length;
+	uint8_t fifo[FIFO_READ_LEN];
+
+	/*
+	 * DIFF[9:0] are number of unread uint16 in FIFO
+	 * mask DIFF and compute total byte len to read from FIFO.
+	 */
+	left = fsts->len & LSM6DSM_FIFO_DIFF_MASK;
+	left *= sizeof(uint16_t);
+	left = (left / OUT_XYZ_SIZE) * OUT_XYZ_SIZE;
+
+	/* Push all data on upper side. */
+	do {
+		/* Fit len to pre-allocated static buffer. */
+		if (left > FIFO_READ_LEN)
+			length = FIFO_READ_LEN;
+		else
+			length = left;
+
+		/* Read data and copy in buffer. */
+		err = st_raw_read_n_noinc(s->port, s->addr,
+					  LSM6DSM_FIFO_DATA_ADDR,
+					  fifo, length);
+		if (err != EC_SUCCESS)
+			return err;
+
+		/* Manage patterns and push data. */
+		push_fifo_data(s, fifo, length);
+		left -= length;
+	} while (left > 0);
+
+	return EC_SUCCESS;
+}
+
+/**
+ * config_fifo - update mode and ODR for FIFO decimator
+ */
+static int config_fifo(const struct motion_sensor_t *accel)
+{
+	int err;
+
+	/* Changing in ODR must stop FIFO. */
+	err = fifo_disable(accel);
+	if (err != EC_SUCCESS)
+		return err;
+
+
+	return fifo_enable(accel);
+}
+#endif /* CONFIG_ACCEL_FIFO */
+
+/**
+ * lsm6dsm_interrupt - interrupt from int1/2 pin of sensor
+ */
+void lsm6dsm_interrupt(enum gpio_signal signal)
+{
+#ifdef CONFIG_ACCEL_FIFO
+	last_interrupt_timestamp = __hw_clock_source_read();
+#endif
+	task_set_event(TASK_ID_MOTIONSENSE,
+		       CONFIG_ACCEL_LSM6DSM_INT_EVENT, 0);
+}
+
+/**
+ * irq_handler - bottom half of the interrupt stack
+ */
+static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+{
+	int ret = EC_SUCCESS;
+
+	if ((s->type != MOTIONSENSE_TYPE_ACCEL) ||
+	    (!(*event & CONFIG_ACCEL_LSM6DSM_INT_EVENT)))
+		return EC_ERROR_NOT_HANDLED;
+
+#ifdef CONFIG_ACCEL_FIFO
+	{
+		struct fstatus fsts;
+		/* Read how many data pattern on FIFO to read and pattern. */
+		ret = st_raw_read_n_noinc(s->port, s->addr,
+				LSM6DSM_FIFO_STS1_ADDR,
+				(uint8_t *)&fsts, sizeof(fsts));
+		if (ret != EC_SUCCESS)
+			return ret;
+		if (fsts.len & (LSM6DSM_FIFO_DATA_OVR | LSM6DSM_FIFO_FULL)) {
+			CPRINTF("[%T %s FIFO Overrun: %04x]\n",
+				s->name, fsts.len);
+		}
+		if (!(fsts.len & LSM6DSM_FIFO_EMPTY))
+			ret = load_fifo(s, &fsts);
+	}
+#endif
+	return ret;
+}
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+
 /**
  * set_range - set full scale range
  * @s: Motion sensor pointer
@@ -99,50 +397,34 @@ static int get_range(const struct motion_sensor_t *s)
  */
 static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
-	int ret, normalized_rate = LSM6DSM_ODR_MIN_VAL;
+	int ret, normalized_rate;
 	struct stprivate_data *data = s->drv_data;
 	uint8_t ctrl_reg, reg_val;
 
 	ctrl_reg = LSM6DSM_ODR_REG(s->type);
+	if (rate > 0) {
+		reg_val = LSM6DSM_ODR_TO_REG(rate);
+		normalized_rate = LSM6DSM_REG_TO_ODR(reg_val);
 
-	if (rate == 0) {
-		/* Power off acc or gyro. */
-		mutex_lock(s->mutex);
-
-		ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK,
-					      LSM6DSM_ODR_0HZ_VAL);
-		if (ret == EC_SUCCESS)
-			data->base.odr = LSM6DSM_ODR_0HZ_VAL;
-
-		mutex_unlock(s->mutex);
-
-		return ret;
-	}
-
-	reg_val = LSM6DSM_ODR_TO_REG(rate);
-	normalized_rate = LSM6DSM_REG_TO_ODR(rate);
-
-	if (rnd && (normalized_rate < rate)) {
-		reg_val++;
-		normalized_rate <<= 1;
-	}
-
-	/* Adjust rounded value for acc and gyro because ODR are shared. */
-	if (reg_val > LSM6DSM_ODR_416HZ_VAL) {
-		reg_val = LSM6DSM_ODR_416HZ_VAL;
-		normalized_rate = LSM6DSM_ODR_MAX_VAL;
-	} else if (reg_val < LSM6DSM_ODR_13HZ_VAL) {
-		reg_val = LSM6DSM_ODR_13HZ_VAL;
-		normalized_rate = LSM6DSM_ODR_MIN_VAL;
+		if (rnd && (normalized_rate < rate)) {
+			reg_val++;
+			normalized_rate *= 2;
+		}
+	} else {
+		reg_val = 0;
+		normalized_rate = 0;
 	}
 
 	mutex_lock(s->mutex);
 	ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, reg_val);
-	if (ret == EC_SUCCESS)
+	if (ret == EC_SUCCESS) {
 		data->base.odr = normalized_rate;
+#ifdef CONFIG_ACCEL_FIFO
+		config_fifo(LSM6DSM_MAIN_SENSOR(s));
+#endif
+	}
 
 	mutex_unlock(s->mutex);
-
 	return ret;
 }
 
@@ -165,8 +447,6 @@ static int is_data_ready(const struct motion_sensor_t *s, int *ready)
 }
 
 /*
- * TODO: Implement FIFO support
- *
  * Is not very efficient to collect the data in read: better have an interrupt
  * and collect the FIFO, even if it has one item: we don't have to check if the
  * sensor is ready (minimize I2C access).
@@ -228,21 +508,33 @@ static int init(const struct motion_sensor_t *s)
 		mutex_lock(s->mutex);
 
 		/* Software reset. */
-		ret = st_write_data_with_mask(s,
-				LSM6DSM_RESET_ADDR,
-				LSM6DSM_RESET_MASK,
-				LSM6DSM_EN_BIT);
+		ret = raw_write8(s->port, s->addr, LSM6DSM_CTRL3_ADDR,
+				LSM6DSM_SW_RESET);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 
-		/* Output data not updated until have been read. */
-		ret = st_write_data_with_mask(s,
-				LSM6DSM_BDU_ADDR,
-				LSM6DSM_BDU_MASK,
-				LSM6DSM_EN_BIT);
+		/*
+		 * Output data not updated until have been read.
+		 * Prefer interrupt to be active low.
+		 */
+		ret = raw_write8(s->port, s->addr, LSM6DSM_CTRL3_ADDR,
+				LSM6DSM_BDU | LSM6DSM_H_L_ACTIVE |
+				LSM6DSM_IF_INC);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 
+#ifdef CONFIG_ACCEL_FIFO
+		ret = fifo_disable(s);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+#endif /* CONFIG_ACCEL_FIFO */
+
+#ifdef CONFIG_ACCEL_INTERRUPTS
+		ret = config_interrupt(s);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+#endif /* CONFIG_ACCEL_INTERRUPTS */
+
 		mutex_unlock(s->mutex);
 	}
 
@@ -252,6 +544,7 @@ static int init(const struct motion_sensor_t *s)
 
 err_unlock:
 	mutex_unlock(s->mutex);
+	CPRINTF("[%T %s: MS Init type:0x%X Error]\n", s->name, s->type);
 
 	return ret;
 }
@@ -266,4 +559,7 @@ const struct accelgyro_drv lsm6dsm_drv = {
 	.get_data_rate = st_get_data_rate,
 	.set_offset = st_set_offset,
 	.get_offset = st_get_offset,
+#ifdef CONFIG_ACCEL_INTERRUPTS
+	.irq_handler = irq_handler,
+#endif /* CONFIG_ACCEL_INTERRUPTS */
 };
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 7f02baa02e..bb332f71b5 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -23,16 +23,10 @@
 #define LSM6DSM_WHO_AM_I_REG		0x0f
 #define LSM6DSM_WHO_AM_I		0x6a
 
-/* Sensor Software Reset Bit */
-#define LSM6DSM_RESET_ADDR		0x12
-#define LSM6DSM_RESET_MASK		0x01
-
 /* COMMON DEFINE FOR ACCEL-GYRO SENSORS */
 #define LSM6DSM_EN_BIT			0x01
 #define LSM6DSM_DIS_BIT			0x00
 
-#define LSM6DSM_BDU_ADDR		0x12
-#define LSM6DSM_BDU_MASK		0x40
 
 #define LSM6DSM_GYRO_OUT_X_L_ADDR	0x22
 #define LSM6DSM_ACCEL_OUT_X_L_ADDR	0x28
@@ -40,6 +34,15 @@
 #define LSM6DSM_CTRL1_ADDR		0x10
 #define LSM6DSM_CTRL2_ADDR		0x11
 #define LSM6DSM_CTRL3_ADDR		0x12
+#define LSM6DSM_SW_RESET			0x01
+#define LSM6DSM_IF_INC				0x04
+#define LSM6DSM_PP_OD				0x10
+#define LSM6DSM_H_L_ACTIVE			0x20
+#define LSM6DSM_BDU				0x40
+
+#define LSM6DSM_CTRL4_ADDR		0x13
+#define LSM6DSM_INT2_ON_INT1_MASK		0x20
+
 #define LSM6DSM_CTRL6_ADDR		0x15
 #define LSM6DSM_CTRL7_ADDR		0x16
 
@@ -50,16 +53,70 @@
 	(LSM6DSM_CTRL1_ADDR + _sensor)
 #define LSM6DSM_ODR_MASK		0xf0
 
-/* Common Acc/Gyro data rate */
-enum lsm6dsm_odr {
-	LSM6DSM_ODR_0HZ_VAL = 0,
-	LSM6DSM_ODR_13HZ_VAL,
-	LSM6DSM_ODR_26HZ_VAL,
-	LSM6DSM_ODR_52HZ_VAL,
-	LSM6DSM_ODR_104HZ_VAL,
-	LSM6DSM_ODR_208HZ_VAL,
-	LSM6DSM_ODR_416HZ_VAL,
-	LSM6DSM_ODR_LIST_NUM
+/* Hardware FIFO size in byte */
+#define LSM6DSM_MAX_FIFO_SIZE		4096
+#define LSM6DSM_MAX_FIFO_LENGTH	(LSM6DSM_MAX_FIFO_SIZE / OUT_XYZ_SIZE)
+
+/* FIFO decimator registers and bitmask */
+#define LSM6DSM_FIFO_CTRL1_ADDR		0x06
+#define LSM6DSM_FIFO_CTRL2_ADDR		0x07
+
+#define LSM6DSM_FIFO_CTRL3_ADDR		0x08
+#define LSM6DSM_FIFO_DEC_XL_OFF			0
+#define LSM6DSM_FIFO_DEC_G_OFF			3
+
+#define LSM6DSM_FIFO_CTRL4_ADDR		0x09
+
+#define LSM6DSM_FIFO_DECIMATOR(_dec) \
+	(_dec < 8 ? _dec : (2 + __builtin_ctz(_dec)))
+
+#define LSM6DSM_INT1_CTRL		0x0d
+#define LSM6DSM_INT_FIFO_TH			0x08
+#define LSM6DSM_INT_FIFO_OVR			0x10
+#define LSM6DSM_INT_FIFO_FULL			0x20
+#define LSM6DSM_INT_SIGMO			0x40
+
+#define LSM6DSM_FIFO_STS1_ADDR		0x3a
+#define LSM6DSM_FIFO_STS2_ADDR		0x3b
+#define LSM6DSM_FIFO_DIFF_MASK			0x07ff
+#define LSM6DSM_FIFO_EMPTY			0x1000
+#define LSM6DSM_FIFO_FULL			0x2000
+#define LSM6DSM_FIFO_DATA_OVR			0x4000
+#define LSM6DSM_FIFO_WATERMARK			0x8000
+#define LSM6DSM_FIFO_NODECIM		0x01
+
+/* Out data register */
+#define LSM6DSM_FIFO_DATA_ADDR		0x3e
+
+/* Registers value for supported FIFO mode */
+#define LSM6DSM_FIFO_MODE_BYPASS_VAL	0x00
+#define LSM6DSM_FIFO_MODE_CONTINUOUS_VAL	0x06
+
+#define LSM6DSM_FIFO_CTRL5_ADDR		0x0a
+#define LSM6DSM_FIFO_CTRL5_ODR_OFF		3
+#define LSM6DSM_FIFO_CTRL5_ODR_MASK \
+	(0xf << LSM6DSM_FIFO_CTRL5_ODR_OFF)
+#define LSM6DSM_FIFO_CTRL5_MODE_MASK		0x07
+
+/* Define ODR FIFO values. Max value is max ODR for sensors
+ * Value is limited to 416 Hz
+ */
+
+/* Registers value for sensor Hub */
+#define LSM6DSM_FUNC_SRC1		0x53
+#define LSM6DSM_SENSORHUB_END_OP	0x01
+
+/* Define device available in FIFO pattern */
+enum dev_fifo {
+	FIFO_DEV_INVALID = -1,
+	FIFO_DEV_GYRO = 0,
+	FIFO_DEV_ACCEL,
+	FIFO_DEV_NUM,
+};
+
+struct fstatus {
+	uint16_t len;
+	uint16_t pattern;
 };
 
 /* Absolute maximum rate for acc and gyro sensors */
@@ -146,4 +203,37 @@ enum lsm6dsm_status {
 
 extern const struct accelgyro_drv lsm6dsm_drv;
 
+void lsm6dsm_interrupt(enum gpio_signal signal);
+
+struct lsm6dsm_fifo_data {
+	/*
+	 * FIFO data order is based on the ODR of each sensors.
+	 * For example Acc @ 52 Hz, Gyro @ 26 Hz Mag @ 13 Hz in FIFO we have
+	 * for each pattern this data samples:
+	 *  ________ _______ _______ _______ ________ _______ _______
+	 * | Gyro_0 | Acc_0 | Mag_0 | Acc_1 | Gyro_1 | Acc_2 | Acc_3 |
+	 * |________|_______|_______|_______|________|_______|_______|
+	 *
+	 * Total samples for each pattern: 2 Gyro, 4 Acc, 1 Mag
+	 */
+	/* Calculated samples in a pattern, based on ODR. */
+	int samples_in_pattern[FIFO_DEV_NUM];
+
+	/* Sum of all samples_in_pattern. */
+	int total_samples_in_pattern;
+};
+
+
+struct lsm6dsm_data {
+	/* Must be first: ST generic accelerometer data. */
+	struct stprivate_data a_data;
+#ifdef CONFIG_ACCEL_FIFO
+	struct lsm6dsm_fifo_data config;
+	struct lsm6dsm_fifo_data current;
+	int next_in_patten;
+#endif
+};
+
+#define LSM6DSM_MAIN_SENSOR(_s) ((_s) - (_s)->type)
+
 #endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */
diff --git a/driver/stm_mems_common.c b/driver/stm_mems_common.c
index c2939ae714..4af0cb4a34 100644
--- a/driver/stm_mems_common.c
+++ b/driver/stm_mems_common.c
@@ -163,10 +163,5 @@ void st_normalize(const struct motion_sensor_t *s, vector_3_t v, uint8_t *data)
 	/* apply offset in the device coordinates */
 	range = s->drv->get_range(s);
 	for (i = X; i <= Z; i++)
-		/*
-		 * TODO(gwendal): only work for accel:
-		 * offsest is in 1/1024th g/LSB, so shift left and divide by
-		 * range to normalize the value.
-		 */
 		v[i] += (drvdata->offset[i] << 5) / range;
 }