driver: lsm6dsm: Integrate LIS2MDL behind LSM6DSM

- Cros set_rate and normalize between LIS2MDL and LSM6DSM
- Remove unused sensor hub function.
- Remove parent field, use macro instead (magnetometer is just after the
gyroscope).

BUG=b:110143516,b:115587004
BRANCH=none
TEST=On meep, check the magnetometer is returning data with shell/python
script.
Check calibration quick in.
Check with AIDA64, compass and sensor app the magnetometer is seen.

Change-Id: I2efef99eda095e33b6a0555b1cbc4ac8fdbfab5d
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/1361992
Reviewed-by: Justin TerAvest <teravest@chromium.org>
Trybot-Ready: Justin TerAvest <teravest@chromium.org>
Tested-by: Justin TerAvest <teravest@chromium.org>

1 files changed, 132 insertions, 89 deletions

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index b53755ada4..49c36d8ec5 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -10,8 +10,10 @@
  */
 
 #include "driver/accelgyro_lsm6dsm.h"
+#include "driver/mag_lis2mdl.h"
 #include "hooks.h"
 #include "hwtimer.h"
+#include "mag_cal.h"
 #include "math_util.h"
 #include "task.h"
 #include "timer.h"
@@ -39,20 +41,21 @@ static inline int get_xyz_reg(enum motionsensor_type type)
  *
  * FIFO threshold on watermark
  *
- * @s: Motion sensor pointer
+ * @accel: Motion sensor pointer to accelerometer.
  */
-static int config_interrupt(const struct motion_sensor_t *s)
+static int config_interrupt(const struct motion_sensor_t *accel)
 {
 	int ret = EC_SUCCESS;
 	int int1_ctrl_val;
 
-	ret = st_raw_read8(s->port, s->addr, LSM6DSM_INT1_CTRL, &int1_ctrl_val);
+	ret = st_raw_read8(accel->port, accel->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 = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL1_ADDR,
+	ret = st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL1_ADDR,
 			 OUT_XYZ_SIZE / sizeof(uint16_t));
 	if (ret != EC_SUCCESS)
 		return ret;
@@ -61,19 +64,20 @@ static int config_interrupt(const struct motion_sensor_t *s)
 #endif /* CONFIG_ACCEL_FIFO */
 
 	return st_raw_write8(
-		s->port, s->addr, LSM6DSM_INT1_CTRL, int1_ctrl_val);
+		accel->port, accel->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.
+ * @accel: Motion sensor pointer: must be MOTIONSENSE_TYPE_ACCEL.
  * @fmode: BYPASS or CONTINUOUS
  */
-int accelgyro_fifo_disable(const struct motion_sensor_t *s)
+static int fifo_disable(const struct motion_sensor_t *accel)
 {
-	return st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR, 0x00);
+	return st_raw_write8(accel->port, accel->addr,
+			     LSM6DSM_FIFO_CTRL5_ADDR, 0x00);
 }
 
 /**
@@ -88,19 +92,22 @@ static void fifo_reset_pattern(struct lsm6dsm_data *private)
 }
 
 /**
- * set_fifo_params - Configure internal FIFO parameters
+ * fifo_enable - Configure internal FIFO parameters
+ * @accel must be the accelerometer sensor.
  *
- * Configure FIFO decimator to have every time the right pattern
+ * Configure FIFO decimators to have every time the right pattern
  * with acc/gyro
  */
