Merge "Adding a recovery mechanism to the accelerometer. It will not hard reset the regulator if it detects a corrupt WAI register, or if there has not been any accelerometer events in 20 minutes." into android-3.10-bringup

3 files changed, 93 insertions, 40 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
index afd696effc2..3ba58d87a29 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -14,6 +14,7 @@
 #include <linux/types.h>
 #include <linux/iio/trigger.h>
 #include <linux/wakelock.h>
+#include <linux/regulator/consumer.h>
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
 #include <linux/i2c.h>
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
@@ -193,6 +194,8 @@ struct lsm6ds3_data {
 
 	const struct st_lsm6ds3_transfer_function *tf;
 	struct st_lsm6ds3_transfer_buffer tb;
+
+	struct regulator *reg_accel;
 };
 
 struct st_lsm6ds3_transfer_function {
@@ -226,6 +229,8 @@ int st_lsm6ds3_set_fifo_mode(struct lsm6ds3_data *cdata, enum fifo_mode fm);
 int st_lsm6ds3_reconfigure_fifo(struct lsm6ds3_data *cdata,
 						bool disable_irq_and_flush);
 
+int st_lsm6ds3_reset(struct lsm6ds3_data *cdata, bool hard_reset);
+
 ssize_t st_lsm6ds3_sysfs_get_hw_fifo_lenght(struct device *dev,
 				struct device_attribute *attr, char *buf);
 ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index 813b5b56536..7298fdd274e 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -24,7 +24,6 @@
 #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>
@@ -1896,7 +1895,13 @@ static ssize_t st_lsm6ds3_sysfs_write_reg(struct device *dev,
 		
 	return size;
 }
-
+static ssize_t st_lsm6ds3_sysfs_reset(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t size)
+{
+	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
+	st_lsm6ds3_reset(sdata->cdata, true);
+	return size;
+}
 static ssize_t st_lsm6ds3_sysfs_read_reg_set(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
@@ -2016,6 +2021,9 @@ static IIO_DEVICE_ATTR(read_reg, S_IRUGO | S_IWUSR,
 		st_lsm6ds3_sysfs_read_reg_get,			
 	       	st_lsm6ds3_sysfs_read_reg_set, 0);
 
+static IIO_DEVICE_ATTR(reset, S_IWUSR,
+		NULL, st_lsm6ds3_sysfs_reset, 0);
+
 static IIO_DEVICE_ATTR(sixd_wake_mask, S_IRUGO | S_IWUSR,
 		st_lsm6ds3_sysfs_sixd_wake_mask_get,			
 	       	st_lsm6ds3_sysfs_sixd_wake_mask_set, 0);
@@ -2035,6 +2043,7 @@ static struct attribute *st_lsm6ds3_accel_attributes[] = {
 	&iio_dev_attr_irq_source.dev_attr.attr,
 	&iio_dev_attr_write_reg.dev_attr.attr,
 	&iio_dev_attr_read_reg.dev_attr.attr,
+	&iio_dev_attr_reset.dev_attr.attr,
 	&iio_dev_attr_sixd_wake_mask.dev_attr.attr,
 	&iio_dev_attr_suspend_samples.dev_attr.attr,
 	&iio_dev_attr_ack_wakeup_irq.dev_attr.attr,
@@ -2141,7 +2150,9 @@ static const struct iio_trigger_ops st_lsm6ds3_trigger_ops = {
 
 static int check_wai(struct lsm6ds3_data *cdata) {
 	u8 wai = 0x00;
+	s64 current_time;
 	int err;
+	struct timespec ts;
 
 	err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true);
 	if (err < 0) {
@@ -2150,8 +2161,75 @@ static int check_wai(struct lsm6ds3_data *cdata) {
 	}
 	if (wai != ST_LSM6DS3_WAI_EXP) {
 		dev_err(cdata->dev, "Who-Am-I value not valid.\n");
-		orderly_poweroff(true);
-		return -EIO;
+		return st_lsm6ds3_reset(cdata, true);
+	}
+
+	get_monotonic_boottime(&ts);
+	current_time = timespec_to_ns(&ts);
+	if(current_time-cdata->timestamp > 20*60*1000*1000*1000) {
+		return st_lsm6ds3_reset(cdata, true);
+	}
+
+	return 0;
+}
+
+int st_lsm6ds3_reset(struct lsm6ds3_data *cdata, bool hard_reset) {
+	int err;
+	struct lsm6ds3_sensor_data *sdata;
+
+	if(hard_reset) {
+		if(!cdata->reg_accel)
+		{
+			cdata->reg_accel = regulator_get(cdata->dev, "vaccel");
+			if (IS_ERR(cdata->reg_accel))
+			{
+				err = PTR_ERR(cdata->reg_accel);
+				dev_err(cdata->dev, "Error %d getting vaccel regulator\n", err);
+				return err;
+			}
+		}
+
+		// Power cycle the accelerometer
+
+		err = regulator_disable(cdata->reg_accel);
+				if (err)
+					return err;
+
+		err = regulator_force_disable(cdata->reg_accel);
+		if (err)
+			return err;
+
+		msleep(100);
+
+		err = regulator_enable(cdata->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;
+
+	msleep(20);
+
+	//	err = check_wai(cdata);
+	//	if (err < 0) {
+	//		dev_err(cdata->dev, "Failed to check Who-Am-I register.\n");
+	//		return err;
+	//	}
+
+	err = st_lsm6ds3_init_sensor(cdata);
+	if (err < 0)
+		return err;
+
+	// Make sure the accel is enabled
+	if(hard_reset)
+	{
+		sdata = iio_priv(cdata->indio_dev[ST_INDIO_DEV_ACCEL]);
+		st_lsm6ds3_enable_sensors(sdata);
 	}
 	return 0;
 }
@@ -2170,43 +2248,10 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 	mutex_init(&cdata->passthrough_lock);
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
 
+	cdata->reg_accel = 0;
 	cdata->fifo_data = 0;
 	cdata->samples_to_keep_on_wake = ST_LSM6DS3_SAMPLES_ON_WAKE_DEFAULT;
 
-//	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;
-
-	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));
 		if (cdata->indio_dev[i] == NULL) {
@@ -2286,7 +2331,7 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 	cdata->indio_dev[ST_INDIO_DEV_TILT]->num_channels =
 					ARRAY_SIZE(st_lsm6ds3_tilt_ch);
 
-	err = st_lsm6ds3_init_sensor(cdata);
+	err = st_lsm6ds3_reset(cdata, false);
 	if (err < 0)
 		goto iio_device_free;
 
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index 815d6df8bd5..75329f01037 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -1824,8 +1824,11 @@ int regulator_force_disable(struct regulator *regulator)
 	mutex_unlock(&rdev->mutex);
 
 	if (rdev->supply)
-		while (rdev->open_count--)
+		while (rdev->open_count > 0)
+		{
+			rdev->open_count--;
 			regulator_disable(rdev->supply);
+		}
 
 	return ret;
 }