driver: icm426xx: discard first data instead of sleeping

Sleeping when turning a sensor on/off is problematic when the
other sensor is running.

Replace sleeping by discarding first events until sensor is
stabilized.

BUG=chromium:1175757
BRANCH=hatch,nami,kukui,dedede,grunt,zork,octopus,volteer
TEST=turn sensor on/off when the other is running using cros-ec
     iio devices

Signed-off-by: Jean-Baptiste Maneyrol <jmaneyrol@invensense.com>
Change-Id: I61801970b12f9fbdcc2cd96cb4df1edae5ed521f
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/2682715
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>
Commit-Queue: Gwendal Grignou <gwendal@chromium.org>
Tested-by: Gwendal Grignou <gwendal@chromium.org>
(cherry picked from commit 71d6b16eff18aab1f7c592a75e5489a4f14b8790)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3128756
Reviewed-by: JuHyun Kim <jkim@invensense.com>
Reviewed-by: Zhuohao Lee <zhuohao@chromium.org>
Commit-Queue: Zhuohao Lee <zhuohao@chromium.org>

3 files changed, 98 insertions, 22 deletions

diff --git a/driver/accelgyro_icm426xx.c b/driver/accelgyro_icm426xx.c
index 411ad86a14..40adaaaace 100644
--- a/driver/accelgyro_icm426xx.c
+++ b/driver/accelgyro_icm426xx.c
@@ -49,6 +49,22 @@ static int icm426xx_normalize(const struct motion_sensor_t *s, intv3_t v,
 	return EC_SUCCESS;
 }
 
+static int icm426xx_check_sensor_stabilized(const struct motion_sensor_t *s,
+					    uint32_t ts)
+{
+	int32_t rem;
+
+	rem = icm_get_sensor_stabilized(s, ts);
+	if (rem == 0)
+		return EC_SUCCESS;
+	if (rem > 0)
+		return EC_ERROR_BUSY;
+
+	/* rem < 0: reset check since ts has passed stabilize_ts */
+	icm_reset_stabilize_ts(s);
+	return EC_SUCCESS;
+}
+
 /* use FIFO threshold interrupt on INT1 */
 #define ICM426XX_FIFO_INT_EN		ICM426XX_FIFO_THS_INT1_EN
 #define ICM426XX_FIFO_INT_STATUS	ICM426XX_FIFO_THS_INT
@@ -221,10 +237,16 @@ static int __maybe_unused icm426xx_load_fifo(struct motion_sensor_t *s,
 		/* exit if error or FIFO is empty */
 		if (size <= 0)
 			return -size;
-		if (accel != NULL)
-			icm426xx_push_fifo_data(st->accel, accel, ts);
-		if (gyro != NULL)
-			icm426xx_push_fifo_data(st->gyro, gyro, ts);
+		if (accel != NULL) {
+			ret = icm426xx_check_sensor_stabilized(st->accel, ts);
+			if (ret == EC_SUCCESS)
+				icm426xx_push_fifo_data(st->accel, accel, ts);
+		}
+		if (gyro != NULL) {
+			ret = icm426xx_check_sensor_stabilized(st->gyro, ts);
+			if (ret == EC_SUCCESS)
+				icm426xx_push_fifo_data(st->gyro, gyro, ts);
+		}
 	}
 
 	return EC_SUCCESS;
@@ -326,7 +348,8 @@ static int icm426xx_config_interrupt(const struct motion_sensor_t *s)
 
 static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable)
 {
-	unsigned int sleep;
+	uint32_t delay, stop_delay;
+	int32_t rem;
 	uint8_t mask, val;
 	int ret;
 
@@ -334,43 +357,50 @@ static int icm426xx_enable_sensor(const struct motion_sensor_t *s, int enable)
 	case MOTIONSENSE_TYPE_ACCEL:
 		mask = ICM426XX_ACCEL_MODE_MASK;
 		if (enable) {
-			sleep = 20;
+			delay = ICM426XX_ACCEL_START_TIME;
+			stop_delay = ICM426XX_ACCEL_STOP_TIME;
 			val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_LOW_POWER);
 		} else {
-			sleep = 0;
+			delay = ICM426XX_ACCEL_STOP_TIME;
 			val = ICM426XX_ACCEL_MODE(ICM426XX_MODE_OFF);
 		}
 		break;
 	case MOTIONSENSE_TYPE_GYRO:
 		mask = ICM426XX_GYRO_MODE_MASK;
 		if (enable) {
-			sleep = 60;
+			delay = ICM426XX_GYRO_START_TIME;
+			stop_delay = ICM426XX_GYRO_STOP_TIME;
 			val = ICM426XX_GYRO_MODE(ICM426XX_MODE_LOW_NOISE);
 		} else {
-			sleep = 150;
+			delay = ICM426XX_GYRO_STOP_TIME;
 			val = ICM426XX_GYRO_MODE(ICM426XX_MODE_OFF);
 		}
 		break;
 	default:
