diff options
Diffstat (limited to 'drivers')
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c | 102 |
1 files changed, 77 insertions, 25 deletions
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c index ead52144441..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> @@ -1104,9 +1106,10 @@ static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata) sdata->c_odr = st_lsm6ds3_odr_table.odr_avl[i].hz; //enable tap and //passthrough since we always want 6d and tap enabled; + break; case ST_INDIO_DEV_SIGN_MOTION: - + dev_info(sdata->cdata->dev, "Enabling 6d and Tap"); err = st_lsm6ds3_write_data_with_mask(sdata->cdata, ST_LSM6DS3_MD1_ADDR, ST_LSM6DS3_6D_MD1_INT_MASK, ST_LSM6DS3_EN_BIT, true); if (err < 0) @@ -1433,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]); @@ -1536,6 +1541,11 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata) #define ST_LSM6DS3_CTRL8_LPF_ON_ACCEL 0x80 #define ST_LSM6DS3_CTRL4_ADDR 0x13 #define ST_LSM6DS3_CTRL4_STOP_ON_FTH_MASK 0x1 + regval = 0x44; // MD1 (6D and Tap on INT1) + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_MD1_ADDR, + 1, ®val, false); + regval = 0x48;//tap threshold err = sdata->cdata->tf->write(sdata->cdata, ST_LSM6DS3_TAP_THS_6D_ADDR, @@ -2129,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; @@ -2147,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)); @@ -2344,16 +2394,16 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata) if (device_may_wakeup(cdata->dev)) enable_irq_wake(cdata->irq); } - err = cdata->tf->read(cdata, - ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); - dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); - dev_info(cdata->dev, "manually setting to 0x44"); - reg_value = 0x44; - err = cdata->tf->write(cdata, - ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); - err = cdata->tf->read(cdata, - ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); - dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); +// err = cdata->tf->read(cdata, +// ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); +// dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); +// dev_info(cdata->dev, "manually setting to 0x44"); +// reg_value = 0x44; +// err = cdata->tf->write(cdata, +// ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); +// err = cdata->tf->read(cdata, +// ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); +// dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); err = cdata->tf->read(cdata, 0xd, 1, ®_value, true); @@ -2422,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); |