diff options
| -rw-r--r-- | baseboard/grunt/baseboard.h | 2 | ||||
| -rw-r--r-- | baseboard/kukui/baseboard.h | 2 | ||||
| -rw-r--r-- | board/careena/board.h | 1 | ||||
| -rw-r--r-- | board/flapjack/board.h | 2 | ||||
| -rw-r--r-- | board/jacuzzi/board.h | 4 | ||||
| -rw-r--r-- | board/kodama/board.h | 4 | ||||
| -rw-r--r-- | board/kukui/board.c | 4 | ||||
| -rw-r--r-- | board/kukui/board.h | 4 | ||||
| -rw-r--r-- | board/liara/board.h | 1 | ||||
| -rw-r--r-- | common/build.mk | 3 | ||||
| -rw-r--r-- | common/motion_sense.c | 409 | ||||
| -rw-r--r-- | common/motion_sense_fifo.c | 403 | ||||
| -rw-r--r-- | driver/accelgyro_bmi160.c | 1 | ||||
| -rw-r--r-- | driver/accelgyro_lsm6dsm.c | 1 | ||||
| -rw-r--r-- | driver/als_si114x.c | 1 | ||||
| -rw-r--r-- | driver/als_tcs3400.c | 1 | ||||
| -rw-r--r-- | driver/sync.c | 1 | ||||
| -rw-r--r-- | include/motion_sense.h | 34 | ||||
| -rw-r--r-- | include/motion_sense_fifo.h | 70 |
19 files changed, 512 insertions, 436 deletions
diff --git a/baseboard/grunt/baseboard.h b/baseboard/grunt/baseboard.h index ceff4d6e71..27d87091b2 100644 --- a/baseboard/grunt/baseboard.h +++ b/baseboard/grunt/baseboard.h @@ -185,11 +185,13 @@ /* Thermal */ #define CONFIG_TEMP_SENSOR_SB_TSI +#ifndef VARIANT_GRUNT_NO_SENSORS /* FIFO size is a power of 2. */ #define CONFIG_ACCEL_FIFO 256 /* Depends on how fast the AP boots and typical ODRs. */ #define CONFIG_ACCEL_FIFO_THRES (CONFIG_ACCEL_FIFO / 3) +#endif /* VARIANT_GRUNT_NO_SENSORS */ #define USB_PD_PORT_ANX74XX 0 #define USB_PD_PORT_PS8751 1 diff --git a/baseboard/kukui/baseboard.h b/baseboard/kukui/baseboard.h index 1c1b8a980c..317c51be51 100644 --- a/baseboard/kukui/baseboard.h +++ b/baseboard/kukui/baseboard.h @@ -140,8 +140,10 @@ #define GPIO_LID_OPEN GPIO_HALL_INT_L /* FIFO size is in power of 2. */ +#ifndef VARIANT_KUKUI_NO_SENSORS #define CONFIG_ACCEL_FIFO 256 #define CONFIG_ACCEL_FIFO_THRES (CONFIG_ACCEL_FIFO / 3) +#endif /* VARIANT_KUKUI_NO_SENSORS */ /* USB PD config */ #define CONFIG_CHARGE_MANAGER diff --git a/board/careena/board.h b/board/careena/board.h index cfe0abd479..f8b225713a 100644 --- a/board/careena/board.h +++ b/board/careena/board.h @@ -9,6 +9,7 @@ #define __CROS_EC_BOARD_H #define VARIANT_GRUNT_TCPC_0_ANX3429 +#define VARIANT_GRUNT_NO_SENSORS #include "baseboard.h" diff --git a/board/flapjack/board.h b/board/flapjack/board.h index 15b52782b0..a7420a55eb 100644 --- a/board/flapjack/board.h +++ b/board/flapjack/board.h @@ -114,9 +114,11 @@ #define CONFIG_TABLET_MODE #define CONFIG_TABLET_MODE_SWITCH +#ifdef SECTION_IS_RW /* FIFO size is in power of 2. */ #define CONFIG_ACCEL_FIFO 256 #define CONFIG_ACCEL_FIFO_THRES (CONFIG_ACCEL_FIFO / 3) +#endif /* SECTION_IS_RW */ /* Wireless Power Charger Config */ #ifdef SECTION_IS_RW diff --git a/board/jacuzzi/board.h b/board/jacuzzi/board.h index bad56feb97..08a7974032 100644 --- a/board/jacuzzi/board.h +++ b/board/jacuzzi/board.h @@ -11,6 +11,10 @@ #define VARIANT_KUKUI_BATTERY_SMART #define VARIANT_KUKUI_CHARGER_ISL9238 +#ifndef SECTION_IS_RW +#define VARIANT_KUKUI_NO_SENSORS +#endif /* SECTION_IS_RW */ + #include "baseboard.h" /* TODO(b:135086465) led implementation */ diff --git a/board/kodama/board.h b/board/kodama/board.h index d865edefcd..44b1cae9ca 100644 --- a/board/kodama/board.h +++ b/board/kodama/board.h @@ -12,6 +12,10 @@ #define VARIANT_KUKUI_CHARGER_MT6370 #define VARIANT_KUKUI_DP_MUX_GPIO +#ifndef SECTION_IS_RW +#define VARIANT_KUKUI_NO_SENSORS +#endif /* SECTION_IS_RW */ + #include "baseboard.h" #define CONFIG_VOLUME_BUTTONS diff --git a/board/kukui/board.c b/board/kukui/board.c index 3d7b514a71..8f54d78f3b 100644 --- a/board/kukui/board.c +++ b/board/kukui/board.c @@ -306,7 +306,7 @@ DECLARE_HOOK(HOOK_INIT, board_rev_init, HOOK_PRIO_INIT_ADC + 1); /* Motion sensors */ /* Mutexes */ -#ifdef SECTION_IS_RW +#ifndef VARIANT_KUKUI_NO_SENSORS static struct mutex g_lid_mutex; static struct bmi160_drv_data_t g_bmi160_data; @@ -486,7 +486,7 @@ const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); const struct motion_sensor_t *motion_als_sensors[] = { &motion_sensors[CLEAR_ALS], }; -#endif /* SECTION_IS_RW */ +#endif /* VARIANT_KUKUI_NO_SENSORS */ void usb_charger_set_switches(int port, enum usb_switch setting) { diff --git a/board/kukui/board.h b/board/kukui/board.h index c017dc6c8c..4fd1a76a50 100644 --- a/board/kukui/board.h +++ b/board/kukui/board.h @@ -17,6 +17,10 @@ #define VARIANT_KUKUI_CHARGER_MT6370 #define VARIANT_KUKUI_DP_MUX_GPIO +#ifndef SECTION_IS_RW +#define VARIANT_KUKUI_NO_SENSORS +#endif /* SECTION_IS_RW */ + #include "baseboard.h" #define CONFIG_VOLUME_BUTTONS diff --git a/board/liara/board.h b/board/liara/board.h index e5260ee06e..b869f9f8c1 100644 --- a/board/liara/board.h +++ b/board/liara/board.h @@ -9,6 +9,7 @@ #define __CROS_EC_BOARD_H #define VARIANT_GRUNT_TCPC_0_ANX3429 +#define VARIANT_GRUNT_NO_SENSORS #include "baseboard.h" diff --git a/common/build.mk b/common/build.mk index 6b999e1fe9..e38f1bdeb8 100644 --- a/common/build.mk +++ b/common/build.mk @@ -17,6 +17,7 @@ common-$(CONFIG_ACCELGYRO_BMI160)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DS0)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DSM)+=math_util.o common-$(CONFIG_ACCELGYRO_LSM6DSO)+=math_util.o +common-$(CONFIG_ACCEL_FIFO)+=motion_sense_fifo.o common-$(CONFIG_ACCEL_LIS2DW12)+=math_util.o common-$(CONFIG_ACCEL_LIS2DH)+=math_util.o common-$(CONFIG_ACCEL_KXCJ9)+=math_util.o @@ -261,4 +262,4 @@ include $(_common_dir)fpsensor/build.mk include $(_common_dir)usbc/build.mk include $(_common_dir)mock/build.mk -common-y+=$(foreach m,$(mock-y),mock/$(m))
\ No newline at end of file +common-y+=$(foreach m,$(mock-y),mock/$(m)) diff --git a/common/motion_sense.c b/common/motion_sense.c index 17806aabfe..424da27011 100644 --- a/common/motion_sense.c +++ b/common/motion_sense.c @@ -19,6 +19,7 @@ #include "math_util.h" #include "mkbp_event.h" #include "motion_sense.h" +#include "motion_sense_fifo.h" #include "motion_lid.h" #include "power.h" #include "queue.h" @@ -63,13 +64,7 @@ static int accel_disp; */ #define MOTION_SENSOR_INT_ADJUSTMENT_US 10 -/* - * Mutex to protect sensor values between host command task and - * motion sense task: - * When we process CMD_DUMP, we want to be sure the motion sense - * task is not updating the sensor values at the same time. - */ -static struct mutex g_sensor_mutex; +struct mutex g_sensor_mutex; /* * Current power level (S0, S3, S5, ...) @@ -83,394 +78,6 @@ 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; -#ifdef CONFIG_ACCEL_FIFO - -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 */ -static int wake_up_needed; - -/* Number of element the AP should collect */ -static int fifo_queue_count; -static int fifo_int_enabled; - -struct queue motion_sense_fifo = QUEUE_NULL(CONFIG_ACCEL_FIFO, - struct ec_response_motion_sensor_data); -static 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; -#ifdef CONFIG_TABLET_MODE - data->flags |= (tablet_get_mode() ? - MOTIONSENSE_SENSOR_FLAG_TABLET_MODE : 0); -#endif - - /* - * 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; - } -} - -enum motion_sense_async_event { - ASYNC_EVENT_FLUSH = MOTIONSENSE_SENSOR_FLAG_FLUSH | - MOTIONSENSE_SENSOR_FLAG_TIMESTAMP, - ASYNC_EVENT_ODR = MOTIONSENSE_SENSOR_FLAG_ODR | - MOTIONSENSE_SENSOR_FLAG_TIMESTAMP, -}; - -static 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(); -} - -static 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 = 0; - - /* 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)); -} - -static 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; -} -#endif - static inline int motion_sensor_in_forced_mode( const struct motion_sensor_t *sensor) { @@ -1369,18 +976,6 @@ void motion_sense_task(void *u) } } -#ifdef CONFIG_ACCEL_FIFO -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); -#endif /*****************************************************************************/ /* Host commands */ 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); + diff --git a/driver/accelgyro_bmi160.c b/driver/accelgyro_bmi160.c index 5fb79194d1..6408bfcc79 100644 --- a/driver/accelgyro_bmi160.c +++ b/driver/accelgyro_bmi160.c @@ -17,6 +17,7 @@ #include "hwtimer.h" #include "i2c.h" #include "math_util.h" +#include "motion_sense_fifo.h" #include "spi.h" #include "task.h" #include "timer.h" diff --git a/driver/accelgyro_lsm6dsm.c b/driver/accelgyro_lsm6dsm.c index e2bba975d6..1a098f3b1e 100644 --- a/driver/accelgyro_lsm6dsm.c +++ b/driver/accelgyro_lsm6dsm.c @@ -15,6 +15,7 @@ #include "hwtimer.h" #include "mag_cal.h" #include "math_util.h" +#include "motion_sense_fifo.h" #include "queue.h" #include "task.h" #include "timer.h" diff --git a/driver/als_si114x.c b/driver/als_si114x.c index 2ac8f1ccbd..4db3fcc9d7 100644 --- a/driver/als_si114x.c +++ b/driver/als_si114x.c @@ -14,6 +14,7 @@ #include "hwtimer.h" #include "i2c.h" #include "math_util.h" +#include "motion_sense_fifo.h" #include "task.h" #include "timer.h" #include "util.h" diff --git a/driver/als_tcs3400.c b/driver/als_tcs3400.c index 6b58735ad2..774efd863c 100644 --- a/driver/als_tcs3400.c +++ b/driver/als_tcs3400.c @@ -12,6 +12,7 @@ #include "hwtimer.h" #include "i2c.h" #include "math_util.h" +#include "motion_sense_fifo.h" #include "task.h" #include "util.h" diff --git a/driver/sync.c b/driver/sync.c index 34df824090..91d161765a 100644 --- a/driver/sync.c +++ b/driver/sync.c @@ -12,6 +12,7 @@ #include "console.h" #include "driver/sync.h" #include "hwtimer.h" +#include "motion_sense_fifo.h" #include "queue.h" #include "task.h" #include "util.h" diff --git a/include/motion_sense.h b/include/motion_sense.h index 8c3f0cdbde..b0e5244e3b 100644 --- a/include/motion_sense.h +++ b/include/motion_sense.h @@ -227,6 +227,14 @@ struct motion_sensor_t { uint32_t max_frequency; }; +/* + * Mutex to protect sensor values between host command task and + * motion sense task: + * When we process CMD_DUMP, we want to be sure the motion sense + * task is not updating the sensor values at the same time. + */ +extern struct mutex g_sensor_mutex; + /* Defined at board level. */ extern struct motion_sensor_t motion_sensors[]; @@ -249,32 +257,6 @@ extern unsigned int motion_min_interval; */ #define MOTION_SENSE_HOOK_PRIO (HOOK_PRIO_DEFAULT) -#ifdef CONFIG_ACCEL_FIFO -extern struct queue motion_sense_fifo; - -/** - * 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. - * - * @param data data to insert in the FIFO - * @param sensor sensor the data comes from - * @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_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. - */ -void motion_sense_fifo_commit_data(void); - -#endif - /** * Take actions at end of sensor initialization: * - print init done status to console, diff --git a/include/motion_sense_fifo.h b/include/motion_sense_fifo.h new file mode 100644 index 0000000000..f0d5f853dd --- /dev/null +++ b/include/motion_sense_fifo.h @@ -0,0 +1,70 @@ +/* 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. + */ + +#ifndef __CROS_EC_MOTION_SENSE_FIFO_H +#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; + +enum motion_sense_async_event { + ASYNC_EVENT_FLUSH = MOTIONSENSE_SENSOR_FLAG_FLUSH | + MOTIONSENSE_SENSOR_FLAG_TIMESTAMP, + ASYNC_EVENT_ODR = MOTIONSENSE_SENSOR_FLAG_ODR | + MOTIONSENSE_SENSOR_FLAG_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. + * + * @param data data to insert in the FIFO + * @param sensor sensor the data comes from + * @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_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. + */ +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. + * + * @param timestamp The timestamp to stage. + */ +void motion_sense_fifo_stage_timestamp(uint32_t timestamp); + +/** + * Get information about the fifo. + * + * @param fifo_info The struct to store the info. + */ +void motion_sense_get_fifo_info( + struct ec_response_motion_sense_fifo_info *fifo_info); + +#endif /* __CROS_EC_MOTION_SENSE_FIFO_H */ |