Adding sysfs entries to set 6d and tap thresholds.

Also some cleaning up

Change-Id: I015d9d86879712dee6bbd02fd238366bcab8a174

2 files changed, 147 insertions, 24 deletions

diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
index 718c8ea4280..25443f64c75 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h
@@ -140,6 +140,8 @@ struct lsm6ds3_data {
 	struct wake_lock tap_wlock;
 	u8 first_irq_from_resume;
 	u8 reg_read;
+	u8 sixd_ths;
+	u8 tap_ths;
 #define SIXD_MASK_VALID_BITS (0x21)
 	u8 sixd_mask;
 	u8 int1_save;
diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
index 12dc8cbf1c7..766ebb42da0 100644
--- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
+++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c
@@ -177,11 +177,22 @@
 #define ST_LSM6DS3_TILT_EN_MASK			0x20
 #define ST_LSM6DS3_TILT_DRDY_IRQ_MASK		0x02
 
-/* 6d Constants */
-
+/* 6d and Tap Constants */
 
 #define ST_LSM6DS3_6D_MD1_INT_MASK		0x04
-#define ST_LSM6DS3_SINGTAP_MD1_INT_MASK		0x40
+#define ST_LSM6DS3_TAP_MD1_INT_MASK		0x48
+#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_SIXD_THS_MASK 0x60
+#define ST_LSM6DS3_TAP_THS_MASK 0x1F
+#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_CTRL4_ADDR 0x13
+#define ST_LSM6DS3_CTRL4_STOP_ON_FTH_MASK 0x1
 
 #define ST_LSM6DS3_ACCEL_SUFFIX_NAME		"accel"
 #define ST_LSM6DS3_GYRO_SUFFIX_NAME		"gyro"
@@ -932,7 +943,7 @@ int st_lsm6ds3_set_drdy_irq(struct lsm6ds3_sensor_data *sdata, bool state)
 						reg_addr, mask, value, true);
 			
 		reg_addr = ST_LSM6DS3_MD1_ADDR;
-		mask = ST_LSM6DS3_SINGTAP_MD1_INT_MASK;
+		mask = ST_LSM6DS3_TAP_MD1_INT_MASK;
 		//written at end of func
 		break;
 	case ST_INDIO_DEV_STEP_COUNTER:
@@ -1080,10 +1091,57 @@ static int st_lsm6ds3_enable_pedometer(struct lsm6ds3_sensor_data *sdata,
 
 }
 
+static int st_lsm6ds3_set_sixd_ths(struct lsm6ds3_data *cdata, int sixd_ths) {
+	int err = 0;
+
+	dev_info(cdata->dev, "Setting 6d ths: %d", sixd_ths);
+	if(sixd_ths < 0 || sixd_ths > 3) {
+		return -EINVAL;
+	} else if(sixd_ths == 0) {
+		err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_MD1_ADDR, ST_LSM6DS3_6D_MD1_INT_MASK, 0, true);
+	} else {
+		err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_MD1_ADDR, ST_LSM6DS3_6D_MD1_INT_MASK, 1, true);
+		if(err < 0) {
+			return -EIO;
+		}
+		err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_TAP_THS_6D_ADDR, ST_LSM6DS3_SIXD_THS_MASK, sixd_ths, true);
+	}
+
+	if(err < 0) {
+		return -EIO;
+	}
+
+	cdata->sixd_ths = sixd_ths;
+
+	return 0;
+}
+
+static int st_lsm6ds3_set_tap_ths(struct lsm6ds3_data *cdata, int tap_ths) {
+	int err = 0;
+
+	dev_info(cdata->dev, "Setting tap ths: %d", tap_ths);
+	if(tap_ths < 0 || tap_ths > 31) {
+		return -EINVAL;
+	} else {
+		err = st_lsm6ds3_write_data_with_mask(cdata, ST_LSM6DS3_TAP_THS_6D_ADDR, ST_LSM6DS3_TAP_THS_MASK, tap_ths, true);
+	}
+
+	if(err < 0) {
+		return -EIO;
+	}
+
+	cdata->tap_ths = tap_ths;
+
+	return 0;
+}
+
 static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata)
 {
+	u8 regval;
 	int err, i;
 
+	dev_info(sdata->cdata->dev, "Enabling sensor: %d", sdata->sindex);
+
 	switch (sdata->sindex) {
 	case ST_INDIO_DEV_ACCEL:
 	case ST_INDIO_DEV_GYRO:
@@ -1097,6 +1155,21 @@ static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata)
 		if (sdata->sindex == ST_INDIO_DEV_ACCEL) {
 			sdata->cdata->accel_samples_to_discard =
 							ST_LSM6DS3_ACCEL_STD;
+
+			dev_info(sdata->cdata->dev, "Enabling tap interrupt.");
+			regval = ST_LSM6DS3_TAP_MD1_INT_MASK;
+			err = sdata->cdata->tf->write(sdata->cdata, ST_LSM6DS3_MD1_ADDR, 1, &regval, false);
+			if(err < 0) {
+				return err;
+			}
+			err = st_lsm6ds3_set_sixd_ths(sdata->cdata, sdata->cdata->sixd_ths);
+			if (err < 0) {
+				return err;
+			}
+			err = st_lsm6ds3_set_tap_ths(sdata->cdata, sdata->cdata->tap_ths);
+			if(err < 0) {
+				return err;
+			}
 		}
 
 		sdata->cdata->gyro_samples_to_discard = ST_LSM6DS3_GYRO_STD;
@@ -1120,7 +1193,7 @@ static int st_lsm6ds3_enable_sensors(struct lsm6ds3_sensor_data *sdata)
 		if (err < 0)
 			return err;
 		err = st_lsm6ds3_write_data_with_mask(sdata->cdata,
-						ST_LSM6DS3_MD1_ADDR, ST_LSM6DS3_SINGTAP_MD1_INT_MASK, ST_LSM6DS3_EN_BIT, true);
+						ST_LSM6DS3_MD1_ADDR, ST_LSM6DS3_TAP_MD1_INT_MASK, ST_LSM6DS3_EN_BIT, true);
 		if (err < 0)
 			return err;
 		break;
