fixing irq latency and buffer sizing

Change-Id: I62951ab83083e71ec4e3fd2b3bb42c5f8d9aa9ce

Signed-off-by: Evan Wilson <evan@oliodevices.com>

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 96cd843b55f..ac126735805 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, &reg_value, true);
+
 #ifdef FIFO_SLEEP_DEBUG	
 	err = cdata->tf->read(cdata,
 			ST_LSM6DS3_FIFO_THR_L_ADDR, 1, &reg_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);