Mindtribe latest accelerometer managementmindtribe_dev

Change-Id: Ia7d76c3c5f7807a37a0e26d5892c0826de8faa84

6 files changed, 153 insertions, 16 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
index ba971628496..4c6fc426fc4 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -137,12 +137,14 @@ struct lsm6ds3_data {
 	int 	wake_lock_initialized;
 	struct wake_lock wlock;
 	struct wake_lock tap_wlock;
+#define RESUME_NORMAL 1
+#define RESUME_FROM_INACTIVE 3
 	u8 first_irq_from_resume;
 	u8 reg_read;
 #define SIXD_MASK_VALID_BITS (0x3f)
 	u8 sixd_mask;
 	u8 int1_save;
-
+	int inactive_wait;
 	u8 *fifo_data;
 	u8 sensors_enabled;
 	u8 gyro_selftest_status;
@@ -212,7 +214,7 @@ struct lsm6ds3_sensor_data {
 	u8 sindex;
 	u8 *buffer_data;
 };
-
+void st_lsm6ds3_set_inactive_detection(struct lsm6ds3_data *cdata, int enable);
 int st_lsm6ds3_write_data_with_mask(struct lsm6ds3_data *cdata,
 				u8 reg_addr, u8 mask, u8 data, bool b_lock);
 
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
index 87cea6f349a..ad5cd7b14cd 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
@@ -194,7 +194,7 @@ void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len, bool
 #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);
+				dev_dbg(cdata->dev, "first accel timestamp:%lld last:%lld ", cdata->accel_timestamp, cdata->timestamp);
 			}
 			
 		}
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index 499fda71e59..a33a6c91d43 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -874,6 +874,10 @@ int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
 {
 	u8 reg_addr, mask, value;
 
+	if((state && sdata->cdata->inactive_wait) && (sdata->sindex != ST_INDIO_DEV_ACCEL)){//allow disables but not enables, except for accelerometer
+		dev_info(sdata->cdata->dev, "drdy setup ignored, inactive_wait:%i", sdata->sindex);
+		return 0;
+	}
 	if (state)
 		value = ST_LSM6DS3_EN_BIT;
 	else
@@ -911,6 +915,9 @@ int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
 		mask = ST_LSM6DS3_FIFO_THR_IRQ_MASK;
 		break;
 	case ST_INDIO_DEV_SIGN_MOTION:
+		if(state && sdata->cdata->inactive_wait){
+			return 0;
+		}
 		if (sdata->cdata->sensors_enabled &
 					(1 << ST_INDIO_DEV_STEP_DETECTOR)){
 			dev_err(sdata->cdata->dev, "can't set up significant motion irq, step detector enabled");
@@ -1568,6 +1575,10 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 #define ST_LSM6DS3_CTRL8_LPF_ON_ACCEL 0x80
 #define ST_LSM6DS3_CTRL4_ADDR 0x13
 #define ST_LSM6DS3_CTRL4_STOP_ON_FTH_MASK 0x1
+//inactivity stuff
+#define ST_LSM6DS3_WAKE_UP_THR_ADDR 0x5B
+#define ST_LSM6DS3_WAKE_UP_DUR_ADDR 0x5C
+
 	regval = 0x50;//tap threshold
 	err = sdata->cdata->tf->write(sdata->cdata,
 					ST_LSM6DS3_TAP_THS_6D_ADDR,
@@ -1578,8 +1589,8 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 					ST_LSM6DS3_INT_DUR2_ADDR,
 					1, &regval, false);
 
-	regval = 0x13; //enable slop_fd to get lpf2 for 6d
-       			// also enable z tap and latch irqs	
+	regval = 0x12; //enable slop_fd to get lpf2 for 6d
+       			// also enable z tap 
 	err = sdata->cdata->tf->write(sdata->cdata,
 					ST_LSM6DS3_TAP_CFG_ADDR,
 					1, &regval, false);
@@ -1588,6 +1599,17 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 	err = sdata->cdata->tf->write(sdata->cdata,
 					ST_LSM6DS3_CTRL8_ADDR ,
 					1, &regval, false);
+
+
+	regval = 0x42;
+#if 0
+	err = sdata->cdata->tf->write(sdata->cdata,
+					ST_LSM6DS3_WAKE_UP_THR_ADDR,
+					1, &regval, false);
+#endif 
+	err = sdata->cdata->tf->write(sdata->cdata,
+					ST_LSM6DS3_WAKE_UP_DUR_ADDR,
+					1, &regval, false);
 	
 	if (err < 0)
 		goto st_lsm6ds3_init_sensor_mutex_unlock;
@@ -1607,6 +1629,20 @@ st_lsm6ds3_init_sensor_mutex_unlock:
 	return err;
 }
 
+void st_lsm6ds3_set_inactive_detection(struct lsm6ds3_data *cdata, int enable)
+{
+	u8 regval = 0;
+	
+	if(enable)
+		regval = 0x42;
+	else
+		regval = 0x00;
+
+	cdata->tf->write(cdata,	ST_LSM6DS3_WAKE_UP_THR_ADDR,
+					1, &regval, false);
+
+}
+
 static int st_lsm6ds3_set_selftest(struct lsm6ds3_sensor_data *sdata, int index)
 {
 	int err;
@@ -2147,6 +2183,7 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 
 	cdata->fifo_data = 0;
 	cdata->samples_to_keep_on_wake = ST_LSM6DS3_SAMPLES_ON_WAKE_DEFAULT;
+	cdata->inactive_wait = 0;
 
 	err = cdata->tf->read(cdata, ST_LSM6DS3_WAI_ADDRESS, 1, &wai, true);
 	if (err < 0) {
@@ -2347,9 +2384,6 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
 	reg_value = 0x44;
 	err = cdata->tf->write(cdata,
 			ST_LSM6DS3_MD1_ADDR, 1, &reg_value, true);
-	err = cdata->tf->read(cdata,
-			ST_LSM6DS3_MD1_ADDR, 1, &reg_value, true);
-	dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err);
 
 	err = cdata->tf->read(cdata,
 			0xd, 1, &reg_value, true);
@@ -2386,6 +2420,19 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
 			0x13, 1, &reg_value, true);
 	dev_info(cdata->dev, "before suspend 0x13 ctrl4: %x err:%i", reg_value, err);
 #endif 
+	if(cdata->inactive_wait){
+		reg_value = 0x80;	
+		err = cdata->tf->write(cdata,
+				ST_LSM6DS3_MD1_ADDR, 1, &reg_value, true);
+	}
+
+	err = cdata->tf->read(cdata,
+			ST_LSM6DS3_MD1_ADDR, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err);
+
+	err = cdata->tf->read(cdata,
+			ST_LSM6DS3_INT1_ADDR, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend int1_ctrl: %x, err:%i", reg_value, err);
 
 	return 0;
 }
@@ -2398,13 +2445,18 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 	struct lsm6ds3_sensor_data *sdata;
 	u8 reg_value;
 
+	if(cdata->inactive_wait){
+	dev_info(cdata->dev, "First wake from hyperactivity wait");
+		cdata->inactive_wait = 0;
+		st_lsm6ds3_set_inactive_detection(cdata, false);
+	}
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
-		if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT))
+		if ((i == ST_INDIO_DEV_SIGN_MOTION && !cdata->int1_save) || (i == ST_INDIO_DEV_TILT))
 			continue;
 
 		sdata = iio_priv(cdata->indio_dev[i]);
 
