diff options
Diffstat (limited to 'driver/accelgyro_bmi160.c')
| -rw-r--r-- | driver/accelgyro_bmi160.c | 19 |
1 files changed, 11 insertions, 8 deletions
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 3c56626b98..1b096c9f50 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -766,7 +766,6 @@ enum fifo_state { #define BMI160_FIFO_BUFFER 64 static uint8_t bmi160_buffer[BMI160_FIFO_BUFFER]; -#define BUFFER_END(_buffer) ((_buffer) + sizeof(_buffer)) /* * Decode the header from the fifo. * Return 0 if we need further processing. @@ -775,9 +774,11 @@ static uint8_t bmi160_buffer[BMI160_FIFO_BUFFER]; * @s: base sensor * @hdr: the header to decode * @bp: current pointer in the buffer, updated when processing the header. + * @ep: pointer to the end of the valid data in the buffer. */ static int bmi160_decode_header(struct motion_sensor_t *s, - enum fifo_header hdr, uint32_t last_ts, uint8_t **bp) + enum fifo_header hdr, uint32_t last_ts, + uint8_t **bp, uint8_t *ep) { if ((hdr & BMI160_FH_MODE_MASK) == BMI160_EMPTY && (hdr & BMI160_FH_PARM_MASK) != 0) { @@ -788,9 +789,9 @@ static int bmi160_decode_header(struct motion_sensor_t *s, if (hdr & (1 << (i + BMI160_FH_PARM_OFFSET))) size += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6); } - if (*bp + size > BUFFER_END(bmi160_buffer)) { + if (*bp + size > ep) { /* frame is not complete, it will be retransmitted. */ - *bp = BUFFER_END(bmi160_buffer); + *bp = ep; return 1; } for (i = MOTIONSENSE_TYPE_MAG; i >= MOTIONSENSE_TYPE_ACCEL; @@ -839,6 +840,7 @@ static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts) uint16_t length; enum fifo_state state = FIFO_HEADER; uint8_t *bp = bmi160_buffer; + uint8_t *ep; uint32_t beginning; @@ -882,6 +884,7 @@ static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts) raw_read_n(s->port, s->addr, BMI160_FIFO_DATA, bmi160_buffer, length); beginning = *(uint32_t *)bmi160_buffer; + ep = bmi160_buffer + length; /* * FIFO is invalid when reading while the sensors are all * suspended. @@ -899,12 +902,12 @@ static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts) return EC_SUCCESS; } - while (bp < bmi160_buffer + length) { + while (bp < ep) { switch (state) { case FIFO_HEADER: { enum fifo_header hdr = *bp++; - if (bmi160_decode_header(s, hdr, last_ts, &bp)) + if (bmi160_decode_header(s, hdr, last_ts, &bp, ep)) continue; /* Other cases */ hdr &= 0xdc; @@ -943,8 +946,8 @@ static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts) state = FIFO_HEADER; break; case FIFO_DATA_TIME: - if (bp + 3 > BUFFER_END(bmi160_buffer)) { - bp = BUFFER_END(bmi160_buffer); + if (bp + 3 > ep) { + bp = ep; continue; } /* We are not requesting timestamp */ |