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 */
 };