motion sense: Calculate loop time based on sensor needs

Currently the motion sense loop bases its sleep time based on the
fastest active sensor. This method has several flaws:

1. It does not take into account any task switching overhead
2. With a mix of interrupt driven and forced sensors the sleep time gets
   recalculated every time there is an interrupt causing the loop to
   oversleep
3. If multiple sensors do not have rates that are in sync the timing of
   the slower sensor will be off. For example if there was a sensor running
   at 50 Hz and one running at 20 Hz the slower sensor would end up being
   sampled at about 16 Hz instead of 20 Hz

This change calculates an ideal read time for every forced mode sensor
and calculates the sleep time based on the nearest read time. Every time
a sensor is read the next read time is calculated based on the ideal read
time not the actual read time so that reading does not drift because of
system load or other overhead.

Conflitcs:
  - remove code in test section added with lid angle calculation
  - common/motion_sense.c: use older code for processing flush.

BUG=b:129159505
TEST=Ran sensor CTS tests on arcada, without this change the
     magnetometer was failing 50 Hz tests at about 38 Hz with 30% jitter
     with this change in place 50 Hz was spot on with about 10% jitter
BRANCH=none

Change-Id: Ia4fccb083713b490518d45e7398eb3be3b957eae
Signed-off-by: Mathew King <mathewk@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/1574786
Reviewed-by: Jett Rink <jettrink@chromium.org>
(cherry picked from commit 0c71c4748699f5f2cb1423ffc07d4c852d04b3fc)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/3128750
Reviewed-by: Zhuohao Lee <zhuohao@chromium.org>
Commit-Queue: I-Fan Wang <ifanw@google.com>

5 files changed, 101 insertions, 85 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 1023409f99..64fc5f6bf8 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -45,13 +45,8 @@ const intv3_t orientation_modes[] = {
 };
 #endif
 
-/*
- * Sampling interval for measuring acceleration and calculating lid angle.
- */
-test_export_static unsigned int motion_interval;
-
 /* Delay between FIFO interruption. */
-static unsigned int motion_int_interval;
+static unsigned int ap_event_interval;
 
 /* Minimum time in between running motion sense task loop. */
 unsigned int motion_min_interval = CONFIG_MOTION_MIN_SENSE_WAIT_TIME * MSEC;
@@ -208,22 +203,19 @@ static inline int motion_sensor_in_forced_mode(
 #endif
 }
 
-
-
 /* Minimal amount of time since last collection before triggering a new one */
 static inline int motion_sensor_time_to_read(const timestamp_t *ts,
 		const struct motion_sensor_t *sensor)
 {
-	int rate_mhz = sensor->drv->get_data_rate(sensor);
-
-	if (rate_mhz == 0)
+	if (sensor->collection_rate == 0)
 		return 0;
+
 	/*
-	 * converting from mHz to us.
-	 * If within 95% of the time, check sensor.
+	 * If the time is within the min motion interval (3 ms) go ahead and
+	 * read from the sensor
 	 */
 	return time_after(ts->le.lo,
-			  sensor->last_collection + SECOND * 950 / rate_mhz);
+			  sensor->next_collection - motion_min_interval);
 }
 
 static enum sensor_config motion_sense_get_ec_config(void)
@@ -296,7 +288,9 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 	 * Reset last collection: the last collection may be so much in the past
 	 * it may appear to be in the future.
 	 */
-	sensor->last_collection = ts.le.lo;
+	odr = sensor->drv->get_data_rate(sensor);
+	sensor->collection_rate = odr > 0 ? SECOND * 1000 / odr : 0;
+	sensor->next_collection = ts.le.lo + sensor->collection_rate;
 	sensor->oversampling = 0;
 	mutex_unlock(&g_sensor_mutex);
 	return 0;
@@ -402,9 +396,9 @@ static int motion_sense_ec_rate(struct motion_sensor_t *sensor)
  *
  * Note: Not static to be tested.
  */
