Merge branch 'android-3.10-bringup' of ssh://internal.oliodevices.com:29418/kernel/omap into android-3.10-bringup

Change-Id: I5b591f445c72ab204001c52fe00d35c93b52e062

4 files changed, 398 insertions, 94 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
index 78fbf76f4c2..479b9188273 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -13,7 +13,7 @@
 
 #include <linux/types.h>
 #include <linux/iio/trigger.h>
-
+#include <linux/wakelock.h>
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
 #include <linux/i2c.h>
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
@@ -120,11 +120,27 @@ struct st_lsm6ds3_transfer_buffer {
 	u8 tx_buf[ST_LSM6DS3_TX_MAX_LENGTH] ____cacheline_aligned;
 };
 
+enum 
+{
+	LSM6DS3_WAKEUP_NONE = 0,
+	LSM6DS3_WAKEUP_TAP = 1,
+	LSM6DS3_WAKEUP_6D  = 2,
+	LSM6DS3_WAKEUP_OTHER = 4
+};
+
 struct lsm6ds3_data {
 	const char *name;
 
 	bool reset_steps;
 	bool sign_motion_event_ready;
+	int  last_wakeup_source;
+	int 	wake_lock_initialized;
+	struct wake_lock wlock;
+	u8 first_irq_from_resume;
+	u8 reg_read;
+#define SIXD_MASK_VALID_BITS (0x3f)
+	u8 sixd_mask;
+	u8 int1_save;
 
 	u8 *fifo_data;
 	u8 sensors_enabled;
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
index 8a7cc5176c2..138868dd5ba 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c
@@ -7,6 +7,7 @@
  *
  * Licensed under the GPL-2.
  */
+#define DEBUG
 
 #include <linux/module.h>
 #include <linux/kernel.h>
@@ -152,20 +153,23 @@ void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len)
 	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)
+	if (!cdata->fifo_data){
+		dev_err(cdata->dev, "No cdata->fifo_data! can't use fifo");
 		return;
-
+	}
 	if (check_fifo_len) {
 		err = cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DIFF_L,
 						2, (u8 *)&read_len, true);
-		if (err < 0)
+		if (err < 0){
+			dev_dbg(cdata->dev, "could not read the fifo length. err: %i", err);
 			return;
+		}
 
 		if (read_len & ST_LSM6DS3_FIFO_DATA_OVR_2REGS) {
-			dev_err(cdata->dev,
-				"data fifo overrun, failed to read it.\n");
-			return;
+			dev_dbg(cdata->dev,
+				"DATA overrun, setting read_len to threshold\n");
+			read_len = cdata->fifo_threshold;
+			//return;
 		}
 
 		read_len &= ST_LSM6DS3_FIFO_DIFF_MASK;
@@ -184,7 +188,7 @@ void st_lsm6ds3_read_fifo(struct lsm6ds3_data *cdata, bool check_fifo_len)
 #endif /* CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT */
 
 		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)
 			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 b6eaa08ae83..bbb024e3a4d 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -25,7 +25,7 @@
 #include <linux/iio/buffer.h>
 #include <linux/iio/events.h>
 #include <asm/unaligned.h>
-
+#include <linux/wakelock.h>
 #include <linux/iio/common/st_sensors.h>
 #include "st_lsm6ds3.h"
 
@@ -99,7 +99,7 @@
 #define ST_LSM6DS3_FIFO_THRESHOLD_IRQ_MASK	0x08
 #define ST_LSM6DS3_FIFO_ODR_ADDR		0x0a
 #define ST_LSM6DS3_FIFO_ODR_MASK		0x78
-#define ST_LSM6DS3_FIFO_ODR_MAX			0x08
+#define ST_LSM6DS3_FIFO_ODR_MAX			0x04 // prev max:0x08
 #define ST_LSM6DS3_FIFO_ODR_OFF			0x00
 #define ST_LSM6DS3_FIFO_DECIMATOR_ADDR		0x08
 #define ST_LSM6DS3_FIFO_ACCEL_DECIMATOR_MASK	0x07
@@ -156,13 +156,8 @@
 #define ST_LSM6DS3_GYRO_STD			6
 
 /* CUSTOM VALUES FOR SIGNIFICANT MOTION SENSOR */
