Adding fixes from Mindtribe (patches 0015 and 0016)

Change-Id: I9217bee0801f311590010fc75463a98f6cd23b32

4 files changed, 150 insertions, 65 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
index 479b9188273..ba971628496 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -136,6 +136,7 @@ struct lsm6ds3_data {
 	int  last_wakeup_source;
 	int 	wake_lock_initialized;
 	struct wake_lock wlock;
+	struct wake_lock tap_wlock;
 	u8 first_irq_from_resume;
 	u8 reg_read;
 #define SIXD_MASK_VALID_BITS (0x3f)
@@ -152,13 +153,14 @@ struct lsm6ds3_data {
 	u8 ext0_samples_in_pattern;
 	u8 ext1_samples_in_pattern;
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
-	u8 accel_samples_to_discard;
-	u8 gyro_samples_to_discard;
+	u32 accel_samples_to_discard;
+	u32 gyro_samples_to_discard;
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
 	u8 ext_samples_to_discard[ST_LSM6DS3_NUM_CLIENTS];
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
 
 	u16 fifo_threshold;
+	u32 samples_to_keep_on_wake;
 
 	int irq;
 
@@ -233,7 +235,7 @@ ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
 int st_lsm6ds3_allocate_rings(struct lsm6ds3_data *cdata);
 void st_lsm6ds3_deallocate_rings(struct lsm6ds3_data *cdata);
 int st_lsm6ds3_trig_set_state(struct iio_trigger *trig, bool state);
-void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len);
+void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len, bool update_discard);
 int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata);
 #define ST_LSM6DS3_TRIGGER_SET_STATE (&st_lsm6ds3_trig_set_state)
 #else /* CONFIG_IIO_BUFFER */
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
index 138868dd5ba..87cea6f349a 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
@@ -146,33 +146,58 @@ static void st_lsm6ds3_parse_fifo_data(struct lsm6ds3_data *cdata, u16 read_len)
 	return;
 }
 
-void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len)
+void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len, bool update_discard)
 {
 	int err;
 #if (CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO > 0)
 	u16 data_remaining, data_to_read;
 #endif /* CONFIG_ST_LSM6DS3_IIO_LIMIT_FIFO */
 	u16 read_len = cdata->fifo_threshold, byte_in_pattern;
-	if (!cdata->fifo_data){
-		dev_err(cdata->dev, "No cdata->fifo_data! can't use fifo");
+
+	if (!cdata->fifo_data)
 		return;
-	}
+
 	if (check_fifo_len) {
 		err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DIFF_L,
 						2, (u8 *)&read_len, true);
-		if (err < 0){
-			dev_dbg(cdata->dev, "could not read the fifo length. err: %i", err);
+		if (err < 0)
 			return;
-		}
 
 		if (read_len & ST_LSM6DS3_FIFO_DATA_OVR_2REGS) {
-			dev_dbg(cdata->dev,
-				"DATA overrun, setting read_len to threshold\n");
-			read_len = cdata->fifo_threshold;
-			//return;
+			u8 reg_value;
+			dev_info(cdata->dev,
+				"DATA overrun, diffreg:[0x%x] avail:[%i]", read_len, read_len & ST_LSM6DS3_FIFO_DIFF_MASK);
+			read_len = read_len & ST_LSM6DS3_FIFO_DIFF_MASK;
+			if(!read_len){
+				read_len = cdata->fifo_threshold;
+				dev_info(cdata->dev, "readlen is zero, setting to :%i", read_len);
+			}
+			err = cdata->tf->read(cdata,
+						6, 1, &reg_value, true);
+			dev_info(cdata->dev, "fifo_thr_L: %x err:%i", reg_value, err);
 		}
-
+		//remove upper bits to get the actual len
 		read_len &= ST_LSM6DS3_FIFO_DIFF_MASK;
+		if(update_discard){
+//currently unused, will re-add
+#ifdef DISCARD_SAMPLES 
+			if(read_len > cdata->samples_to_keep_on_wake){
+				cdata->accel_samples_to_discard = read_len - cdata->samples_to_keep_on_wake;
+				cdata->accel_samples_to_discard /= 3; //2 bytes/sample, 3 chan per accel
+				dev_info(cdata->dev, "Discard: %i samples (%i bytes)", cdata->accel_samples_to_discard,
+						cdata->accel_samples_to_discard *2 *3);
+			}
+#else
+
+			cdata->accel_samples_to_discard = 0; 
+							     
+#endif 
+			cdata->accel_timestamp = cdata->timestamp - (cdata->accel_deltatime * (read_len/3)); //backdate timestamp
+			if(update_discard){
+				dev_info(cdata->dev, "first accel timestamp:%lld last:%lld ", cdata->accel_timestamp, cdata->timestamp);
+			}
+			
+		}
 		read_len *= ST_LSM6DS3_BYTE_FOR_CHANNEL;
 
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
@@ -189,7 +214,7 @@ void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len)
 
 		read_len = (read_len / byte_in_pattern) * byte_in_pattern;
 		dev_dbg(cdata->dev, "Fifo avail: %i thresh: %i", read_len, cdata->fifo_threshold);
