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>

13 files changed, 375 insertions, 255 deletions

diff --git a/board/fleex/board.c b/board/fleex/board.c
index cbf0f3be70..8026c1c007 100644
--- a/board/fleex/board.c
+++ b/board/fleex/board.c
@@ -119,8 +119,7 @@ const mat33_fp_t lid_standard_ref = {
 
 /* sensor private data */
 static struct stprivate_data g_lis2dh_data;
-static struct lsm6dsm_data lsm6dsm_g_data;
-static struct lsm6dsm_data lsm6dsm_a_data;
+static struct lsm6dsm_data lsm6dsm_data;
 
 /* Drivers */
 struct motion_sensor_t motion_sensors[] = {
@@ -160,7 +159,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_a_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_ACCEL),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.rot_standard_ref = &base_standard_ref,
@@ -189,7 +189,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_g_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_GYRO),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.default_range = 1000 | ROUND_UP_FLAG, /* dps */
diff --git a/board/meep/board.c b/board/meep/board.c
index 72f7f2d864..00f7bc49b6 100644
--- a/board/meep/board.c
+++ b/board/meep/board.c
@@ -130,10 +130,8 @@ const mat33_fp_t mag_standard_ref = {
 };
 
 /* sensor private data */
-static struct kionix_accel_data g_kx022_data;
-static struct lsm6dsm_data lsm6dsm_g_data;
-static struct lsm6dsm_data lsm6dsm_a_data;
-static struct lsm6dsm_data lis2mdl_data;
+static struct kionix_accel_data kx022_data;
+static struct lsm6dsm_data lsm6dsm_data;
 
 /* Drivers */
 struct motion_sensor_t motion_sensors[] = {
@@ -145,7 +143,7 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_LID,
 		.drv = &kionix_accel_drv,
 		.mutex = &g_lid_mutex,
-		.drv_data = &g_kx022_data,
+		.drv_data = &kx022_data,
 		.port = I2C_PORT_SENSOR,
 		.addr = KX022_ADDR1,
 		.rot_standard_ref = &lid_standrd_ref,
@@ -170,7 +168,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_a_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_ACCEL),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.rot_standard_ref = &base_standard_ref,
@@ -199,7 +198,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_g_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_GYRO),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.default_range = 1000 | ROUND_UP_FLAG, /* dps */
@@ -211,16 +211,15 @@ struct motion_sensor_t motion_sensors[] = {
 	[BASE_MAG] = {
 		.name = "Base Mag",
 		.active_mask = SENSOR_ACTIVE_S0_S3,
-		.chip = MOTIONSENSE_CHIP_LSM6DSM,
+		.chip = MOTIONSENSE_CHIP_LIS2MDL,
 		.type = MOTIONSENSE_TYPE_MAG,
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lis2mdl_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lis2mdl_data,
-		.parent = &motion_sensors[BASE_ACCEL],
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data, MOTIONSENSE_TYPE_MAG),
 		.port = I2C_PORT_SENSOR,
-		.addr = LIS2MDL_ADDR0,
-		.default_range = LIS2MDL_RANGE,
+		.addr = LSM6DSM_ADDR0,
+		.default_range = 1 << 11, /* 16LSB / uT, fixed */
 		.rot_standard_ref = &mag_standard_ref,
 		.min_frequency = LIS2MDL_ODR_MIN_VAL,
 		.max_frequency = LIS2MDL_ODR_MAX_VAL,
diff --git a/board/meep/board.h b/board/meep/board.h
index e15016ce86..9beb1872e3 100644
--- a/board/meep/board.h
+++ b/board/meep/board.h
@@ -27,10 +27,15 @@
 /* Sensors */
 #define CONFIG_ACCEL_KX022		/* Lid accel */
 #define CONFIG_ACCELGYRO_LSM6DSM	/* Base accel */
+#define CONFIG_MAG_LSM6DSM_LIS2MDL	/* Magnetometer behind base accel */
+#define CONFIG_ACCELGYRO_SEC_ADDR LIS2MDL_ADDR0
 #define CONFIG_SENSORHUB_LSM6DSM
-#define CONFIG_MAG_LIS2MDL
+#define CONFIG_MAG_CALIBRATE
+#define CONFIG_FPU
+
 /* Sensors without hardware FIFO are in forced mode */
 #define CONFIG_ACCEL_FORCE_MODE_MASK (1 << LID_ACCEL)
+
 #define CONFIG_DYNAMIC_MOTION_SENSOR_COUNT
 
 #define CONFIG_LID_ANGLE
diff --git a/board/phaser/board.c b/board/phaser/board.c
index 671d4705f6..3ced43da7e 100644
--- a/board/phaser/board.c
+++ b/board/phaser/board.c
@@ -106,8 +106,7 @@ const mat33_fp_t standard_rot_ref = {
 
 /* sensor private data */
 static struct stprivate_data g_lis2dh_data;
-static struct lsm6dsm_data lsm6dsm_g_data;
-static struct lsm6dsm_data lsm6dsm_a_data;
+static struct lsm6dsm_data lsm6dsm_data;
 
 /* Drivers */
 struct motion_sensor_t motion_sensors[] = {
@@ -147,7 +146,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_a_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_ACCEL),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.rot_standard_ref = &standard_rot_ref,
@@ -176,7 +176,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_g_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_GYRO),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.default_range = 1000 | ROUND_UP_FLAG, /* dps */
diff --git a/board/yorp/board.c b/board/yorp/board.c
index 4f0c0e8fad..3a9091aef4 100644
--- a/board/yorp/board.c
+++ b/board/yorp/board.c
@@ -105,8 +105,7 @@ const mat33_fp_t base_standard_ref = {
 
 /* sensor private data */
 static struct kionix_accel_data g_kx022_data;
-static struct lsm6dsm_data lsm6dsm_g_data;
-static struct lsm6dsm_data lsm6dsm_a_data;
+static struct lsm6dsm_data lsm6dsm_data;
 
 /* Drivers */
 struct motion_sensor_t motion_sensors[] = {
@@ -143,7 +142,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_a_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_ACCEL),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.rot_standard_ref = &base_standard_ref,
@@ -172,7 +172,8 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_BASE,
 		.drv = &lsm6dsm_drv,
 		.mutex = &g_base_mutex,
-		.drv_data = &lsm6dsm_g_data,
+		.drv_data = LSM6DSM_ST_DATA(lsm6dsm_data,
+				MOTIONSENSE_TYPE_GYRO),
 		.port = I2C_PORT_SENSOR,
 		.addr = LSM6DSM_ADDR0,
 		.default_range = 1000 | ROUND_UP_FLAG, /* dps */
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,
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index 8575bfe158..a9191bdfb4 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -9,6 +9,9 @@
 #define __CROS_EC_ACCELGYRO_LSM6DSM_H
 
 #include "stm_mems_common.h"
+#include "mag_cal.h"
+#include "mag_bmm150.h"
+#include "mag_lis2mdl.h"
 
 #define LSM6DSM_I2C_ADDR(__x)		(__x << 1)
 
@@ -178,7 +181,7 @@ enum dev_fifo {
 	FIFO_DEV_INVALID = -1,
 	FIFO_DEV_GYRO = 0,
 	FIFO_DEV_ACCEL,
-#ifdef CONFIG_MAG_LIS2MDL
+#ifdef CONFIG_LSM6DSM_SEC_I2C
 	FIFO_DEV_MAG,
 #endif
 	FIFO_DEV_NUM,
@@ -247,7 +250,7 @@ struct fstatus {
 
 /* FS register address/mask for Acc/Gyro sensors */
 #define LSM6DSM_RANGE_REG(_sensor)  (LSM6DSM_ACCEL_FS_ADDR + (_sensor))
-#define LSM6DSM_RANGE_MASK  		0x0c
+#define LSM6DSM_RANGE_MASK		0x0c
 
 /* Status register bitmask for Acc/Gyro data ready */
 enum lsm6dsm_status {
@@ -260,7 +263,7 @@ enum lsm6dsm_status {
 #define LSM6DSM_STS_GDA_MASK		0x02
 
 /* Sensor resolution in number of bits: fixed 16 bit */
-#define LSM6DSM_RESOLUTION      	16
+#define LSM6DSM_RESOLUTION		16
 
 extern const struct accelgyro_drv lsm6dsm_drv;
 
@@ -285,15 +288,40 @@ struct lsm6dsm_fifo_data {
 };
 
 /*
- * Please refer to b:110013316, motion_sensor_t.drv_data field should
- * use this data type pointer rather than stprivate_data type pointer.
- * Use stprivate_data type will lead to random corrupted runtime data
- * since stprivate_data is smaller than required once CONFIG_ACCEL_FIFO
- * is defined.
+ * lsm6dsm_data is used for accel gyro and the sensor connect to a LSM6DSM.
+ *
+ * +---- lsm6dsm_data ------------------------------------------------+
+ * | +--- stprivate_data ---+                                         |
+ * | |                      | ST common data for accelerometer        |
+ * | +----------------------+                                         |
+ * | +--- stprivate_data ---+                                         |
+ * | |                      | ST common data for gyroscope            |
+ * | +----------------------+                                         |
+ * | +--- stprivate_data ---+                                         |
+ * | |                      | ST common data for LIS2MDL magnetomer   |
+ * | +----------------------+ (optional)                              |
+ * |                                                                  |
+ * | Fifo Information                                                 |
+ * |                                                                  |
+ * | +----- Magnetometer information -----------------------------+   |
+ * | | +--- mag_cal_t ------+                                     |   |
+ * | | |                    | Data for online calibration         |   |
+ * | | +--------------------+                                     |   |
+ * | | Other privata data                                         |   |
+ * | +------------------------------------------------------------+   |
+ * +------------------------------------------------------------------+
+ *
+ * In motion_sensors array, use LSM6DSM_ST_DATA to point drv_data
+ * to the right st_data structure.
  */
 struct lsm6dsm_data {
-	/* Must be first: ST generic accelerometer data. */
-	struct stprivate_data a_data;
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+	/* LIS2MDL uses st_mems_common and needs stprivate_data */
+	struct stprivate_data st_data[3];
+#else
+	/* BMM150 doesn't use st_mems_common; no stprivate_data */
+	struct stprivate_data st_data[2];
+#endif
 #ifdef CONFIG_ACCEL_FIFO
 	struct lsm6dsm_fifo_data config;
 	struct lsm6dsm_fifo_data current;
@@ -304,6 +332,17 @@ struct lsm6dsm_data {
 	 */
 	unsigned int samples_to_discard[FIFO_DEV_NUM];
 #endif
+#if defined(CONFIG_LSM6DSM_SEC_I2C) && defined(CONFIG_MAG_CALIBRATE)
+	union {
+#ifdef CONFIG_MAG_LSM6DSM_BMM150
+		struct bmm150_private_data   compass;
+#endif
+#ifdef CONFIG_MAG_LSM6DSM_LIS2MDL
+		struct lis2mdl_private_data  compass;
+#endif
+		struct mag_cal_t             cal;
+	};
+#endif  /* CONFIG_MAG_CALIBRATE */
 };
 
 /*
@@ -314,11 +353,18 @@ struct lsm6dsm_data {
  */
 #define LSM6DSM_DISCARD_SAMPLES 4
 
+#define LSM6DSM_ST_DATA(g, type) (&(&(g))->st_data[(type)])
+
 #define LSM6DSM_MAIN_SENSOR(_s) ((_s) - (_s)->type)
 
-#ifdef CONFIG_ACCEL_FIFO
-int accelgyro_fifo_disable(const struct motion_sensor_t *s);
-int accelgyro_fifo_enable(const struct motion_sensor_t *s);
-#endif /* CONFIG_ACCEL_FIFO */
+#define LSM6DSM_GET_DATA(_s) \
+	((struct lsm6dsm_data *)(LSM6DSM_MAIN_SENSOR(_s))->drv_data)
+
+#if defined(CONFIG_LSM6DSM_SEC_I2C) && defined(CONFIG_MAG_CALIBRATE)
+#define LIS2MDL_CAL(_s) (&LSM6DSM_GET_DATA(_s)->cal)
+#endif
+
+int lsm6dsm_set_data_rate(const struct motion_sensor_t *s, int rate, int rnd);
+
 
 #endif /* __CROS_EC_ACCELGYRO_LSM6DSM_H */
diff --git a/driver/mag_bmm150.h b/driver/mag_bmm150.h
index bedd66d9b8..c15cbc065b 100644
--- a/driver/mag_bmm150.h
+++ b/driver/mag_bmm150.h
@@ -108,8 +108,9 @@ struct bmm150_comp_registers {
 };
 
 struct bmm150_private_data {
-	struct bmm150_comp_registers comp;
+	/* lsm6dsm_data union requires cal be first element */
 	struct mag_cal_t             cal;
+	struct bmm150_comp_registers comp;
 };
 #define BMM150_COMP_REG(_s) \
 	(&BMI160_GET_DATA(_s)->compass.comp)
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,
 };
diff --git a/driver/mag_lis2mdl.h b/driver/mag_lis2mdl.h
index 13e1b422e4..9d1941de6a 100644
--- a/driver/mag_lis2mdl.h
+++ b/driver/mag_lis2mdl.h
@@ -8,11 +8,20 @@
 #ifndef __CROS_EC_MAG_LIS2MDL_H
 #define __CROS_EC_MAG_LIS2MDL_H
 
+#include "accelgyro.h"
+#include "mag_cal.h"
+#include "stm_mems_common.h"
+
 #define LIS2MDL_I2C_ADDR(__x)		(__x << 1)
 
+/*
+ * 7-bit address is 0011110Xb. Where 'X' is determined
+ * by the voltage on the ADDR pin
+ */
 #define LIS2MDL_ADDR0			LIS2MDL_I2C_ADDR(0x1e)
 #define LIS2MDL_ADDR1			LIS2MDL_I2C_ADDR(0x1f)
 
+/* Registers */
 #define LIS2MDL_WHO_AM_I_REG		0x4f
 #define LIS2MDL_WHO_AM_I		0x40
 
@@ -24,15 +33,36 @@
 #define LIS2MDL_STATUS_REG		0x67
 #define LIS2MDL_OUT_REG			0x68
 
-#define LIS2MDL_RANGE			4915
-#define LIS2MDL_RESOLUTION		16
+#define	LIS2DSL_RESOLUTION		16
+/*
+ * Maximum sensor data range (milligauss):
+ * Spec is 1.5 mguass / LSB, so 0.15 uT / LSB.
+ * Calibration code is set to 16LSB/ut, [0.0625 uT/LSB]
+ * Apply a multiplier to change the unit
+ */
+#define LIS2MDL_RATIO(_in) (((_in) * 24) / 10)
+
+
+struct lis2mdl_private_data {
+	/* lsm6dsm_data union requires cal be first element */
+	struct mag_cal_t cal;
+#ifdef CONFIG_MAG_BMI160_LIS2MDL
+	intv3_t          hn;   /* last sample for offset compensation */
+	int              hn_valid;
+#endif
+};
+
 
 #define LIS2MDL_ODR_MIN_VAL	10000
-#define LIS2MDL_ODR_MAX_VAL	100000
+#define LIS2MDL_ODR_MAX_VAL	50000
 #if (CONFIG_EC_MAX_SENSOR_FREQ_MILLIHZ <= LIS2MDL_ODR_MAX_VAL)
 #error "EC too slow for magnetometer"
 #endif
 
+void lis2mdl_normalize(const struct motion_sensor_t *s,
+		       intv3_t v,
+		       uint8_t *data);
+
 extern const struct accelgyro_drv lis2mdl_drv;
 
 #endif /* __CROS_EC_MAG_LIS2MDL_H */
diff --git a/driver/sensorhub_lsm6dsm.c b/driver/sensorhub_lsm6dsm.c
index c9989b8c0b..f663f5a5ae 100644
--- a/driver/sensorhub_lsm6dsm.c
+++ b/driver/sensorhub_lsm6dsm.c
@@ -188,39 +188,12 @@ static int config_slv0_read(const struct motion_sensor_t *s, uint8_t addr,
 	return EC_SUCCESS;
 }
 
-int sensorhub_set_ext_data_rate(const struct motion_sensor_t *s,
-					int rate, int rnd, int *ret_rate)
-{
-	int xl_rate;
-	int ret = EC_SUCCESS;
-
-	if (!s || s->parent)
-		return EC_ERROR_INVAL;
-
-	xl_rate = st_get_data_rate(s);
-	*ret_rate = MIN(rate, xl_rate);
-#ifdef CONFIG_ACCEL_FIFO
-	ret = accelgyro_fifo_disable(s);
-	if (ret != EC_SUCCESS)
-		return ret;
-	/*
-	 * NOTE: the ODR on the pass-through sensor does not currently change.
-	 * If it does, that step needs to be performed here
-	 */
-	ret = accelgyro_fifo_enable(s);
-#endif
-	return ret;
-}
-
 int sensorhub_config_ext_reg(const struct motion_sensor_t *s,
 				uint8_t slv_addr, uint8_t reg, uint8_t val)
 {
 	int ret;
 	int tmp;
 
-	if (!s || s->parent)
-		return EC_ERROR_INVAL;
-
 	ret = enable_i2c_pass_through(s, &tmp);
 	if (ret != EC_SUCCESS) {
 		CPRINTF("%s: %s type:0x%x ENI2C error ret: %d\n",
@@ -239,9 +212,6 @@ int sensorhub_config_slv0_read(const struct motion_sensor_t *s,
 	int tmp_xl_cfg;
 	int ret;
 
-	if (!s || s->parent)
-		return EC_ERROR_INVAL;
-
 	if (len <= 0 || len > OUT_XYZ_SIZE) {
 		CPRINTF("%s: %s type:0x%x Invalid length: %d\n",
 			__func__, s->name, s->type, len);
@@ -288,14 +258,10 @@ out_restore_ctrl1:
 	return ret;
 }
 
-int sensorhub_slv0_data_read(const struct motion_sensor_t *s, intv3_t v)
+int sensorhub_slv0_data_read(const struct motion_sensor_t *s, uint8_t *raw)
 {
-	uint8_t raw[OUT_XYZ_SIZE];
 	int ret;
 
-	if (!s || s->parent)
-		return EC_ERROR_INVAL;
-
 	/*
 	 * Accel/Gyro is already reading slave 0 data into the sensorhub1
 	 * register as soon as the accel is in power-up mode. So return the
@@ -308,9 +274,6 @@ int sensorhub_slv0_data_read(const struct motion_sensor_t *s, intv3_t v)
 			__func__, s->name, s->type, ret);
 		return ret;
 	}
-
-	/* Apply precision, sensitivity and rotation vector */
-	st_normalize(s, v, raw);
 	return EC_SUCCESS;
 }
 
@@ -321,9 +284,6 @@ int sensorhub_check_and_rst(const struct motion_sensor_t *s, uint8_t slv_addr,
 	int ret, tmp;
 	int tmp_master_cfg;
 
-	if (!s || s->parent)
-		return EC_ERROR_INVAL;
-
 	ret = enable_i2c_pass_through(s, &tmp_master_cfg);
 	if (ret != EC_SUCCESS) {
 		CPRINTF("%s: %s type:0x%x ENI2C error ret: %d\n",
diff --git a/driver/sensorhub_lsm6dsm.h b/driver/sensorhub_lsm6dsm.h
index 6bb318e281..a86ca6f9c3 100644
--- a/driver/sensorhub_lsm6dsm.h
+++ b/driver/sensorhub_lsm6dsm.h
@@ -15,21 +15,6 @@
 #include "motion_sense.h"
 
 /**
- * Set the output data rate of the external sensor that is attached
- * to the sensor hub to the requested rate.
- *
- * @param s Pointer to external motion sensor's data structure.
- * @param rate Preferred output data rate from the sensor.
- * @param rnd Flag to indicate rounding to the nearest supported
- *            output data rate by the sensor.
- * @param ret_rate Rate configured by the sensor hub depending on the
- *                 accelerometer data rate.
- * @return EC_SUCCESS on success, EC error codes on failure.
- */
-int sensorhub_set_ext_data_rate(const struct motion_sensor_t *s,
-					int rate, int rnd, int *ret_rate);
-
-/**
  * Configure the register of an external sensor that is attached to sensor
  * hub with a specific value.
  *
@@ -60,10 +45,10 @@ int sensorhub_config_slv0_read(const struct motion_sensor_t *s,
  * of the sensor hub.
  *
  * @param s Pointer to external motion sensor's data structure.
- * @param v Vector to hold the data from the external sensor.
+ * @param raw Vector to hold the data from the external sensor.
  * @return EC_SUCCESS on success, EC error codes on failure.
  */
-int sensorhub_slv0_data_read(const struct motion_sensor_t *s, intv3_t v);
+int sensorhub_slv0_data_read(const struct motion_sensor_t *s, uint8_t *raw);
 
 /**
  * Check the identity of the external sensor and then reset the external
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 6733fee8dc..2291784c1c 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -89,12 +89,6 @@ struct motion_sensor_t {
 	const struct accelgyro_drv *drv;
 	struct mutex *mutex;
 	void *drv_data;
-	/*
-	 * For use by motion sensors in cascade mode to refer
-	 * to their master/parent motion sensor. If a motion
-	 * sensor is not in cascade mode, leave it as NULL.
-	 */
-	const struct motion_sensor_t *parent;
 
 	/* i2c port */
 	uint8_t port;