diff options
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 59 |
1 files changed, 51 insertions, 8 deletions
diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c index cdf5535c7d..ecc58887b8 100644 --- a/driver/accelgyro_lsm6dsm.c +++ b/driver/accelgyro_lsm6dsm.c @@ -22,6 +22,8 @@ #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args) #define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args) +#define IS_FSTS_EMPTY(s) ((s).len & LSM6DSM_FIFO_EMPTY) + #ifdef CONFIG_ACCEL_FIFO static volatile uint32_t last_interrupt_timestamp; #endif @@ -306,7 +308,8 @@ static void push_fifo_data(struct motion_sensor_t *accel, uint8_t *fifo, } } -static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts) +static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts, + uint32_t *last_fifo_read_ts) { uint32_t int_ts = last_interrupt_timestamp; int err, left, length; @@ -338,6 +341,7 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts) err = st_raw_read_n_noinc(s->port, s->addr, LSM6DSM_FIFO_DATA_ADDR, fifo, length); + *last_fifo_read_ts = __hw_clock_source_read(); if (err != EC_SUCCESS) return err; @@ -354,21 +358,44 @@ static int load_fifo(struct motion_sensor_t *s, const struct fstatus *fsts) return EC_SUCCESS; } + +static int is_fifo_empty(struct motion_sensor_t *s, struct fstatus *fsts) +{ + int res; + + if (s->flags & MOTIONSENSE_FLAG_INT_SIGNAL) + return gpio_get_level(s->int_signal); + CPRINTS("Interrupt signal not set for %s", s->name); + res = st_raw_read_n_noinc(s->port, s->addr, + LSM6DSM_FIFO_STS1_ADDR, + (int8_t *)fsts, sizeof(*fsts)); + /* If we failed to read the FIFO size assume empty. */ + if (res != EC_SUCCESS) + return 1; + return IS_FSTS_EMPTY(*fsts); +} + #endif /* CONFIG_ACCEL_FIFO */ -/** - * lsm6dsm_interrupt - interrupt from int1/2 pin of sensor - */ -void lsm6dsm_interrupt(enum gpio_signal signal) +static void handle_interrupt_for_fifo(uint32_t ts) { #ifdef CONFIG_ACCEL_FIFO - last_interrupt_timestamp = __hw_clock_source_read(); + if (time_after(ts, last_interrupt_timestamp)) + last_interrupt_timestamp = ts; #endif task_set_event(TASK_ID_MOTIONSENSE, CONFIG_ACCEL_LSM6DSM_INT_EVENT, 0); } /** + * lsm6dsm_interrupt - interrupt from int1/2 pin of sensor + */ +void lsm6dsm_interrupt(enum gpio_signal signal) +{ + handle_interrupt_for_fifo(__hw_clock_source_read()); +} + +/** * irq_handler - bottom half of the interrupt stack */ static int irq_handler(struct motion_sensor_t *s, uint32_t *event) @@ -382,18 +409,34 @@ static int irq_handler(struct motion_sensor_t *s, uint32_t *event) #ifdef CONFIG_ACCEL_FIFO { struct fstatus fsts; + uint32_t last_fifo_read_ts; + uint32_t triggering_interrupt_timestamp = + last_interrupt_timestamp; + /* Read how many data pattern on FIFO to read and pattern. */ ret = st_raw_read_n_noinc(s->port, s->addr, LSM6DSM_FIFO_STS1_ADDR, (uint8_t *)&fsts, sizeof(fsts)); if (ret != EC_SUCCESS) return ret; + last_fifo_read_ts = __hw_clock_source_read(); if (fsts.len & (LSM6DSM_FIFO_DATA_OVR | LSM6DSM_FIFO_FULL)) { CPRINTF("[%T %s FIFO Overrun: %04x]\n", s->name, fsts.len); } - if (!(fsts.len & LSM6DSM_FIFO_EMPTY)) - ret = load_fifo(s, &fsts); + if (!IS_FSTS_EMPTY(fsts)) + ret = load_fifo(s, &fsts, &last_fifo_read_ts); + + /* + * Check if FIFO isn't empty and we never got an interrupt. + * This can happen if new entries were added to the FIFO after + * the count was read, but before the FIFO was cleared out. + * In the long term it might be better to use the last + * spread timestamp instead. + */ + if (!is_fifo_empty(s, &fsts) && + triggering_interrupt_timestamp == last_interrupt_timestamp) + handle_interrupt_for_fifo(last_fifo_read_ts); } #endif return ret; |