-		if (read_len > cdata->fifo_threshold)
+		if (!update_discard && (read_len > cdata->fifo_threshold))
 			read_len = cdata->fifo_threshold;
 	}
 
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index bbb024e3a4d..499fda71e59 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -202,6 +202,8 @@
 		IIO_DEVICE_ATTR(name, S_IRUGO, \
 			st_lsm6ds3_sysfs_scale_avail, NULL , 0);
 
+#define ST_LSM6DS3_SAMPLES_ON_WAKE_DEFAULT 52
+
 static struct st_lsm6ds3_selftest_table {
 	char *string_mode;
 	u8 accel_value;
@@ -240,7 +242,7 @@ static struct st_lsm6ds3_odr_table {
 	.mask[ST_INDIO_DEV_ACCEL] = ST_LSM6DS3_ACCEL_ODR_MASK,
 	.addr[ST_INDIO_DEV_GYRO] = ST_LSM6DS3_GYRO_ODR_ADDR,
 	.mask[ST_INDIO_DEV_GYRO] = ST_LSM6DS3_GYRO_ODR_MASK,
-	//hack , remove
+	//leaving 416 in slot 0 makes it default
 	.odr_avl[0] = { .hz = 416, .value = ST_LSM6DS3_ODR_416HZ_VAL },
 	.odr_avl[1] = { .hz = 52, .value = ST_LSM6DS3_ODR_52HZ_VAL },
 	.odr_avl[2] = { .hz = 104, .value = ST_LSM6DS3_ODR_104HZ_VAL },
@@ -248,6 +250,22 @@ static struct st_lsm6ds3_odr_table {
 	.odr_avl[4] = { .hz = 416, .value = ST_LSM6DS3_ODR_416HZ_VAL },
 };
 
+u32 fifo_odr[]  = 
+{
+	[0] = 0, //disabled
+	[1] = 13,
+        [2] = 26,
+        [3] = 52,
+        [4] = 104, 
+	[5] = 208,
+       	[6] = 416,
+	[7] = 833,
+       	[8] = 1666,
+	[9] = 3330,
+	[10]= 6660
+
+};
+
 struct st_lsm6ds3_fs_reg {
 	unsigned int gain;
 	u8 value;
@@ -665,8 +683,7 @@ int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata)
 						(sdata_accel->c_odr / min_odr);
 		num_pattern_accel = MAX(fifo_len_accel /
 					cdata->accel_samples_in_pattern, 1);
-		//TODO: this needs to reflect fifo odr rates and not c_odr
-		cdata->accel_deltatime = (1000000000ULL / sdata_accel->c_odr);
+		cdata->accel_deltatime = (1000000000ULL / fifo_odr[ST_LSM6DS3_FIFO_ODR_MAX]);
 		accel_decimator = max_odr / sdata_accel->c_odr;
 	} else
 		cdata->accel_samples_in_pattern = 0;
@@ -803,7 +820,8 @@ int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata)
 	cdata->fifo_data = 0;
 
 	if (fifo_len > 0) {
-		cdata->fifo_data = kmalloc(cdata->fifo_threshold, GFP_KERNEL);
+		//lets us read the entire fifo and not just up to threshold
+		cdata->fifo_data = kmalloc(ST_LSM6DS3_MAX_FIFO_SIZE, GFP_KERNEL); 
 		if (!cdata->fifo_data)
 			return -ENOMEM;
 	}
