motion: Lower jitter of Sensor->EC timestamp

Instead getting the time for each sample in the task code, we should be
getting it as soon as the sensor reported it added it to its fifo (so
sensor just finished integration).

Because of that each sensor should provide the time when it provides a
sample, ideally from an accurate spot like an interrupt.

Deprecate motion_sense_fifo_add_unit (without a timestamp) in favour of
motion_sense_fifo_add_data (which adds the timestamps). Update all
relevant sensors to use the new api.

Note: for now I focused on the BMI160, where I actually made it get the
time in the interrupt. The other sensors were made to use the new api,
but still don't record the time in the right place (though it's not any
worse than before).

BUG=b:67743747
TEST=In the kernel, fifo_info->info.timestamp still has sane values.
TEST=CTS should still pass
BRANCH=master

Change-Id: I9829343f8702e00cc19f9c88134fa1f258c9e1e9
Signed-off-by: Alexandru M Stan <amstan@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/807331
Commit-Ready: ChromeOS CL Exonerator Bot <chromiumos-cl-exonerator@appspot.gserviceaccount.com>
Reviewed-by: Gwendal Grignou <gwendal@chromium.org>

5 files changed, 58 insertions, 23 deletions

diff --git a/common/motion_sense.c b/common/motion_sense.c
index 24ac31c964..89a7f870df 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -104,9 +104,12 @@ struct queue motion_sense_fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO,
 		struct ec_response_motion_sensor_data);
 static int motion_sense_fifo_lost;
 
-static void motion_sense_insert_timestamp(void);
-
-void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
+/*
+ * Do not use this function directly if you just want to add sensor data, use
+ * motion_sense_fifo_add_data instead to get a proper timestamp too.
+ */
+static void motion_sense_fifo_add_unit(
+				struct ec_response_motion_sensor_data *data,
 				struct motion_sensor_t *sensor,
 				int valid_data)
 {
@@ -139,11 +142,6 @@ void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
 	}
 	mutex_unlock(&g_sensor_mutex);
 	if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP) {
-		/*
-		 * Fist, send a timestamp to be sure the event will not
-		 * be tied to an old one.
-		 */
-		motion_sense_insert_timestamp();
 		wake_up_needed = 1;
 	}
 #ifdef CONFIG_TABLET_MODE
@@ -166,15 +164,23 @@ static void motion_sense_insert_flush(struct motion_sensor_t *sensor)
 	motion_sense_fifo_add_unit(&vector, sensor, 0);
 }
 
-static void motion_sense_insert_timestamp(void)
+static void motion_sense_insert_timestamp(uint32_t timestamp)
 {
 	struct ec_response_motion_sensor_data vector;
 	vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
-	vector.timestamp = __hw_clock_source_read();
+	vector.timestamp = timestamp;
 	vector.sensor_num = 0;
 	motion_sense_fifo_add_unit(&vector, NULL, 0);
 }
 
+void motion_sense_fifo_add_data(struct ec_response_motion_sensor_data *data,
+				struct motion_sensor_t *sensor,
+				int valid_data,
+				uint32_t time) {
+	motion_sense_insert_timestamp(time);
+	motion_sense_fifo_add_unit(data, sensor, valid_data);
+}
+
 static void motion_sense_get_fifo_info(
 		struct ec_response_motion_sense_fifo_info *fifo_info)
 {
@@ -732,7 +738,8 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
 				vector.data[X] = v[X];
 				vector.data[Y] = v[Y];
 				vector.data[Z] = v[Z];
-				motion_sense_fifo_add_unit(&vector, sensor, 3);
+				motion_sense_fifo_add_data(&vector, sensor, 3,
+						   __hw_clock_source_read());
 			}
 			sensor->last_collection = ts->le.lo;
 		} else {
@@ -812,7 +819,8 @@ static void check_and_queue_gestures(uint32_t *event)
 		vector.activity = MOTIONSENSE_ACTIVITY_DOUBLE_TAP;
 		vector.state = 1; /* triggered */
 		vector.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID;
-		motion_sense_fifo_add_unit(&vector, NULL, 0);
+		motion_sense_fifo_add_data(&vector, NULL, 0,
+					   __hw_clock_source_read());
 #endif
 		/* Call board specific function to process tap */
 		sensor_board_proc_double_tap();
@@ -829,7 +837,8 @@ static void check_and_queue_gestures(uint32_t *event)
 		vector.activity = MOTIONSENSE_ACTIVITY_SIG_MOTION;
 		vector.state = 1; /* triggered */
 		vector.sensor_num = MOTION_SENSE_ACTIVITY_SENSOR_ID;
-		motion_sense_fifo_add_unit(&vector, NULL, 0);
+		motion_sense_fifo_add_data(&vector, NULL, 0,
+					   __hw_clock_source_read());
 #endif
 		/* Disable further detection */
 		activity_sensor = &motion_sensors[CONFIG_GESTURE_SIGMO];
@@ -854,7 +863,8 @@ static void check_and_queue_gestures(uint32_t *event)
 				MOTIONSENSE_ORIENTATION_UNKNOWN)) {
 			SET_ORIENTATION_UPDATED(sensor);
 			vector.state = GET_ORIENTATION(sensor);
-			motion_sense_fifo_add_unit(&vector, NULL, 0);
+			motion_sense_fifo_add_data(&vector, NULL, 0,
+						   __hw_clock_source_read());
 #ifdef CONFIG_DEBUG_ORIENTATION
 			{
 				static const char * const mode_strs[] = {
@@ -973,7 +983,8 @@ void motion_sense_task(void *u)
 		     time_after(ts_end_task.le.lo,
 				ts_last_int.le.lo + motion_int_interval))) {
 			if (!fifo_flush_needed)
-				motion_sense_insert_timestamp();
+				motion_sense_insert_timestamp(
+					__hw_clock_source_read());
 			fifo_flush_needed = 0;
 			ts_last_int = ts_end_task;
 			/*
@@ -1194,7 +1205,7 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
 			 * Send an event to have a timestamp inserted in the
 			 * FIFO.
 			 */
-			motion_sense_insert_timestamp();
+			motion_sense_insert_timestamp(__hw_clock_source_read());
 #endif
 			sensor->config[SENSOR_CONFIG_AP].odr =
 				in->sensor_odr.data |
diff --git a/driver/accel_lis2dh.c b/driver/accel_lis2dh.c
index 6cb2ff87ce..0393d95429 100644
--- a/driver/accel_lis2dh.c
+++ b/driver/accel_lis2dh.c
@@ -11,6 +11,7 @@
 #include "common.h"
 #include "console.h"
 #include "hooks.h"
+#include "hwtimer.h"
 #include "i2c.h"
 #include "math_util.h"
 #include "task.h"
@@ -197,7 +198,12 @@ static int load_fifo(struct motion_sensor_t *s)
 			vect.data[2] = axis[2];
 			vect.flags = 0;
 			vect.sensor_num = 0;
-			motion_sense_fifo_add_unit(&vect, s, 3);
+			motion_sense_fifo_add_data(&vect, s, 3,
+						   __hw_clock_source_read());
+			/*
+			 * TODO: get time at a more accurate spot.
+			 * Like in lis2dh_interrupt
+			 */
 		}
 	} while(!done);
 
diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c
index 384357bf29..72b8b2a902 100644
--- a/driver/accelgyro_bmi160.c
+++ b/driver/accelgyro_bmi160.c
@@ -14,6 +14,7 @@
 #include "driver/accelgyro_bmi160.h"
 #include "driver/mag_bmm150.h"
 #include "hooks.h"
+#include "hwtimer.h"
 #include "i2c.h"
 #include "math_util.h"
 #include "spi.h"
@@ -25,6 +26,10 @@
 #define CPRINTF(format, args...) cprintf(CC_ACCEL, format, ## args)
 #define CPRINTS(format, args...) cprints(CC_ACCEL, format, ## args)
 
+#ifdef CONFIG_ACCEL_FIFO
+static uint32_t last_interrupt_timestamp;
+#endif
+
 /*
  * Struct for pairing an engineering value with the register value for a
  * parameter.
@@ -820,7 +825,8 @@ static int bmi160_decode_header(struct motion_sensor_t *s,
 				vector.data[Y] = v[Y];
 				vector.data[Z] = v[Z];
 				vector.sensor_num = i + (s - motion_sensors);
-				motion_sense_fifo_add_unit(&vector, s + i, 3);
+				motion_sense_fifo_add_data(&vector, s + i, 3,
+						last_interrupt_timestamp);
 				*bp += (i == MOTIONSENSE_TYPE_MAG ? 8 : 6);
 			}
 		}
@@ -955,6 +961,9 @@ static int load_fifo(struct motion_sensor_t *s)
  */
 void bmi160_interrupt(enum gpio_signal signal)
 {
+#ifdef CONFIG_ACCEL_FIFO
+	last_interrupt_timestamp = __hw_clock_source_read();
+#endif
 	task_set_event(TASK_ID_MOTIONSENSE,
 		       CONFIG_ACCELGYRO_BMI160_INT_EVENT, 0);
 }
diff --git a/driver/als_si114x.c b/driver/als_si114x.c
index 5d9e595f07..567caa917c 100644
--- a/driver/als_si114x.c
+++ b/driver/als_si114x.c
@@ -11,6 +11,7 @@
 #include "console.h"
 #include "driver/als_si114x.h"
 #include "hooks.h"
+#include "hwtimer.h"
 #include "i2c.h"
 #include "math_util.h"
 #include "task.h"
@@ -150,7 +151,12 @@ static int si114x_read_results(struct motion_sensor_t *s, int nb)
 	for (i = nb; i < 3; i++)
 		vector.data[i] = 0;
 	vector.sensor_num = s - motion_sensors;
-	motion_sense_fifo_add_unit(&vector, s, nb);
+	motion_sense_fifo_add_data(&vector, s, nb,
+				   __hw_clock_source_read());
+	/*
+	 * TODO: get time at a more accurate spot.
+	 * Like in si114x_interrupt
+	 */
 #else
 	/* We need to copy raw_xyz into xyz with mutex */
 #endif
diff --git a/include/motion_sense.h b/include/motion_sense.h
index 9d727a62de..07687feb5a 100644
--- a/include/motion_sense.h
+++ b/include/motion_sense.h
@@ -180,15 +180,18 @@ extern unsigned int motion_min_interval;
 extern struct queue motion_sense_fifo;
 
 /**
- * Interrupt function for lid accelerometer.
+ * Add new actual data to the fifo, including a timestamp.
  *
  * @param data data to insert in the FIFO
  * @param sensor sensor the data comes from
- * @valid_data data should be copied into the public sensor vector
+ * @param valid_data data should be copied into the public sensor vector
+ * @param time accurate time (ideally measured in an interrupt) the sample
+ *             was taken at
  */
-void motion_sense_fifo_add_unit(struct ec_response_motion_sensor_data *data,
+void motion_sense_fifo_add_data(struct ec_response_motion_sensor_data *data,
 				struct motion_sensor_t *sensor,
-				int valid_data);
+				int valid_data,
+				uint32_t time);
 
 #endif