-/******
- * WARNING
- * Evan: Switching Sig motion to use Tap interrupt
- */
-#define ST_LSM6DS3_SIGN_MOTION_EN_ADDR		0x58
-#define ST_LSM6DS3_SIGN_MOTION_EN_MASK		0x02
-#define ST_LSM6DS3_SIGN_MOTION_DRDY_IRQ_MASK 0x40
+#define ST_LSM6DS3_SIGN_MOTION_EN_ADDR		0x19
+#define ST_LSM6DS3_SIGN_MOTION_EN_MASK		0x01
 
 /* CUSTOM VALUES FOR STEP DETECTOR SENSOR */
 #define ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK	0x80
@@ -181,6 +176,12 @@
 #define ST_LSM6DS3_TILT_EN_MASK			0x20
 #define ST_LSM6DS3_TILT_DRDY_IRQ_MASK		0x02
 
+/* 6d Constants */
+
+
+#define ST_LSM6DS3_6D_MD1_INT_MASK		0x04
+#define ST_LSM6DS3_SINGTAP_MD1_INT_MASK		0x40
+
 #define ST_LSM6DS3_ACCEL_SUFFIX_NAME		"accel"
 #define ST_LSM6DS3_GYRO_SUFFIX_NAME		"gyro"
 #define ST_LSM6DS3_STEP_COUNTER_SUFFIX_NAME	"step_c"
@@ -239,7 +240,8 @@ 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,
-	.odr_avl[0] = { .hz = 26, .value = ST_LSM6DS3_ODR_26HZ_VAL },
+	//hack , remove
+	.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 },
 	.odr_avl[3] = { .hz = 208, .value = ST_LSM6DS3_ODR_208HZ_VAL },
@@ -663,6 +665,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);
 		accel_decimator = max_odr / sdata_accel->c_odr;
 	} else
@@ -780,6 +783,7 @@ int st_lsm6ds3_set_fifo_decimators_and_threshold(struct lsm6ds3_data *cdata)
 
 	if (fifo_len > 0) {
 		fifo_threshold = fifo_len / 2;
+		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,
 					1, (u8 *)&fifo_threshold, true);
@@ -850,7 +854,6 @@ EXPORT_SYMBOL(st_lsm6ds3_reconfigure_fifo);
 
 int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
 {
-	int err;
 	u8 reg_addr, mask, value;
 
 	if (state)
@@ -890,18 +893,36 @@ 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 (sdata->cdata->sensors_enabled &
-		//			(1 << ST_INDIO_DEV_STEP_DETECTOR))
-		//	return 0;
-		// The double tap needs to be enabled in-order for latched mode to work
-		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
-					ST_LSM6DS3_MD1_ADDR,
-					0x08,
-					ST_LSM6DS3_EN_BIT, true);
-		if (err < 0)
-			return err;
+		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");
+			return 0;
+		}
+
+		dev_info(sdata->cdata->dev,"--sig mot irq setup: %i", value);
+		if(value == 0 ){
+			//need this so that sig motion can reregister before we sleep,
+			//otherwise we'll sleep and never wake up
+			wake_lock_timeout(&sdata->cdata->wlock, msecs_to_jiffies(1000));
+			dev_err(sdata->cdata->dev, "IRQ disabled for md1 sig motion");
+			//dump_stack();	
+		
+		}
+#if 0
+		reg_addr = ST_LSM6DS3_INT1_ADDR;
+		mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK;
+		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
+		reg_addr = ST_LSM6DS3_MD1_ADDR;
+		mask = ST_LSM6DS3_6D_MD1_INT_MASK;
+		st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						reg_addr, mask, value, true);
+			
 		reg_addr = ST_LSM6DS3_MD1_ADDR;
-		mask = ST_LSM6DS3_SIGN_MOTION_DRDY_IRQ_MASK;
+		mask = ST_LSM6DS3_SINGTAP_MD1_INT_MASK;
+		//written at end of func
 		break;
 	case ST_INDIO_DEV_STEP_COUNTER:
 		reg_addr = ST_LSM6DS3_INT2_ADDR;
@@ -911,7 +932,7 @@ int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
 		if (sdata->cdata->sensors_enabled &
 					(1 << ST_INDIO_DEV_SIGN_MOTION))
 			return 0;
-
+		dev_info(sdata->cdata->dev, "setting up STEP count irq");
 		reg_addr = ST_LSM6DS3_INT1_ADDR;
 		mask = ST_LSM6DS3_STEP_DETECTOR_DRDY_IRQ_MASK;
 		break;
@@ -1087,31 +1108,26 @@ static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata)
 		if (err < 0)
 			return err;
 