-static int motion_sense_set_motion_intervals(void)
+static void motion_sense_set_motion_intervals(void)
 {
-	int i, sensor_ec_rate, ec_rate = 0, ec_int_rate = 0;
+	int i, sensor_ec_rate, ec_int_rate = 0;
 	struct motion_sensor_t *sensor;
 	for (i = 0; i < motion_sensor_count; ++i) {
 		sensor = &motion_sensors[i];
@@ -415,28 +409,20 @@ static int motion_sense_set_motion_intervals(void)
 		    (sensor->drv->get_data_rate(sensor) == 0))
 			continue;
 
-		sensor_ec_rate = motion_sense_ec_rate(sensor);
-		if (sensor_ec_rate == 0)
-			continue;
-		if (ec_rate == 0 || sensor_ec_rate < ec_rate)
-			ec_rate = sensor_ec_rate;
-
 		sensor_ec_rate = motion_sense_select_ec_rate(
 				sensor, SENSOR_CONFIG_AP, 1);
 		if (ec_int_rate == 0 ||
 		    (sensor_ec_rate && sensor_ec_rate < ec_int_rate))
 			ec_int_rate = sensor_ec_rate;
 	}
-	motion_interval = ec_rate;
 
-	motion_int_interval =
+	ap_event_interval =
 		MAX(0, ec_int_rate - MOTION_SENSOR_INT_ADJUSTMENT_US);
 	/*
 	 * Wake up the motion sense task: we want to sensor task to take
 	 * in account the new period right away.
 	 */
 	task_wake(TASK_ID_MOTIONSENSE);
-	return motion_interval;
 }
 
 static inline int motion_sense_init(struct motion_sensor_t *sensor)
@@ -711,6 +697,28 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
 	return sensor->drv->read(sensor, sensor->raw_xyz);
 }
 
+
+static inline void increment_sensor_collection(struct motion_sensor_t *sensor,
+					       const timestamp_t *ts)
+{
+	sensor->next_collection += sensor->collection_rate;
+
+	while (time_after(ts->le.lo, sensor->next_collection)) {
+		/*
+		 * If we get here it means that we completely missed a sensor
+		 * collection time and we attempt to recover by incrementing
+		 * the next collection time until we recover. This should not
+		 * happen and if it does it means that the ec cannot handle the
+		 * requested data rate.
+		 */
+
+		CPRINTS("%s Missed data collection at %u - rate: %d",
+			sensor->name, sensor->next_collection,
+			sensor->collection_rate);
+		sensor->next_collection += sensor->collection_rate;
+	}
+}
+
 static int motion_sense_process(struct motion_sensor_t *sensor,
 				uint32_t *event,
 				const timestamp_t *ts)
@@ -743,7 +751,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 				motion_sense_fifo_add_data(&vector, sensor, 3,
 						   __hw_clock_source_read());
 			}
-			sensor->last_collection = ts->le.lo;
+			increment_sensor_collection(sensor, ts);
 		} else {
 			ret = EC_ERROR_BUSY;
 		}
@@ -761,7 +769,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		if (motion_sensor_time_to_read(ts, sensor)) {
 			/* Get latest data for local calculation */
 			ret = motion_sense_read(sensor);
-			sensor->last_collection = ts->le.lo;
+			increment_sensor_collection(sensor, ts);
 		} else {
 			ret = EC_ERROR_BUSY;
 		}
