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>

1 files changed, 340 insertions, 44 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) {
-		/* 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 (rate > 0) {
+		reg_val = LSM6DSM_ODR_TO_REG(rate);
+		normalized_rate = LSM6DSM_REG_TO_ODR(reg_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,20 +508,32 @@ 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.
+		 * 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 */
 
-		/* Output data not updated until have been read. */
-		ret = st_write_data_with_mask(s,
-				LSM6DSM_BDU_ADDR,
-				LSM6DSM_BDU_MASK,
-				LSM6DSM_EN_BIT);
+#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 */
 };