-int accelgyro_fifo_enable(const struct motion_sensor_t *s)
+static int fifo_enable(const struct motion_sensor_t *accel)
 {
+	const struct motion_sensor_t *s;
 	int err, i, rate;
-	uint8_t decimator[FIFO_DEV_NUM] = { 0 };
+	uint8_t decimators[FIFO_DEV_NUM] = { 0 };
+	unsigned int odrs[FIFO_DEV_NUM];
 	unsigned int min_odr = LSM6DSM_ODR_MAX_VAL;
 	unsigned int max_odr = 0;
 	uint8_t odr_reg_val;
-	struct lsm6dsm_data *private = s->drv_data;
+	struct lsm6dsm_data *private = LSM6DSM_GET_DATA(accel);
 	/* In FIFO sensors are mapped in a different way. */
 	uint8_t agm_maps[] = {
 		MOTIONSENSE_TYPE_GYRO,
@@ -112,11 +119,13 @@ int accelgyro_fifo_enable(const struct motion_sensor_t *s)
 	/* 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(s + agm_maps[i]);
+		s = accel + agm_maps[i];
+		rate = s->drv->get_data_rate(s);
 		if (rate > 0) {
 			min_odr = MIN(min_odr, rate);
 			max_odr = MAX(max_odr, rate);
 		}
+		odrs[i] = rate;
 	}
 
 	if (max_odr == 0) {
@@ -127,16 +136,17 @@ int accelgyro_fifo_enable(const struct motion_sensor_t *s)
 	/* FIFO ODR must be set before the decimation factors */
 	odr_reg_val = LSM6DSM_ODR_TO_REG(max_odr) <<
 					LSM6DSM_FIFO_CTRL5_ODR_OFF;
-	err = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR,
+	err = st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL5_ADDR,
 			    odr_reg_val);
 
 	/* 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(s + agm_maps[i]);
-		if (rate > 0) {
-			private->config.samples_in_pattern[i] = rate / min_odr;
-			decimator[i] = LSM6DSM_FIFO_DECIMATOR(max_odr / rate);
+		if (odrs[i] > 0) {
+			private->config.samples_in_pattern[i] =
+				odrs[i] / min_odr;
+			decimators[i] =
+				LSM6DSM_FIFO_DECIMATOR(max_odr / odrs[i]);
 			private->config.total_samples_in_pattern +=
 				private->config.samples_in_pattern[i];
 			private->samples_to_discard[i] =
@@ -146,18 +156,32 @@ int accelgyro_fifo_enable(const struct motion_sensor_t *s)
 			private->config.samples_in_pattern[i] = 0;
 		}
 	}
-	st_raw_write8(s->port, s->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
-	st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL4_ADDR,
-			decimator[FIFO_DEV_MAG]);
+	st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL3_ADDR,
+		      (decimators[FIFO_DEV_GYRO] << LSM6DSM_FIFO_DEC_G_OFF) |
+		      (decimators[FIFO_DEV_ACCEL] << LSM6DSM_FIFO_DEC_XL_OFF));
+#ifdef CONFIG_LSM6DSM_SEC_I2C
+	st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL4_ADDR,
+			decimators[FIFO_DEV_MAG]);
+
+	/*
+	 * FIFO ODR is limited by odr of gyro or accel.
+	 * If we are sampling magnetometer faster than gyro or accel,
+	 * bump up ODR of accel. Thanks to decimation we will still measure at
+	 * the specified ODR.
+	 * Contrary to gyroscope, sampling faster will not affect measurements.
+	 * Set the ODR behind the back of set/get_data_rate.
+	 */
+	if (max_odr > MAX(odrs[FIFO_DEV_ACCEL], odrs[FIFO_DEV_GYRO])) {
+		st_write_data_with_mask(accel, LSM6DSM_ODR_REG(accel->type),
+				LSM6DSM_ODR_MASK,
+				LSM6DSM_ODR_TO_REG(max_odr));
+	}
 #endif /* CONFIG_MAG_LSM6DSM_LIS2MDL */
 	/*
 	 * After ODR and decimation values are set, continuous mode can be
 	 * enabled
 	 */
-	err = st_raw_write8(s->port, s->addr, LSM6DSM_FIFO_CTRL5_ADDR,
+	err = st_raw_write8(accel->port, accel->addr, LSM6DSM_FIFO_CTRL5_ADDR,
 			    odr_reg_val | LSM6DSM_FIFO_MODE_CONTINUOUS_VAL);
 	if (err != EC_SUCCESS)
 		return err;
@@ -213,7 +237,8 @@ static int fifo_next(struct lsm6dsm_data *private)
 static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
 			   uint16_t flen, uint32_t int_ts)
 {
-	struct lsm6dsm_data *private = accel->drv_data;
+	struct motion_sensor_t *s;
+	struct lsm6dsm_data *private = LSM6DSM_GET_DATA(accel);
 	/* In FIFO sensors are mapped in a different way. */
 	uint8_t agm_maps[] = {
 		MOTIONSENSE_TYPE_GYRO,
@@ -239,18 +264,28 @@ static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo,
 			private->samples_to_discard[next_fifo]--;
 		} else {
 			id = agm_maps[next_fifo];
-			axis = (accel + id)->raw_xyz;
+			s = accel + id;
+			axis = s->raw_xyz;
 
 			/* Apply precision, sensitivity and rotation. */
-			st_normalize(accel + id, axis, fifo);
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+			if (s->type == MOTIONSENSE_TYPE_MAG) {
+				lis2mdl_normalize(s, axis, fifo);
+				rotate(axis, *s->rot_standard_ref, axis);
+			} else
+#endif
+			{
+				st_normalize(s, 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,
-						   int_ts);
+			vect.sensor_num = s - motion_sensors;
+			motion_sense_fifo_add_data(&vect, s, 3, int_ts);
 		}
 
 		fifo += OUT_XYZ_SIZE;
@@ -361,19 +396,11 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 {
 	int err;
 	uint8_t ctrl_reg, reg_val;
-	/*
-	 * Since 'stprivate_data a_data;' is the first member of lsm6dsm_data,
-	 * the address of lsm6dsm_data is the same as a_data's. Using this
-	 * fact, we can do the following conversion. This conversion is equal
-	 * to:
-	 *     struct lsm6dsm_data *lsm_data = s->drv_data;
-	 *     struct stprivate_data *data = &lsm_data->a_data;
-	 */
 	struct stprivate_data *data = s->drv_data;
 	int newrange = range;
 
-	ctrl_reg = LSM6DSM_RANGE_REG(s->type);
-	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
+	switch (s->type) {
+	case MOTIONSENSE_TYPE_ACCEL:
 		/* Adjust and check rounded value for acc. */
 		if (rnd && (newrange < LSM6DSM_ACCEL_NORMALIZE_FS(newrange)))
 			newrange *= 2;
@@ -382,7 +409,8 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 			newrange = LSM6DSM_ACCEL_FS_MAX_VAL;
 
 		reg_val = LSM6DSM_ACCEL_FS_REG(newrange);
-	} else {
+		break;
+	case MOTIONSENSE_TYPE_GYRO:
 		/* Adjust and check rounded value for gyro. */
 		reg_val = LSM6DSM_GYRO_FS_REG(range);
 		if (rnd && (range > LSM6DSM_GYRO_NORMALIZE_FS(reg_val)))
@@ -391,16 +419,19 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 		if (reg_val > LSM6DSM_GYRO_FS_MAX_REG_VAL)
 			reg_val = LSM6DSM_GYRO_FS_MAX_REG_VAL;
 		newrange = LSM6DSM_GYRO_NORMALIZE_FS(reg_val);
+		break;
+	default:
+		return EC_RES_INVALID_PARAM;
 	}
 
+	ctrl_reg = LSM6DSM_RANGE_REG(s->type);
 	mutex_lock(s->mutex);
 	err = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_RANGE_MASK, reg_val);
 	if (err == EC_SUCCESS)
 		/* Save internally gain for speed optimization. */
 		data->base.range = newrange;
 	mutex_unlock(s->mutex);
-
-	return EC_SUCCESS;
+	return err;
 }
 
 /**
@@ -411,50 +442,36 @@ static int set_range(const struct motion_sensor_t *s, int range, int rnd)
  */
 static int get_range(const struct motion_sensor_t *s)
 {
-	/*
-	 * Since 'stprivate_data a_data;' is the first member of lsm6dsm_data,
-	 * the address of lsm6dsm_data is the same as a_data's. Using this
-	 * fact, we can do the following conversion. This conversion is equal
-	 * to:
-	 *     struct lsm6dsm_data *lsm_data = s->drv_data;
-	 *     struct stprivate_data *data = &lsm_data->a_data;
-	 */
 	struct stprivate_data *data = s->drv_data;
 
 	return data->base.range;
 }
 
 /**
- * set_data_rate
+ * lsm6dsm_set_data_rate
  * @s: Motion sensor pointer
  * @range: Rate (mHz)
  * @rnd: Round up/down flag
  *
- * For mag in cascade with lsm6dsm/l we use acc trigger and FIFO decimator
+ * For mag in cascade with lsm6dsm/l we use acc trigger and FIFO decimators
  */
-static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
+int lsm6dsm_set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 {
-	int ret, normalized_rate = 0;
-	/*
-	 * Since 'stprivate_data a_data;' is the first member of lsm6dsm_data,
-	 * the address of lsm6dsm_data is the same as a_data's. Using this
-	 * fact, we can do the following conversion. This conversion is equal
-	 * to:
-	 *     struct lsm6dsm_data *lsm_data = s->drv_data;
-	 *     struct stprivate_data *data = &lsm_data->a_data;
-	 */
+	int ret = EC_SUCCESS, normalized_rate = 0;
+#ifdef CONFIG_ACCEL_FIFO
+	const struct motion_sensor_t *accel = LSM6DSM_MAIN_SENSOR(s);
+#endif
 	struct stprivate_data *data = s->drv_data;
 	uint8_t ctrl_reg, reg_val = 0;
 
 #ifdef CONFIG_ACCEL_FIFO
 	/* FIFO must be disabled before setting any ODR values */
-	ret = accelgyro_fifo_disable(LSM6DSM_MAIN_SENSOR(s));
+	ret = fifo_disable(accel);
 	if (ret != EC_SUCCESS) {
 		CPRINTS("Failed to disable FIFO. Error: %d", ret);
 		return ret;
 	}
 #endif
-	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);
@@ -465,16 +482,55 @@ static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 		}
 		if (normalized_rate < LSM6DSM_ODR_MIN_VAL ||
 		    normalized_rate > MIN(LSM6DSM_ODR_MAX_VAL,
-			    CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ))
+				CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ))
 			return EC_RES_INVALID_PARAM;
 	}
 