-//		if ((sdata->cdata->sensors_enabled & ~(1 << sdata->sindex)) &
-//					ST_LSM6DS3_PEDOMETER_DEPENDENCY) {
-//			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
-//						ST_LSM6DS3_PEDOMETER_EN_ADDR,
-//						ST_LSM6DS3_PEDOMETER_EN_MASK,
-//						ST_LSM6DS3_DIS_BIT, true);
-//			if (err < 0)
-//				return err;
-//
-//			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
-//						ST_LSM6DS3_PEDOMETER_EN_ADDR,
-//						ST_LSM6DS3_PEDOMETER_EN_MASK,
-//						ST_LSM6DS3_EN_BIT, true);
-//			if (err < 0)
-//				return err;
-//		} else {
-//			err = st_lsm6ds3_enable_pedometer(sdata, true);
-//			if (err < 0)
-//				return err;
-//		}
+		if ((sdata->cdata->sensors_enabled & ~(1 << sdata->sindex)) &
+					ST_LSM6DS3_PEDOMETER_DEPENDENCY) {
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_PEDOMETER_EN_ADDR,
+						ST_LSM6DS3_PEDOMETER_EN_MASK,
+						ST_LSM6DS3_DIS_BIT, true);
+			if (err < 0)
+				return err;
 
-		// This sets the TAP threshold
-		err = st_lsm6ds3_write_data_with_mask(sdata->cdata, 0x59, 0x1F, 0x04, true);
-		if (err < 0)
-			return err;
+			err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
+						ST_LSM6DS3_PEDOMETER_EN_ADDR,
+						ST_LSM6DS3_PEDOMETER_EN_MASK,
+						ST_LSM6DS3_EN_BIT, true);
+			if (err < 0)
+				return err;
+		} else {
+			err = st_lsm6ds3_enable_pedometer(sdata, true);
+			if (err < 0)
+				return err;
+		}
 
 		break;
 	case ST_INDIO_DEV_STEP_COUNTER:
@@ -1422,6 +1438,7 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 {
 	int err, i;
 	u8 default_reg_value = 0;
+	u8 regval = 0;
 	struct lsm6ds3_sensor_data *sdata;
 
 	mutex_init(&cdata->tb.buf_lock);
@@ -1522,11 +1539,55 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 					ST_LSM6DS3_DIS_BIT, false);
 	if (err < 0)
 		goto st_lsm6ds3_init_sensor_mutex_unlock;