@@ -824,7 +842,7 @@ int st_lsm6ds3_reconfigure_fifo(struct lsm6ds3_data *cdata,
 
 	mutex_lock(&cdata->fifo_lock);
 
-	st_lsm6ds3_read_fifo(cdata, true);
+	st_lsm6ds3_read_fifo(cdata, true, false);
 
 	err = st_lsm6ds3_set_fifo_mode(cdata, BYPASS);
 	if (err < 0)
@@ -914,7 +932,7 @@ int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
 		st_lsm6ds3_write_data_with_mask(sdata->cdata,
 						reg_addr, mask, value, true);
 #endif 
-//TODO: move this to init since we don't need to set up the parameters every time
+		//TODO: move this to init since we don't need to set up the parameters every time
 		reg_addr = ST_LSM6DS3_MD1_ADDR;
 		mask = ST_LSM6DS3_6D_MD1_INT_MASK;
 		st_lsm6ds3_write_data_with_mask(sdata->cdata,
@@ -1548,9 +1566,8 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 #define ST_LSM6DS3_CTRL8_ADDR 0x17
 #define ST_LSM6DS3_CTRL8_LPF_ON_6D 0x01	
 #define ST_LSM6DS3_CTRL8_LPF_ON_ACCEL 0x80
-
-#define ST_LSM6DS3_CTRL4C_ADDR 0x13
-
+#define ST_LSM6DS3_CTRL4_ADDR 0x13
+#define ST_LSM6DS3_CTRL4_STOP_ON_FTH_MASK 0x1
 	regval = 0x50;//tap threshold
 	err = sdata->cdata->tf->write(sdata->cdata,
 					ST_LSM6DS3_TAP_THS_6D_ADDR,
@@ -1572,11 +1589,6 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 					ST_LSM6DS3_CTRL8_ADDR ,
 					1, &regval, false);
 	
-	regval = 1;//todo fix this because it overwrites int2_on_int1 bit 
-	err = sdata->cdata->tf->write(sdata->cdata,
-					ST_LSM6DS3_CTRL4C_ADDR ,
-					1, &regval, false);
-
 	if (err < 0)
 		goto st_lsm6ds3_init_sensor_mutex_unlock;
 
@@ -1842,7 +1854,7 @@ ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
 
 	mutex_lock(&sdata->cdata->fifo_lock);
 
-	st_lsm6ds3_read_fifo(sdata->cdata, true);
+	st_lsm6ds3_read_fifo(sdata->cdata, true, false);
 
 	mutex_unlock(&sdata->cdata->fifo_lock);
 
@@ -1884,7 +1896,7 @@ static ssize_t st_lsm6ds3_sysfs_read_reg_set(struct device *dev,
 {
 	int ret ;
 	struct lsm6ds3_sensor_data *sdata = iio_priv(dev_get_drvdata(dev));
-	uint32_t waddr, wval;
+	uint32_t waddr;
 	ret = sscanf(buf, "%i", &waddr);
 	dev_info(dev, "read: 0x%x ", waddr);
 	if(ret != 1){
@@ -1934,6 +1946,33 @@ static ssize_t st_lsm6ds3_sysfs_sixd_wake_mask_set(struct device *dev,
 		
 	return size;
 }
+
+ssize_t st_lsm6ds3_sysfs_suspend_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->samples_to_keep_on_wake);
+	return ret;
+}
+
+
+static ssize_t st_lsm6ds3_sysfs_suspend_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){
+		return -EINVAL;
+	}
+
+	sdata->cdata->samples_to_keep_on_wake = 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);
@@ -1973,6 +2012,10 @@ 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);
 