-	mutex_lock(s->mutex);
-	ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK, reg_val);
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+	/*
+	 * TODO(b:110143516) Improve data rate selection:
+	 * Sensor is always running at 100Hz, even when not used.
+	 */
+	if (s->type == MOTIONSENSE_TYPE_MAG) {
+		struct mag_cal_t *cal = LIS2MDL_CAL(s);
+
+#ifdef CONFIG_ACCEL_FIFO
+		/*
+		 * Accelormeter rate may have been changed when setting the
+		 * FIFO.
+		 * Put back correct rate if rate of magnetometer changes.
+		 */
+		data->base.odr = 0;
+		accel->drv->set_data_rate(accel,
+				accel->drv->get_data_rate(accel), 0);
+#endif
+
+		init_mag_cal(cal);
+		/*
+		 * Magnetometer ODR is calculating at 100Hz, but we are reading
+		 * less often.
+		 */
+		if (normalized_rate > 0)
+			cal->batch_size = MAX(
+				MAG_CAL_MIN_BATCH_SIZE,
+				(normalized_rate * 1000) /
+					MAG_CAL_MIN_BATCH_WINDOW_US);
+		else
+			cal->batch_size = 0;
+		CPRINTS("Batch size: %d", cal->batch_size);
+		mutex_lock(s->mutex);
+	} else
+#endif
+	{
+		mutex_lock(s->mutex);
+		ctrl_reg = LSM6DSM_ODR_REG(s->type);
+		ret = st_write_data_with_mask(s, ctrl_reg, LSM6DSM_ODR_MASK,
+					      reg_val);
+	}
 	if (ret == EC_SUCCESS) {
 		data->base.odr = normalized_rate;
 #ifdef CONFIG_ACCEL_FIFO
-		ret = accelgyro_fifo_enable(LSM6DSM_MAIN_SENSOR(s));
+		ret = fifo_enable(accel);
 		if (ret != EC_SUCCESS)
 			CPRINTS("Failed to enable FIFO. Error: %d", ret);
 #endif
@@ -531,27 +587,19 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	xyz_reg = get_xyz_reg(s->type);
 
 	/* Read data bytes starting at xyz_reg. */
-	ret = st_raw_read_n_noinc(s->port, s->addr, xyz_reg, raw, OUT_XYZ_SIZE);
+	ret = st_raw_read_n_noinc(s->port, s->addr, xyz_reg, raw,
+			OUT_XYZ_SIZE);
 	if (ret != EC_SUCCESS)
 		return ret;
 
 	/* Apply precision, sensitivity and rotation vector. */
 	st_normalize(s, v, raw);
-
 	return EC_SUCCESS;
 }
 
 static int init(const struct motion_sensor_t *s)
 {
 	int ret = 0, tmp;
-	/*
-	 * Since 'stprivate_data a_data;' is the first member of lsm6dsm_data,
-	 * the address of lsm6dsm_data is the same as a_data's. Using this
-	 * fact, we can do the following conversion. This conversion is equal
-	 * to:
-	 *     struct lsm6dsm_data *lsm_data = s->drv_data;
-	 *     struct stprivate_data *data = &lsm_data->a_data;
-	 */
 	struct stprivate_data *data = s->drv_data;
 	uint8_t ctrl_reg, reg_val = 0;
 
@@ -592,12 +640,7 @@ static int init(const struct motion_sensor_t *s)
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 
-#ifdef CONFIG_MAG_LIS2MDL
-		/*
-		 * TODO: Check for pass-through mode instead of magnetometer
-		 * config.
-		 */
-
+#ifdef CONFIG_LSM6DSM_SEC_I2C
 		/*
 		 * Reboot to reload memory content as pass-through mode can get
 		 * stuck.
@@ -638,7 +681,7 @@ static int init(const struct motion_sensor_t *s)
 			goto err_unlock;
 
 #ifdef CONFIG_ACCEL_FIFO
-		ret = accelgyro_fifo_disable(s);
+		ret = fifo_disable(s);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 #endif /* CONFIG_ACCEL_FIFO */
@@ -669,7 +712,7 @@ const struct accelgyro_drv lsm6dsm_drv = {
 	.set_range = set_range,
 	.get_range = get_range,
 	.get_resolution = st_get_resolution,
-	.set_data_rate = set_data_rate,
+	.set_data_rate = lsm6dsm_set_data_rate,
 	.get_data_rate = st_get_data_rate,
 	.set_offset = st_set_offset,
 	.get_offset = st_get_offset,