+//set up tap stuff
+#define ST_LSM6DS3_TAP_CFG_ADDR 0x58
+#define ST_LSM6DS3_TAP_CFG_XYZ_MASK (0xE)
+	//1 is for latch enable
+#define ST_LSM6DS3_TAP_THS_6D_ADDR 0x59
+#define ST_LSM6DS3_INT_DUR2_ADDR 0x5A
+#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
+
+	regval = 0x50;//tap threshold
+	err = sdata->cdata->tf->write(sdata->cdata,
+					ST_LSM6DS3_TAP_THS_6D_ADDR,
+					1, &regval, false);
+
+	regval = 0x6; //quiet and shock times
+	err = sdata->cdata->tf->write(sdata->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	
+	err = sdata->cdata->tf->write(sdata->cdata,
+					ST_LSM6DS3_TAP_CFG_ADDR,
+					1, &regval, false);
+
+	regval = ST_LSM6DS3_CTRL8_LPF_ON_6D | ST_LSM6DS3_CTRL8_LPF_ON_ACCEL; 
+	err = sdata->cdata->tf->write(sdata->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;
 
+
+
+	//
 	mutex_unlock(&cdata->bank_registers_lock);
 
 	sdata->c_odr = 0;
 
+
 	return 0;
 
 st_lsm6ds3_init_sensor_mutex_unlock:
@@ -1790,6 +1851,89 @@ ssize_t st_lsm6ds3_sysfs_flush_fifo(struct device *dev,
 	return size;
 }
 
+ssize_t st_lsm6ds3_sysfs_get_irq_src(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, "%d\n", sdata->cdata->last_wakeup_source);
+	sdata->cdata->last_wakeup_source = 0; //clear flags
+	return ret;
+}
+
+
+static ssize_t st_lsm6ds3_sysfs_write_reg(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 waddr, wval;
+	ret = sscanf(buf, "%i %i", &waddr, &wval);
+	dev_info(sdata->cdata->dev, "read: 0x%x 0x%x", waddr, wval);
+	if(ret != 2){
+		dev_err(sdata->cdata->dev,"error, write_reg usage: addr val");
+		return -EINVAL;
+	}
+
+	sdata->cdata->tf->write(sdata->cdata, waddr, 1, (u8 *)&wval, true);
+		
+	return size;
+}
+
+static ssize_t st_lsm6ds3_sysfs_read_reg_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 waddr, wval;
+	ret = sscanf(buf, "%i", &waddr);
+	dev_info(dev, "read: 0x%x ", waddr);
+	if(ret != 1){
+		dev_err(sdata->cdata->dev,"error, read_reg usage: addr ");
+		return -EINVAL;
+	}
+
+	sdata->cdata->tf->read(sdata->cdata, waddr, 1, &sdata->cdata->reg_read, true);
+		
+	return size;
+}
+
+
+ssize_t st_lsm6ds3_sysfs_read_reg_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, "%d\n", sdata->cdata->reg_read);
+	return ret;
+}
+
+ssize_t st_lsm6ds3_sysfs_sixd_wake_mask_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->sixd_mask);
+	return ret;
+}
+
+
+static ssize_t st_lsm6ds3_sysfs_sixd_wake_mask_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 maskval;
+	ret = sscanf(buf, "%i", &maskval);
+	dev_info(sdata->cdata->dev, "read: 0x%x ", maskval);
+	if(ret != 1 || maskval ){
+		dev_info(sdata->cdata->dev, "read: 0x%x ", maskval);
+		dev_err(sdata->cdata->dev,"error, sixd_mask usage: mask sig mot events from"); 
+		dev_err(sdata->cdata->dev,"ZH | ZL | YH | YL | XH | XL \n");
+		return -EINVAL;
+	}
+
+	sdata->cdata->sixd_mask = maskval & SIXD_MASK_VALID_BITS;	
+		
+	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);
@@ -1812,6 +1956,23 @@ static IIO_DEVICE_ATTR(self_test, S_IWUSR | S_IRUGO,
 				st_lsm6ds3_sysfs_get_selftest_status,
 				st_lsm6ds3_sysfs_set_selftest_status, 0);
 
+static IIO_DEVICE_ATTR(irq_source, S_IRUGO,
+				 st_lsm6ds3_sysfs_get_irq_src,
+						NULL, 0);
+
+
+static IIO_DEVICE_ATTR(write_reg, S_IWUSR,
+				NULL, st_lsm6ds3_sysfs_write_reg, 0);
+
+
+static IIO_DEVICE_ATTR(read_reg, S_IRUGO | S_IWUSR,
+		st_lsm6ds3_sysfs_read_reg_get,			
+	       	st_lsm6ds3_sysfs_read_reg_set, 0);
+
+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 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,
@@ -1820,6 +1981,10 @@ static struct attribute *st_lsm6ds3_accel_attributes[] = {
 	&iio_dev_attr_self_test.dev_attr.attr,
 	&iio_dev_attr_hw_fifo_lenght.dev_attr.attr,
 	&iio_dev_attr_flush.dev_attr.attr,
+	&iio_dev_attr_irq_source.dev_attr.attr,
+	&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,
 	NULL,
 };
 
@@ -1929,6 +2094,7 @@ 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");
 
 #ifdef CONFIG_ST_LSM6DS3_IIO_MASTER_SUPPORT
 	mutex_init(&cdata->passthrough_lock);
@@ -1969,6 +2135,8 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 		dev_info(cdata->dev, "driver use DRDY int pin 1\n");
 	}
 
+	cdata->sixd_mask = SIXD_MASK_VALID_BITS;
+
 	cdata->indio_dev[ST_INDIO_DEV_ACCEL]->name =
 		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,
 					ST_LSM6DS3_ACCEL_SUFFIX_NAME);
@@ -2094,27 +2262,78 @@ int st_lsm6ds3_common_suspend(struct lsm6ds3_data *cdata)
 	int err, i;
 	u8 tmp_sensors_enabled;
 	struct lsm6ds3_sensor_data *sdata;
+	u8 reg_value;
 
 	tmp_sensors_enabled = cdata->sensors_enabled;
+	dev_info(cdata->dev, "entering suspend");
 
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
+		sdata = iio_priv(cdata->indio_dev[i]);
 		if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT))
 			continue;