@@ -1536,25 +1609,6 @@ static int st_lsm6ds3_init_sensor(struct lsm6ds3_data *cdata)
 	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_CTRL4_ADDR 0x13
-#define ST_LSM6DS3_CTRL4_STOP_ON_FTH_MASK 0x1
-	regval = 0x44; // MD1 (6D and Tap on INT1)
-	err = sdata->cdata->tf->write(sdata->cdata,
-					ST_LSM6DS3_MD1_ADDR,
-					1, &regval, false);
-
-	regval = 0x48;//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,
@@ -1908,6 +1962,61 @@ static ssize_t st_lsm6ds3_sysfs_reset(struct device *dev,
 	st_lsm6ds3_reset(sdata->cdata, true);
 	return size;
 }
+
+static ssize_t st_lsm6ds3_sysfs_sixd_ths_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));
+	int sixd_ths;
+	ret = sscanf(buf, "%i", &sixd_ths);
+	if(ret != 1){
+		dev_err(sdata->cdata->dev,"error, sixd_ths range:  0-4");
+		return -EINVAL;
+	}
+
+	ret = st_lsm6ds3_set_sixd_ths(sdata->cdata, sixd_ths);
+	if(ret < 0)
+		return ret;
+	return size;
+}
+
+
+ssize_t st_lsm6ds3_sysfs_sixd_ths_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->sixd_ths);
+	return ret;
+}
+
+static ssize_t st_lsm6ds3_sysfs_tap_ths_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));
+	int tap_ths;
+	ret = sscanf(buf, "%i", &tap_ths);
+	if(ret != 1){
+		dev_err(sdata->cdata->dev,"error, tap_ths range:  0-31");
+		return -EINVAL;
+	}
+
+	ret = st_lsm6ds3_set_tap_ths(sdata->cdata, tap_ths);
+	if(ret < 0)
+		return ret;
+	return size;
+}
+
+
+ssize_t st_lsm6ds3_sysfs_tap_ths_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->tap_ths);
+	return ret;
+}
+
 static ssize_t st_lsm6ds3_sysfs_read_reg_set(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t size)
 {
@@ -2057,6 +2166,12 @@ static IIO_DEVICE_ATTR(read_reg, S_IRUGO | S_IWUSR,
 static IIO_DEVICE_ATTR(reset, S_IWUSR,
 		NULL, st_lsm6ds3_sysfs_reset, 0);
 
+static IIO_DEVICE_ATTR(sixd_ths, S_IRUGO | S_IWUSR,
+		st_lsm6ds3_sysfs_sixd_ths_get, st_lsm6ds3_sysfs_sixd_ths_set, 0);
+
+static IIO_DEVICE_ATTR(tap_ths, S_IRUGO | S_IWUSR,
+		st_lsm6ds3_sysfs_tap_ths_get, st_lsm6ds3_sysfs_tap_ths_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);
@@ -2081,6 +2196,8 @@ 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_reset.dev_attr.attr,
+	&iio_dev_attr_sixd_ths.dev_attr.attr,
+	&iio_dev_attr_tap_ths.dev_attr.attr,
 	&iio_dev_attr_sixd_wake_mask.dev_attr.attr,
 	&iio_dev_attr_suspend_samples.dev_attr.attr,
 	&iio_dev_attr_ack_wakeup_irq.dev_attr.attr,
@@ -2216,6 +2333,7 @@ int st_lsm6ds3_reset(struct lsm6ds3_data *cdata, bool hard_reset) {
 	int err;
 	struct lsm6ds3_sensor_data *sdata;
 
+	dev_info(cdata->dev, "Resetting accel. Hard reset: %b", hard_reset);
 	if(hard_reset) {
 		if(!cdata->reg_accel)
 		{
@@ -2312,7 +2430,10 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq)
 		dev_info(cdata->dev, "driver use DRDY int pin 1\n");
 	}
 
+	// default values
 	cdata->sixd_mask = 0x21;
+	cdata->sixd_ths = 0x2;
+	cdata->tap_ths = 0x8;
 
 	cdata->indio_dev[ST_INDIO_DEV_ACCEL]->name =
 		kasprintf(GFP_KERNEL, "%s_%s", cdata->name,