diff options
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3.h | 18 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c | 20 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c | 325 | ||||
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c | 129 |
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, ®val, false); + + regval = 0x6; //quiet and shock times + err = sdata->cdata->tf->write(sdata->cdata, + ST_LSM6DS3_INT_DUR2_ADDR, + 1, ®val, 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, ®val, 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, ®val, 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, ®val, 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, ®_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, ®_value, true); + err = cdata->tf->read(cdata, + ST_LSM6DS3_MD1_ADDR, 1, ®_value, true); + dev_info(cdata->dev, "before suspend md1: %x, err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0xd, 1, ®_value, true); + dev_info(cdata->dev, "before suspend int1: %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0x58, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0x58 tap enable: %x err:%i", reg_value, err); + err = cdata->tf->read(cdata, + 0x10, 1, ®_value, true); + dev_info(cdata->dev, "ctrl1_xl: %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0xa, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0xA fifo mode: %x err:%i", reg_value, err); + err = cdata->tf->read(cdata, + 0x13, 1, ®_value, true); + dev_info(cdata->dev, "before suspend 0x13 ctrl4_c : %x err:%i", reg_value, err); + + err = cdata->tf->read(cdata, + 0x06, 1, ®_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, ®_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, ®_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; } |