BACKPORT: common: motion: Fix forced mode computation

When the sensor is defined to be used in forced mode, ec rate was not
calculated properly: if the AP rate was rounded up, ec_rate requested by
the AP would always be 0. If the EC rate is 0, the sensor may potientally
never be queried.

Also, when the sensor was disable for a long time, the last timestamp of
collection may appear to be in the future, so collection was not
initiated. (long time more than 35 minutes, less than 71 minutes).

We still see instance where the sensor seems locked up.
accelinit would not help because the state machine was not reseted, fix
that.

BRANCH=smaug, cyan
BUG=chrome-os-partner:45627, b:27849483
TEST=With accelerate 3/4, check the value is now correct.
Check proximity sensor is not stuck 45 minutes after last collection.

Change-Id: Ia6805b75f67b048cb0b42c0f91a73dfaf94a254f
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/305823
Reviewed-by: Alec Berg <alecaberg@chromium.org>
(cherry picked from commit 917effebbf4f64607a94b10490c143817bf10801)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/359388

1 files changed, 19 insertions, 14 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 6aeb04bc10..2095882a6f 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -96,7 +96,7 @@ void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
 
 	/* For valid sensors, check if AP really needs this data */
 	if (valid_data) {
-		/* Use Hz, conversion to FP will overflow with mHz */
+		/* Use Hz, conversion to FP will overflow with kHz */
 		fp_t ap_odr =
 			fp_div(BASE_ODR(sensor->config[SENSOR_CONFIG_AP].odr),
 			       1000);
@@ -188,7 +188,7 @@ static inline int motion_sensor_time_to_read(const timestamp_t *ts,
 	if (rate == 0)
 		return 0;
 	/*
-	 * converting from mHz to us, need 1e9,
+	 * converting from kHz to us, need 1e9,
 	 * If within 95% of the time, check sensor.
 	 */
 	return time_after(ts->le.lo,
@@ -219,6 +219,7 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 {
 	int roundup = 0, ec_odr = 0, odr = 0;
 	enum sensor_config config_id;
+	timestamp_t ts = get_time();
 
 	/* We assume the sensor is initialized */
 
@@ -238,21 +239,27 @@ int motion_sense_set_data_rate(struct motion_sensor_t *sensor)
 		sensor->name, odr, roundup, config_id,
 		BASE_ODR(sensor->config[SENSOR_CONFIG_AP].odr));
 	sensor->oversampling = 0;
+	/*
+	 * 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;
 	return sensor->drv->set_data_rate(sensor, odr, roundup);
 }
 
-static inline int motion_sense_select_ec_rate(
+static int motion_sense_select_ec_rate(
 		const struct motion_sensor_t *sensor,
 		enum sensor_config config_id)
 {
 #ifdef CONFIG_ACCEL_FORCE_MODE_MASK
-	if (CONFIG_ACCEL_FORCE_MODE_MASK & (1 << (sensor - motion_sensors)))
+	if (CONFIG_ACCEL_FORCE_MODE_MASK & (1 << (sensor - motion_sensors))) {
+		int rate = BASE_ODR(sensor->config[config_id].odr);
 		/* we have to run ec at the sensor frequency rate.*/
-		if (sensor->config[config_id].odr > 0)
-			return 1000000 / sensor->config[config_id].odr;
+		if (rate > 0)
+			return 1000000 / rate;
 		else
 			return 0;
-	else
+	} else
 #endif
 		return sensor->config[config_id].ec_rate;
 }
@@ -275,9 +282,9 @@ static int motion_sense_ec_rate(struct motion_sensor_t *sensor)
 	ec_rate_from_cfg = motion_sense_select_ec_rate(
 			sensor, motion_sense_get_ec_config());
 
-	if ((ec_rate == 0 && ec_rate_from_cfg != 0) ||
-	    (ec_rate_from_cfg != 0 && ec_rate_from_cfg < ec_rate))
-		ec_rate = ec_rate_from_cfg;
+	if (ec_rate_from_cfg != 0)
+		if (ec_rate == 0 || ec_rate_from_cfg < ec_rate)
+			ec_rate = ec_rate_from_cfg;
 	return ec_rate * MSEC;
 }
 
@@ -308,7 +315,8 @@ int motion_sense_set_motion_intervals(void)
 		if (ec_rate == 0 || sensor_ec_rate < ec_rate)
 			ec_rate = sensor_ec_rate;
 
-		sensor_ec_rate = sensor->config[SENSOR_CONFIG_AP].ec_rate;
+		sensor_ec_rate = motion_sense_select_ec_rate(
+				sensor, SENSOR_CONFIG_AP);
 		if (ec_int_rate_ms == 0 ||
 		    (sensor_ec_rate && sensor_ec_rate < ec_int_rate_ms))
 			ec_int_rate_ms = sensor_ec_rate;
@@ -341,9 +349,7 @@ static inline int motion_sense_init(struct motion_sensor_t *sensor)
 	if (ret != EC_SUCCESS) {
 		sensor->state = SENSOR_INIT_ERROR;
 	} else {
-		timestamp_t ts = get_time();
 		sensor->state = SENSOR_INITIALIZED;
-		sensor->last_collection = ts.le.lo;
 		motion_sense_set_data_rate(sensor);
 	}
 	return ret;
@@ -637,7 +643,6 @@ void motion_sense_task(void)
 					continue;
 				}
 
-				ts_begin_task = get_time();
 				ret = motion_sense_process(sensor, &event,
 						&ts_begin_task);
 				if (ret != EC_SUCCESS)