diff options
author | Yuval Peress <peress@chromium.org> | 2019-07-15 11:40:47 -0600 |
---|---|---|
committer | Commit Bot <commit-bot@chromium.org> | 2019-08-23 00:25:33 +0000 |
commit | a2e7b77b3b62d9f663eb1f916fe9ab36bef06dbe (patch) | |
tree | 0508e2e77f23ba861090a53d1b99306fe9ad3046 /common/motion_sense_fifo.c | |
parent | 3a2044d812f26a1ac0302a5eeb66eaf9433e78ba (diff) | |
download | chrome-ec-a2e7b77b3b62d9f663eb1f916fe9ab36bef06dbe.tar.gz |
common: Move fifo logic out of motion_sense.c
This change is needed to allow better testing of the fifo
behavior. Additionally, motion_sense_fifo.c will only be compiled
if CONFIG_ACCEL_FIFO is defined. This behaviour requires a few
small changes to several boards and baseboards to make
sure that we only define CONFIG_ACCEL_FIFO when the MOTIONSENSE
task is present (some times that may be only in one section RW or
RO).
BUG=b:137758297
BRANCH=None
TEST=buildall and ran CTS on arcada
Change-Id: I2f7e4e436ba9568a35b7a0b2c8d53a73f198ba73
Signed-off-by: Yuval Peress <peress@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/1704163
Reviewed-by: Alexandru M Stan <amstan@chromium.org>
Commit-Queue: Alexandru M Stan <amstan@chromium.org>
Diffstat (limited to 'common/motion_sense_fifo.c')
-rw-r--r-- | common/motion_sense_fifo.c | 403 |
1 files changed, 403 insertions, 0 deletions
diff --git a/common/motion_sense_fifo.c b/common/motion_sense_fifo.c new file mode 100644 index 0000000000..00c2f5a8cc --- /dev/null +++ b/common/motion_sense_fifo.c @@ -0,0 +1,403 @@ +/* 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. + */ + +#include "console.h" +#include "hwtimer.h" +#include "mkbp_event.h" +#include "motion_sense_fifo.h" +#include "queue.h" +#include "tablet_mode.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, struct ec_response_motion_sensor_data); +int motion_sense_fifo_lost; + +/** + * Staged metadata for the motion_sense_fifo. + * @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 + * staged. + * @sample_count: The total number of sensor readings per sensor that are + * currently staged. + * @requires_spreading: Flag used to shortcut the commit process. This should be + * true iff at least one of sample_count[] > 1 + */ +struct fifo_staged { + uint32_t read_ts; + uint16_t count; + uint8_t sample_count[SENSOR_COUNT]; + uint8_t requires_spreading; +}; +static struct fifo_staged fifo_staged; + +static inline struct ec_response_motion_sensor_data * +get_motion_sense_fifo_head(void) +{ + return ((struct ec_response_motion_sensor_data *) + motion_sense_fifo.buffer) + + (motion_sense_fifo.state->head & + motion_sense_fifo.unit_bytes); +} + +/** + * Pop one entry from the motion sense fifo. Poping will give priority to + * committed data (data residing between the head and tail of the queue). If no + * committed data is available (all the data is staged), then this function will + * remove the oldest staged data by moving both the head and tail. + * + * As a side-effect of this function, it'll updated any appropriate lost and + * count variables. + * + * WARNING: This function MUST be called from within a locked context of + * g_sensor_mutex. + */ +static void motion_sense_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); + + /* Check that we have something to pop. */ + if (!initial_count && !fifo_staged.count) + return; + + /* + * If all the data is staged (nothing in the committed queue), we'll + * need to move the head and the tail over to simulate poping from the + * staged data. + */ + if (!initial_count) + queue_advance_tail(&motion_sense_fifo, 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++; + + /* Increment lost counter if we have valid data. */ + if (!is_timestamp(head)) + motion_sensors[head->sensor_num].lost++; + + /* + * We're done if the initial count was non-zero and we only advanced the + * head. Else, decrement the staged count and update staged metadata. + */ + if (initial_count) + return; + + fifo_staged.count--; + + /* If we removed a timestamp there's nothing else for us to do. */ + if (is_timestamp(head)) + return; + + /* + * Decrement sample count, if the count was 2 before, we might not need + * to spread anymore. Loop through and check. + */ + if (--fifo_staged.sample_count[head->sensor_num] < 2) { + int i; + + fifo_staged.requires_spreading = 0; + for (i = 0; i < SENSOR_COUNT; i++) { + if (fifo_staged.sample_count[i] > 1) { + fifo_staged.requires_spreading = 1; + break; + } + } + } +} + +static void motion_sense_fifo_ensure_space(void) +{ + /* If we already have space just bail. */ + if (queue_space(&motion_sense_fifo) > fifo_staged.count) + return; + + /* + * Pop at least 1 spot, but if all the following conditions are met we + * will continue to pop: + * 1. We're operating with tight timestamps. + * 2. The new head isn't a timestamp. + * 3. We have data that we can possibly pop. + * + * Removing more than one entry is needed because if we are using tight + * timestamps and we pop a timestamp, then the next head is data, the AP + * would assign a bad timestamp to it. + */ + do { + motion_sense_fifo_pop(); + } while (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) && + !is_timestamp(get_motion_sense_fifo_head()) && + queue_count(&motion_sense_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. + */ +static void motion_sense_fifo_stage_unit( + struct ec_response_motion_sensor_data *data, + struct motion_sensor_t *sensor, + int valid_data) +{ + struct queue_chunk chunk; + int i; + + mutex_lock(&g_sensor_mutex); + + for (i = 0; i < valid_data; i++) + sensor->xyz[i] = data->data[i]; + + /* For valid sensors, check if AP really needs this data */ + if (valid_data) { + int removed; + + if (sensor->oversampling_ratio == 0) { + mutex_unlock(&g_sensor_mutex); + return; + } + removed = sensor->oversampling++; + sensor->oversampling %= sensor->oversampling_ratio; + if (removed != 0) { + mutex_unlock(&g_sensor_mutex); + return; + } + } + + /* Make sure we have room for the data */ + motion_sense_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); + + /* + * 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); + + if (!chunk.buffer) { + /* + * This should never happen since we already ensured there was + * space, but if there was a bug, we don't want to write to + * address 0. Just don't add any data to the queue instead. + */ + CPRINTS("Failed to get write chunk for new fifo data!"); + return; + } + + /* + * Save the data to the writable block and increment count. This data + * will now reside AFTER the tail of the queue and will not be visible + * to the AP until the motion_sense_fifo_commit_data() function is + * called. Because count is incremented, the following staged data will + * be written to the next available block and this one will remain + * staged. + */ + memcpy(chunk.buffer, data, motion_sense_fifo.unit_bytes); + fifo_staged.count++; + + /* + * If we're using tight timestamps, and the current entry isn't a + * timestamp we'll increment the sample_count for the given sensor. + * If the new per-sensor sample count is greater than 1, we'll need to + * spread. + */ + if (IS_ENABLED(CONFIG_SENSOR_TIGHT_TIMESTAMPS) && + !is_timestamp(data) && + ++fifo_staged.sample_count[data->sensor_num] > 1) + fifo_staged.requires_spreading = 1; +} + +void motion_sense_insert_async_event(struct motion_sensor_t *sensor, + enum motion_sense_async_event evt) +{ + struct ec_response_motion_sensor_data vector; + + vector.flags = evt; + vector.timestamp = __hw_clock_source_read(); + vector.sensor_num = sensor - motion_sensors; + + motion_sense_fifo_stage_unit(&vector, sensor, 0); + motion_sense_fifo_commit_data(); +} + +void motion_sense_fifo_stage_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); +} + +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); + } + 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; +} + +void motion_sense_fifo_commit_data(void) +{ + /* + * Static data to use off stack. Note that next_timestamp should persist + * and is only updated if the timestamp from the sensor is greater. + */ + static uint32_t data_periods[SENSOR_COUNT]; + static uint32_t next_timestamp[SENSOR_COUNT]; + struct ec_response_motion_sensor_data *data; + int i, window, sensor_num; + + /* Nothing staged, no work to do. */ + if (!fifo_staged.count) + return; + + /* + * If per-sensor event counts are never more than 1, no spreading is + * needed. This will also catch cases where tight timestamps aren't + * used. + */ + if (!fifo_staged.requires_spreading) + goto flush_data_end; + + data = motion_sense_peek_fifo_staged(0); + + /* + * Spreading only makes sense if tight timestamps are used. In such case + * entries are expected to be ordered: timestamp then data. If the first + * entry isn't a timestamp we must have gotten out of sync. Just commit + * all the data and skip the spreading. + */ + if (!is_timestamp(data)) { + CPRINTS("Spreading skipped, first entry is not a timestamp"); + goto flush_data_end; + } + + window = time_until(data->timestamp, fifo_staged.read_ts); + + /* Update the data_periods as needed for this flush. */ + for (i = 0; i < SENSOR_COUNT; i++) { + int period; + + /* Skip empty sensors. */ + if (!fifo_staged.sample_count[i]) + continue; + + period = motion_sensors[i].collection_rate; + /* + * 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]); + data_periods[i] = period; + } + + /* + * 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 + * or more timestamps followed by exactly 1 data entry. We'll loop + * through the timestamps until we get to data. We only need to update + * the timestamp right before it to keep things correct. + */ + for (i = 0; i < fifo_staged.count; i++) { + data = motion_sense_peek_fifo_staged(i); + + /* Skip timestamp, we don't know the sensor number yet. */ + if (is_timestamp(data)) + continue; + + /* Get the sensor number and point to the timestamp entry. */ + sensor_num = data->sensor_num; + data = motion_sense_peek_fifo_staged(i - 1); + + /* If the timestamp is after our computed next, skip ahead. */ + if (time_after(data->timestamp, next_timestamp[sensor_num])) + next_timestamp[sensor_num] = data->timestamp; + + /* 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); + 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) +{ + fifo_info->size = motion_sense_fifo.buffer_units; + mutex_lock(&g_sensor_mutex); + fifo_info->count = fifo_queue_count; + fifo_info->total_lost = motion_sense_fifo_lost; + mutex_unlock(&g_sensor_mutex); + fifo_info->timestamp = mkbp_last_event_time; +} + +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); + return sizeof(data->sensor_fifo); +} + +DECLARE_EVENT_SOURCE(EC_MKBP_EVENT_SENSOR_FIFO, motion_sense_get_next_event); + |