+		if(i == ST_INDIO_DEV_ACCEL){
+			//stop the fifo
+			err = st_lsm6ds3_set_drdy_irq(sdata, false);
+			cdata->int1_save = 1;
+			continue; //do not disable 
+		}
 
-		sdata = iio_priv(cdata->indio_dev[i]);
 
 		err = st_lsm6ds3_set_enable(sdata, false);
+		//todo: see if this is good
+		err = st_lsm6ds3_set_drdy_irq(sdata, false);
 		if (err < 0)
 			return err;
 	}
 	cdata->sensors_enabled = tmp_sensors_enabled;
+
 #endif /* CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP */
 
 	if (cdata->sensors_enabled & ST_LSM6DS3_WAKE_UP_SENSORS) {
 		if (device_may_wakeup(cdata->dev))
 			enable_irq_wake(cdata->irq);
 	}
+	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);
+	dev_info(cdata->dev, "manually setting to 0x44");
+	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);
+	dev_info(cdata->dev, "before suspend int1: %x err:%i", reg_value, err);
+
+	err = cdata->tf->read(cdata,
+			0x58, 1, &reg_value, true);
+	dev_info(cdata->dev, "before suspend 0x58 tap enable: %x err:%i", reg_value, err);
+	err = cdata->tf->read(cdata,
+			0x10, 1, &reg_value, true);
+	dev_info(cdata->dev, "ctrl1_xl: %x err:%i", reg_value, err);
+
+        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);
+
+
+
+	reg_value = 624; //set fifo to 1 second of data (104hz fifo)
+	err = cdata->tf->write(cdata,
+			ST_LSM6DS3_FIFO_THR_L_ADDR, 1, &reg_value, true);
+
+	cdata->first_irq_from_resume = 1;
 	return 0;
 }
 EXPORT_SYMBOL(st_lsm6ds3_common_suspend);
@@ -2124,6 +2343,7 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 #ifndef CONFIG_ST_LSM6DS3_IIO_SENSORS_WAKEUP
 	int err, i;
 	struct lsm6ds3_sensor_data *sdata;
+	u8 reg_value;
 
 	for (i = 0; i < ST_INDIO_DEV_NUM; i++) {
 		if ((i == ST_INDIO_DEV_SIGN_MOTION) || (i == ST_INDIO_DEV_TILT))
@@ -2131,6 +2351,12 @@ int st_lsm6ds3_common_resume(struct lsm6ds3_data *cdata)
 
 		sdata = iio_priv(cdata->indio_dev[i]);
 
+		if(i == ST_INDIO_DEV_ACCEL && 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;
+		}
+
 		if ((1 << sdata->sindex) & cdata->sensors_enabled) {
 			err = st_lsm6ds3_set_enable(sdata, true);
 			if (err < 0)
@@ -2143,6 +2369,9 @@ 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)
+	err = cdata->tf->write(cdata,
+			ST_LSM6DS3_FIFO_THR_L_ADDR, 1, &reg_value, true);
 
 	return 0;
 }
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
index f5108a7526e..ccb215c20d0 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c
@@ -8,27 +8,38 @@
  * Licensed under the GPL-2.
  */
 
+#define DEBUG //TODO: remove
+
 #include <linux/kernel.h>
+#include <linux/device.h>
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/trigger.h>
 #include <linux/interrupt.h>
 #include <linux/iio/events.h>
+#include <linux/wakelock.h>
 
 #include "st_lsm6ds3.h"
 
 #define ST_LSM6DS3_SRC_FUNC_ADDR		0x53
 #define ST_LSM6DS3_FIFO_DATA_AVL_ADDR		0x3b
-#define ST_LSM6DS3_TAP_FUNC_ADDR		0x1C
 
-#define ST_LSM6DS3_SRC_SIGN_MOTION_DATA_AVL 0x40
 #define ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL	0x10
 #define ST_LSM6DS3_SRC_TILT_DATA_AVL		0x20
 #define ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL	0x80
 #define ST_LSM6DS3_FIFO_DATA_AVL		0x80
 #define ST_LSM6DS3_FIFO_DATA_OVR		0x40
 
+#define ST_LSM6DS3_TAP_SRC_ADDR			0x1c
+#define ST_LSM6DS3_TAP_SRC_DETECTED_MASK	(1<<6)
+#define ST_LSM6DS3_TAP_SRC_SINGLE_TAP_MASK	(1<<5)
+#define ST_LSM6DS3_TAP_SRC_Z_AXIS_MASK		(1<<0)
+
+#define ST_LSM6DS3_6D_SRC_ADDR			0x1d
+
+
+
 static struct workqueue_struct *st_lsm6ds3_wq;
 
 void st_lsm6ds3_flush_works()
