UPSTREAM: motion: Add sample frequency per sensor

Store at which frequency each sensor should be sampled.
This frequency is different from the sensor frequency:
- sensor frequency: frequency at which the sensor produce information.
- sensor sampling frequency: frequency at the which the EC gater
information.
If 2 sensors must be sampled at very different frequency,
we don't want to oversample the slow one, and filling the
software FIFO unnecessarily.

BRANCH=smaug
TEST=Unit test. Check that frequency is correct when sensor
frequencies change from IIO driver.
BUG=chrome-os-partner:39900

Change-Id: I4272963413f53d4ca004e26639dc7a2affd317eb
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/284616
Reviewed-by: Alec Berg <alecaberg@chromium.org>
(cherry picked from commit c0f78b4c0aca20203fefbc96c7e52c709455c06b)
Signed-off-by: Gwendal Grignou <gwendal@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/370502

5 files changed, 222 insertions, 137 deletions

diff --git a/board/samus/board.c b/board/samus/board.c
index e32860c185..d2763557fd 100644
--- a/board/samus/board.c
+++ b/board/samus/board.c
@@ -299,7 +299,8 @@ struct motion_sensor_t motion_sensors[] = {
 	 .rot_standard_ref = &base_standard_ref,
 	 .default_config = {
 		 .odr = 119000,
-		 .range = 2
+		 .range = 2,
+		 .ec_rate = SUSPEND_SAMPLING_INTERVAL,
 	 }
 	},
 
@@ -315,7 +316,8 @@ struct motion_sensor_t motion_sensors[] = {
 	 .rot_standard_ref = &lid_standard_ref,
 	 .default_config = {
 		 .odr = 100000,
-		 .range = 2
+		 .range = 2,
+		 .ec_rate = SUSPEND_SAMPLING_INTERVAL,
 	 }
 	},
 
@@ -331,7 +333,8 @@ struct motion_sensor_t motion_sensors[] = {
 	 .rot_standard_ref = NULL,
 	 .default_config = {
 		 .odr = 119000,
-		 .range = 2000
+		 .range = 2000,
+		 .ec_rate = SUSPEND_SAMPLING_INTERVAL,
 	 }
 	},
 
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 8e82dd0fe9..b53fe1b1cd 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -30,48 +30,24 @@
 #define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)
 #define CPRINTF(format, args...) cprintf(CC_MOTION_SENSE, format, ## args)
 
-/* Minimum time in between running motion sense task loop. */
-#define MIN_MOTION_SENSE_WAIT_TIME (1 * MSEC)
-
-/* Time to wait in between failed attempts to initialize sensors */
-#define TASK_MOTION_SENSE_WAIT_TIME (500 * MSEC)
-
-
-/* Bounds for setting the sensor polling interval. */
-#define MIN_POLLING_INTERVAL_MS 1
-
-/* Define sensor sampling interval in suspend. */
-#ifdef CONFIG_GESTURE_DETECTION
-#define SUSPEND_SAMPLING_INTERVAL (CONFIG_GESTURE_SAMPLING_INTERVAL_MS * MSEC)
-#elif defined(CONFIG_ACCEL_FIFO)
-#define SUSPEND_SAMPLING_INTERVAL (1000 * MSEC)
-#else
-#define SUSPEND_SAMPLING_INTERVAL (100 * MSEC)
-#endif
-
-/* Accelerometer polling intervals based on chipset state. */
-#ifdef CONFIG_ACCEL_FIFO
-/*
- * TODO(crbug.com/498352):
- * For now, we collect the data every accel_interval.
- * For non-FIFO sensor, we should set accel_interval at a lower value.
- * But if we have a mix a FIFO and non FIFO sensors, setting it
- * to low will increase the number of interrupts.
- */
-static int accel_interval_ap_on = 1000 * MSEC;
-#else
-static int accel_interval_ap_on = 10 * MSEC;
-#endif
 /*
  * Sampling interval for measuring acceleration and calculating lid angle.
- * Set to accel_interval_ap_on when ap is on.
  */
-static int accel_interval;
+unsigned accel_interval;
 
 #ifdef CONFIG_CMD_ACCEL_INFO
 static int accel_disp;
 #endif
 
