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, 113 insertions, 59 deletions

diff --git a/driver/mag_lis2mdl.c b/driver/mag_lis2mdl.c
index de0650cd38..99415e9695 100644
--- a/driver/mag_lis2mdl.c
+++ b/driver/mag_lis2mdl.c
@@ -12,59 +12,63 @@
 #include "common.h"
 #include "driver/mag_lis2mdl.h"
 #include "driver/sensorhub_lsm6dsm.h"
+#include "driver/accelgyro_lsm6dsm.h"
 #include "driver/stm_mems_common.h"
 #include "task.h"
 
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
 #ifndef CONFIG_SENSORHUB_LSM6DSM
 #error "Need Sensor Hub LSM6DSM support"
 #endif
+#endif
 
-static int irq_handler(struct motion_sensor_t *s, uint32_t *event)
+void lis2mdl_normalize(const struct motion_sensor_t *s,
+		       intv3_t v,
+		       uint8_t *raw)
 {
-	if (!s->parent) {
-		/* Sensor is not supported in direct connection/slave mode */
-		return EC_ERROR_UNIMPLEMENTED;
+	struct mag_cal_t *cal = LIS2MDL_CAL(s);
+	int i;
+#ifdef CONFIG_MAG_BMI160_LIS2MDL
+	struct lis2mdl_private_data *private = LIS2MDL_DATA(s);
+	intv3_t hn1;
+
+	hn1[X] = ((int16_t)((raw[1] << 8) | raw[0]));
+	hn1[Y] = ((int16_t)((raw[3] << 8) | raw[2]));
+	hn1[Z] = ((int16_t)((raw[5] << 8) | raw[4]));
+
+	/* Only when LIS2MDL is in forced mode */
+	if (private->hn_valid) {
+		for (i = X; i <= Z; i++)
+			v[i] = (hn1[i] + private->hn[i]) / 2;
+		memcpy(private->hn, hn1, sizeof(intv3_t));
+	} else {
+		private->hn_valid = 1;
+		memcpy(v, hn1, sizeof(intv3_t));
 	}
-	/*
-	 * Magnetometer in cascade mode. The main sensor should take
-	 * care of interrupt situation.
-	 */
-	return EC_SUCCESS;
-}
-
-static int set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
-{
-	int ret = EC_ERROR_UNIMPLEMENTED;
-	/*
-	 * 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;
-
-	if (!s->parent)
-		return ret;
+#else
+	v[X] = ((int16_t)((raw[1] << 8) | raw[0]));
+	v[Y] = ((int16_t)((raw[3] << 8) | raw[2]));
+	v[Z] = ((int16_t)((raw[5] << 8) | raw[4]));
+#endif
+	for (i = X; i <= Z; i++)
+		v[i] = LIS2MDL_RATIO(v[i]);
 
-	mutex_lock(s->mutex);
-	ret = sensorhub_set_ext_data_rate(s->parent, rate, rnd,
-						&data->base.odr);
-	mutex_unlock(s->mutex);
+	mag_cal_update(cal, v);
 
-	return ret;
+	v[X] += cal->bias[X];
+	v[Y] += cal->bias[Y];
+	v[Z] += cal->bias[Z];
 }
 
 static int set_range(const struct motion_sensor_t *s, int range, int rnd)
 {
 	struct stprivate_data *data = s->drv_data;
 
-	if (range != LIS2MDL_RANGE)
+	/* Range is fixed by hardware */
+	if (range != s->default_range)
 		return EC_ERROR_INVAL;
 
-	/* Range is fixed to LIS2MDL_RANGE by hardware */
-	data->base.range = LIS2MDL_RANGE;
+	data->base.range = range;
 	return EC_SUCCESS;
 }
 
@@ -75,69 +79,119 @@ static int get_range(const struct motion_sensor_t *s)
 	return data->base.range;
 }
 
