ST lsm6ds3 fixes. Moved IRQ registration to end of probe, changed 6d push function to use '01,01,01' as the 6d signifier

Change-Id: Ia162ef14178cda4d9a19a72159f1d0e3bcbbcd0f

3 files changed, 35 insertions, 27 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
index eb080d4b8f9..b89141689a1 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
@@ -160,7 +160,7 @@ void st_lsm6ds3_push_tap_to_fifo(struct lsm6ds3_data *cdata)
 
 void st_lsm6ds3_push_d6d_to_fifo(struct lsm6ds3_data *cdata)
 {
-	u8 d6d_signature[6] = {1}; 
+	u8 d6d_signature[6] = {0,1,0,1,0,1};
 	dev_info(cdata->dev, "Sending 6d event through accel data @:%llu", cdata->accel_timestamp-1);
 	st_lsm6ds3_push_data_with_timestamp(
 		cdata, ST_INDIO_DEV_ACCEL,
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index aca1924a38b..113efe2d4b4 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -2244,13 +2244,6 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 	if (err < 0)
 		goto iio_device_free;
 
-	if (irq > 0) {
-		err = st_lsm6ds3_allocate_triggers(cdata,
-							ST_LSM6DS3_TRIGGER_OPS);
-		if (err < 0)
-			goto deallocate_ring;
-	}
-
 	for (n = 0; n < ST_INDIO_DEV_NUM; n++) {
 		err = iio_device_register(cdata->indio_dev[n]);
 		if (err)
@@ -2263,6 +2256,18 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 
 	device_init_wakeup(cdata->dev, true);
 
+	/* 
+	 * We need to do this last, since the IRQs will be enabled 
+	 * immediately once request_threaded_irq returns.
+	 */
+
+	if (irq > 0) {
+		err = st_lsm6ds3_allocate_triggers(cdata,
+							ST_LSM6DS3_TRIGGER_OPS);
+		if (err < 0)
+			goto iio_device_unregister_and_trigger_deallocate;
+		        /* goto deallocate_ring; */
+	}
 	return 0;
 
 iio_device_unregister_and_trigger_deallocate:
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
index 4ee21595362..de189237ae4 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -149,7 +149,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	int ignore_event = 0;
 
 	cdata = container_of((struct work_struct *)data_work,
-						struct lsm6ds3_data, data_work);
+			     struct lsm6ds3_data, data_work);
 
 	wake_irq = last_wakeup_reason_test(cdata->irq);
 	mutex_lock(&cdata->fifo_lock); 
@@ -159,34 +159,38 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1,
 							&src_fifo, true);
 
-	dev_dbg(cdata->dev, "ST irq start :src_value, 6d, tap:%x %x %x",src_value, d6d_src_reg, tap_src_reg);
+	dev_dbg(cdata->dev, "ST irq start :src_value, 6d, tap:%x %x %x",
+		src_value, d6d_src_reg, tap_src_reg);
 
 	if(d6d_src_reg & ST_LSM6DS3_6D_SRC_DETECTED_MASK){
 #ifdef WAKE_STATS_DEBUG_INFO
 		update_orient_stats(d6d_src_reg, cdata->timestamp);
 #endif
-		dev_info(cdata->dev, "D6D IRQ val:0x%x mask:0x%x",  SIXD_MASK_VALID_BITS & d6d_src_reg, cdata->sixd_mask);
+		dev_info(cdata->dev, "D6D IRQ val:0x%x mask:0x%x",
+			 SIXD_MASK_VALID_BITS & d6d_src_reg, cdata->sixd_mask);
+
 		if(cdata->sixd_mask & d6d_src_reg){
 			d6d_event = 1;
 			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_6D;
-		}
-		else{
+		} else {
 			ignore_event = 1;
-			dev_info(cdata->dev, "ignoring 6d interrupt, wrong axis. mask: 0x%x", cdata->sixd_mask);
-		} 
-
+			dev_info(cdata->dev, "ignoring 6d interrupt, wrong axis. mask: 0x%x", 
+				 cdata->sixd_mask);
+		}
 	}
+
 	if(tap_src_reg & ST_LSM6DS3_TAP_SRC_DETECTED_MASK){
 		dev_info(cdata->dev, "TAP IRQ");
-		if(tap_src_reg & (ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK | ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK)){
+		if(tap_src_reg & (ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK |
+				  ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK)){
 			tap_event = 1;
 			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
 			dev_info(cdata->dev, "Valid Tap");
-		}
-		else { 
+		} else {
 			dev_info(cdata->dev, "Ignoring tap");
 		}
 	}
+	
 	if(cdata->first_irq_from_resume && wake_irq){
 #ifdef WAKE_STATS_DEBUG_INFO
 		wakeup_irq_count++;
@@ -196,8 +200,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 			tap_event = 1;
 			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
 			dev_info(cdata->dev, "Valid Tap from sleep");
-		}
-
+		}	
 	}
 
 	if(!ignore_event && (tap_event || d6d_event) && cdata->first_irq_from_resume){
@@ -209,7 +212,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 		//	st_lsm6ds3_push_d6d_to_fifo(cdata);
 		//}
 	}
-	else if(d6d_event && !ignore_event){//negative roll 
+	else if(d6d_event && !ignore_event) { //negative roll 
 		wake_lock_timeout(&cdata->tap_wlock,msecs_to_jiffies(200));
 #ifdef WAKE_STATS_DEBUG_INFO
 		wakeup_irq_keepawake_count++;
@@ -276,7 +279,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 }
 
 int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
-				const struct iio_trigger_ops *trigger_ops)
+				 const struct iio_trigger_ops *trigger_ops)
 {
 	int err, i, n;
 
@@ -303,7 +306,7 @@ int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
 	}
 
 	err = request_threaded_irq(cdata->irq, st_lsm6ds3_save_timestamp, NULL,
-					IRQF_TRIGGER_HIGH, cdata->name, cdata);
+				   IRQF_TRIGGER_HIGH, cdata->name, cdata);
 	if (err)
 		goto deallocate_trigger;
 
@@ -316,9 +319,9 @@ int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
 		}
 		cdata->indio_dev[n]->trig = cdata->trig[n];
 	}
-
+	
 	return 0;
-
+	
 free_irq:
 	free_irq(cdata->irq, cdata);
 	for (n--; n >= 0; n--)
@@ -326,7 +329,7 @@ free_irq:
 deallocate_trigger:
 	for (i--; i >= 0; i--)
 		iio_trigger_free(cdata->trig[i]);
-
+	
 	return err;
 }
 EXPORT_SYMBOL(st_lsm6ds3_allocate_triggers);