+#define SENSOR_EC_RATE(_sensor) \
+	((_sensor)->active == SENSOR_ACTIVE_S0 ? \
+	 (_sensor)->runtime_config.ec_rate : \
+	 (_sensor)->default_config.ec_rate)
+
+/* Minimal amount of time since last collection before triggering a new one */
+#define SENSOR_EC_RATE_THRES(_sensor) \
+	(SENSOR_EC_RATE(_sensor) * 9 / 10)
+
 /*
  * Mutex to protect sensor values between host command task and
  * motion sense task:
@@ -132,6 +108,54 @@ static void motion_sense_get_fifo_info(
 }
 #endif
 
+/*
+ * motion_sense_set_accel_interval
+ *
+ * Set the wake up interval for the motion sense thread.
+ * It is set to the highest frequency one of the sensors need to be polled at.
+ *
+ * driving_sensor: In S0, indicates which sensor has its EC sampling rate
+ * changed. In S3, it is hard coded, so in S3 driving_sensor should be NULL.
+ *
+ * Note: Not static to be tested.
+ */
+int motion_sense_set_accel_interval(
+		struct motion_sensor_t *driving_sensor,
+		unsigned data)
+{
+	int i;
+	struct motion_sensor_t *sensor;
+	if (driving_sensor)
+		driving_sensor->runtime_config.ec_rate = data;
+
+	for (i = 0; i < motion_sensor_count; ++i) {
+		sensor = &motion_sensors[i];
+		if (sensor == driving_sensor)
+			continue;
+		if (SENSOR_EC_RATE(sensor) < data)
+			data = SENSOR_EC_RATE(sensor);
+	}
+	accel_interval = data;
+	return data;
+}
+
+static void motion_sense_startup(void)
+{
+	int i;
+	struct motion_sensor_t *sensor;
+	for (i = 0; i < motion_sensor_count; ++i) {
+		sensor = &motion_sensors[i];
+		sensor->state = SENSOR_NOT_INITIALIZED;
+		sensor->active = SENSOR_ACTIVE_S5;
+
+		memcpy(&sensor->runtime_config, &sensor->default_config,
+		       sizeof(sensor->runtime_config));
+	}
+	motion_sense_set_accel_interval(NULL, MAX_MOTION_SENSE_WAIT_TIME);
+}
+DECLARE_HOOK(HOOK_INIT, motion_sense_startup,
+	     MOTION_SENSE_HOOK_PRIO);
+
 static void motion_sense_shutdown(void)
 {
 	int i;
@@ -140,14 +164,15 @@ static void motion_sense_shutdown(void)
 	for (i = 0; i < motion_sensor_count; i++) {
 		sensor = &motion_sensors[i];
 		sensor->active = SENSOR_ACTIVE_S5;
-		sensor->runtime_config.odr    = sensor->default_config.odr;
-		sensor->runtime_config.range  = sensor->default_config.range;
+		memcpy(&sensor->runtime_config, &sensor->default_config,
+		       sizeof(sensor->runtime_config));
 		if ((sensor->state == SENSOR_INITIALIZED) &&
 		   !(sensor->active_mask & sensor->active)) {
 			sensor->drv->set_data_rate(sensor, 0, 0);
 			sensor->state = SENSOR_NOT_INITIALIZED;
 		}
 	}
+	motion_sense_set_accel_interval(NULL, MAX_MOTION_SENSE_WAIT_TIME);
 }
 DECLARE_HOOK(HOOK_CHIPSET_SHUTDOWN, motion_sense_shutdown,
 	     MOTION_SENSE_HOOK_PRIO);
@@ -157,14 +182,12 @@ static void motion_sense_suspend(void)
 	int i;
 	struct motion_sensor_t *sensor;
 
-	accel_interval = SUSPEND_SAMPLING_INTERVAL;
-
 	for (i = 0; i < motion_sensor_count; i++) {
 		sensor = &motion_sensors[i];
 
-		/* if it is in s5, don't enter suspend */
+		/* if we are comming from S5, don't enter suspend */
 		if (sensor->active == SENSOR_ACTIVE_S5)
-			continue;
+			return;
 
 		sensor->active = SENSOR_ACTIVE_S3;
 
@@ -175,6 +198,7 @@ static void motion_sense_suspend(void)
 			sensor->state = SENSOR_NOT_INITIALIZED;
 		}
 	}