@@ -897,6 +905,7 @@ void motion_sense_task(void *u)
 {
 	int i, ret, wait_us;
 	timestamp_t ts_begin_task, ts_end_task;
+	int32_t time_diff;
 	uint32_t event = 0;
 	uint16_t ready_status;
 	struct motion_sensor_t *sensor;
@@ -970,7 +979,6 @@ void motion_sense_task(void *u)
 		update_sense_data(lpc_status, &sample_id);
 #endif
 
-		ts_end_task = get_time();
 #ifdef CONFIG_ACCEL_FIFO
 		/*
 		 * Ask the host to flush the queue if
@@ -981,14 +989,14 @@ void motion_sense_task(void *u)
 		if (fifo_flush_needed || wake_up_needed ||
 		    event & TASK_EVENT_MOTION_ODR_CHANGE ||
 		    queue_space(&motion_sense_fifo) < CONFIG_ACCEL_FIFO_THRES ||
-		    (motion_int_interval > 0 &&
-		     time_after(ts_end_task.le.lo,
-				ts_last_int.le.lo + motion_int_interval))) {
+		    (ap_event_interval > 0 &&
+		     time_after(ts_begin_task.le.lo,
+				ts_last_int.le.lo + ap_event_interval))) {
 			if (!fifo_flush_needed)
 				motion_sense_insert_timestamp(
 					__hw_clock_source_read());
 			fifo_flush_needed = 0;
-			ts_last_int = ts_end_task;
+			ts_last_int = ts_begin_task;
 			/*
 			 * Count the number of event the AP is allowed to
 			 * collect.
@@ -1012,25 +1020,36 @@ void motion_sense_task(void *u)
 #endif
 		}
 #endif
-		if (motion_interval > 0) {
-			/*
-			 * Delay appropriately to keep sampling time
-			 * consistent.
-			 */
-			wait_us = motion_interval -
-				(ts_end_task.val - ts_begin_task.val);
 
-			/* and it cannnot be negative */
-			wait_us = MAX(wait_us, 0);
+		ts_end_task = get_time();
+		wait_us = -1;
+
+		for (i = 0; i < motion_sensor_count; i++) {
+			struct motion_sensor_t *sensor = &motion_sensors[i];
+
+			if (!motion_sensor_in_forced_mode(sensor) ||
+			   sensor->collection_rate == 0)
+				continue;
+
+			time_diff = time_until(ts_end_task.le.lo,
+					       sensor->next_collection);
 
+			/* We missed our collection time so wake soon */
+			if (time_diff <= 0) {
+				wait_us = 0;
+				break;
+			}
+
+			if (wait_us == -1 || wait_us > time_diff)
+				wait_us = time_diff;
+		}
+
+		if (wait_us >= 0 && wait_us < motion_min_interval) {
 			/*
-			 * Guarantee some minimum delay to allow other lower
-			 * priority tasks to run.
-			 */
-			if (wait_us < motion_min_interval)
-				wait_us = motion_min_interval;
-		} else {
-			wait_us = -1;
+			* Guarantee some minimum delay to allow other lower
+			* priority tasks to run.
+			*/
+			wait_us = motion_min_interval;
 		}
 
 		event = task_wait_event(wait_us);
@@ -1633,8 +1652,7 @@ static int command_accel_data_rate(int argc, char **argv)
 			 sensor->drv->get_data_rate(sensor));
 		ccprintf("EC rate for sensor %d: %d\n", id,
 			 motion_sense_ec_rate(sensor));
-		ccprintf("Current EC rate: %d\n", motion_interval);
-		ccprintf("Current Interrupt rate: %d\n", motion_int_interval);
+		ccprintf("Current Interrupt rate: %d\n", ap_event_interval);
 	}
 
 	return EC_SUCCESS;
@@ -1710,7 +1728,6 @@ DECLARE_CONSOLE_COMMAND(accelinit, command_accel_init,
 #ifdef CONFIG_CMD_ACCEL_INFO
 static int command_display_accel_info(int argc, char **argv)
 {
-	char *e;
 	int val;
 
 	if (argc > 3)
@@ -1724,21 +1741,6 @@ static int command_display_accel_info(int argc, char **argv)
 		accel_disp = val;
 	}
 
-	/*
-	 * Second arg changes the accel task time interval. Note accel
-	 * sampling interval will be clobbered when chipset suspends or
-	 * resumes.
-	 */
-	if (argc > 2) {
-		val = strtoi(argv[2], &e, 0);
-		if (*e)
-			return EC_ERROR_PARAM2;
-
-		motion_interval = val * MSEC;
-		task_wake(TASK_ID_MOTIONSENSE);
-
-	}
-
 	return EC_SUCCESS;
 }
 DECLARE_CONSOLE_COMMAND(accelinfo, command_display_accel_info,
diff --git a/driver/als_si114x.c b/driver/als_si114x.c
index 17f178080f..be3f090db3 100644
--- a/driver/als_si114x.c
+++ b/driver/als_si114x.c
@@ -295,6 +295,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 	case SI114X_NOT_READY:
 		ret = EC_ERROR_NOT_POWERED;
 	}