-static int read(const struct motion_sensor_t *s, intv3_t v)
+/**
+ * set_offset - Set data offset
+ * @s: Motion sensor pointer
+ * @offset: offset vector
+ * @temp: Temp
+ */
+static int set_offset(const struct motion_sensor_t *s,
+		      const int16_t *offset, int16_t temp)
 {
-	int ret = EC_ERROR_UNIMPLEMENTED;
 
-	if (!s->parent)
-		return ret;
+#ifdef CONFIG_LSM6DSM_SEC_I2C
+	struct mag_cal_t *cal = LIS2MDL_CAL(s);
+#endif
+
+	cal->bias[X] = offset[X];
+	cal->bias[Y] = offset[Y];
+	cal->bias[Z] = offset[Z];
+	rotate_inv(cal->bias, *s->rot_standard_ref, cal->bias);
+	return EC_SUCCESS;
+}
+
+/**
+ * get_offset - Get data offset
+ * @s: Motion sensor pointer
+ * @offset: offset vector
+ * @temp: Temp
+ */
+static int get_offset(const struct motion_sensor_t *s,
+		      int16_t *offset, int16_t *temp)
+{
+#ifdef CONFIG_LSM6DSM_SEC_I2C
+	struct mag_cal_t *cal = LIS2MDL_CAL(s);
+#endif
+	intv3_t offset_int;
 
+	rotate(cal->bias, *s->rot_standard_ref, offset_int);
+	offset[X] = offset_int[X];
+	offset[Y] = offset_int[Y];
+	offset[Z] = offset_int[Z];
+	*temp = EC_MOTION_SENSE_INVALID_CALIB_TEMP;
+	return EC_SUCCESS;
+}
+
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+int lis2mdl_thru_lsm6dsm_read(const struct motion_sensor_t *s, intv3_t v)
+{
+	int ret;
+	uint8_t raw[OUT_XYZ_SIZE];
+	/*
+	 * This is mostly for debugging, read happens through LSM6DSM/BMI160
+	 * FIFO.
+	 */
 	mutex_lock(s->mutex);
-	ret = sensorhub_slv0_data_read(s->parent, v);
+	ret = sensorhub_slv0_data_read(LSM6DSM_MAIN_SENSOR(s), raw);
 	mutex_unlock(s->mutex);
-
+	lis2mdl_normalize(s, v, raw);
+	rotate(v, *s->rot_standard_ref, v);
 	return ret;
 }
 
-static int init(const struct motion_sensor_t *s)
+int lis2mdl_thru_lsm6dsm_init(const struct motion_sensor_t *s)
 {
 	int ret = EC_ERROR_UNIMPLEMENTED;
+	struct mag_cal_t *cal = LIS2MDL_CAL(s);
 	struct stprivate_data *data = s->drv_data;
 
-	if (!s->parent)
-		return ret;
-
 	mutex_lock(s->mutex);
 	/* Magnetometer in cascade mode */
-	ret = sensorhub_check_and_rst(s->parent, s->addr,
+	ret = sensorhub_check_and_rst(
+			LSM6DSM_MAIN_SENSOR(s),
+			CONFIG_ACCELGYRO_SEC_ADDR,
 			LIS2MDL_WHO_AM_I_REG, LIS2MDL_WHO_AM_I,
 			LIS2MDL_CFG_REG_A_ADDR, LIS2MDL_SW_RESET);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	ret = sensorhub_config_ext_reg(s->parent, s->addr,
+	ret = sensorhub_config_ext_reg(
+			LSM6DSM_MAIN_SENSOR(s),
+			CONFIG_ACCELGYRO_SEC_ADDR,
 			LIS2MDL_CFG_REG_A_ADDR,
 			LIS2MDL_ODR_100HZ | LIS2MDL_CONT_MODE);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	ret = sensorhub_config_slv0_read(s->parent, s->addr,
-				LIS2MDL_OUT_REG, OUT_XYZ_SIZE);
+	ret = sensorhub_config_slv0_read(
+			LSM6DSM_MAIN_SENSOR(s),
+			CONFIG_ACCELGYRO_SEC_ADDR,
+			LIS2MDL_OUT_REG, OUT_XYZ_SIZE);
 	if (ret != EC_SUCCESS)
 		goto err_unlock;
 
-	/* Set default resolution to 16 bit */
-	data->resol = LIS2MDL_RESOLUTION;
-	/* Range is fixed to LIS2MDL_RANGE by hardware */
-	data->base.range = LIS2MDL_RANGE;
 	mutex_unlock(s->mutex);
-
+	init_mag_cal(cal);
+	cal->radius = 0.0f;
+	data->resol = LIS2DSL_RESOLUTION;
 	return sensor_init_done(s);
 
 err_unlock:
 	mutex_unlock(s->mutex);
 	return ret;
 }
+#endif  /* CONFIG_MAG_LSM6DSM_LIS2MDL */
 
 const struct accelgyro_drv lis2mdl_drv = {
-	.init = init,
-	.read = read,
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+	.init = lis2mdl_thru_lsm6dsm_init,
+	.read = lis2mdl_thru_lsm6dsm_read,
+	.set_data_rate = lsm6dsm_set_data_rate,
+#endif
 	.set_range = set_range,
 	.get_range = get_range,
 	.get_resolution = st_get_resolution,
-	.set_data_rate = set_data_rate,
 	.get_data_rate = st_get_data_rate,
-	.set_offset = st_set_offset,
-	.get_offset = st_get_offset,
-	.irq_handler = irq_handler,
+	.set_offset = set_offset,
+	.get_offset = get_offset,
 };