-		if(i == ST_INDIO_DEV_ACCEL && cdata->int1_save == 1 ){
+		if(((i == ST_INDIO_DEV_ACCEL)||(i == ST_INDIO_DEV_SIGN_MOTION)) && cdata->int1_save == 1 ){
 			//todo fix how this is set, we're always clearing and setting
 			err = st_lsm6ds3_set_drdy_irq(sdata, true);
 			cdata->int1_save = 0;
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
index 2e7af62ed47..5e20cc722e8 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -19,6 +19,8 @@
 #include <linux/interrupt.h>
 #include <linux/iio/events.h>
 #include <linux/wakelock.h>
+#include <linux/wakeup_reason.h>
+#include <linux/kfifo.h>
 
 #include "st_lsm6ds3.h"
 
@@ -38,7 +40,9 @@
 
 #define ST_LSM6DS3_6D_SRC_ADDR			0x1d
 
-
+#define WAKE_LOG_EVENTS 8 //must be power of 2 for kfifo
+static DECLARE_KFIFO(wake_time_fifo, s64, WAKE_LOG_EVENTS);
+#define HYPERACTIVITY_WINDOW_NS 5000000000L
 
 static struct workqueue_struct *st_lsm6ds3_wq;
 
@@ -47,6 +51,32 @@ void st_lsm6ds3_flush_works()
 	flush_workqueue(st_lsm6ds3_wq);
 }
 
+int hyperactivity_check(struct lsm6ds3_data *cdata)
+{
+	int i, fifo_len;
+	int recent_count = 0;
+	struct timespec ts;
+	s64 ctime;
+	s64 wtime[WAKE_LOG_EVENTS] = {0};
+	get_monotonic_boottime(&ts);
+	ctime = timespec_to_ns(&ts);
+	fifo_len = kfifo_len(&wake_time_fifo);
+	dev_dbg(cdata->dev, "HYP_CHECK FIFO_LEN: %i/%i, time:%lld", fifo_len, kfifo_size(&wake_time_fifo), ctime);
+	if(fifo_len < WAKE_LOG_EVENTS)
+		return 0;
+
+	kfifo_out(&wake_time_fifo, wtime, WAKE_LOG_EVENTS);
+	for(i=0; i < WAKE_LOG_EVENTS; i++){
+		dev_dbg(cdata->dev, "wtime[%i]:%lld", i, wtime[i]);
+		if(ctime - wtime[i] < HYPERACTIVITY_WINDOW_NS){
+			recent_count++;
+			kfifo_in(&wake_time_fifo, &wtime[i], 1);
+		}	
+	}
+	dev_dbg(cdata->dev, "recent count:%i", recent_count);
+	return (recent_count == WAKE_LOG_EVENTS);
+}
+
 irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
 {
 	struct timespec ts;
@@ -69,14 +99,24 @@ 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;
+	int woken_from_sleep = 0;
+	int hyperactive = 0;
 
 
 	cdata = container_of((struct work_struct *)data_work,
 						struct lsm6ds3_data, data_work);
+  
+	
+	if(cdata->first_irq_from_resume && last_wakeup_reason_test(cdata->irq)){
+		woken_from_sleep = 1;
+	}
 
 	if(!wake_lock_active(&cdata->wlock))
 		wake_lock(&cdata->wlock);
-	 mutex_lock(&cdata->fifo_lock); 
+
+
+	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);
@@ -107,7 +147,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 			dev_info(cdata->dev, "Ignoring tap");
 		}
 	}
