SW reset the Accelerometer if it's in a bad state. Check this on every resume

Change-Id: Icd61eac39e1cfe42446c44622c92aed6df0b5b1d

2 files changed, 60 insertions, 15 deletions

diff --git a/arch/arm/boot/dts/omap3_h1.dts b/arch/arm/boot/dts/omap3_h1.dts
index ee19b23f0cc..7b922bb66b0 100644
--- a/arch/arm/boot/dts/omap3_h1.dts
+++ b/arch/arm/boot/dts/omap3_h1.dts
@@ -385,7 +385,6 @@
 			regulator-name = "vaccel";
 			regulator-min-microvolt = <2700000>;
 			regulator-max-microvolt = <2700000>;
-			regulator-always-on;
 		};
 
 		/* vdig2_reg: regulator@6 {}; unused */
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index 9f2527bad93..813b5b56536 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -24,6 +24,8 @@
 #include <linux/iio/trigger.h>
 #include <linux/iio/buffer.h>
 #include <linux/iio/events.h>
+#include <linux/regulator/consumer.h>
+#include <linux/reboot.h>
 #include <asm/unaligned.h>
 #include <linux/wakelock.h>
 #include <linux/iio/common/st_sensors.h>
@@ -1434,11 +1436,13 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 	cdata->reset_steps = false;
 	cdata->sign_motion_event_ready = false;
 
-	err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_RESET_ADDR,
-				ST_LSM6DS3_RESET_MASK, ST_LSM6DS3_EN_BIT, true);
-	if (err < 0)
-		return err;
-
+/* No need to do another SW_RESET, we added that in the probe function
+ *
+ *	err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_RESET_ADDR,
+ *				ST_LSM6DS3_RESET_MASK, ST_LSM6DS3_EN_BIT, true);
+ *	if (err < 0)
+ *		return err;
+ */
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
 		sdata = iio_priv(cdata->indio_dev[i]);
 
@@ -2135,9 +2139,25 @@ static const struct iio_trigger_ops st_lsm6ds3_trigger_ops = {
 #define ST_LSM6DS3_TRIGGER_OPS NULL
 #endif
 
+static int check_wai(struct lsm6ds3_data *cdata) {
+	u8 wai = 0x00;
+	int err;
+
+	err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true);
+	if (err < 0) {
+		dev_err(cdata->dev, "failed to read Who-Am-I register.\n");
+		return err;
+	}
+	if (wai != ST_LSM6DS3_WAI_EXP) {
+		dev_err(cdata->dev, "Who-Am-I value not valid.\n");
+		orderly_poweroff(true);
+		return -EIO;
+	}
+	return 0;
+}
+
 int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 {
-	u8 wai = 0x00;
 	int i, n, err;
 	struct lsm6ds3_sensor_data *sdata;
 
@@ -2153,15 +2173,39 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 	cdata->fifo_data = 0;
 	cdata->samples_to_keep_on_wake = ST_LSM6DS3_SAMPLES_ON_WAKE_DEFAULT;
 
-	err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true);
-	if (err < 0) {
-		dev_err(cdata->dev, "failed to read Who-Am-I register.\n");
+//	reg_accel = regulator_get(cdata->dev, "vaccel");
+//	if (IS_ERR(reg_accel)) {
+//		err = PTR_ERR(reg_accel);
+//		dev_err(cdata->dev, "Error %d getting vaccel regulator\n", err);
+//		return err;
+//	}
+//
+//	// Power cycle the accelerometer and do a SW reset
+//
+//	err = regulator_force_disable(reg_accel);
+//	if (err)
+//		return err;
+//
+//	msleep(100);
+//
+//	err = regulator_enable(reg_accel);
+//	if (err)
+//		return err;
+//
+//	msleep(100);
+
+	err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_RESET_ADDR,
+				ST_LSM6DS3_RESET_MASK, ST_LSM6DS3_EN_BIT, true);
+	if (err < 0)
 		return err;
-	}
-	if (wai != ST_LSM6DS3_WAI_EXP) {
-		dev_err(cdata->dev, "Who-Am-I value not valid.\n");
-		return -ENODEV;
-	}
+
+	msleep(20);
+
+//	err = check_wai(cdata);
+//	if (err < 0) {
+//		dev_err(cdata->dev, "Failed to check Who-Am-I register.\n");
+//		return err;
+//	}
 
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
 		cdata->indio_dev[i] = iio_device_alloc(sizeof(*sdata));
@@ -2428,6 +2472,8 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 	}
 #endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */
 
+	check_wai(cdata);
+
 	if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) {
 		if (device_may_wakeup(cdata->dev))
 			disable_irq_wake(cdata->irq);