driver: bmi160: Read the FIFO more efficiently

Instead of blindy reading 64 bytes, even when only one sample is
available, first read the amount of data in the FIFO:
- if less than the buffer, just read that amount, and return when
  processed.
- if more, read what the buffer can hold, return to be called again.
  by the interrupt routine.
Return an error when we have to flush the FIFO when it looks incorrect.

BUG=b:73557414,b:80284952
BRANCH=scarlet,poppy
TEST=Check BMI160 is not stuck, run CtsHardwareTestCases suite.
Check remaining failures are related to timestamp only.

Change-Id: Ic2f48978f73f7aaf9996ec20d219a75dca5e5112
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1128555
Reviewed-by: Daisuke Nojiri <dnojiri@chromium.org>

1 files changed, 108 insertions, 83 deletions

diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 671ca1f572..82ef7b0117 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -826,112 +826,138 @@ static int bmi160_decode_header(struct motion_sensor_t *s,
  * We put raw data in hub fifo and process data from there.
  * @s Pointer to sensor data.
  *
+ * Read only up to  bmi160_buffer. If more reads are needed, we will be called
+ * again by the interrupt routine.
+ *
  * NOTE: If a new driver supports this function, be sure to add a check
  * for spoof_mode in order to load the sensor stack with the spoofed
  * data.  See accelgyro_bmi160.c::load_fifo for an example.
  */
 static int load_fifo(struct motion_sensor_t *s, uint32_t last_ts)
 {
-	int done = 0;
 	struct bmi160_drv_data_t *data = BMI160_GET_DATA(s);
+	uint16_t length;
+	enum fifo_state state = FIFO_HEADER;
+	uint8_t *bp = bmi160_buffer;
+	uint32_t beginning;
+
 
 	if (s->type != MOTIONSENSE_TYPE_ACCEL)
 		return EC_SUCCESS;
 
-	do {
-		enum fifo_state state = FIFO_HEADER;
-		uint8_t *bp = bmi160_buffer;
-		uint32_t beginning;
+	raw_read_n(s->port, s->addr, BMI160_FIFO_LENGTH_0,
+			(uint8_t *)&length, sizeof(length));
+	length &= BMI160_FIFO_LENGTH_MASK;
 
-		if (!(data->flags &
-		      (BMI160_FIFO_ALL_MASK << BMI160_FIFO_FLAG_OFFSET))) {
-			/*
-			 * The FIFO was disable while were processing it.
-			 *
-			 * Flush potential left over:
-			 * When sensor is resumed, we won't read old data.
-			 */
-			raw_write8(s->port, s->addr, BMI160_CMD_REG,
-				   BMI160_CMD_FIFO_FLUSH);
-			return EC_SUCCESS;
-		}
+	/*
+	 * We have not requested timestamp, no extra frame to read.
+	 * if we have too much to read, read the whole buffer.
+	 */
+	if (length == 0) {
+		CPRINTS("unexpected empty FIFO");
+		return EC_SUCCESS;
+	}
+
+	/* Add one byte to get an empty FIFO frame.*/
+	length++;
 
-		raw_read_n(s->port, s->addr, BMI160_FIFO_DATA, bmi160_buffer,
-				sizeof(bmi160_buffer));
-		beginning = *(uint32_t *)bmi160_buffer;
+	if (length > sizeof(bmi160_buffer))
+		CPRINTS("unexpected large FIFO: %d", length);
+	length = MIN(length, sizeof(bmi160_buffer));
+
+	if (!(data->flags &
+	     (BMI160_FIFO_ALL_MASK << BMI160_FIFO_FLAG_OFFSET))) {
 		/*
-		 * FIFO is invalid when reading while the sensors are all
-		 * suspended.
-		 * Instead of returning the empty frame, it can return a
-		 * pattern that looks like a valid header: 84 or 40.
-		 * If we see those, assume the sensors have been disabled
-		 * while this thread was running.
+		 * The FIFO was disabled while we were processing it.
+		 *
+		 * Flush potential left over:
+		 * When sensor is resumed, we won't read old data.
 		 */
-		if (beginning == 0x84848484 ||
-		    (beginning & 0xdcdcdcdc) == 0x40404040) {
-			CPRINTS("Suspended FIFO: accel ODR/rate: %d/%d: 0x%08x",
+		raw_write8(s->port, s->addr, BMI160_CMD_REG,
+				BMI160_CMD_FIFO_FLUSH);
+		return EC_SUCCESS;
+	}
+
+
+	raw_read_n(s->port, s->addr, BMI160_FIFO_DATA, bmi160_buffer,
+			length);
+	beginning = *(uint32_t *)bmi160_buffer;
+	/*
+	 * FIFO is invalid when reading while the sensors are all
+	 * suspended.
+	 * Instead of returning the empty frame, it can return a
+	 * pattern that looks like a valid header: 84 or 40.
+	 * If we see those, assume the sensors have been disabled
+	 * while this thread was running.
+	 */
+	if (beginning == 0x84848484 ||
+			(beginning & 0xdcdcdcdc) == 0x40404040) {
+		CPRINTS("Suspended FIFO: accel ODR/rate: %d/%d: 0x%08x",
 				BASE_ODR(s->config[SENSOR_CONFIG_AP].odr),
 				get_data_rate(s),
 				beginning);
-			return EC_SUCCESS;
-		}
+		return EC_SUCCESS;
+	}
 
-		while (!done && bp != BUFFER_END(bmi160_buffer)) {
-			switch (state) {
-			case FIFO_HEADER: {
-				enum fifo_header hdr = *bp++;
-				if (bmi160_decode_header(s, hdr, last_ts, &bp))
-					continue;
-				/* Other cases */
-				hdr &= 0xdc;
-				switch (hdr) {
-				case BMI160_EMPTY:
-					done = 1;
-					break;
-				case BMI160_SKIP:
-					state = FIFO_DATA_SKIP;
-					break;
-				case BMI160_TIME:
-					state = FIFO_DATA_TIME;
-					break;
-				case BMI160_CONFIG:
-					state = FIFO_DATA_CONFIG;
-					break;
-				default:
-					CPRINTS("Unknown header: 0x%02x @ %d",
-						hdr, bp - bmi160_buffer);
-					raw_write8(s->port, s->addr,
-						   BMI160_CMD_REG,
-						   BMI160_CMD_FIFO_FLUSH);
-					done = 1;
-				}
+	while (bp < bmi160_buffer + length) {
+		switch (state) {
+		case FIFO_HEADER: {
+			enum fifo_header hdr = *bp++;
+
+			if (bmi160_decode_header(s, hdr, last_ts, &bp))
+				continue;
+			/* Other cases */
+			hdr &= 0xdc;
+			switch (hdr) {
+			case BMI160_EMPTY:
+				return EC_SUCCESS;
+			case BMI160_SKIP:
+				state = FIFO_DATA_SKIP;
 				break;
-			}
-			case FIFO_DATA_SKIP:
-				CPRINTS("skipped %d frames", *bp++);
-				state = FIFO_HEADER;
+			case BMI160_TIME:
+				state = FIFO_DATA_TIME;
 				break;
-			case FIFO_DATA_CONFIG:
-				CPRINTS("config change: 0x%02x", *bp++);
-				state = FIFO_HEADER;
-				break;
-			case FIFO_DATA_TIME:
-				if (bp + 3 > BUFFER_END(bmi160_buffer)) {
-					bp = BUFFER_END(bmi160_buffer);
-					continue;
-				}
-				/* We are not requesting timestamp */
-				CPRINTS("timestamp %d", (bp[2] << 16) |
-					(bp[1] << 8) | bp[0]);
-				state = FIFO_HEADER;
-				bp += 3;
+			case BMI160_CONFIG:
+				state = FIFO_DATA_CONFIG;
 				break;
 			default:
-				CPRINTS("Unknown data: 0x%02x", *bp++);
-				state = FIFO_HEADER;
+				CPRINTS("Unknown header: 0x%02x @ %d",
+						hdr, bp - bmi160_buffer);
+				raw_write8(s->port, s->addr,
+						BMI160_CMD_REG,
+						BMI160_CMD_FIFO_FLUSH);
+				return EC_ERROR_NOT_HANDLED;
 			}
+			break;
 		}
-	} while (!done);
+		case FIFO_DATA_SKIP:
+			CPRINTS("@ %d - %d, skipped %d frames",
+					bp - bmi160_buffer, length, *bp);
+			bp++;
+			state = FIFO_HEADER;
+			break;
+		case FIFO_DATA_CONFIG:
+			CPRINTS("@ %d - %d, config change: 0x%02x",
+					bp - bmi160_buffer, length, *bp);
+			bp++;
+			state = FIFO_HEADER;
+			break;
+		case FIFO_DATA_TIME:
+			if (bp + 3 > BUFFER_END(bmi160_buffer)) {
+				bp = BUFFER_END(bmi160_buffer);
+				continue;
+			}
+			/* We are not requesting timestamp */
+			CPRINTS("timestamp %d", (bp[2] << 16) |
+					(bp[1] << 8) | bp[0]);
+			state = FIFO_HEADER;
+			bp += 3;
+			break;
+		default:
+			CPRINTS("Unknown data: 0x%02x", *bp++);
+			state = FIFO_HEADER;
+		}
+	}
 	return EC_SUCCESS;
 }
 #endif  /* CONFIG_ACCEL_FIFO */
@@ -1143,8 +1169,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	ret = raw_read_n(s->port, s->addr, get_xyz_reg(s->type), data, 6);
 
 	if (ret != EC_SUCCESS) {
-		CPRINTF("[%T %s type:0x%X RD XYZ Error %d]",
-			s->name, s->type, ret);
+		CPRINTS("%s: type:0x%X RD XYZ Error %d", s->name, s->type, ret);
 		return ret;
 	}
 	normalize(s, v, data);