driver: lsm6dsm: add reboot at init time

Without this patch, the pass-through mode will get stuck after sysjump.
Force a reboot of the lsm6dsm accelerometer side at init() to allow
magnetometer initialization to complete.

BUG=b:115587004
BRANCH=octopus
TEST=execute "sysjump RW" then make sure magnetometer initial passed
and no errors as below:
[855.944737 Base Accel: MS Done Init type:0x0 range:2]
[855.947773 Base Accel ODR: 13000 - roundup 1 from config 1 [AP 0]]
[855.949904 Base Gyro: MS Done Init type:0x1 range:1000]
[855.952509 Base Gyro ODR: 0 - roundup 0 from config 0 [AP 0]]
sensorhub_check_and_rst: Base Accel type:0x0 WAIR error ret: 1
sensorhub_check_and_rst: Base Accel type:0x0 WAIR error ret: 1
sensorhub_check_and_rst: Base Accel type:0x0 WAIR error ret: 1
[855.993153 Base Mag: 3: init failed: 1]

Change-Id: I7870acb674457dfcf247a3fa231e82a7123303bf
Signed-off-by: Devin Lu <Devin.Lu@quantatw.com>
Reviewed-on: https://chromium-review.googlesource.com/1389994
Reviewed-by: Justin TerAvest <teravest@chromium.org>

2 files changed, 51 insertions, 0 deletions

diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c
index 59a46cdaac..b53755ada4 100644
--- a/driver/accelgyro_lsm6dsm.c
+++ b/driver/accelgyro_lsm6dsm.c
@@ -553,6 +553,7 @@ static int init(const struct motion_sensor_t *s)
 	 *     struct stprivate_data *data = &lsm_data->a_data;
 	 */
 	struct stprivate_data *data = s->drv_data;
+	uint8_t ctrl_reg, reg_val = 0;
 
 	ret = st_raw_read8(s->port, s->addr, LSM6DSM_WHO_AM_I_REG, &tmp);
 	if (ret != EC_SUCCESS)
@@ -571,12 +572,61 @@ static int init(const struct motion_sensor_t *s)
 	if (s->type == MOTIONSENSE_TYPE_ACCEL) {
 		mutex_lock(s->mutex);
 
+		/* Software reset procedure. */
+		reg_val = LSM6DSM_ODR_TO_REG(LSM6DSM_ODR_MIN_VAL);
+		ctrl_reg = LSM6DSM_ODR_REG(MOTIONSENSE_TYPE_ACCEL);
+
+		/* Power OFF gyro. */
+		ret = st_raw_write8(s->port, s->addr, LSM6DSM_CTRL2_ADDR, 0);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+
+		/* Power ON Accel. */
+		ret = st_raw_write8(s->port, s->addr, ctrl_reg, reg_val);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+
 		/* Software reset. */
 		ret = st_raw_write8(s->port, s->addr, LSM6DSM_CTRL3_ADDR,
 				LSM6DSM_SW_RESET);
 		if (ret != EC_SUCCESS)
 			goto err_unlock;
 
+#ifdef CONFIG_MAG_LIS2MDL
+		/*
+		 * TODO: Check for pass-through mode instead of magnetometer
+		 * config.
+		 */
+
+		/*
+		 * Reboot to reload memory content as pass-through mode can get
+		 * stuck.
+		 * Direct to the AN: See "AN4987 - LSM6DSM: always-on 3D
+		 * accelerometer and 3D gyroscope".
+		 */
+
+		/* Power ON Accel. */
+		ret = st_raw_write8(s->port, s->addr, ctrl_reg, reg_val);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+
+		ret = st_raw_write8(s->port, s->addr, LSM6DSM_CTRL3_ADDR,
+				LSM6DSM_BOOT);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+
+		/*
+		 * Refer to AN4987, wait 15ms for accelerometer to doing full
+		 * reboot.
+		 */
+		msleep(15);
+
+		/* Power OFF Accel. */
+		ret = st_raw_write8(s->port, s->addr, ctrl_reg, 0);
+		if (ret != EC_SUCCESS)
+			goto err_unlock;
+#endif
+
 		/*
 		 * Output data not updated until have been read.
 		 * Prefer interrupt to be active low.
diff --git a/driver/accelgyro_lsm6dsm.h b/driver/accelgyro_lsm6dsm.h
index fd7f59c3b3..0d0480a9cb 100644
--- a/driver/accelgyro_lsm6dsm.h
+++ b/driver/accelgyro_lsm6dsm.h
@@ -46,6 +46,7 @@
 #define LSM6DSM_PP_OD				0x10
 #define LSM6DSM_H_L_ACTIVE			0x20
 #define LSM6DSM_BDU				0x40
+#define LSM6DSM_BOOT				0x80
 
 #define LSM6DSM_CTRL4_ADDR		0x13
 #define LSM6DSM_INT2_ON_INT1_MASK		0x20