adding in suspend_again

1 files changed, 84 insertions, 10 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
index a97a5da517c..6fe42ec20fb 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -19,6 +19,7 @@
 #include <linux/iio/events.h>
 #include <linux/wakelock.h>
 #include <linux/wakeup_reason.h>
+#include <linux/suspend.h>
 
 #include "st_lsm6ds3.h"
 
@@ -56,13 +57,82 @@ irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
 	cdata->timestamp = timespec_to_ns(&ts);
 	cdata->accel_timestamp  = cdata->timestamp;
 	cdata->irq_timestamp = ts.tv_sec;
-	queue_work(st_lsm6ds3_wq, &cdata->data_work);
 
-	disable_irq_nosync(irq);
+	if(cdata->first_irq_from_resume && cdata->last_wakeup_source == LSM6DS3_WAKEUP_OTHER) {
+		dev_info(cdata->dev, "No valid wakeup. Ignoring IRQ.");
+		cdata->first_irq_from_resume = 0;
+	} else {
+		dev_info(cdata->dev, "IRQ handled. Scheduling work.");
+		queue_work(st_lsm6ds3_wq, &cdata->data_work);
+		disable_irq_nosync(irq);
+	}
 
 	return IRQ_HANDLED;
 }
 
+void st_lsm6ds3_set_wake_triggers(struct lsm6ds3_data* cdata)
+{
+	u8 d6d_src_reg = 0x00, tap_src_reg = 0x00, func_src_reg = 0x00, wake_up_src_reg = 0x00;
+	u8 d6d_event = 0;
+	u8 tap_event = 0;
+	int wake_irq;
+	int ignore_event = 0;
+
+	wake_irq = last_wakeup_reason_test(cdata->irq);
+	// if not the proper wakeup irq, don't set wake trigger
+	if(!wake_irq) {
+		return;
+	}
+
+	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, &func_src_reg, true);
+	cdata->tf->read(cdata, 0x1b, 1, &wake_up_src_reg, true);
+	dev_info(cdata->dev, "ST irq start: src_value: 0x%x, 6d: 0x%x, tap: 0x%x, wakeup: 0x%x",
+			func_src_reg, d6d_src_reg, tap_src_reg, wake_up_src_reg);
+
+	if(d6d_src_reg & ST_LSM6DS3_6D_SRC_DETECTED_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 {
+			ignore_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
+			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)){
+			tap_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
+			dev_info(cdata->dev, "Valid Tap");
+		} else {
+			dev_info(cdata->dev, "Ignoring tap");
+		}
+	}
+
+	if(cdata->first_irq_from_resume && wake_irq){
+		if(!d6d_event && !tap_event && !ignore_event){
+			dev_info(cdata->dev, "No event from first resume, assuming lost TAP");
+			tap_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
+			dev_info(cdata->dev, "Valid Tap from sleep");
+		}
+	}
+}
+
+void st_lsm6ds3_clear_wake_triggers(struct lsm6ds3_data* cdata)
+{
+	cdata->first_irq_from_resume = 1;
+	cdata->last_wakeup_source = 0;
+}
 #ifdef WAKE_STATS_DEBUG_INFO
 static int wakeup_irq_count = 0;
 static int wakeup_irq_stayawake_count = 0;
@@ -143,7 +213,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 {
 	struct lsm6ds3_data *cdata;
 	u8 d6d_src_reg, tap_src_reg;
-	u8 src_value = 0x00, src_fifo = 0x00;
+	u8 src_value = 0x00, src_fifo = 0x00, wake_up_src_reg;
 	u8 d6d_event = 0;
 	u8 tap_event = 0;
 	int wake_irq;
@@ -157,11 +227,13 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	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);
+	cdata->tf->read(cdata, 0x1b, 1, &wake_up_src_reg, true);
 	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_info(cdata->dev, "IRQ scheduled work running.");
+	dev_info(cdata->dev, "ST irq start: src_value: 0x%x, 6d: 0x%x, tap: 0x%x, wakeup: 0x%x",
+		src_value, d6d_src_reg, tap_src_reg, wake_up_src_reg);
 
 	if(d6d_src_reg & ST_LSM6DS3_6D_SRC_DETECTED_MASK){
 #ifdef WAKE_STATS_DEBUG_INFO
@@ -185,7 +257,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 		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;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
 			dev_info(cdata->dev, "Valid Tap");
 		} else {
 			dev_info(cdata->dev, "Ignoring tap");
@@ -193,15 +265,17 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	}
 	
 	if(cdata->first_irq_from_resume && wake_irq){
-#ifdef WAKE_STATS_DEBUG_INFO
-		wakeup_irq_count++;
-#endif 
 		if(!d6d_event && !tap_event && !ignore_event){
 			dev_info(cdata->dev, "No event from first resume, assuming lost TAP");
 			tap_event = 1;
-			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_OTHER;
 			dev_info(cdata->dev, "Valid Tap from sleep");
 		}	
+		pr_info("posting susp again decision: %i", ignore_event);
+		//susp_again_post_decision(ignore_event);
+#ifdef WAKE_STATS_DEBUG_INFO
+		wakeup_irq_count++;
+#endif 
 	}
 
 	if(!ignore_event && (tap_event || d6d_event) && cdata->first_irq_from_resume){