driver: lis2mdl: fix magnetometer range/resolution

Change drv_data pointer to st_private_data, to make LIS2MDL driver
compliant with other ST driver.

In standalone mode we have:
 +--- lis2mdl_private_data ---+
 | struct mag_cal_t cal;      | <-------- LIS2MDL_CAL(s)
 |                            |              |
 | struct stprivate_data data;| <-------- s->drv_data
 +----------------------------+

BRANCH=None
BUG=b:132288982
TEST=Check scale returns 0.000625000
Checks figure-8 calibration is working
Checks CTS verifier test "Magnetic Field Measurements Tests" pass.

Change-Id: Iaba99b50cb0bf9bfc76f67cef0da8843a86f4838
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1616884
Legacy-Commit-Queue: Commit Bot <commit-bot@chromium.org>
Reviewed-by: Jett Rink <jettrink@chromium.org>
Reviewed-by: Yuval Peress <peress@chromium.org>

3 files changed, 13 insertions, 17 deletions

diff --git a/board/arcada_ish/board.c b/board/arcada_ish/board.c
index f5e3d3ef0b..8d3d13bb9c 100644
--- a/board/arcada_ish/board.c
+++ b/board/arcada_ish/board.c
@@ -136,7 +136,7 @@ struct motion_sensor_t motion_sensors[] = {
 		.location = MOTIONSENSE_LOC_LID,
 		.drv = &lis2mdl_drv,
 		.mutex = &g_lid_mag_mutex,
-		.drv_data = &lis2mdl_a_data,
+		.drv_data = LIS2MDL_ST_DATA(lis2mdl_a_data),
 		.port = I2C_PORT_SENSOR,
 		.addr = LIS2MDL_ADDR,
 		.default_range = 1 << 11,	/* 16LSB / uT, fixed  */
diff --git a/driver/mag_lis2mdl.c b/driver/mag_lis2mdl.c
index 2fd35612be..1dedb23882 100644
--- a/driver/mag_lis2mdl.c
+++ b/driver/mag_lis2mdl.c
@@ -257,6 +257,7 @@ int lis2mdl_read(const struct motion_sensor_t *s, intv3_t v)
 int lis2mdl_init(const struct motion_sensor_t *s)
 {
 	int ret = EC_ERROR_UNKNOWN, who_am_i, count = LIS2MDL_STARTUP_MS;
+	struct stprivate_data *data = s->drv_data;
 	struct mag_cal_t *cal = LIS2MDL_CAL(s);
 
 	/* Check who am I value */
@@ -293,7 +294,7 @@ int lis2mdl_init(const struct motion_sensor_t *s)
 
 	init_mag_cal(cal);
 	cal->radius = 0.0f;
-	LIS2MDL_ST_DATA(s)->base.odr = 0;
+	data->resol = LIS2DSL_RESOLUTION;
 	return sensor_init_done(s);
 
 lis2mdl_init_error:
@@ -321,7 +322,7 @@ int lis2mdl_set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 	int ret = EC_SUCCESS, normalized_rate = 0;
 	uint8_t reg_val = 0;
 	struct mag_cal_t *cal = LIS2MDL_CAL(s);
-	struct stprivate_data *data = LIS2MDL_ST_DATA(s);
+	struct stprivate_data *data = s->drv_data;
 
 	if (rate > 0) {
 		if (rnd)
@@ -375,11 +376,6 @@ int lis2mdl_set_data_rate(const struct motion_sensor_t *s, int rate, int rnd)
 	return ret;
 }
 
-int get_data_rate(const struct motion_sensor_t *s)
-{
-	return LIS2MDL_ST_DATA(s)->base.odr;
-}
-
 #endif /* CONFIG_MAG_LIS2MDL */
 
 const struct accelgyro_drv lis2mdl_drv = {
@@ -387,15 +383,14 @@ const struct accelgyro_drv lis2mdl_drv = {
 	.init = lis2mdl_thru_lsm6dsm_init,
 	.read = lis2mdl_thru_lsm6dsm_read,
 	.set_data_rate = lsm6dsm_set_data_rate,
-	.get_data_rate = st_get_data_rate,
 #else /* CONFIG_MAG_LSM6DSM_LIS2MDL */
 	.init = lis2mdl_init,
 	.read = lis2mdl_read,
 	.set_data_rate = lis2mdl_set_data_rate,
-	.get_data_rate = get_data_rate,
 #endif /* !CONFIG_MAG_LSM6DSM_LIS2MDL */
 	.set_range = set_range,
 	.get_range = get_range,
+	.get_data_rate = st_get_data_rate,
 	.get_resolution = st_get_resolution,
 	.set_offset = set_offset,
 	.get_offset = get_offset,
diff --git a/driver/mag_lis2mdl.h b/driver/mag_lis2mdl.h
index 0a415ceebd..f87b4e1f91 100644
--- a/driver/mag_lis2mdl.h
+++ b/driver/mag_lis2mdl.h
@@ -59,21 +59,22 @@
 struct lis2mdl_private_data {
 	/* lsm6dsm_data union requires cal be first element */
 	struct mag_cal_t cal;
+#ifndef CONFIG_LSM6DSM_SEC_I2C
 	struct stprivate_data data;
+#endif
 #ifdef CONFIG_MAG_BMI160_LIS2MDL
 	intv3_t          hn;   /* last sample for offset compensation */
 	int              hn_valid;
 #endif
 };
 
-#define LIS2MDL_GET_DATA(_s) \
-	((struct lis2mdl_private_data *)(_s->drv_data))
-
-#define LIS2MDL_ST_DATA(_s) \
-	(&(LIS2MDL_GET_DATA(_s)->data))
+#ifndef CONFIG_LSM6DSM_SEC_I2C
+#define LIS2MDL_ST_DATA(g) (&((g).data))
 
-#if !defined(CONFIG_LSM6DSM_SEC_I2C) && defined(CONFIG_MAG_CALIBRATE)
-#define LIS2MDL_CAL(_s) (&LIS2MDL_GET_DATA(_s)->cal)
+#ifdef CONFIG_MAG_CALIBRATE
+#define LIS2MDL_CAL(_s) \
+	(&(DOWNCAST(s->drv_data, struct lis2mdl_private_data, data)->cal))
+#endif
 #endif