+static IIO_DEVICE_ATTR(suspend_samples, S_IRUGO | S_IWUSR,
+		st_lsm6ds3_sysfs_suspend_samples_get,			
+	       	st_lsm6ds3_sysfs_suspend_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,
@@ -1985,6 +2028,7 @@ static struct attribute *st_lsm6ds3_accel_attributes[] = {
 	&iio_dev_attr_write_reg.dev_attr.attr,
 	&iio_dev_attr_read_reg.dev_attr.attr,
 	&iio_dev_attr_sixd_wake_mask.dev_attr.attr,
+	&iio_dev_attr_suspend_samples.dev_attr.attr,
 	NULL,
 };
 
@@ -2095,12 +2139,14 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 	mutex_init(&cdata->bank_registers_lock);
 	mutex_init(&cdata->fifo_lock);
 	wake_lock_init(&cdata->wlock, WAKE_LOCK_SUSPEND, "st_lsm6ds3_kdriv_lock");
+	wake_lock_init(&cdata->tap_wlock, WAKE_LOCK_SUSPEND, "st_lsm6ds3_kdriv_tap_lock");
 
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
 	mutex_init(&cdata->passthrough_lock);
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
 
 	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) {
@@ -2263,6 +2309,7 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
 	u8 tmp_sensors_enabled;
 	struct lsm6ds3_sensor_data *sdata;
 	u8 reg_value;
+	u16 sleep_fifo_size;
 
 	tmp_sensors_enabled = cdata->sensors_enabled;
 	dev_info(cdata->dev, "entering suspend");
@@ -2314,26 +2361,32 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
 	err = cdata->tf->read(cdata,
 			0x10, 1, &reg_value, true);
 	dev_info(cdata->dev, "ctrl1_xl: %x err:%i", reg_value, err);
+	cdata->first_irq_from_resume = 1;
 
-        err = cdata->tf->read(cdata,
-                        0xa, 1, &reg_value, true);
-        dev_info(cdata->dev, "before suspend 0xA fifo mode: %x err:%i", reg_value, err);
-
-        err = cdata->tf->read(cdata,
-                        0x13, 1, &reg_value, true);
-        dev_info(cdata->dev, "before suspend 0x13 ctrl4_c : %x err:%i", reg_value, err);
-
-        err = cdata->tf->read(cdata,
-                        0x06, 1, &reg_value, true);
-        dev_info(cdata->dev, "before suspend 0x6 fifo_fth : %x err:%i", reg_value, err);
-
-
+	//limit wakeup fifo
+	sleep_fifo_size = cdata->samples_to_keep_on_wake * 3; //3 samples/chan 
+	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,
+			ST_LSM6DS3_FIFO_THR_L_ADDR, 1, &reg_value, true);
+	reg_value = sleep_fifo_size >> 8 ;
+	err = cdata->tf->write(cdata,
+			ST_LSM6DS3_FIFO_THR_H_ADDR, 1, &reg_value, true);
+		
 
-	reg_value = 624; //set fifo to 1 second of data (104hz fifo)
+	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);
+	dev_info(cdata->dev, "before suspend fifo_thr_L: %x err:%i", reg_value, err);
+
+	err = cdata->tf->read(cdata,
+			0x13, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend 0x13 ctrl4: %x err:%i", reg_value, err);
+#endif 
 
-	cdata->first_irq_from_resume = 1;
 	return 0;
 }
 EXPORT_SYMBOL(st_lsm6ds3_common_suspend);
