From b6602fe7788593d7c5e02ae7bb5616ebc6f63f2c Mon Sep 17 00:00:00 2001 From: mattis fjallstrom Date: Wed, 9 Dec 2015 15:44:40 -0800 Subject: ST lsm6ds3 fixes. Moved IRQ registration to end of probe, changed 6d push function to use '01,01,01' as the 6d signifier Change-Id: Ia162ef14178cda4d9a19a72159f1d0e3bcbbcd0f Signed-off-by: Evan Wilson --- drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c | 2 +- drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c | 19 +++++++----- drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c | 41 +++++++++++++------------ 3 files changed, 35 insertions(+), 27 deletions(-) diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c index eb080d4b8f9..b89141689a1 100644 --- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_buffer.c @@ -160,7 +160,7 @@ void st_lsm6ds3_push_tap_to_fifo(struct lsm6ds3_data *cdata) void st_lsm6ds3_push_d6d_to_fifo(struct lsm6ds3_data *cdata) { - u8 d6d_signature[6] = {1}; + u8 d6d_signature[6] = {0,1,0,1,0,1}; dev_info(cdata->dev, "Sending 6d event through accel data @:%llu", cdata->accel_timestamp-1); st_lsm6ds3_push_data_with_timestamp( cdata, ST_INDIO_DEV_ACCEL, diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c index 306a1ca68e4..ead52144441 100644 --- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_core.c @@ -2244,13 +2244,6 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq) if (err < 0) goto iio_device_free; - if (irq > 0) { - err = st_lsm6ds3_allocate_triggers(cdata, - ST_LSM6DS3_TRIGGER_OPS); - if (err < 0) - goto deallocate_ring; - } - for (n = 0; n < ST_INDIO_DEV_NUM; n++) { err = iio_device_register(cdata->indio_dev[n]); if (err) @@ -2263,6 +2256,18 @@ int st_lsm6ds3_common_probe(struct lsm6ds3_data *cdata, int irq) device_init_wakeup(cdata->dev, true); + /* + * We need to do this last, since the IRQs will be enabled + * immediately once request_threaded_irq returns. + */ + + if (irq > 0) { + err = st_lsm6ds3_allocate_triggers(cdata, + ST_LSM6DS3_TRIGGER_OPS); + if (err < 0) + goto iio_device_unregister_and_trigger_deallocate; + /* goto deallocate_ring; */ + } return 0; iio_device_unregister_and_trigger_deallocate: diff --git a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c index 8aab9911596..a818b14b275 100644 --- a/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c +++ b/drivers/iio/imu/st_lsm6ds3/st_lsm6ds3_trigger.c @@ -149,7 +149,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work) int ignore_event = 0; cdata = container_of((struct work_struct *)data_work, - struct lsm6ds3_data, data_work); + struct lsm6ds3_data, data_work); wake_irq = last_wakeup_reason_test(cdata->irq); mutex_lock(&cdata->fifo_lock); @@ -159,34 +159,38 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work) cdata->tf->read(cdata, ST_LSM6DS3_FIFO_DATA_AVL_ADDR, 1, &src_fifo, true); - dev_dbg(cdata->dev, "ST irq start :src_value, 6d, tap:%x %x %x",src_value, d6d_src_reg, tap_src_reg); + 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 & ST_LSM6DS3_6D_SRC_DETECTED_MASK){ #ifdef WAKE_STATS_DEBUG_INFO update_orient_stats(d6d_src_reg, cdata->timestamp); #endif - dev_info(cdata->dev, "D6D IRQ val:0x%x mask:0x%x", SIXD_MASK_VALID_BITS & d6d_src_reg, cdata->sixd_mask); + dev_info(cdata->dev, "D6D IRQ val:0x%x mask:0x%x", + SIXD_MASK_VALID_BITS & d6d_src_reg, cdata->sixd_mask); + if(cdata->sixd_mask & d6d_src_reg){ d6d_event = 1; cdata->last_wakeup_source |= LSM6DS3_WAKEUP_6D; - } - else{ + } else { ignore_event = 1; - dev_info(cdata->dev, "ignoring 6d interrupt, wrong axis. mask: 0x%x", cdata->sixd_mask); - } - + 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)){ + 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 { + } else { dev_info(cdata->dev, "Ignoring tap"); } } + if(cdata->first_irq_from_resume && wake_irq){ #ifdef WAKE_STATS_DEBUG_INFO wakeup_irq_count++; @@ -196,8 +200,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work) tap_event = 1; cdata->last_wakeup_source |= LSM6DS3_WAKEUP_TAP; dev_info(cdata->dev, "Valid Tap from sleep"); - } - + } } if(!ignore_event && (tap_event || d6d_event) && cdata->first_irq_from_resume){ @@ -209,7 +212,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work) st_lsm6ds3_push_d6d_to_fifo(cdata); } } - else if(d6d_event && !ignore_event){//negative roll + else if(d6d_event && !ignore_event) { //negative roll wake_lock_timeout(&cdata->tap_wlock,msecs_to_jiffies(200)); #ifdef WAKE_STATS_DEBUG_INFO wakeup_irq_keepawake_count++; @@ -276,7 +279,7 @@ static void st_lsm6ds3_irq_management(struct work_struct *data_work) } int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, - const struct iio_trigger_ops *trigger_ops) + const struct iio_trigger_ops *trigger_ops) { int err, i, n; @@ -303,7 +306,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_HIGH, cdata->name, cdata); if (err) goto deallocate_trigger; @@ -316,9 +319,9 @@ int st_lsm6ds3_allocate_triggers(struct lsm6ds3_data *cdata, } cdata->indio_dev[n]->trig = cdata->trig[n]; } - + return 0; - + free_irq: free_irq(cdata->irq, cdata); for (n--; n >= 0; n--) @@ -326,7 +329,7 @@ free_irq: deallocate_trigger: for (i--; i >= 0; i--) iio_trigger_free(cdata->trig[i]); - + return err; } EXPORT_SYMBOL(st_lsm6ds3_allocate_triggers); -- cgit v1.2.3-70-g09d2