+	motion_sense_set_accel_interval(NULL, MAX_MOTION_SENSE_WAIT_TIME);
 }
 DECLARE_HOOK(HOOK_CHIPSET_SUSPEND, motion_sense_suspend,
 	     MOTION_SENSE_HOOK_PRIO);
@@ -184,8 +208,6 @@ static void motion_sense_resume(void)
 	int i;
 	struct motion_sensor_t *sensor;
 
-	accel_interval = accel_interval_ap_on;
-
 	for (i = 0; i < motion_sensor_count; i++) {
 		sensor = &motion_sensors[i];
 		sensor->active = SENSOR_ACTIVE_S0;
@@ -195,6 +217,7 @@ static void motion_sense_resume(void)
 					sensor->runtime_config.odr, 1);
 		}
 	}
+	motion_sense_set_accel_interval(NULL, MAX_MOTION_SENSE_WAIT_TIME);
 }
 DECLARE_HOOK(HOOK_CHIPSET_RESUME, motion_sense_resume,
 	     MOTION_SENSE_HOOK_PRIO);
@@ -258,10 +281,13 @@ static inline void motion_sense_init(struct motion_sensor_t *sensor)
 		ret = sensor->drv->init(sensor);
 	} while ((ret != EC_SUCCESS) && (--cnt > 0));
 
-	if (ret != EC_SUCCESS)
+	if (ret != EC_SUCCESS) {
 		sensor->state = SENSOR_INIT_ERROR;
-	else
+	} else {
+		timestamp_t ts = get_time();
 		sensor->state = SENSOR_INITIALIZED;
+		sensor->last_collection = ts.val;
+	}
 }
 
 static int motion_sense_read(struct motion_sensor_t *sensor)
@@ -275,6 +301,7 @@ static int motion_sense_read(struct motion_sensor_t *sensor)
 
 static int motion_sense_process(struct motion_sensor_t *sensor,
 				uint32_t event,
+				const timestamp_t *ts,
 				int *flush_needed)
 {
 	int ret = EC_SUCCESS;
@@ -288,13 +315,18 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 	if (sensor->drv->load_fifo != NULL) {
 		/* Load fifo is filling raw_xyz sensor vector */
 		sensor->drv->load_fifo(sensor);
-	} else {
+	} else if (ts->val - sensor->last_collection >=
+		   SENSOR_EC_RATE_THRES(sensor)) {
+		struct ec_response_motion_sensor_data vector;
+		sensor->last_collection = ts->val;
 		ret = motion_sense_read(sensor);
-		/* Put data in fifo.
-		 * Depending on the frequency on that particular sensor,
-		 * we may not do it all the time.
-		 * TODO(chromium:498352)
-		 */
+		vector.flags = 0;
+		vector.data[X] = sensor->raw_xyz[X];
+		vector.data[Y] = sensor->raw_xyz[Y];
+		vector.data[Z] = sensor->raw_xyz[Z];
+		motion_sense_fifo_add_unit(&vector, sensor);
+	} else {
+		ret = EC_ERROR_BUSY;
 	}
 	if (event & TASK_EVENT_MOTION_FLUSH_PENDING) {
 		int flush_pending;
@@ -305,8 +337,14 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 		}
 	}
 #else
-	/* Get latest data for local calculation */
-	ret = motion_sense_read(sensor);
+	if (ts->val - sensor->last_collection >=
+	    SENSOR_EC_RATE_THRES(sensor)) {
+		sensor->last_collection = ts->val;
+		/* Get latest data for local calculation */
+		ret = motion_sense_read(sensor);
+	} else {
+		ret = EC_ERROR_BUSY;
+	}
 #endif
 	return ret;
 }