@@ -2369,10 +2422,14 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 		if (device_may_wakeup(cdata->dev))
 			disable_irq_wake(cdata->irq);
 	}
-	reg_value = 624/8; //set fifo to 1 second of data (104hz fifo)
+	
+	
+	mutex_lock(&cdata->fifo_lock);
+	reg_value = 0; //clear stop on fth 
 	err = cdata->tf->write(cdata,
-			ST_LSM6DS3_FIFO_THR_L_ADDR, 1, &reg_value, true);
+			ST_LSM6DS3_CTRL4_ADDR, 1, &reg_value, true);
 
+	mutex_unlock(&cdata->fifo_lock);
 	return 0;
 }
 EXPORT_SYMBOL(st_lsm6ds3_common_resume);
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
index ccb215c20d0..2e7af62ed47 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -69,7 +69,6 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	u8 src_value = 0x00, src_fifo = 0x00;
 	u8 d6d_event = 0;
 	u8 tap_event = 0;
-	//u8 read_buff[2] = {0};
 
 
 	cdata = container_of((struct work_struct *)data_work,
@@ -77,10 +76,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 
 	if(!wake_lock_active(&cdata->wlock))
 		wake_lock(&cdata->wlock);
-
-	//cdata->tf->read(cdata, ST_LSM6DS3_TAP_SRC_ADDR, 2, &read_buff[0], true);
-	//tap_src_reg = read_buff[0];
-	//d6d_src_reg = read_buff[1];
+	 mutex_lock(&cdata->fifo_lock); 
 	cdata->tf->read(cdata, ST_LSM6DS3_6D_SRC_ADDR, 1, &d6d_src_reg, true);
 	cdata->tf->read(cdata, ST_LSM6DS3_TAP_SRC_ADDR, 1, &tap_src_reg, true);
 	cdata->tf->read(cdata, ST_LSM6DS3_SRC_FUNC_ADDR, 1, &src_value, true);
@@ -119,25 +115,29 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
 			dev_info(cdata->dev, "Valid Tap from sleep");
 		}
+
+
 	}
 
 	if (src_fifo & ST_LSM6DS3_FIFO_DATA_AVL) {
-		dev_dbg(cdata->dev, "Fifo data available");
-		st_lsm6ds3_read_fifo(cdata, true);
+		if(cdata->first_irq_from_resume){
+			st_lsm6ds3_read_fifo(cdata, true, true);
+		}
+		else{
+			st_lsm6ds3_read_fifo(cdata, true, false);
+		}
 	}
 //significant motion event processing
 	if(d6d_event || tap_event){
-		dev_info(cdata->dev, "Sending sig mot event");
-		wake_lock_timeout(&cdata->wlock,msecs_to_jiffies(500));
-		if (cdata->sign_motion_event_ready) {
-			iio_push_event(cdata->indio_dev[
-				ST_INDIO_DEV_SIGN_MOTION],
-				IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION,
-				0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
-				cdata->accel_timestamp);
+		dev_info(cdata->dev, "Sending sig mot event; ready:%i",cdata->sign_motion_event_ready);
+		wake_lock_timeout(&cdata->tap_wlock,msecs_to_jiffies(1500));
+		iio_push_event(cdata->indio_dev[
+			ST_INDIO_DEV_SIGN_MOTION],
+			IIO_UNMOD_EVENT_CODE(IIO_SIGN_MOTION,
+			0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
+			cdata->accel_timestamp);
 
 			cdata->sign_motion_event_ready = false;
-		}
 	}
 
 	if (src_value & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) {
@@ -171,6 +171,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	}
 
 	enable_irq(cdata->irq);
+	mutex_unlock(&cdata->fifo_lock); 
 	if(wake_lock_active(&cdata->wlock))
 		wake_unlock(&cdata->wlock);