-	if(cdata->first_irq_from_resume){
+	if(woken_from_sleep){
 		dev_info(cdata->dev, "First IRQ from a RESUME");
 		if(!d6d_event && !tap_event){
 			dev_info(cdata->dev, "No event from first resume, assuming lost TAP");
@@ -124,11 +164,13 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 			st_lsm6ds3_read_fifo(cdata, true, true);
 		}
 		else{
-			st_lsm6ds3_read_fifo(cdata, true, false);
+			st_lsm6ds3_read_fifo(cdata, true, true);
 		}
 	}
 //significant motion event processing
-	if(d6d_event || tap_event){
+	if(!cdata->inactive_wait && (d6d_event || tap_event)){
+		kfifo_in(&wake_time_fifo, &cdata->timestamp, 1);
+		hyperactive = hyperactivity_check(cdata);	
 		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[
@@ -170,6 +212,17 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 				cdata->timestamp);
 	}
 
+	if(hyperactive){
+		uint8_t reg_value;
+		int err;
+
+		dev_info(cdata->dev, "Hyperactivity triggered, masking irqs except inactivity and fifo");
+		cdata->inactive_wait = 1;
+		reg_value = 0x80;//fifo is in another register
+		err = cdata->tf->write(cdata,
+			0x5e, 1, &reg_value, true);	
+		st_lsm6ds3_set_inactive_detection(cdata, 1);
+	}
 	enable_irq(cdata->irq);
 	mutex_unlock(&cdata->fifo_lock); 
 	if(wake_lock_active(&cdata->wlock))
@@ -191,6 +244,8 @@ int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata,
 		return -EINVAL;
 
 	INIT_WORK(&cdata->data_work, st_lsm6ds3_irq_management);
+	INIT_KFIFO(wake_time_fifo);
+
 
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
 		cdata->trig[i] = iio_trigger_alloc("%s-trigger",
@@ -207,7 +262,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_RISING, cdata->name, cdata);
 	if (err)
 		goto deallocate_trigger;
 
diff --git a/include/linux/wakeup_reason.h b/include/linux/wakeup_reason.h
index 7ce50f0debc..b58723ab7c7 100644
--- a/include/linux/wakeup_reason.h
+++ b/include/linux/wakeup_reason.h
@@ -19,5 +19,7 @@
 #define _LINUX_WAKEUP_REASON_H
 
 void log_wakeup_reason(int irq);
+int get_last_wakeup_reason();
+int last_wakeup_reason_test(int reason);
 
 #endif /* _LINUX_WAKEUP_REASON_H */
diff --git a/kernel/power/wakeup_reason.c b/kernel/power/wakeup_reason.c
index 9823d9ccde4..635ed61926c 100644
--- a/kernel/power/wakeup_reason.c
+++ b/kernel/power/wakeup_reason.c
@@ -63,6 +63,32 @@ static struct attribute_group attr_group = {
 	.attrs = attrs,
 };
 
+
+
+int get_last_wakeup_reason()
+{
+	if(irq_count)
+		return irq_list[irq_count-1];
+	else
+		return -EINVAL;
+}
+
+int last_wakeup_reason_test(int reason)
+{
+	int irq_no;
+	unsigned long flags;
+	int ret = 0;
+	spin_lock_irqsave(&resume_reason_lock, flags);
+	for (irq_no = 0; irq_no < irq_count; irq_no++) {
+		if(reason == irq_list[irq_no]){
+			ret = 1;
+			break;
+		}
+	}
+	spin_unlock_irqrestore(&resume_reason_lock, flags);
+	return ret;
+}
+
 /*
  * logs all the wake up reasons to the kernel
  * stores the irqs to expose them to the userspace via sysfs