diff options
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h | 1 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c | 55 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c | 2 |
3 files changed, 58 insertions, 0 deletions
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h index 2533a9b78de..718c8ea4280 100644 --- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h @@ -162,6 +162,7 @@ struct lsm6ds3_data { u16 fifo_threshold; u32 samples_to_keep_on_wake; + u32 min_latency_samples; int irq; diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c index 68e5df1de3f..df19c4e08d3 100644 --- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c @@ -204,6 +204,7 @@ st_lsm6ds3_sysfs_scale_avail, NULL , 0); #define ST_LSM6DS3_SAMPLES_ON_WAKE_DEFAULT 52 +#define ST_LSM6DS3_MIN_LATENCY_SAMPLES 12 static struct st_lsm6ds3_selftest_table { char *string_mode; @@ -801,6 +802,9 @@ int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata) if (fifo_len > 0) { fifo_threshold = fifo_len / 2; + if(fifo_threshold > cdata->min_latency_samples) + fifo_threshold = cdata->min_latency_samples; + dev_info(cdata->dev,"setting FIFO length/threshold: 0x%x 0x%x ", fifo_len, fifo_threshold); err = cdata->tf->write(cdata, ST_LSM6DS3_FIFO_THR_L_ADDR, @@ -1984,6 +1988,33 @@ static ssize_t st_lsm6ds3_sysfs_suspend_samples_set(struct device *dev, return size; } + +ssize_t st_lsm6ds3_sysfs_min_latency_samples_get(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + ssize_t ret = sprintf(buf, "0x%02x\n", sdata->cdata->min_latency_samples); + return ret; +} + + +static ssize_t st_lsm6ds3_sysfs_min_latency_samples_set(struct device *dev, + struct device_attribute *attr, const char *buf, size_t size) +{ + int ret ; + struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev)); + uint32_t val; + ret = sscanf(buf, "%i", &val); + dev_info(sdata->cdata->dev, "read: 0x%x ", val); + if(ret != 1 || val <= 1){ + return -EINVAL; + } + + sdata->cdata->min_latency_samples = val; + + return size; +} + static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ(); static ST_LSM6DS3_DEV_ATTR_SAMP_FREQ_AVAIL(); static ST_LSM6DS3_DEV_ATTR_SCALE_AVAIL(in_accel_scale_available); @@ -2032,6 +2063,10 @@ static IIO_DEVICE_ATTR(suspend_samples, S_IRUGO | S_IWUSR, st_lsm6ds3_sysfs_suspend_samples_get, st_lsm6ds3_sysfs_suspend_samples_set, 0); +static IIO_DEVICE_ATTR(min_latency_samples, S_IRUGO | S_IWUSR, + st_lsm6ds3_sysfs_min_latency_samples_get, + st_lsm6ds3_sysfs_min_latency_samples_set, 0); + static struct attribute *st_lsm6ds3_accel_attributes[] = { &iio_dev_attr_sampling_frequency_available.dev_attr.attr, &iio_dev_attr_in_accel_scale_available.dev_attr.attr, @@ -2047,6 +2082,7 @@ static struct attribute *st_lsm6ds3_accel_attributes[] = { &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, + &iio_dev_attr_min_latency_samples.dev_attr.attr, NULL, }; @@ -2254,6 +2290,18 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq) cdata->reg_accel = 0; cdata->fifo_data = 0; cdata->samples_to_keep_on_wake = ST_LSM6DS3_SAMPLES_ON_WAKE_DEFAULT; + cdata->min_latency_samples = ST_LSM6DS3_MIN_LATENCY_SAMPLES; + + + 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"); + return -ENODEV; + } for (i = 0; i < ST_INDIO_DEV_NUM; i++) { cdata->indio_dev[i] = iio_device_alloc(sizeof(*sdata)); @@ -2467,6 +2515,12 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata) //limit wakeup fifo sleep_fifo_size = cdata->samples_to_keep_on_wake * 3; //3 samples/chan + if(cdata->samples_to_keep_on_wake >= cdata->indio_dev[ST_INDIO_DEV_ACCEL]->buffer->length){ + //Resize one or the other if you get this warning + dev_err(cdata->dev, "Error: Buffer length(%i) is to small to hold %i samples in sleep.", + cdata->indio_dev[ST_INDIO_DEV_ACCEL]->buffer->length, cdata->samples_to_keep_on_wake); + } + dev_info(cdata->dev , "Setting sleep fifo size to: %i, samples: %i", sleep_fifo_size, cdata->samples_to_keep_on_wake); reg_value = sleep_fifo_size & 0xFF; err = cdata->tf->write(cdata, @@ -2479,6 +2533,7 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata) reg_value = ST_LSM6DS3_CTRL4_STOP_ON_FTH_MASK; //set stop on fth to limit fifo err = cdata->tf->write(cdata, ST_LSM6DS3_CTRL4_ADDR, 1, ®_value, true); + #ifdef FIFO_SLEEP_DEBUG err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_THR_L_ADDR, 1, ®_value, true); diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c index 7c497447640..6afc3fe55d5 100644 --- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c @@ -270,6 +270,8 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work) if(cdata->first_irq_from_resume){ cdata->first_irq_from_resume = 0; + //reset the fifo threshold + //fifo stop on threshold is cleared in common_resume st_lsm6ds3_reconfigure_fifo(cdata, false); #ifdef WAKE_STATS_DEBUG_INFO print_wake_stats(cdata); |