@@ -321,48 +359,26 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 void motion_sense_task(void)
 {
 	int i, ret, wait_us, fifo_flush_needed = 0;
-	static timestamp_t ts_begin_task, ts_end_task;
+	timestamp_t ts_begin_task, ts_end_task;
 	uint8_t *lpc_status;
 	uint16_t *lpc_data;
-	uint32_t event;
+	uint32_t event = 0;
 	int sample_id = 0;
 	int rd_cnt;
 	struct motion_sensor_t *sensor;
 #ifdef CONFIG_ACCEL_FIFO
-	static timestamp_t ts_last_int;
+	timestamp_t ts_last_int;
 #endif
 
 	lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
 	lpc_data = (uint16_t *)host_get_memmap(EC_MEMMAP_ACC_DATA);
-
-	for (i = 0; i < motion_sensor_count; ++i) {
-		sensor = &motion_sensors[i];
-		sensor->state = SENSOR_NOT_INITIALIZED;
-
-		sensor->runtime_config.odr = sensor->default_config.odr;
-		sensor->runtime_config.range = sensor->default_config.range;
-	}
-
 	set_present(lpc_status);
 
-	if (chipset_in_state(CHIPSET_STATE_ON)) {
-		/* Update the sensor current active state to S0. */
-		for (i = 0; i < motion_sensor_count; ++i) {
-			sensor = &motion_sensors[i];
-			sensor->active = SENSOR_ACTIVE_S0;
-		}
-
-		accel_interval = accel_interval_ap_on;
-	} else {
-		/* sensor->active already initializes to SENSOR_ACTIVE_S5 */
-		accel_interval = SUSPEND_SAMPLING_INTERVAL;
-	}
-
 	wait_us = accel_interval;
 #ifdef CONFIG_ACCEL_FIFO
 	ts_last_int = get_time();
 #endif
-	while ((event = task_wait_event(wait_us))) {
+	do {
 		ts_begin_task = get_time();
 		rd_cnt = 0;
 		for (i = 0; i < motion_sensor_count; ++i) {
@@ -374,7 +390,9 @@ void motion_sense_task(void)
 				if (sensor->state == SENSOR_NOT_INITIALIZED)
 					motion_sense_init(sensor);
 
+				ts_begin_task = get_time();
 				ret = motion_sense_process(sensor, event,
+						&ts_begin_task,
 						&fifo_flush_needed);
 				if (ret != EC_SUCCESS)
 					continue;
@@ -462,8 +480,7 @@ void motion_sense_task(void)
 		 */
 		if (wait_us < MIN_MOTION_SENSE_WAIT_TIME)
 			wait_us = MIN_MOTION_SENSE_WAIT_TIME;
-
-	}
+	} while ((event = task_wait_event(wait_us)));
 }
 
 #ifdef CONFIG_ACCEL_FIFO
@@ -561,21 +578,24 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 		break;
 
 	case MOTIONSENSE_CMD_EC_RATE:
+		sensor = host_sensor_id_to_motion_sensor(
+				in->sensor_odr.sensor_num);
+		if (sensor == NULL)
+			return EC_RES_INVALID_PARAM;
+
 		/*
 		 * Set new sensor sampling rate when AP is on, if the data arg
 		 * has a value.
 		 */
 		if (in->ec_rate.data != EC_MOTION_SENSE_NO_VALUE) {
 			/* Bound the new sampling rate. */
-			data = in->ec_rate.data;
-			if (data < MIN_POLLING_INTERVAL_MS)
-				data = MIN_POLLING_INTERVAL_MS;
-
-			accel_interval_ap_on = data * MSEC;
-			accel_interval = data * MSEC;
+			motion_sense_set_accel_interval(
+					sensor,
+					MAX(in->ec_rate.data * MSEC,
+					    MIN_MOTION_SENSE_WAIT_TIME));
 		}
 
-		out->ec_rate.ret = accel_interval_ap_on / MSEC;
+		out->ec_rate.ret = sensor->runtime_config.ec_rate / MSEC;
 
 		args->response_size = sizeof(out->ec_rate);
 		break;
@@ -597,14 +617,15 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 						in->sensor_odr.data);
 				return EC_RES_INVALID_PARAM;
 			}
+			/*
+			 * To be sure timestamps are calculated properly,
+			 * Send an event to have a timestamp inserted in the
+			 * FIFO.
+			 */
+			task_set_event(TASK_ID_MOTIONSENSE,
+					TASK_EVENT_MOTION_ODR_CHANGE, 0);
 		}
 
