diff options
Diffstat (limited to 'drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c')
| -rw-r--r-- | drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c | 67 |
1 files changed, 61 insertions, 6 deletions
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, ®_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; |