diff options
| -rw-r--r-- | driver/accelgyro_lsm6dso.c | 55 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dso.h | 10 |
2 files changed, 28 insertions, 37 deletions
diff --git a/driver/accelgyro_lsm6dso.c b/driver/accelgyro_lsm6dso.c index ca4d85edb8..fbce687f2c 100644 --- a/driver/accelgyro_lsm6dso.c +++ b/driver/accelgyro_lsm6dso.c @@ -159,32 +159,25 @@ static void push_fifo_data(struct motion_sensor_t *main_s, uint8_t *fifo, motion_sense_fifo_stage_data(&vect, sensor, 3, saved_ts); } -static inline int load_fifo(struct motion_sensor_t *s, - const struct lsm6dso_fstatus *fsts, +static inline int load_fifo(struct motion_sensor_t *main_s, + const uint16_t fifo_len, uint32_t saved_ts) { - uint8_t fifo[FIFO_READ_LEN], *ptr; - int i, err, read_len = 0, word_len, fifo_len; - uint16_t fifo_depth; - - fifo_depth = fsts->len & LSM6DSO_FIFO_DIFF_MASK; - fifo_len = fifo_depth * LSM6DSO_FIFO_SAMPLE_SIZE; - while (read_len < fifo_len) { - word_len = GENERIC_MIN(fifo_len - read_len, sizeof(fifo)); - err = st_raw_read_n_noinc(s->port, s->i2c_spi_addr_flags, + uint8_t fifo[LSM6DSO_FIFO_SAMPLE_SIZE]; + int i, err; + + for (i = 0; i < fifo_len; i++) { + err = st_raw_read_n_noinc(main_s->port, + main_s->i2c_spi_addr_flags, LSM6DSO_FIFO_DATA_ADDR_TAG, - fifo, word_len); + fifo, LSM6DSO_FIFO_SAMPLE_SIZE); if (err != EC_SUCCESS) return err; - for (i = 0; i < word_len; i += LSM6DSO_FIFO_SAMPLE_SIZE) { - ptr = &fifo[i]; - push_fifo_data(LSM6DSO_MAIN_SENSOR(s), ptr, saved_ts); - } - read_len += word_len; + push_fifo_data(main_s, fifo, saved_ts); } - return read_len; + return EC_SUCCESS; } /** @@ -234,31 +227,37 @@ void lsm6dso_interrupt(enum gpio_signal signal) */ static int irq_handler(struct motion_sensor_t *s, uint32_t *event) { - int ret = EC_SUCCESS; + int ret = EC_SUCCESS, fifo_len = 0; struct lsm6dso_fstatus fsts; + bool has_read_fifo = false; - if (((s->type != MOTIONSENSE_TYPE_ACCEL) && - (s->type != MOTIONSENSE_TYPE_GYRO)) || + if ((s->type != MOTIONSENSE_TYPE_ACCEL) || (!(*event & CONFIG_ACCEL_LSM6DSO_INT_EVENT))) return EC_ERROR_NOT_HANDLED; - if (IS_ENABLED(CONFIG_ACCEL_FIFO)) { + if (!IS_ENABLED(CONFIG_ACCEL_FIFO)) + return EC_SUCCESS; + + do { /* Read how many data patterns on FIFO to read. */ ret = st_raw_read_n_noinc(s->port, s->i2c_spi_addr_flags, LSM6DSO_FIFO_STS1_ADDR, (uint8_t *)&fsts, sizeof(fsts)); if (ret != EC_SUCCESS) - return ret; + break; if (fsts.len & (LSM6DSO_FIFO_DATA_OVR | LSM6DSO_FIFO_FULL)) CPRINTS("%s FIFO Overrun: %04x", s->name, fsts.len); - if (fsts.len & LSM6DSO_FIFO_DIFF_MASK) - ret = load_fifo(s, &fsts, last_interrupt_timestamp); + fifo_len = fsts.len & LSM6DSO_FIFO_DIFF_MASK; + if (fifo_len) { + ret = load_fifo(s, fifo_len, last_interrupt_timestamp); + has_read_fifo = true; + } + } while (fifo_len != 0 && ret == EC_SUCCESS); - if (IS_ENABLED(CONFIG_ACCEL_FIFO) && ret > 0) - motion_sense_fifo_commit_data(); - } + if (ret == EC_SUCCESS && has_read_fifo) + motion_sense_fifo_commit_data(); return ret; } diff --git a/driver/accelgyro_lsm6dso.h b/driver/accelgyro_lsm6dso.h index a786f5adf0..9a58fe7d36 100644 --- a/driver/accelgyro_lsm6dso.h +++ b/driver/accelgyro_lsm6dso.h @@ -60,10 +60,6 @@ (LSM6DSO_CTRL1_ADDR + (_sensor)) #define LSM6DSO_ODR_MASK 0xf0 -/* Hardware FIFO size in byte */ -#define LSM6DSO_MAX_FIFO_SIZE 4096 -#define LSM6DSO_MAX_FIFO_LENGTH (LSM6DSO_MAX_FIFO_SIZE / OUT_XYZ_SIZE) - /* FIFO decimator registers and bitmask */ #define LSM6DSO_FIFO_CTRL1_ADDR 0x07 #define LSM6DSO_FIFO_CTRL2_ADDR 0x08 @@ -104,11 +100,8 @@ enum lsm6dso_dev_fifo { }; /* Define FIFO data pattern, tag and len */ -#define LSM6DSO_SAMPLE_SIZE 6 -#define LSM6DSO_TS_SAMPLE_SIZE 4 #define LSM6DSO_TAG_SIZE 1 -#define LSM6DSO_FIFO_SAMPLE_SIZE LSM6DSO_SAMPLE_SIZE + LSM6DSO_TAG_SIZE -#define LSM6DSO_MAX_FIFO_DEPTH 416 +#define LSM6DSO_FIFO_SAMPLE_SIZE (OUT_XYZ_SIZE + LSM6DSO_TAG_SIZE) enum lsm6dso_tag_fifo { LSM6DSO_GYRO_TAG = 0x01, @@ -222,7 +215,6 @@ struct lsm6dso_data { /* Macro to initialize motion_sensors structure */ #define LSM6DSO_ST_DATA(g, type) (&((g).st_data[type])) -#define LSM6DSO_MAIN_SENSOR(_s) ((_s) - (_s)->type) extern const struct accelgyro_drv lsm6dso_drv; |