-		/* To be sure timestamps are calculated properly,
-		 * Send an event to have a timestamp inserted in the FIFO.
-		 */
-		task_set_event(TASK_ID_MOTIONSENSE,
-			       TASK_EVENT_MOTION_ODR_CHANGE, 0);
-
 		sensor->drv->get_data_rate(sensor, &data);
 
 		/* Save configuration parameter: ODR */
@@ -671,6 +692,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 				in->sensor_odr.sensor_num);
 		if (sensor == NULL)
 			return EC_RES_INVALID_PARAM;
+
 		atomic_add(&sensor->flush_pending, 1);
 
 		task_set_event(TASK_ID_MOTIONSENSE,
diff --git a/include/ec_commands.h b/include/ec_commands.h
index 950b820b78..c6e33f046e 100644
--- a/include/ec_commands.h
+++ b/include/ec_commands.h
@@ -1381,7 +1381,13 @@ enum motionsense_command {
 
 	/*
 	 * EC Rate command is a setter/getter command for the EC sampling rate
-	 * of all motion sensors in milliseconds.
+	 * in milliseconds.
+	 * It is per sensor, the EC run sample task  at the minimum of all
+	 * sensors EC_RATE.
+	 * For sensors without hardware FIFO, EC_RATE should be equals to 1/ODR
+	 * to collect all the sensor samples.
+	 * For sensor with hardware FIFO, EC_RATE is used as the maximal delay
+	 * to process of all motion sensors in milliseconds.
 	 */
 	MOTIONSENSE_CMD_EC_RATE = 2,
 
@@ -1539,16 +1545,14 @@ struct ec_params_motion_sense {
 		} dump;
 
 		/*
-		 * Used for MOTIONSENSE_CMD_EC_RATE and
-		 * MOTIONSENSE_CMD_KB_WAKE_ANGLE.
+		 * Used for MOTIONSENSE_CMD_KB_WAKE_ANGLE.
 		 */
 		struct {
 			/* Data to set or EC_MOTION_SENSE_NO_VALUE to read.
-			 * ec_rate: polling rate in ms.
 			 * kb_wake_angle: angle to wakup AP.
 			 */
 			int16_t data;
-		} ec_rate, kb_wake_angle;
+		} kb_wake_angle;
 
 		/* Used for MOTIONSENSE_CMD_INFO, MOTIONSENSE_CMD_DATA
 		 * and MOTIONSENSE_CMD_PERFORM_CALIB. */
@@ -1557,8 +1561,8 @@ struct ec_params_motion_sense {
 		} info, data, fifo_flush, perform_calib;
 
 		/*
-		 * Used for MOTIONSENSE_CMD_SENSOR_ODR and
-		 * MOTIONSENSE_CMD_SENSOR_RANGE.
+		 * Used for MOTIONSENSE_CMD_EC_RATE, MOTIONSENSE_CMD_SENSOR_ODR
+		 * and MOTIONSENSE_CMD_SENSOR_RANGE.
 		 */
 		struct {
 			uint8_t sensor_num;
@@ -1570,7 +1574,7 @@ struct ec_params_motion_sense {
 
 			/* Data to set or EC_MOTION_SENSE_NO_VALUE to read. */
 			int32_t data;
-		} sensor_odr, sensor_range;
+		} ec_rate, sensor_odr, sensor_range;
 
 		/* Used for MOTIONSENSE_CMD_SENSOR_OFFSET */
 		struct __attribute__((__packed__)) {
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 1aeb569dd0..82a492719c 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -28,11 +28,35 @@ enum sensor_state {
 #define SENSOR_ACTIVE_S0_S3 (SENSOR_ACTIVE_S3 | SENSOR_ACTIVE_S0)
 #define SENSOR_ACTIVE_S0_S3_S5 (SENSOR_ACTIVE_S0_S3 | SENSOR_ACTIVE_S5)
 
+/* Events the motion sense task may have to process.*/
+#define TASK_EVENT_MOTION_FLUSH_PENDING TASK_EVENT_CUSTOM(1)
+#define TASK_EVENT_MOTION_INTERRUPT     TASK_EVENT_CUSTOM(2)
+#define TASK_EVENT_MOTION_ODR_CHANGE    TASK_EVENT_CUSTOM(4)
+
+/* Define sensor sampling interval in suspend. */
+#ifdef CONFIG_GESTURE_DETECTION
+#define SUSPEND_SAMPLING_INTERVAL (CONFIG_GESTURE_SAMPLING_INTERVAL_MS * MSEC)
+#elif defined(CONFIG_ACCEL_FIFO)
+#define SUSPEND_SAMPLING_INTERVAL (1000 * MSEC)
+#else
+#define SUSPEND_SAMPLING_INTERVAL (100 * MSEC)
+#endif
+
+/* Minimum time in between running motion sense task loop. */
+#define MIN_MOTION_SENSE_WAIT_TIME (3 * MSEC)
+#define MAX_MOTION_SENSE_WAIT_TIME (60000 * MSEC)
+
 struct motion_data_t {
 	/* data rate the sensor will measure, in mHz */
 	int odr;
 	/* range of measurement in SI */
 	int range;
+	/* delay between collection by EC, in ms.
+	 * For non FIFO sensor, should be nead 1e6/odr to
+	 * collect events.
+	 * For sensor with FIFO, can be much longer.
+	 */
+	unsigned ec_rate;
 };
 
 struct motion_sensor_t {
@@ -68,11 +92,26 @@ struct motion_sensor_t {
 	 * FIFO info has been transmitted.
 	 */
 	uint16_t lost;
+
+	/*
+	 * Time since iast 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.
+	 */
+	int last_collection;
 };
 
 /* Defined at board level. */
 extern struct motion_sensor_t motion_sensors[];
-extern const unsigned int motion_sensor_count;
+extern const unsigned motion_sensor_count;
+
+/* For testing purposes: export the sampling interval. */
+extern unsigned accel_interval;
+int motion_sense_set_accel_interval(
+		struct motion_sensor_t *driving_sensor,
+		unsigned data);
 
 /*
  * Priority of the motion sense resume/suspend hooks, to be sure associated
@@ -103,10 +142,4 @@ void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
 				const struct motion_sensor_t *sensor);
 
 #endif
-
-/* Events the motion sense task may have to process.*/
-#define TASK_EVENT_MOTION_FLUSH_PENDING TASK_EVENT_CUSTOM(1)
-#define TASK_EVENT_MOTION_INTERRUPT     TASK_EVENT_CUSTOM(2)
-#define TASK_EVENT_MOTION_ODR_CHANGE    TASK_EVENT_CUSTOM(4)
-
 #endif /* __CROS_EC_MOTION_SENSE_H */
diff --git a/test/motion_lid.c b/test/motion_lid.c
index 84a539e226..d06a76135d 100644
--- a/test/motion_lid.c
+++ b/test/motion_lid.c
@@ -19,6 +19,18 @@
 #include "timer.h"
 #include "util.h"
 
+
+/*
+ * Period in us for the motion task period.
+ * The task will read the vectors at that interval
+ */
+#define TEST_LID_EC_RATE (SUSPEND_SAMPLING_INTERVAL / 10)
+
+/*
+ * Time in ms to wait for the task to read the vectors.
+ */
+#define TEST_LID_SLEEP_RATE (TEST_LID_EC_RATE / (5 * MSEC))
+
 /*****************************************************************************/
 /* Mock functions */
 static int accel_init(const struct motion_sensor_t *s)
@@ -109,10 +121,11 @@ struct motion_sensor_t motion_sensors[] = {
 	 .rot_standard_ref = &base_standard_ref,
 	 .default_config = {
 		 .odr = 119000,
-		 .range = 2
+		 .range = 2,
+		 .ec_rate = SUSPEND_SAMPLING_INTERVAL,
 	 }
 	},
-	{.name = "base",
+	{.name = "lid",
 	 .active_mask = SENSOR_ACTIVE_S0,
 	 .chip = MOTIONSENSE_CHIP_KXCJ9,
 	 .type = MOTIONSENSE_TYPE_ACCEL,
@@ -124,7 +137,8 @@ struct motion_sensor_t motion_sensors[] = {
 	 .rot_standard_ref = &lid_standard_ref,
 	 .default_config = {
 		 .odr = 119000,
-		 .range = 2
+		 .range = 2,
+		 .ec_rate = SUSPEND_SAMPLING_INTERVAL,
 	 }
 	},
 };
@@ -132,19 +146,38 @@ const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
 
 /*****************************************************************************/
 /* Test utilities */
-static int test_lid_angle(void)
+static void wait_for_valid_sample(void)
 {
-	uint8_t *lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
 	uint8_t sample;
+	uint8_t *lpc_status = host_get_memmap(EC_MEMMAP_ACC_STATUS);
+
+	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
+	msleep(TEST_LID_EC_RATE/MSEC);
+	task_wake(TASK_ID_MOTIONSENSE);
+	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
+		msleep(TEST_LID_SLEEP_RATE);
+}
+
+static int test_lid_angle(void)
+{
 
 	struct motion_sensor_t *base = &motion_sensors[0];
 	struct motion_sensor_t *lid = &motion_sensors[1];
 
 	/* Go to S3 state */
-	hook_notify(HOOK_CHIPSET_STARTUP);
+	TEST_ASSERT(accel_interval == SUSPEND_SAMPLING_INTERVAL);
+	TEST_ASSERT(motion_sensors[0].active == SENSOR_ACTIVE_S5);
 
 	/* Go to S0 state */
 	hook_notify(HOOK_CHIPSET_RESUME);
+	TEST_ASSERT(accel_interval == SUSPEND_SAMPLING_INTERVAL);
+	TEST_ASSERT(motion_sensors[0].active == SENSOR_ACTIVE_S0);
+
+	motion_sense_set_accel_interval(base, TEST_LID_EC_RATE);
+	TEST_ASSERT(accel_interval == TEST_LID_EC_RATE);
+
+	motion_sense_set_accel_interval(lid, TEST_LID_EC_RATE);
+	TEST_ASSERT(accel_interval == TEST_LID_EC_RATE);
 
 	/*
 	 * Set the base accelerometer as if it were sitting flat on a desk
@@ -156,32 +189,28 @@ static int test_lid_angle(void)
 	lid->xyz[X] = 0;
 	lid->xyz[Y] = 0;
 	lid->xyz[Z] = 1000;
-	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
+	/* Initial wake up, like init does */
 	task_wake(TASK_ID_MOTIONSENSE);
-	msleep(5);
+
+	/* wait for the EC sampling period to expire   */
+	msleep(TEST_LID_EC_RATE/MSEC);
 	task_wake(TASK_ID_MOTIONSENSE);
-	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
-		msleep(5);
+
+	wait_for_valid_sample();
 	TEST_ASSERT(motion_lid_get_angle() == 0);
 
 	/* Set lid open to 90 degrees. */
 	lid->xyz[X] = -1000;
 	lid->xyz[Y] = 0;
 	lid->xyz[Z] = 0;
-	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
-	task_wake(TASK_ID_MOTIONSENSE);
-	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
-		msleep(5);
+	wait_for_valid_sample();
 	TEST_ASSERT(motion_lid_get_angle() == 90);
 
 	/* Set lid open to 225. */
 	lid->xyz[X] = 500;
 	lid->xyz[Y] = 0;
 	lid->xyz[Z] = -500;
-	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
-	task_wake(TASK_ID_MOTIONSENSE);
-	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
-		msleep(5);
+	wait_for_valid_sample();
 	TEST_ASSERT(motion_lid_get_angle() == 225);
 
 	/*
@@ -191,10 +220,7 @@ static int test_lid_angle(void)
 	base->xyz[X] = 0;
 	base->xyz[Y] = 1000;
 	base->xyz[Z] = 0;
-	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
-	task_wake(TASK_ID_MOTIONSENSE);
-	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
-		msleep(5);
+	wait_for_valid_sample();
 	TEST_ASSERT(motion_lid_get_angle() == LID_ANGLE_UNRELIABLE);
 
 	/*
@@ -207,10 +233,7 @@ static int test_lid_angle(void)
 	lid->xyz[X] = -500;
 	lid->xyz[Y] = -400;
 	lid->xyz[Z] = -300;
-	sample = *lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK;
-	task_wake(TASK_ID_MOTIONSENSE);
-	while ((*lpc_status & EC_MEMMAP_ACC_STATUS_SAMPLE_ID_MASK) == sample)
-		msleep(5);
+	wait_for_valid_sample();
 	TEST_ASSERT(motion_lid_get_angle() == 180);
 
 	return EC_SUCCESS;