summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--common/motion_sense.c138
-rw-r--r--common/motion_sense_fifo.c258
-rw-r--r--include/motion_sense_fifo.h80
-rw-r--r--test/build.mk3
-rw-r--r--test/motion_sense_fifo.c328
-rw-r--r--test/motion_sense_fifo.tasklist10
-rw-r--r--test/test_config.h8
7 files changed, 617 insertions, 208 deletions
diff --git a/common/motion_sense.c b/common/motion_sense.c
index 650f2a7617..c74971a14b 100644
--- a/common/motion_sense.c
+++ b/common/motion_sense.c
@@ -78,6 +78,9 @@ static void print_spoof_mode_status(int id);
/* Flags to control whether to send an ODR change event for a sensor */
static uint32_t odr_event_required;
+/* Number of element the AP should collect */
+__maybe_unused static int fifo_int_enabled;
+
static inline int motion_sensor_in_forced_mode(
const struct motion_sensor_t *sensor)
{
@@ -610,6 +613,36 @@ static inline void increment_sensor_collection(struct motion_sensor_t *sensor,
}
}
+/**
+ * Commit the data in a sensor's raw_xyz vector. This operation might have
+ * different meanings depending on the CONFIG_ACCEL_FIFO flag.
+ *
+ * @param s Pointer to the sensor.
+ */
+static void motion_sense_push_raw_xyz(struct motion_sensor_t *s)
+{
+ if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
+ struct ec_response_motion_sensor_data vector;
+ int *v = s->raw_xyz;
+
+ vector.flags = 0;
+ vector.sensor_num = s - motion_sensors;
+ if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE) &&
+ s->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE)
+ v = s->spoof_xyz;
+ mutex_lock(&g_sensor_mutex);
+ memcpy(vector.data, v, sizeof(v));
+ mutex_unlock(&g_sensor_mutex);
+ motion_sense_fifo_stage_data(&vector, s, 3,
+ __hw_clock_source_read());
+ motion_sense_fifo_commit_data();
+ } else {
+ mutex_lock(&g_sensor_mutex);
+ memcpy(s->xyz, s->raw_xyz, sizeof(s->xyz));
+ mutex_unlock(&g_sensor_mutex);
+ }
+}
+
static int motion_sense_process(struct motion_sensor_t *sensor,
uint32_t *event,
const timestamp_t *ts)
@@ -632,58 +665,24 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
ret = sensor->drv->irq_handler(sensor, event);
}
#endif
- if (IS_ENABLED(CONFIG_ACCEL_FIFO)) {
- if (motion_sensor_in_forced_mode(sensor)) {
- if (motion_sensor_time_to_read(ts, sensor)) {
- struct ec_response_motion_sensor_data vector;
- int *v = sensor->raw_xyz;
-
- ret = motion_sense_read(sensor);
- if (ret == EC_SUCCESS) {
- vector.flags = 0;
- vector.sensor_num = sensor -
- motion_sensors;
- if (IS_ENABLED(CONFIG_ACCEL_SPOOF_MODE)
- && sensor->flags &
- MOTIONSENSE_FLAG_IN_SPOOF_MODE)
- v = sensor->spoof_xyz;
- vector.data[X] = v[X];
- vector.data[Y] = v[Y];
- vector.data[Z] = v[Z];
- motion_sense_fifo_stage_data(
- &vector, sensor, 3,
- __hw_clock_source_read());
- motion_sense_fifo_commit_data();
- }
- increment_sensor_collection(sensor, ts);
- } else {
- ret = EC_ERROR_BUSY;
- }
+ if (motion_sensor_in_forced_mode(sensor)) {
+ if (motion_sensor_time_to_read(ts, sensor)) {
+ ret = motion_sense_read(sensor);
+ increment_sensor_collection(sensor, ts);
+ } else {
+ ret = EC_ERROR_BUSY;
}
- if (*event & TASK_EVENT_MOTION_FLUSH_PENDING) {
- int flush_pending = atomic_read_clear(
- &sensor->flush_pending);
- for (; flush_pending > 0; flush_pending--) {
- motion_sense_insert_async_event(sensor,
- ASYNC_EVENT_FLUSH);
- }
- }
- } else {
- if (motion_sensor_in_forced_mode(sensor)) {
- if (motion_sensor_time_to_read(ts, sensor)) {
- /* Get latest data for local calculation */
- ret = motion_sense_read(sensor);
- increment_sensor_collection(sensor, ts);
- } else {
- ret = EC_ERROR_BUSY;
- }
- if (ret == EC_SUCCESS) {
- mutex_lock(&g_sensor_mutex);
- memcpy(sensor->xyz, sensor->raw_xyz,
- sizeof(sensor->xyz));
- mutex_unlock(&g_sensor_mutex);
- }
+ if (ret == EC_SUCCESS)
+ motion_sense_push_raw_xyz(sensor);
+ }
+ if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
+ *event & TASK_EVENT_MOTION_FLUSH_PENDING) {
+ int flush_pending = atomic_read_clear(&sensor->flush_pending);
+
+ for (; flush_pending > 0; flush_pending--) {
+ motion_sense_fifo_insert_async_event(
+ sensor, ASYNC_EVENT_FLUSH);
}
}
@@ -692,7 +691,7 @@ static int motion_sense_process(struct motion_sensor_t *sensor,
motion_sense_set_data_rate(sensor);
motion_sense_set_motion_intervals();
if (IS_ENABLED(CONFIG_ACCEL_FIFO))
- motion_sense_insert_async_event(
+ motion_sense_fifo_insert_async_event(
sensor, ASYNC_EVENT_ODR);
}
return ret;
@@ -902,25 +901,18 @@ void motion_sense_task(void *u)
* - we haven't done it for a while.
*/
if (IS_ENABLED(CONFIG_ACCEL_FIFO) &&
- (motion_sense_fifo_is_wake_up_needed() ||
+ (motion_sense_fifo_wake_up_needed ||
event & (TASK_EVENT_MOTION_ODR_CHANGE |
TASK_EVENT_MOTION_FLUSH_PENDING) ||
+ motion_sense_fifo_over_thres() ||
(ap_event_interval > 0 &&
time_after(ts_begin_task.le.lo,
ts_last_int.le.lo + ap_event_interval)))) {
if ((event & TASK_EVENT_MOTION_FLUSH_PENDING) == 0) {
- motion_sense_fifo_stage_timestamp(
+ motion_sense_fifo_add_timestamp(
__hw_clock_source_read());
- motion_sense_fifo_commit_data();
}
ts_last_int = ts_begin_task;
- /*
- * Count the number of event the AP is allowed to
- * collect.
- */
- mutex_lock(&g_sensor_mutex);
- fifo_queue_count = queue_count(&motion_sense_fifo);
- mutex_unlock(&g_sensor_mutex);
#ifdef CONFIG_MKBP_EVENT
/*
* Send an event if we know we are in S0 and the kernel
@@ -930,9 +922,9 @@ void motion_sense_task(void *u)
*/
if ((fifo_int_enabled &&
sensor_active == SENSOR_ACTIVE_S0) ||
- wake_up_needed) {
+ motion_sense_fifo_wake_up_needed) {
mkbp_send_event(EC_MKBP_EVENT_SENSOR_FIFO);
- wake_up_needed = 0;
+ motion_sense_fifo_wake_up_needed = 0;
}
#endif /* CONFIG_MKBP_EVENT */
}
@@ -1267,12 +1259,11 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
args->response_size = sizeof(out->fifo_info);
break;
}
- motion_sense_get_fifo_info(&out->fifo_info);
+ motion_sense_fifo_get_info(&out->fifo_info, 1);
for (i = 0; i < motion_sensor_count; i++) {
out->fifo_info.lost[i] = motion_sensors[i].lost;
motion_sensors[i].lost = 0;
}
- motion_sense_fifo_lost = 0;
args->response_size = sizeof(out->fifo_info) +
sizeof(uint16_t) * motion_sensor_count;
break;
@@ -1280,17 +1271,12 @@ static int host_cmd_motion_sense(struct host_cmd_handler_args *args)
case MOTIONSENSE_CMD_FIFO_READ:
if (!IS_ENABLED(CONFIG_ACCEL_FIFO))
return EC_RES_INVALID_PARAM;
- mutex_lock(&g_sensor_mutex);
- reported = MIN((args->response_max - sizeof(out->fifo_read)) /
- motion_sense_fifo.unit_bytes,
- MIN(queue_count(&motion_sense_fifo),
- in->fifo_read.max_data_vector));
- reported = queue_remove_units(&motion_sense_fifo,
- out->fifo_read.data, reported);
- mutex_unlock(&g_sensor_mutex);
- out->fifo_read.number_data = reported;
- args->response_size = sizeof(out->fifo_read) + reported *
- motion_sense_fifo.unit_bytes;
+ out->fifo_read.number_data = motion_sense_fifo_read(
+ args->response_max - sizeof(out->fifo_read),
+ in->fifo_read.max_data_vector,
+ out->fifo_read.data,
+ &(args->response_size));
+ args->response_size += sizeof(out->fifo_read);
break;
case MOTIONSENSE_CMD_FIFO_INT_ENABLE:
if (!IS_ENABLED(CONFIG_ACCEL_FIFO))
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c
index ec4c63b6de..938a4d142c 100644
--- a/common/motion_sense_fifo.c
+++ b/common/motion_sense_fifo.c
@@ -7,30 +7,14 @@
#include "hwtimer.h"
#include "mkbp_event.h"
#include "motion_sense_fifo.h"
-#include "queue.h"
#include "tablet_mode.h"
+#include "task.h"
#include "util.h"
#define CPRINTS(format, args...) cprints(CC_MOTION_SENSE, format, ## args)
-static inline int is_timestamp(struct ec_response_motion_sensor_data *data)
-{
- return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
-}
-
-/* Need to wake up the AP */
-int wake_up_needed;
-
-/* Number of element the AP should collect */
-int fifo_queue_count;
-int fifo_int_enabled;
-
-struct queue motion_sense_fifo = QUEUE_NULL(
- CONFIG_ACCEL_FIFO_SIZE, struct ec_response_motion_sensor_data);
-int motion_sense_fifo_lost;
-
/**
- * Staged metadata for the motion_sense_fifo.
+ * Staged metadata for the fifo queue.
* @read_ts: The timestamp at which the staged data was read. This value will
* serve as the upper bound for spreading
* @count: The total number of motion_sense_fifo entries that are currently
@@ -46,15 +30,39 @@ struct fifo_staged {
uint8_t sample_count[SENSOR_COUNT];
uint8_t requires_spreading;
};
+
+/** Queue to hold the data to be sent to the AP. */
+struct queue fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO_SIZE,
+ struct ec_response_motion_sensor_data);
+/** Count of the number of entries lost due to a small queue. */
+static int fifo_lost;
+/** Metadata for the fifo, used for staging and spreading data. */
static struct fifo_staged fifo_staged;
-static inline struct ec_response_motion_sensor_data *
-get_motion_sense_fifo_head(void)
+/** Need to wake up the AP. */
+int motion_sense_fifo_wake_up_needed;
+
+/**
+ * Check whether or not a give sensor data entry is a timestamp or not.
+ *
+ * @param data The data entry to check.
+ * @return 1 if the entry is a timestamp, 0 otherwise.
+ */
+static inline int is_timestamp(struct ec_response_motion_sensor_data *data)
+{
+ return data->flags & MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+}
+
+/**
+ * Convinience function to get the head of the fifo. This function makes no
+ * guarantee on whether or not the entry is valid.
+ *
+ * @return Pointer to the head of the fifo.
+ */
+static inline struct ec_response_motion_sensor_data *get_fifo_head(void)
{
- return ((struct ec_response_motion_sensor_data *)
- motion_sense_fifo.buffer) +
- (motion_sense_fifo.state->head &
- motion_sense_fifo.unit_bytes);
+ return ((struct ec_response_motion_sensor_data *) fifo.buffer) +
+ (fifo.state->head & fifo.buffer_units_mask);
}
/**
@@ -69,11 +77,10 @@ get_motion_sense_fifo_head(void)
* WARNING: This function MUST be called from within a locked context of
* g_sensor_mutex.
*/
-static void motion_sense_fifo_pop(void)
+static void fifo_pop(void)
{
- struct ec_response_motion_sensor_data *head =
- get_motion_sense_fifo_head();
- const size_t initial_count = queue_count(&motion_sense_fifo);
+ struct ec_response_motion_sensor_data *head = get_fifo_head();
+ const size_t initial_count = queue_count(&fifo);
/* Check that we have something to pop. */
if (!initial_count && !fifo_staged.count)
@@ -85,13 +92,19 @@ static void motion_sense_fifo_pop(void)
* staged data.
*/
if (!initial_count)
- queue_advance_tail(&motion_sense_fifo, 1);
+ queue_advance_tail(&fifo, 1);
/*
+ * If we're about to pop a wakeup flag, we should remember it as though
+ * it was committed.
+ */
+ if (head->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
+ motion_sense_fifo_wake_up_needed = 1;
+ /*
* By not using queue_remove_unit we're avoiding an un-necessary memcpy.
*/
- queue_advance_head(&motion_sense_fifo, 1);
- motion_sense_fifo_lost++;
+ queue_advance_head(&fifo, 1);
+ fifo_lost++;
/* Increment lost counter if we have valid data. */
if (!is_timestamp(head))
@@ -127,10 +140,13 @@ static void motion_sense_fifo_pop(void)
}
}
-static void motion_sense_fifo_ensure_space(void)
+/**
+ * Make sure that the fifo has at least 1 empty spot to stage data into.
+ */
+static void fifo_ensure_space(void)
{
/* If we already have space just bail. */
- if (queue_space(&motion_sense_fifo) > fifo_staged.count)
+ if (queue_space(&fifo) > fifo_staged.count)
return;
/*
@@ -145,17 +161,21 @@ static void motion_sense_fifo_ensure_space(void)
* would assign a bad timestamp to it.
*/
do {
- motion_sense_fifo_pop();
+ fifo_pop();
} while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) &&
- !is_timestamp(get_motion_sense_fifo_head()) &&
- queue_count(&motion_sense_fifo) + fifo_staged.count);
+ !is_timestamp(get_fifo_head()) &&
+ queue_count(&fifo) + fifo_staged.count);
}
-/*
- * Do not use this function directly if you just want to add sensor data, use
- * motion_sense_fifo_stage_data instead to get a proper timestamp too.
+/**
+ * Stage a single data unit to the motion sense fifo. Note that for the AP to
+ * see this data, it must be committed.
+ *
+ * @param data The data to stage.
+ * @param sensor The sensor that generated the data
+ * @param valid_data The number of readable data entries in the data.
*/
-static void motion_sense_fifo_stage_unit(
+void fifo_stage_unit(
struct ec_response_motion_sensor_data *data,
struct motion_sensor_t *sensor,
int valid_data)
@@ -178,29 +198,26 @@ static void motion_sense_fifo_stage_unit(
}
removed = sensor->oversampling++;
sensor->oversampling %= sensor->oversampling_ratio;
- if (removed != 0) {
+ if (removed) {
mutex_unlock(&g_sensor_mutex);
return;
}
}
/* Make sure we have room for the data */
- motion_sense_fifo_ensure_space();
+ fifo_ensure_space();
mutex_unlock(&g_sensor_mutex);
- if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
- wake_up_needed = 1;
if (IS_ENABLED(CONFIG_TABLET_MODE))
data->flags |= (tablet_get_mode() ?
- MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
+ MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0);
/*
* Get the next writable block in the fifo. We don't need to lock this
* because it will always be past the tail and thus the AP will never
* read this until motion_sense_fifo_commit_data() is called.
*/
- chunk = queue_get_write_chunk(
- &motion_sense_fifo, fifo_staged.count);
+ chunk = queue_get_write_chunk(&fifo, fifo_staged.count);
if (!chunk.buffer) {
/*
@@ -220,7 +237,7 @@ static void motion_sense_fifo_stage_unit(
* be written to the next available block and this one will remain
* staged.
*/
- memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes);
+ memcpy(chunk.buffer, data, fifo.unit_bytes);
fifo_staged.count++;
/*
@@ -235,52 +252,68 @@ static void motion_sense_fifo_stage_unit(
fifo_staged.requires_spreading = 1;
}
-void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
- enum motion_sense_async_event evt)
+/**
+ * Stage an entry representing a single timestamp.
+ *
+ * @param timestamp The timestamp to add to the fifo.
+ */
+static void fifo_stage_timestamp(uint32_t timestamp)
{
struct ec_response_motion_sensor_data vector;
- vector.flags = evt;
+ vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
+ vector.timestamp = timestamp;
+ vector.sensor_num = 0;
+ fifo_stage_unit(&vector, NULL, 0);
+}
+
+/**
+ * Peek into the staged data at a given offset. This function performs no bound
+ * checking and is purely for confinience.
+ *
+ * @param offset The offset into the staged data to peek into.
+ * @return Pointer to the entry at the given offset.
+ */
+static inline struct ec_response_motion_sensor_data *
+peek_fifo_staged(size_t offset)
+{
+ return (struct ec_response_motion_sensor_data *)
+ queue_get_write_chunk(&fifo, offset).buffer;
+}
+
+void motion_sense_fifo_insert_async_event(
+ struct motion_sensor_t *sensor,
+ enum motion_sense_async_event event)
+{
+ struct ec_response_motion_sensor_data vector;
+
+ vector.flags = event;
vector.timestamp = __hw_clock_source_read();
vector.sensor_num = sensor - motion_sensors;
- motion_sense_fifo_stage_unit(&vector, sensor, 0);
+ fifo_stage_unit(&vector, sensor, 0);
motion_sense_fifo_commit_data();
}
-void motion_sense_fifo_stage_timestamp(uint32_t timestamp)
+inline void motion_sense_fifo_add_timestamp(uint32_t timestamp)
{
- struct ec_response_motion_sensor_data vector;
-
- vector.flags = MOTIONSENSE_SENSOR_FLAG_TIMESTAMP;
- vector.timestamp = timestamp;
- vector.sensor_num = 0;
- motion_sense_fifo_stage_unit(&vector, NULL, 0);
+ fifo_stage_timestamp(timestamp);
+ motion_sense_fifo_commit_data();
}
-void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
- struct motion_sensor_t *sensor,
- int valid_data,
- uint32_t time)
+void motion_sense_fifo_stage_data(
+ struct ec_response_motion_sensor_data *data,
+ struct motion_sensor_t *sensor,
+ int valid_data,
+ uint32_t time)
{
if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS)) {
/* First entry, save the time for spreading later. */
if (!fifo_staged.count)
fifo_staged.read_ts = __hw_clock_source_read();
- motion_sense_fifo_stage_timestamp(time);
+ fifo_stage_timestamp(time);
}
- motion_sense_fifo_stage_unit(data, sensor, valid_data);
-}
-
-/**
- * Peek into the staged data at a given offset. This function performs no bound
- * checking and is purely for convenience.
- */
-static inline struct ec_response_motion_sensor_data *
-motion_sense_peek_fifo_staged(size_t offset)
-{
- return (struct ec_response_motion_sensor_data *)
- queue_get_write_chunk(&motion_sense_fifo, offset).buffer;
+ fifo_stage_unit(data, sensor, valid_data);
}
void motion_sense_fifo_commit_data(void)
@@ -291,6 +324,7 @@ void motion_sense_fifo_commit_data(void)
*/
static uint32_t data_periods[SENSOR_COUNT];
static uint32_t next_timestamp[SENSOR_COUNT];
+ static uint16_t next_timestamp_initialized;
struct ec_response_motion_sensor_data *data;
int i, window, sensor_num;
@@ -306,7 +340,7 @@ void motion_sense_fifo_commit_data(void)
if (!fifo_staged.requires_spreading)
goto flush_data_end;
- data = motion_sense_peek_fifo_staged(0);
+ data = peek_fifo_staged(0);
/*
* Spreading only makes sense if tight timestamps are used. In such case
@@ -316,6 +350,7 @@ void motion_sense_fifo_commit_data(void)
*/
if (!is_timestamp(data)) {
CPRINTS("Spreading skipped, first entry is not a timestamp");
+ fifo_staged.requires_spreading = 0;
goto flush_data_end;
}
@@ -334,14 +369,16 @@ void motion_sense_fifo_commit_data(void)
* Clamp the sample period to the MIN of collection_rate and the
* window length / sample counts.
*/
- if (window)
- period = MIN(period,
- window / fifo_staged.sample_count[i]);
+ if (window && fifo_staged.sample_count[i] > 1)
+ period = MIN(
+ period,
+ window / (fifo_staged.sample_count[i] - 1));
data_periods[i] = period;
}
+flush_data_end:
/*
- * Spread the timestamps.
+ * Conditionally spread the timestamps.
*
* If we got this far that means that the tight timestamps config is
* enabled. This means that we can expect the staged entries to have 1
@@ -349,45 +386,58 @@ void motion_sense_fifo_commit_data(void)
* through the timestamps until we get to data. We only need to update
* the timestamp right before it to keep things correct.
*/
+ next_timestamp_initialized = 0;
for (i = 0; i < fifo_staged.count; i++) {
- data = motion_sense_peek_fifo_staged(i);
+ data = peek_fifo_staged(i);
+ if (data->flags & MOTIONSENSE_SENSOR_FLAG_WAKEUP)
+ motion_sense_fifo_wake_up_needed = 1;
/* Skip timestamp, we don't know the sensor number yet. */
- if (is_timestamp(data))
+ if (is_timestamp(data) || !fifo_staged.requires_spreading)
continue;
/* Get the sensor number and point to the timestamp entry. */
sensor_num = data->sensor_num;
- data = motion_sense_peek_fifo_staged(i - 1);
+ data = peek_fifo_staged(i - 1);
- /* If the timestamp is after our computed next, skip ahead. */
- if (time_after(data->timestamp, next_timestamp[sensor_num]))
+ /*
+ * If this is the first time we're seeing a timestamp for this
+ * sensor or the timestamp is after our computed next, skip
+ * ahead.
+ */
+ if (!(next_timestamp_initialized & BIT(sensor_num)) ||
+ time_after(data->timestamp, next_timestamp[sensor_num])) {
next_timestamp[sensor_num] = data->timestamp;
+ next_timestamp_initialized |= BIT(sensor_num);
+ }
/* Spread the timestamp and compute the expected next. */
data->timestamp = next_timestamp[sensor_num];
next_timestamp[sensor_num] += data_periods[sensor_num];
}
-flush_data_end:
/* Advance the tail and clear the staged metadata. */
mutex_lock(&g_sensor_mutex);
- queue_advance_tail(&motion_sense_fifo, fifo_staged.count);
+ queue_advance_tail(&fifo, fifo_staged.count);
mutex_unlock(&g_sensor_mutex);
/* Reset metadata for next staging cycle. */
memset(&fifo_staged, 0, sizeof(fifo_staged));
}
-void motion_sense_get_fifo_info(
- struct ec_response_motion_sense_fifo_info *fifo_info)
+void motion_sense_fifo_get_info(
+ struct ec_response_motion_sense_fifo_info *fifo_info,
+ int reset)
{
- fifo_info->size = motion_sense_fifo.buffer_units;
+ fifo_info->size = fifo.buffer_units;
mutex_lock(&g_sensor_mutex);
- fifo_info->count = fifo_queue_count;
- fifo_info->total_lost = motion_sense_fifo_lost;
+ fifo_info->count = queue_count(&fifo);
+ fifo_info->total_lost = fifo_lost;
mutex_unlock(&g_sensor_mutex);
fifo_info->timestamp = mkbp_last_event_time;
+
+ if (reset)
+ fifo_lost = 0;
}
static int motion_sense_get_next_event(uint8_t *out)
@@ -395,14 +445,28 @@ static int motion_sense_get_next_event(uint8_t *out)
union ec_response_get_next_data *data =
(union ec_response_get_next_data *)out;
/* out is not padded. It has one byte for the event type */
- motion_sense_get_fifo_info(&data->sensor_fifo.info);
+ motion_sense_fifo_get_info(&data->sensor_fifo.info, 0);
return sizeof(data->sensor_fifo);
}
DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event);
-inline int motion_sense_fifo_is_wake_up_needed(void)
+inline int motion_sense_fifo_over_thres(void)
+{
+ return queue_space(&fifo) < CONFIG_ACCEL_FIFO_THRES;
+}
+
+int motion_sense_fifo_read(int capacity_bytes, int max_count, void *out,
+ uint16_t *out_size)
{
- return queue_space(&motion_sense_fifo) < CONFIG_ACCEL_FIFO_THRES ||
- wake_up_needed;
+ int count;
+
+ mutex_lock(&g_sensor_mutex);
+ count = MIN(capacity_bytes / fifo.unit_bytes,
+ MIN(queue_count(&fifo), max_count));
+ count = queue_remove_units(&fifo, out, count);
+ mutex_unlock(&g_sensor_mutex);
+ *out_size = count * fifo.unit_bytes;
+
+ return count;
}
diff --git a/include/motion_sense_fifo.h b/include/motion_sense_fifo.h
index 6b306b5f29..d47c23af94 100644
--- a/include/motion_sense_fifo.h
+++ b/include/motion_sense_fifo.h
@@ -7,14 +7,8 @@
#define __CROS_EC_MOTION_SENSE_FIFO_H
#include "motion_sense.h"
-#include "task.h"
-
-extern struct queue motion_sense_fifo;
-extern int wake_up_needed;
-extern int fifo_int_enabled;
-extern int fifo_queue_count;
-extern int motion_sense_fifo_lost;
+/** Allowed async events. */
enum motion_sense_async_event {
ASYNC_EVENT_FLUSH = MOTIONSENSE_SENSOR_FLAG_FLUSH |
MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
@@ -22,6 +16,26 @@ enum motion_sense_async_event {
MOTIONSENSE_SENSOR_FLAG_TIMESTAMP,
};
+/** Whether or not we need to wake up the AP. */
+extern int motion_sense_fifo_wake_up_needed;
+
+/**
+ * Insert an async event into the fifo.
+ *
+ * @param sensor The sensor that generated the async event.
+ * @param event The event to insert.
+ */
+void motion_sense_fifo_insert_async_event(
+ struct motion_sensor_t *sensor,
+ enum motion_sense_async_event event);
+
+/**
+ * Insert a timestamp into the fifo.
+ *
+ * @param timestamp The timestamp to insert.
+ */
+void motion_sense_fifo_add_timestamp(uint32_t timestamp);
+
/**
* Stage data to the fifo, including a timestamp. This data will not be
* available to the AP until motion_sense_fifo_commit_data is called.
@@ -32,46 +46,46 @@ enum motion_sense_async_event {
* @param time accurate time (ideally measured in an interrupt) the sample
* was taken at
*/
-void motion_sense_fifo_stage_data(struct ec_response_motion_sensor_data *data,
- struct motion_sensor_t *sensor,
- int valid_data,
- uint32_t time);
+void motion_sense_fifo_stage_data(
+ struct ec_response_motion_sensor_data *data,
+ struct motion_sensor_t *sensor,
+ int valid_data,
+ uint32_t time);
/**
- * Commits all staged data to the fifo. If multiple readings were placed using
- * the same timestamps, they will be spread out.
+ * Commit all the currently staged data to the fifo. Doing so makes it readable
+ * to the AP.
*/
void motion_sense_fifo_commit_data(void);
/**
- * Insert an async event into the fifo.
- *
- * @param sensor Pointer to the sensor generating the event.
- * @param evt The event to insert.
- */
-void motion_sense_insert_async_event(struct motion_sensor_t *sensor,
- enum motion_sense_async_event evt);
-
-/**
- * Stage a timestamp into the fifo.
+ * Get information about the fifo.
*
- * @param timestamp The timestamp to stage.
+ * @param fifo_info The struct to modify with the current information about the
+ * fifo.
+ * @param reset Whether or not to reset statistics after reading them.
*/
-void motion_sense_fifo_stage_timestamp(uint32_t timestamp);
+void motion_sense_fifo_get_info(
+ struct ec_response_motion_sense_fifo_info *fifo_info,
+ int reset);
/**
- * Get information about the fifo.
+ * Check whether or not the fifo has gone over its threshold.
*
- * @param fifo_info The struct to store the info.
+ * @return 1 if yes, 0 for no.
*/
-void motion_sense_get_fifo_info(
- struct ec_response_motion_sense_fifo_info *fifo_info);
+int motion_sense_fifo_over_thres(void);
/**
- * Checks if either the AP should be woken up due to the fifo.
+ * Read available committed entries from the fifo.
*
- * @return 1 if the AP should be woken up, 0 otherwise.
+ * @param capacity_bytes The number of bytes available to be written to `out`.
+ * @param max_count The maximum number of entries to be placed in `out`.
+ * @param out The target to copy the data into.
+ * @param out_size The number of bytes written to `out`.
+ * @return The number of entries written to `out`.
*/
-int motion_sense_fifo_is_wake_up_needed(void);
+int motion_sense_fifo_read(int capacity_bytes, int max_count, void *out,
+ uint16_t *out_size);
-#endif /* __CROS_EC_MOTION_SENSE_FIFO_H */
+#endif /*__CROS_EC_MOTION_SENSE_FIFO_H */
diff --git a/test/build.mk b/test/build.mk
index ea0fef7fc1..9e139995a4 100644
--- a/test/build.mk
+++ b/test/build.mk
@@ -46,6 +46,7 @@ test-list-host += math_util
test-list-host += motion_angle
test-list-host += motion_angle_tablet
test-list-host += motion_lid
+test-list-host += motion_sense_fifo
test-list-host += mutex
test-list-host += nvmem
test-list-host += pingpong
@@ -84,7 +85,6 @@ test-list-host += vboot
test-list-host += x25519
endif
-
aes-y=aes.o
base32-y=base32.o
battery_get_params_smart-y=battery_get_params_smart.o
@@ -118,6 +118,7 @@ math_util-y=math_util.o
motion_angle-y=motion_angle.o motion_angle_data_literals.o motion_common.o
motion_angle_tablet-y=motion_angle_tablet.o motion_angle_data_literals_tablet.o motion_common.o
motion_lid-y=motion_lid.o
+motion_sense_fifo-y=motion_sense_fifo.o
mutex-y=mutex.o
nvmem-y=nvmem.o nvmem_tpm2_mock.o
pingpong-y=pingpong.o
diff --git a/test/motion_sense_fifo.c b/test/motion_sense_fifo.c
new file mode 100644
index 0000000000..84f52d614c
--- /dev/null
+++ b/test/motion_sense_fifo.c
@@ -0,0 +1,328 @@
+/* Copyright 2019 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ *
+ * Test motion_sense_fifo.
+ */
+
+#include "stdio.h"
+#include "motion_sense_fifo.h"
+#include "test_util.h"
+#include "util.h"
+#include "hwtimer.h"
+
+struct motion_sensor_t motion_sensors[] = {
+ [BASE] = {},
+ [LID] = {},
+};
+
+const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
+
+uint32_t mkbp_last_event_time;
+
+static struct ec_response_motion_sensor_data data[CONFIG_ACCEL_FIFO_SIZE];
+static uint16_t data_bytes_read;
+
+static int test_insert_async_event(void)
+{
+ int read_count;
+
+ motion_sense_fifo_insert_async_event(motion_sensors, ASYNC_EVENT_FLUSH);
+ motion_sense_fifo_insert_async_event(motion_sensors + 1,
+ ASYNC_EVENT_ODR);
+
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 2, "%d");
+ TEST_EQ(data_bytes_read,
+ 2 * sizeof(struct ec_response_motion_sensor_data), "%d");
+
+ TEST_BITS_SET(data[0].flags, ASYNC_EVENT_FLUSH);
+ TEST_BITS_CLEARED(data[0].flags, MOTIONSENSE_SENSOR_FLAG_ODR);
+ TEST_EQ(data[0].sensor_num, 0, "%d");
+
+ TEST_BITS_SET(data[1].flags, ASYNC_EVENT_ODR);
+ TEST_BITS_CLEARED(data[1].flags, MOTIONSENSE_SENSOR_FLAG_FLUSH);
+ TEST_EQ(data[1].sensor_num, 1, "%d");
+
+ return EC_SUCCESS;
+}
+
+static int test_wake_up_needed(void)
+{
+ data[0].flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP;
+
+ motion_sense_fifo_stage_data(data, motion_sensors, 0, 100);
+ TEST_EQ(motion_sense_fifo_wake_up_needed, 0, "%d");
+
+ motion_sense_fifo_commit_data();
+ TEST_EQ(motion_sense_fifo_wake_up_needed, 1, "%d");
+
+ return EC_SUCCESS;
+}
+
+static int test_wake_up_needed_overflow(void)
+{
+ int i;
+
+ data[0].flags = MOTIONSENSE_SENSOR_FLAG_WAKEUP;
+ motion_sense_fifo_stage_data(data, motion_sensors, 0, 100);
+
+ data[0].flags = 0;
+ /*
+ * Using CONFIG_ACCEL_FIFO_SIZE / 2 since 2 entries are inserted per
+ * 'data':
+ * - a timestamp
+ * - the data
+ */
+ for (i = 0; i < (CONFIG_ACCEL_FIFO_SIZE / 2); i++)
+ motion_sense_fifo_stage_data(data, motion_sensors, 0, 101 + i);
+
+ TEST_EQ(motion_sense_fifo_wake_up_needed, 1, "%d");
+ return EC_SUCCESS;
+}
+
+static int test_adding_timestamp(void)
+{
+ int read_count;
+
+ motion_sense_fifo_add_timestamp(100);
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+
+ TEST_EQ(read_count, 1, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, 100, "%u");
+ return EC_SUCCESS;
+}
+
+static int test_stage_data_sets_xyz(void)
+{
+ motion_sensors->oversampling_ratio = 1;
+ motion_sensors->oversampling = 0;
+ data->data[0] = 1;
+ data->data[1] = 2;
+ data->data[2] = 3;
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+
+ TEST_EQ(motion_sensors->xyz[0], 1, "%d");
+ TEST_EQ(motion_sensors->xyz[1], 2, "%d");
+ TEST_EQ(motion_sensors->xyz[2], 3, "%d");
+
+ return EC_SUCCESS;
+}
+
+static int test_stage_data_removed_oversample(void)
+{
+ int read_count;
+
+ motion_sensors->oversampling_ratio = 2;
+ motion_sensors->oversampling = 0;
+ data->data[0] = 1;
+ data->data[1] = 2;
+ data->data[2] = 3;
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+
+ data->data[0] = 4;
+ data->data[1] = 5;
+ data->data[2] = 6;
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 110);
+ motion_sense_fifo_commit_data();
+
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 3, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, 100, "%u");
+ TEST_BITS_CLEARED(data[1].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[1].data[0], 1, "%d");
+ TEST_EQ(data[1].data[1], 2, "%d");
+ TEST_EQ(data[1].data[2], 3, "%d");
+ TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[2].timestamp, 110, "%u");
+
+ return EC_SUCCESS;
+}
+
+static int test_stage_data_remove_all_oversampling(void)
+{
+ int read_count;
+
+ motion_sensors->oversampling_ratio = 0;
+ motion_sensors->oversampling = 0;
+ data->data[0] = 1;
+ data->data[1] = 2;
+ data->data[2] = 3;
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+
+ data->data[0] = 4;
+ data->data[1] = 5;
+ data->data[2] = 6;
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 110);
+ motion_sense_fifo_commit_data();
+
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 2, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, 100, "%u");
+ TEST_BITS_SET(data[1].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[1].timestamp, 110, "%u");
+
+ return EC_SUCCESS;
+}
+
+static int test_stage_data_evicts_data_with_timestamp(void)
+{
+ int i, read_count;
+
+ /* Fill the fifo */
+ motion_sensors->oversampling_ratio = 1;
+ for (i = 0; i < CONFIG_ACCEL_FIFO_SIZE / 2; i++)
+ motion_sense_fifo_stage_data(data, motion_sensors,
+ 3, i * 100);
+
+ /* Add a single entry (should evict 2) */
+ motion_sense_fifo_add_timestamp(CONFIG_ACCEL_FIFO_SIZE * 100);
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, CONFIG_ACCEL_FIFO_SIZE - 1, "%d");
+ TEST_BITS_SET(data->flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data->timestamp, 100, "%u");
+ TEST_BITS_SET(data[CONFIG_ACCEL_FIFO_SIZE - 2].flags,
+ MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[CONFIG_ACCEL_FIFO_SIZE - 2].timestamp,
+ CONFIG_ACCEL_FIFO_SIZE * 100, "%u");
+
+ return EC_SUCCESS;
+}
+
+static int test_add_data_no_spreading_when_different_sensors(void)
+{
+ int read_count;
+
+ motion_sensors[0].oversampling_ratio = 1;
+ motion_sensors[1].oversampling_ratio = 1;
+
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+ motion_sense_fifo_stage_data(data, motion_sensors + 1, 3, 100);
+ motion_sense_fifo_commit_data();
+
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 4, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, 100, "%u");
+ TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[2].timestamp, 100, "%u");
+
+ return EC_SUCCESS;
+}
+
+static int test_add_data_no_spreading_different_timestamps(void)
+{
+ int read_count;
+
+ motion_sensors[0].oversampling_ratio = 1;
+
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 100);
+ motion_sense_fifo_stage_data(data, motion_sensors, 3, 120);
+ motion_sense_fifo_commit_data();
+
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 4, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, 100, "%u");
+ TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[2].timestamp, 120, "%u");
+
+ return EC_SUCCESS;
+}
+
+static int test_spread_data_in_window(void)
+{
+ uint32_t now;
+ int read_count;
+
+ motion_sensors[0].oversampling_ratio = 1;
+ motion_sensors[0].collection_rate = 20000; /* ns */
+ now = __hw_clock_source_read();
+
+ motion_sense_fifo_stage_data(data, motion_sensors, 3,
+ now - 18000);
+ motion_sense_fifo_stage_data(data, motion_sensors, 3,
+ now - 18000);
+ motion_sense_fifo_commit_data();
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 4, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, now - 18000, "%u");
+ TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[2].timestamp, now, "%u");
+
+ return EC_SUCCESS;
+}
+
+static int test_spread_data_by_collection_rate(void)
+{
+ const uint32_t now = __hw_clock_source_read();
+ int read_count;
+
+ motion_sensors[0].oversampling_ratio = 1;
+ motion_sensors[0].collection_rate = 20000; /* ns */
+ motion_sense_fifo_stage_data(data, motion_sensors, 3,
+ now - 20500);
+ motion_sense_fifo_stage_data(data, motion_sensors, 3,
+ now - 20500);
+ motion_sense_fifo_commit_data();
+ read_count = motion_sense_fifo_read(
+ sizeof(data), CONFIG_ACCEL_FIFO_SIZE,
+ data, &data_bytes_read);
+ TEST_EQ(read_count, 4, "%d");
+ TEST_BITS_SET(data[0].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[0].timestamp, now - 20500, "%u");
+ TEST_BITS_SET(data[2].flags, MOTIONSENSE_SENSOR_FLAG_TIMESTAMP);
+ TEST_EQ(data[2].timestamp, now - 500, "%u");
+
+ return EC_SUCCESS;
+}
+
+void before_test(void)
+{
+ motion_sense_fifo_commit_data();
+ motion_sense_fifo_read(sizeof(data), CONFIG_ACCEL_FIFO_SIZE, &data,
+ &data_bytes_read);
+ motion_sense_fifo_wake_up_needed = 0;
+ memset(data, 0, sizeof(data));
+}
+
+void run_test(void)
+{
+ test_reset();
+
+ RUN_TEST(test_insert_async_event);
+ RUN_TEST(test_wake_up_needed);
+ RUN_TEST(test_wake_up_needed_overflow);
+ RUN_TEST(test_adding_timestamp);
+ RUN_TEST(test_stage_data_sets_xyz);
+ RUN_TEST(test_stage_data_removed_oversample);
+ RUN_TEST(test_stage_data_remove_all_oversampling);
+ RUN_TEST(test_stage_data_evicts_data_with_timestamp);
+ RUN_TEST(test_add_data_no_spreading_when_different_sensors);
+ RUN_TEST(test_add_data_no_spreading_different_timestamps);
+ RUN_TEST(test_spread_data_in_window);
+ RUN_TEST(test_spread_data_by_collection_rate);
+
+ test_print_result();
+}
+
diff --git a/test/motion_sense_fifo.tasklist b/test/motion_sense_fifo.tasklist
new file mode 100644
index 0000000000..0e3696c3f0
--- /dev/null
+++ b/test/motion_sense_fifo.tasklist
@@ -0,0 +1,10 @@
+/* Copyright 2019 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+/**
+ * See CONFIG_TASK_LIST in config.h for details.
+ */
+#define CONFIG_TEST_TASK_LIST \
+ TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE)
diff --git a/test/test_config.h b/test/test_config.h
index b83d834056..89c928f1f0 100644
--- a/test/test_config.h
+++ b/test/test_config.h
@@ -82,8 +82,14 @@
#define CONFIG_SHA256
#endif
+#ifdef TEST_MOTION_SENSE_FIFO
+#define CONFIG_ACCEL_FIFO
+#define CONFIG_ACCEL_FIFO_SIZE 256
+#define CONFIG_ACCEL_FIFO_THRES 10
+#endif
+
#if defined(TEST_MOTION_LID) || defined(TEST_MOTION_ANGLE) || \
- defined(TEST_MOTION_ANGLE_TABLET)
+ defined(TEST_MOTION_ANGLE_TABLET) || defined(TEST_MOTION_SENSE_FIFO)
enum sensor_id {
BASE,
LID,