+#if 0 /* This code is incorrect https://crbug.com/956569 */
 	if (ret == EC_ERROR_ACCESS_DENIED &&
 	    s->type == MOTIONSENSE_TYPE_LIGHT) {
 		timestamp_t ts_now = get_time();
@@ -315,6 +316,7 @@ static int read(const struct motion_sensor_t *s, intv3_t v)
 			init(s);
 		}
 	}
+#endif
 	return ret;
 }
 
diff --git a/include/motion_sense.h b/include/motion_sense.h
index b7290e7bb4..6584ca05a6 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -146,13 +146,17 @@ struct motion_sensor_t {
 	uint16_t lost;
 
 	/*
-	 * Time since last collection:
-	 * For sensor with hardware FIFO,  time since last sample
-	 * has move from the hardware FIFO to the FIFO (used if fifo rate != 0).
-	 * For sensor without FIFO, time since the last event was collect
-	 * from sensor registers.
+	 * For sensors in forced mode the ideal time to collect the next
+	 * measurement.
+	 *
+	 * This is unused with sensors that interrupt the ec like hw fifo chips.
+	 */
+	uint32_t next_collection;
+
+	/*
+	 * The time in us between collection measurements
 	 */
-	uint32_t last_collection;
+	uint32_t collection_rate;
 
 	/* Minimum supported sampling frequency in miliHertz for this sensor */
 	uint32_t min_frequency;
diff --git a/include/timer.h b/include/timer.h
index fb7800ce2e..35e63686d2 100644
--- a/include/timer.h
+++ b/include/timer.h
@@ -133,6 +133,23 @@ void force_time(timestamp_t ts);
 void timer_print_info(void);
 
 /**
+ * Returns a free running millisecond clock counter, which matches tpm2
+ * library expectations.
+ */
+clock_t clock(void);
+
+/**
+ * Compute how far to_time is from from_time with rollover taken into account
+ *
+ * Return us until to_time given from_time, if negative then to_time has
+ * passeed from_time.
+ */
+static inline int time_until(uint32_t from_time, uint32_t to_time)
+{
+	return (int32_t)(to_time - from_time);
+}
+
+/**
  * Returns the number of microseconds that have elapsed from a start time.
  *
  * This function is for timing short delays typically of a few milliseconds
@@ -142,27 +159,21 @@ void timer_print_info(void);
  * hour. After that, the value returned will wrap.
  *
  * @param start		Start time to compare against
- * @return number of microseconds that have elspsed since that start time
+ * @return number of microseconds that have elapsed since that start time
  */
 static inline unsigned time_since32(timestamp_t start)
 {
-	return get_time().le.lo - start.le.lo;
+	return time_until(start.le.lo, get_time().le.lo);
 }
 
 /**
- * Returns a free running millisecond clock counter, which matches tpm2
- * library expectations.
- */
-clock_t clock(void);
-
-/**
  * To compare time and deal with rollover
  *
  * Return true if a is after b.
  */
 static inline int time_after(uint32_t a, uint32_t b)
 {
-	return (int32_t)(b - a) < 0;
+	return time_until(a, b) < 0;
 }
 
 #endif  /* __CROS_EC_TIMER_H */
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 3282812e86..1436c489b1 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -21,7 +21,6 @@
 #include "util.h"
 
 extern enum chipset_state_mask sensor_active;
-extern unsigned motion_interval;
 
 /*
  * Period in us for the motion task period.
@@ -179,7 +178,6 @@ static int test_lid_angle(void)
 	hook_notify(HOOK_CHIPSET_SHUTDOWN);
 	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S5);
 	TEST_ASSERT(accel_get_data_rate(lid) == 0);
-	TEST_ASSERT(motion_interval == 0);
 
 	/* Go to S0 state */
 	hook_notify(HOOK_CHIPSET_SUSPEND);
@@ -187,7 +185,6 @@ static int test_lid_angle(void)
 	msleep(1000);
 	TEST_ASSERT(sensor_active == SENSOR_ACTIVE_S0);
 	TEST_ASSERT(accel_get_data_rate(lid) == (119000 | ROUND_UP_FLAG));
-	TEST_ASSERT(motion_interval == TEST_LID_EC_RATE);
 
 	/*
 	 * Set the base accelerometer as if it were sitting flat on a desk