@@ -43,6 +54,7 @@ irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
 
 	get_monotonic_boottime(&ts);
 	cdata->timestamp = timespec_to_ns(&ts);
+	cdata->accel_timestamp  = cdata->timestamp;
 	queue_work(st_lsm6ds3_wq, &cdata->data_work);
 
 	disable_irq_nosync(irq);
@@ -53,31 +65,79 @@ irqreturn_t st_lsm6ds3_save_timestamp(int irq, void *private)
 static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 {
 	struct lsm6ds3_data *cdata;
-	u8 src_value = 0x00, src_fifo = 0x00, tap_value = 0x00, tap_cfg = 0x00, int1_cfg = 0x00, tap_thresh = 0x00;
+	u8 d6d_src_reg, tap_src_reg;
+	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,
 						struct lsm6ds3_data, 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];
+	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, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1,
 							&src_fifo, true);
-	cdata->tf->read(cdata, ST_LSM6DS3_TAP_FUNC_ADDR, 1,
-								&tap_value, true);
-	cdata->tf->read(cdata, 0x58, 1,
-									&tap_cfg, true);
-	cdata->tf->read(cdata, 0x5E, 1,
-										&int1_cfg, true);
-	cdata->tf->read(cdata, 0x59, 1,
-											&tap_thresh, true);
+
+	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 & (1<<6)){
+		dev_info(cdata->dev, "D6D IRQ");
+		if(cdata->sixd_mask & d6d_src_reg){
+			d6d_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_6D;
+		}
+		else{
+			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_TAP;
+			dev_info(cdata->dev, "Valid Tap");
+		}
+		else { 
+			dev_info(cdata->dev, "Ignoring tap");
+		}
+	}
+	if(cdata->first_irq_from_resume){
+		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");
+			tap_event = 1;
+			cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP;
+			dev_info(cdata->dev, "Valid Tap from sleep");
+		}
+	}
 
 	if (src_fifo & ST_LSM6DS3_FIFO_DATA_AVL) {
-		if (src_fifo & ST_LSM6DS3_FIFO_DATA_OVR) {
-			st_lsm6ds3_set_fifo_mode(cdata, BYPASS);
-			st_lsm6ds3_set_fifo_mode(cdata, CONTINUOS);
-			dev_err(cdata->dev,
-				"data fifo overrun, reduce fifo size.\n");
-		} else
-			st_lsm6ds3_read_fifo(cdata, false);
+		dev_dbg(cdata->dev, "Fifo data available");
+		st_lsm6ds3_read_fifo(cdata, true);
+	}
+//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);
+
+			cdata->sign_motion_event_ready = false;
+		}
 	}
 
 	if (src_value & ST_LSM6DS3_SRC_STEP_DETECTOR_DATA_AVL) {
@@ -86,27 +146,18 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 				0, IIO_EV_TYPE_THRESH, IIO_EV_DIR_EITHER),
 				cdata->timestamp);
 
-//		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->timestamp);
-//
-//			cdata->sign_motion_event_ready = false;
-//		}
-	}
+		if (cdata->sign_motion_event_ready) {
+            dev_info(cdata->dev, "significant motion irq, pushing event (disable this?)");
+			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->timestamp);
 
-	if (cdata->sign_motion_event_ready &&
-			(tap_value & ST_LSM6DS3_SRC_SIGN_MOTION_DATA_AVL)) {
-		printk("Firing Sig motion event. Tap value: %x, Tap cfg: %x, Int cfg: %x, Tap thresh: %x\n", tap_value, tap_cfg, int1_cfg, tap_thresh);
-		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->timestamp);
-		cdata->sign_motion_event_ready = false;
+			cdata->sign_motion_event_ready = false;
+		}
 	}
+
 	if (src_value & ST_LSM6DS3_SRC_STEP_COUNTER_DATA_AVL) {
 		iio_trigger_poll_chained(
 				cdata->trig[ST_INDIO_DEV_STEP_COUNTER], 0);
@@ -120,6 +171,10 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work)
 	}
 
 	enable_irq(cdata->irq);
+	if(wake_lock_active(&cdata->wlock))
+		wake_unlock(&cdata->wlock);
+
+	cdata->first_irq_from_resume = 0;
 	return;
 }