driver: lsm6dso: Fix FIFO processing

Add a loop in irq_handler to match datasheet documentation:
"""
1. Read the FIFO_STATUS1 and FIFO_STATUS2 registers to check how many
words are stored in the FIFO. This information is contained in the
DIFF_FIFO_[9:0] bits.
2. For each word in FIFO, read the FIFO word (tag and output data) and
interpret it on the basis of the FIFO
tag.
3. Go to step 1.
"""
We need to read the FIFO length at the end of the IRQ handler to be
sure a new item has not been put while the FIFO was being read.
Otherwise, we will not get interrupt and will stop processing FIFO when
the FIFO status register still shows the FIFO as not empty.

However, using linux kernel st_lsm6dsx_read_tagged_fifo() as model,
read only one word at a time:
When reading 2 7bytes word at time, the FIFO would still claim one entry
remains: The 3rd entry would be a copy of the accelerometer data, but
with a corrupted z axis.That why we fail the data test, the gravity
vector is not measured at 9.81m/s^2.

There was a bug in the interrupt handler that would read the FIFO twice,
which was presenting the data: both gyro and accel at the same ODR:
[7623.475256 Base Accel FIFO status: 8002]
[7623.476548 Base Gyro sending vector num: 2
    raw: 0x00000007 - 0xFFFFFFF6 - 0xFFFFFFF9]
[7623.477526 Base Accel sending vector num: 1
    raw: 0xFFFFFFCC - 0xFFFFE275 - 0xFFFFE818]
[7623.479271 Base Accel FIFO status: 8001]
[7623.480333 Base Accel sending vector num: 1
    raw: 0xFFFFFFCC - 0xFFFFE275 - 0x00000C18]
[7623.481699 Base Accel FIFO status: 0000]
We must only run the irq_hanlder for the main/accelerometer sensor.

Restructure irq_hanlder to match other driver, like accelgyro_bm160.c.

BUG=b:195700255,b:192219470
BRANCH=dedede
TEST=Running tast run <ip> hardware.SensorIioserviceHard
Check we get the right amount of samples.

Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Change-Id: I5820dec4c7c5ccb1ebeda854398738d1af60290c
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3150679
Reviewed-by: Keith Short <keithshort@chromium.org>

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;