summaryrefslogtreecommitdiff
path: root/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'drivers')
-rw-r--r--drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h5
-rw-r--r--drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c123
-rw-r--r--drivers/regulator/core.c5
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;
}
generated by cgit v1.2.3-70-g09d2 (git 2.51.2) at 2025-10-31 16:52:05 +0000