-		return -EC_ERROR_INVAL;
+		return EC_ERROR_INVAL;
+	}
+
+	/* check stop delay and sleep if required */
+	if (enable) {
+		rem = icm_get_sensor_stabilized(s, __hw_clock_source_read());
+		/* rem > stop_delay means counter rollover */
+		if (rem > 0 && rem <= stop_delay)
+			usleep(rem);
 	}
 
 	mutex_lock(s->mutex);
 
 	ret = icm_field_update8(s, ICM426XX_REG_PWR_MGMT0, mask, val);
-	/* when turning sensor on block any register write for 200 us */
-	if (ret == EC_SUCCESS && enable)
-		usleep(200);
+	if (ret == EC_SUCCESS) {
+		icm_set_stabilize_ts(s, delay);
+		/* when turning sensor on block any register write for 200 us */
+		if (enable)
+			usleep(200);
+	}
 
 	mutex_unlock(s->mutex);
 
-	if (ret != EC_SUCCESS)
-		return ret;
-
-	if (sleep)
-		msleep(sleep);
-
-	return EC_SUCCESS;
+	return ret;
 }
 
 static int icm426xx_set_data_rate(const struct motion_sensor_t *s, int rate,
@@ -741,9 +771,13 @@ static int icm426xx_read(const struct motion_sensor_t *s, intv3_t v)
 		return EC_ERROR_INVAL;
 	}
 
-	/* read data registers */
+	/* read data registers if sensor is stabilized */
 	mutex_lock(s->mutex);
-	ret = icm_read_n(s, reg, raw, sizeof(raw));
+
+	ret = icm426xx_check_sensor_stabilized(s, __hw_clock_source_read());
+	if (ret == EC_SUCCESS)
+		ret = icm_read_n(s, reg, raw, sizeof(raw));
+
 	mutex_unlock(s->mutex);
 	if (ret != EC_SUCCESS)
 		return ret;
diff --git a/driver/accelgyro_icm426xx.h b/driver/accelgyro_icm426xx.h
index 90f75184ec..0c1949e3f0 100644
--- a/driver/accelgyro_icm426xx.h
+++ b/driver/accelgyro_icm426xx.h
@@ -31,6 +31,14 @@
 #define ICM426XX_GYRO_FS_MIN_VAL	125
 #define ICM426XX_GYRO_FS_MAX_VAL	2000
 
+/* accel stabilization time in us */
+#define ICM426XX_ACCEL_START_TIME	20000
+#define ICM426XX_ACCEL_STOP_TIME	0
+
+/* gyro stabilization time in us */
+#define ICM426XX_GYRO_START_TIME	60000
+#define ICM426XX_GYRO_STOP_TIME		150000
+
 /* Reg value from Accel FS in G */
 #define ICM426XX_ACCEL_FS_TO_REG(_fs)	((_fs) < 2 ? 3 : \
 					(_fs) > 16 ? 0 : \
diff --git a/driver/accelgyro_icm_common.h b/driver/accelgyro_icm_common.h
index aa48864102..5f2db67df1 100644
--- a/driver/accelgyro_icm_common.h
+++ b/driver/accelgyro_icm_common.h
@@ -9,6 +9,8 @@
 #define __CROS_EC_ACCELGYRO_ICM_COMMON_H
 
 #include "accelgyro.h"
+#include "hwtimer.h"
+#include "timer.h"
 
 #ifdef CONFIG_ACCEL_FIFO
 /* reserve maximum 4 samples of 16 bytes */
@@ -21,6 +23,7 @@ struct icm_drv_data_t {
 	struct accelgyro_saved_data_t saved_data[2];
 	struct motion_sensor_t *accel;
 	struct motion_sensor_t *gyro;
+	uint32_t stabilize_ts[2];
 	uint8_t bank;
 	uint8_t fifo_en;
 	uint8_t fifo_buffer[ICM_FIFO_BUFFER] __aligned(sizeof(long));
@@ -92,4 +95,35 @@ int icm_get_data_rate(const struct motion_sensor_t *s);
 ssize_t icm_fifo_decode_packet(const void *packet, const uint8_t **accel,
 		const uint8_t **gyro);
 
+static inline void icm_set_stabilize_ts(const struct motion_sensor_t *s,
+					uint32_t delay)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	uint32_t stabilize_ts;
+
+	stabilize_ts = __hw_clock_source_read() + delay;
+	/* prevent 0 value used for disabling time checking */
+	st->stabilize_ts[s->type] = stabilize_ts | 1;
+}
+
+static inline void icm_reset_stabilize_ts(const struct motion_sensor_t *s)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+
+	st->stabilize_ts[s->type] = 0;
+}
+
+static inline
+int32_t icm_get_sensor_stabilized(const struct motion_sensor_t *s,
+				  uint32_t ts)
+{
+	struct icm_drv_data_t *st = ICM_GET_DATA(s);
+	uint32_t stabilize_ts = st->stabilize_ts[s->type];
+
+	if (stabilize_ts == 0)
+		return 0;
+
+	return time_until(ts, stabilize_ts);
+}
+
 #endif	/* __CROS_EC_ACCELGYRO_ICM_COMMON_H */