diff --git a/.clang-format b/.clang-format index fdcd735fe283f..dc561d739b9c0 100644 --- a/.clang-format +++ b/.clang-format @@ -80,7 +80,7 @@ IncludeCategories: Priority: 3 IndentCaseLabels: false IndentWidth: 8 -InsertBraces: true +#InsertBraces: true SpaceBeforeParens: ControlStatementsExceptControlMacros SortIncludes: Never UseTab: Always diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index b1ac7a12873b4..ed909f3659b1d 100644 --- a/drivers/sensor/CMakeLists.txt +++ b/drivers/sensor/CMakeLists.txt @@ -135,13 +135,8 @@ add_subdirectory_ifdef(CONFIG_TMD2620 tmd2620) add_subdirectory_ifdef(CONFIG_ZEPHYR_NTC_THERMISTOR zephyr_thermistor) add_subdirectory_ifdef(CONFIG_S11059 s11059) -if(CONFIG_USERSPACE OR CONFIG_SENSOR_SHELL OR CONFIG_SENSOR_SHELL_BATTERY) -# The above if() is needed or else CMake would complain about -# empty library. - zephyr_library() +zephyr_library_sources(default_rtio_sensor.c) zephyr_library_sources_ifdef(CONFIG_USERSPACE sensor_handlers.c) zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL sensor_shell.c) zephyr_library_sources_ifdef(CONFIG_SENSOR_SHELL_BATTERY shell_battery.c) - -endif() diff --git a/drivers/sensor/Kconfig b/drivers/sensor/Kconfig index 4a2349accb5e9..2b3f8cf1c3021 100644 --- a/drivers/sensor/Kconfig +++ b/drivers/sensor/Kconfig @@ -5,6 +5,8 @@ menuconfig SENSOR bool "Sensor drivers" + select RTIO + select RTIO_SYS_MEM_BLOCKS help Include sensor drivers in system config diff --git a/drivers/sensor/akm09918c/akm09918c.c b/drivers/sensor/akm09918c/akm09918c.c index 759c00c91f484..708619cd35da8 100644 --- a/drivers/sensor/akm09918c/akm09918c.c +++ b/drivers/sensor/akm09918c/akm09918c.c @@ -15,11 +15,31 @@ #include #include "akm09918c.h" +#include "akm09918c_decoder.h" +#include "akm09918c_internal.h" #include "akm09918c_reg.h" +#include "akm09918c_rtio.h" LOG_MODULE_REGISTER(AKM09918C, CONFIG_SENSOR_LOG_LEVEL); static int akm09918c_sample_fetch(const struct device *dev, enum sensor_channel chan) +{ + return akm09918c_sample_fetch_helper(dev, chan, &data->x_sample, &data->y_sample, + &data->z_sample); +} + +/** + * @brief Perform the bus transaction to fetch samples + * + * @param dev Sensor device to operate on + * @param chan Channel ID to fetch + * @param x Location to write X channel sample or null. + * @param y Location to write Y channel sample or null. + * @param z Location to write Z channel sample or null. + * @return int 0 if successful or error code + */ +static int akm09918c_sample_fetch_helper(const struct device *dev, enum sensor_channel chan, + int16_t *x, int16_t *y, int16_t *z) { struct akm09918c_data *data = dev->data; const struct akm09918c_config *cfg = dev->config; @@ -54,9 +74,12 @@ static int akm09918c_sample_fetch(const struct device *dev, enum sensor_channel return -EBUSY; } - data->x_sample = sys_le16_to_cpu(buf[1] | (buf[2] << 8)); - data->y_sample = sys_le16_to_cpu(buf[3] | (buf[4] << 8)); - data->z_sample = sys_le16_to_cpu(buf[5] | (buf[6] << 8)); + if (x) + *x = sys_le16_to_cpu(buf[1] | (buf[2] << 8)); + if (y) + *y = sys_le16_to_cpu(buf[3] | (buf[4] << 8)); + if (z) + *z = sys_le16_to_cpu(buf[5] | (buf[6] << 8)); return 0; } @@ -204,6 +227,314 @@ static int akm09918c_init(const struct device *dev) return 0; } +int akm09981c_convert_raw_to_q31(enum sensor_channel chan, int16_t reading, q31_t *out) +{ + /* Convert and store reading with separate whole and fractional parts */ + struct sensor_value val; + + switch (chan) { + case SENSOR_CHAN_MAGN_XYZ: + case SENSOR_CHAN_MAGN_X: + case SENSOR_CHAN_MAGN_Y: + case SENSOR_CHAN_MAGN_Z: + akm09918c_convert(&val, reading); + *out = (val.val1 << AKM09918C_RTIO_SHIFT | val.val2); + break; + default: + return -ENOTSUP; + } + + return 0; +} + +/** + * @brief Count number of samples needed to fulfill requested list of channels (a single + * channel like SENSOR_CHAN_MAGN_XYZ may count as three). + * + * @param channels Pointer to channel list + * @param num_channels Length of channel list + * @return int Number of samples needed + */ +static int compute_num_samples(const enum sensor_channel *channels, size_t num_channels) +{ + int num_samples = 0; + + for (size_t i = 0; i < num_channels; ++i) { + num_samples += SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1; + } + + return num_samples; +} + +/** + * @brief Calculate the minimum size RTIO buffer needed to fulfill the request. + * + * @param num_channels Count of requested channels (i.e. number of channel headers) + * @param num_output_samples Count of requested samples (i.e. payload) + * @return uint32_t Size of buffer needed to store the above + */ +static uint32_t compute_min_buf_len(size_t num_channels, int num_output_samples) +{ + return sizeof(struct sensor_data_generic_header) + + (num_channels * sizeof(struct sensor_data_generic_channel)) + + (num_output_samples * sizeof(int16_t)); +} + +/** + * @brief Process an RTIO submission queue event. + * + * @param sensor Reference to the AKM09918C sensor being targetted + * @param iodev_sqe Reference to RTIO submission queue + * @return int 0 if successful or error code + */ +int akm09918c_submit(const struct device *sensor, struct rtio_iodev_sqe *iodev_sqe) +{ + + const struct sensor_read_config *cfg = iodev_sqe->sqe->iodev->data; + const enum sensor_channel *const channels = cfg->channels; + + const size_t num_channels = cfg->count; + int num_output_samples = compute_num_samples(channels, num_channels); + uint32_t min_buf_len = compute_min_buf_len(num_channels, num_output_samples); + + uint8_t *buf; + uint32_t buf_len; + + int ret; + + /* Get RTIO memory buffer */ + int ret = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len); + if (ret) { + LOG_ERR("rtio_sqe_rx_buf failed with code %d", ret); + + rtio_iodev_sqe_err(iodev_sqe, ret); + return ret; + } + + /* Insert header */ + struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buf; + + header->timestamp_ns = k_ticks_to_ns_floor64(k_uptime_ticks()); + header->num_channels = num_channels; + + /* Loop through and add channel header(s) */ + for (int i = 0; i < num_channels; i++) { + header->channel_info[i].channel = channels[i]; + header->channel_info[i].shift = AKM09918C_RTIO_SHIFT; + } + + /* Fetch and insert data for each channel */ + uint16_t *sample_data = buf + sizeof(struct sensor_data_generic_header) + + (num_channels * sizeof(struct sensor_data_generic_header)); + + for (i = 0; i < num_channels; i++) { + switch (channels[i]) { + case SENSOR_CHAN_MAGN_X: + ret = akm09918c_sample_fetch_helper(sensor, channels[i], &sample_data, NULL, + NULL); + sample_data++; + break; + case SENSOR_CHAN_MAGN_Y: + ret = akm09918c_sample_fetch_helper(sensor, channels[i], NULL, &sample_data, + NULL); + sample_data++; + break; + case SENSOR_CHAN_MAGN_Z: + ret = akm09918c_sample_fetch_helper(sensor, channels[i], NULL, NULL, + &sample_data); + sample_data++; + break; + case SENSOR_CHAN_MAGN_XYZ: + ret = akm09918c_sample_fetch_helper(sensor, channels[i], &sample_data[0], + &sample_data[1], &sample_data[2]); + sample_data += 3; + break; + } + + if (ret) { + LOG_ERR("Failed to fetch channel %d (%d)", channels[i], ret); + + rtio_iodev_sqe_err(iodev_sqe, ret); + return ret; + } + + sample_data += chan_samples; + } + + rtio_iodev_sqe_ok(iodev_sqe, 0); + + return 0; +} + +static void akm09918c_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe->iodev->data; + const struct device *dev = cfg->sensor; + const struct sensor_driver_api *api = dev->api; + + if (api->submit != NULL) { + // TODO: Implement Sensor v2 + api->submit(dev, iodev_sqe); + } else { + akm09918c_submit(dev, iodev_sqe); + } +} + +struct rtio_iodev_api __akm09918c_iodev_api = { + .submit = akm09918c_iodev_submit, +}; + +/** + * @brief Decode an RTIO sensor data buffer and extract samples in Q31 format. + * + * @param buffer Buffer received through RTIO context + * @param fit Frame iterator + * @param cit Channel iterator + * @param channels Pointer to list of channels + * @param values Pointer to memory for writing out decoded samples + * @param max_count Maximum number of Q31 samples to write to `values`. + * @return int 0 if successful or error code. + */ +static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, + uint8_t max_count) +{ + const struct sensor_data_generic_header *header = + (const struct sensor_data_generic_header *)buffer; + const int16_t *samples = + (const int16_t *)(buffer + sizeof(struct sensor_data_generic_header) + + header->num_channels * + sizeof(struct sensor_data_generic_channel)); + + int total_samples = 0; + + if (!fit || !*fit) { + /* TODO: support reading multiple frames in one buffer. For now, consider it an + * error if `fit > 0`. + */ + LOG_ERR("frame iterator NULL or non-zero"); + return -EINVAL; + } + + /* Track sample offset in the buffer */ + int sample_offset = 0; + + /* Calculate how many samples exist in buffer. */ + for (int i = 0; i < header->num_channels; ++i) { + int num_samples_in_channel = + SENSOR_CHANNEL_3_AXIS(header->channel_info[i].channel) ? 3 : 1; + + total_samples += num_samples_in_channel; + + if (i < *cit) { + /* While looping, calculate sample offset for the current value of the + * channel iterator. */ + sample_offset += num_samples_in_channel; + } + } + + int offset_in_triplet = 0; + + /* Loop through channels, ensuring the channel iterator does not overrun the end of the + * frame and that no more than `max_count` samples get written out. + */ + for (int i = 0; *cit < total_samples && i < max_count; i++) { + enum sensor_channel channel = header->channel_info[*cit].channel; + + if (SENSOR_CHANNEL_3_AXIS(channel)) { + channels[i] = channel - 3 + offset_in_triplet; + values[i] = akm09981c_convert_raw_to_q31( + samples[sample_offset + offset_in_triplet]); + offset_in_triplet++; + if (offset_in_triplet == 3) { + *cit++; + sample_offset += 3; + offset_in_triplet = 0; + } + } else { + channels[i] = channel; + values[i] = akm09981c_convert_raw_to_q31(samples[sample_offset]); + sample_offset++; + *cit++; + } + } + + if (*cit >= num_samples) { + /* All samples in frame have been read. Reset channel iterator and advance frame + * iterator. */ + *fit++; + *cit = 0; + } + return 0; +} + +/** + * @brief Get the number of frames from the RTIO sensor data buffer. We only support one-shot mode + * for now, so this will always be 1. + * + * @param buffer Data buffer. + * @param frame_count Output param for frame count. + * @return int 0 if successful or error code. + */ +static int get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + *frame_count = 1; + return 0; +} + +/** + * @brief Get the timestamp value from an RTIO sensor data buffer + * + * @param buffer Data buffer. + * @param timestamp_ns Output param for timestamp. + * @return int 0 if successful or error code. + */ +static int get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + *timestamp_ns = ((struct sensor_data_generic_header *)buffer)->timestamp_ns; + return 0; +} + +/** + * @brief Get the shift offset used for encoding Q31 ints. All channels use a fixed shift of + * AKM09918C, so just verify the channel is supported + * + * @param buffer Unused + * @param channel_type Channel ID enum + * @param shift Output param to write shift to + * @return int 0 if successful, -EINVAL if unsupported channel. + */ +static int get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift) +{ + ARG_UNUSED(buffer); + + switch (channel_type) { + case SENSOR_CHAN_MAGN_XYZ: + case SENSOR_CHAN_MAGN_X: + case SENSOR_CHAN_MAGN_Y: + case SENSOR_CHAN_MAGN_Z: + *shift = AKM09918C_RTIO_SHIFT; + return 0; + } + + /* Unsupported channel */ + return -EINVAL; +} + +struct sensor_decoder_api akm09981c_decoder = { + .get_frame_count = get_frame_count, + .get_timestamp = get_timestamp, + .get_shift = get_shift, + .decode = decode, +}; + +int akm09981c_get_decoder(const struct device *dev, struct sensor_decoder_api *decoder) +{ + *decoder = akm09981c_decoder; + + return 0; +} + #define AKM09918C_DEFINE(inst) \ static struct akm09918c_data akm09918c_data_##inst; \ \ diff --git a/drivers/sensor/akm09918c/akm09918c.h b/drivers/sensor/akm09918c/akm09918c.h index 9bb1b6db40621..a0088d8d7566c 100644 --- a/drivers/sensor/akm09918c/akm09918c.h +++ b/drivers/sensor/akm09918c/akm09918c.h @@ -16,6 +16,7 @@ /* Conversion values */ #define AKM09918C_MICRO_GAUSS_PER_BIT INT64_C(500) +#define AKM09918C_RTIO_SHIFT (16) struct akm09918c_data { int16_t x_sample; diff --git a/drivers/sensor/default_rtio_sensor.c b/drivers/sensor/default_rtio_sensor.c new file mode 100644 index 0000000000000..183489723ea5c --- /dev/null +++ b/drivers/sensor/default_rtio_sensor.c @@ -0,0 +1,371 @@ +/* + * Copyright (c) 2023 Google LLC. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include +#include + +LOG_MODULE_REGISTER(rtio_sensor, CONFIG_SENSOR_LOG_LEVEL); + +static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe); + +static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe->iodev->data; + const struct device *dev = cfg->sensor; + const struct sensor_driver_api *api = dev->api; + + if (api->submit != NULL) { + api->submit(dev, iodev_sqe); + } else if (!cfg->is_streaming) { + sensor_submit_fallback(dev, iodev_sqe); + } +} + +struct rtio_iodev_api __sensor_iodev_api = { + .submit = sensor_iodev_submit, +}; + +/** + * @brief Compute the number of samples needed for the given channels + * + * @param[in] channels Array of channels requested + * @param[in] num_channels Number of channels on the @p channels array + * @return The number of samples required to read the given channels + */ +static inline int compute_num_samples(const enum sensor_channel *channels, size_t num_channels) +{ + int num_samples = 0; + + for (size_t i = 0; i < num_channels; ++i) { + num_samples += SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1; + } + + return num_samples; +} + +/** + * @brief Compute the minimum number of bytes needed + * + * @param[in] num_output_samples The number of samples to represent + * @return The number of bytes needed for this sample frame + */ +static inline uint32_t compute_min_buf_len(size_t num_channels, int num_output_samples) +{ + return sizeof(struct sensor_data_generic_header) + (num_output_samples * sizeof(q31_t)) + + (num_channels * sizeof(struct sensor_data_generic_channel)); +} + +/** + * @brief Fallback function for retrofiting old drivers to rtio + * + * @param[in] dev The sensor device to read + * @param[in] iodev_sqe The read submission queue event + */ +static void sensor_submit_fallback(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) +{ + const struct sensor_read_config *cfg = iodev_sqe->sqe->iodev->data; + const enum sensor_channel *const channels = cfg->channels; + const size_t num_channels = cfg->num_channels; + int num_output_samples = compute_num_samples(channels, num_channels); + uint32_t min_buf_len = compute_min_buf_len(num_channels, num_output_samples); + uint64_t timestamp_ns = k_ticks_to_ns_floor64(k_uptime_ticks()); + int rc = sensor_sample_fetch(dev); + uint8_t *buf; + uint32_t buf_len; + + /* Check that the fetch succeeded */ + if (rc != 0) { + LOG_ERR("Failed to fetch samples"); + rtio_iodev_sqe_err(iodev_sqe, rc); + return; + } + + /* Get the buffer for the frame, it may be allocated dynamically by the rtio context */ + rc = rtio_sqe_rx_buf(iodev_sqe, min_buf_len, min_buf_len, &buf, &buf_len); + if (rc != 0) { + LOG_ERR("Failed to get a read buffer of size %u bytes", min_buf_len); + rtio_iodev_sqe_err(iodev_sqe, rc); + return; + } + + /* Set the timestamp and num_channels */ + struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buf; + + header->timestamp_ns = timestamp_ns; + header->num_channels = num_channels; + + uint8_t current_channel_index = 0; + q31_t *q = (q31_t *)(buf + sizeof(struct sensor_data_generic_header) + + num_channels * sizeof(struct sensor_data_generic_channel)); + + for (size_t i = 0; i < num_channels; ++i) { + struct sensor_value value[3]; + int num_samples = SENSOR_CHANNEL_3_AXIS(channels[i]) ? 3 : 1; + + /* Get the current channel requested by the user */ + rc = sensor_channel_get(dev, channels[i], value); + if (rc != 0) { + LOG_ERR("Failed to get channel %d", channels[i]); + rtio_iodev_sqe_err(iodev_sqe, rc); + return; + } + + /* Get the largest absolute value reading to set the scale for the channel */ + uint32_t header_scale = 0; + + for (int sample = 0; sample < num_samples; ++sample) { + uint32_t scale = (uint32_t)llabs((int64_t)value[sample].val1 + 1); + + header_scale = MAX(header_scale, scale); + } + header->channel_info[i].channel = channels[i]; + header->channel_info[i].shift = ilog2(header_scale - 1) + 1; + + /* + * Spread the q31 values. This is needed because some channels are 3D. If + * the user specified one of those then num_samples will be 3; and we need to + * produce 3 separate readings. + */ + for (int sample = 0; sample < num_samples; ++sample) { + /* Convert the value to micro-units */ + int64_t value_u = + value[sample].val1 * INT64_C(1000000) + value[sample].val2; + + /* Convert to q31 using the shift */ + q[current_channel_index + sample] = + ((value_u * ((INT64_C(1) << 31) - 1)) / 1000000) >> + header->channel_info[i].shift; + + LOG_DBG("value[%d]=%s%d.%06d, q[%d]=%d", sample, value_u < 0 ? "-" : "", + abs((int)(value_u / 1000000)), abs((int)(value_u % 1000000)), + current_channel_index + sample, q[current_channel_index + sample]); + } + current_channel_index += num_samples; + } + rtio_iodev_sqe_ok(iodev_sqe, 0); +} + +void z_sensor_processing_loop(struct rtio *ctx, sensor_processing_callback_t cb) +{ + void *userdata = NULL; + uint8_t *buf = NULL; + uint32_t buf_len = 0; + int rc; + + /* Wait for a CQE */ + struct rtio_cqe *cqe = rtio_cqe_consume_block(ctx); + + /* Cache the data from the CQE */ + rc = cqe->result; + userdata = cqe->userdata; + rtio_cqe_get_mempool_buffer(ctx, cqe, &buf, &buf_len); + + /* Release the CQE */ + rtio_cqe_release(ctx); + + /* Call the callback */ + cb(rc, buf, buf_len, userdata); + + /* Release the memory */ + rtio_release_buffer(ctx, buf, buf_len); +} + +/** + * @brief Default decoder get frame count + * + * Default reader can only ever service a single frame at a time. + * + * @param[in] buffer The data buffer to parse + * @param[out] frame_count The number of frames in the buffer (always 1) + * @return 0 in all cases + */ +static int get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + *frame_count = 1; + return 0; +} + +/** + * @brief Default decoder get the timestamp of the first frame + * + * @param[in] buffer The data buffer to parse + * @param[out] timestamp_ns The timestamp of the first frame + * @return 0 in all cases + */ +static int get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + *timestamp_ns = ((struct sensor_data_generic_header *)buffer)->timestamp_ns; + return 0; +} + +/** + * @brief Check if @p needle is a subchannel of @p haystack + * + * Returns true when: + * - @p haystack and @p needle are the same + * - @p haystack is a 3D channel and @p needle is a subchannel. For example: @p haystack is + * :c:enum:`SENSOR_CHAN_ACCEL_XYZ` and @p needle is :c:enum:`SENSOR_CHAN_ACCEL_X`. + * + * @param needle The channel to search + * @param haystack The potential superset of channels. + * @return true When @p needle can be found in @p haystack + * @return false Otherwise + */ +static inline bool is_subchannel(enum sensor_channel needle, enum sensor_channel haystack) +{ + if (needle == haystack) { + return true; + } + if (!SENSOR_CHANNEL_3_AXIS(haystack)) { + return false; + } + return (haystack == SENSOR_CHAN_ACCEL_XYZ && + (needle == SENSOR_CHAN_ACCEL_X || needle == SENSOR_CHAN_ACCEL_Y || + needle == SENSOR_CHAN_ACCEL_Z)) || + (haystack == SENSOR_CHAN_GYRO_XYZ && + (needle == SENSOR_CHAN_GYRO_X || needle == SENSOR_CHAN_GYRO_Y || + needle == SENSOR_CHAN_GYRO_Z)) || + (haystack == SENSOR_CHAN_MAGN_XYZ && + (needle == SENSOR_CHAN_MAGN_X || needle == SENSOR_CHAN_MAGN_Y || + needle == SENSOR_CHAN_MAGN_Z)); +} + +/** + * @brief Default decoder get the bitshift of the given channel (if possible) + * + * @param[in] buffer The data buffer to parse + * @param[in] channel_type The channel to query + * @param[out] shift The bitshift for the q31 value + * @return 0 on success + * @return -EINVAL if the @p channel_type couldn't be found + */ +static int get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift) +{ + struct sensor_data_generic_header *header = (struct sensor_data_generic_header *)buffer; + + for (uint8_t i = 0; i < header->num_channels; ++i) { + if (is_subchannel(channel_type, header->channel_info[i].channel)) { + *shift = header->channel_info[i].shift; + return 0; + } + } + return -EINVAL; +} + +/** + * @brief Compute the channel_info index and offset in the header + * + * This is a helper function for decoding which allows mapping the current channel iterator @p cit + * to the current channel_info object in the header along with a potential offset if the channel is + * a 3D channel. + * + * @param[in] header The data header to parse + * @param[in] cit The channel iterator where decoding is starting + * @param[out] index The index of the @p header's channel_info matching the @p cit + * @param[out] offset the offset of the channel value if the channel value is a 3D channel, + * 0 otherwise. + * @return 0 on success + * @return -EINVAL if the index couldn't be computed + */ +static int compute_channel_info_index(const struct sensor_data_generic_header *header, + sensor_channel_iterator_t cit, int *index, int *offset) +{ + sensor_channel_iterator_t i = { 0 }; + + *index = 0; + do { + sensor_channel_iterator_t last_i = i; + + i += SENSOR_CHANNEL_3_AXIS(header->channel_info[*index].channel) ? 3 : 1; + + if (i > cit) { + *offset = cit - last_i; + return 0; + } + *index += 1; + } while (*index < header->num_channels); + + return -EINVAL; +} + +/** + * @brief Default decoder decode N samples + * + * Decode up to N samples starting at the provided @p fit and @p cit. The appropriate channel types + * and q31 values will be placed in @p values and @p channels respectively. + * + * @param[in] buffer The data buffer to decode + * @param[in,out] fit The starting frame iterator + * @param[in,out] cit The starting channel iterator + * @param[out] channels The decoded channel types + * @param[out] values The decoded q31 values + * @param[in] max_count The maximum number of values to decode + * @return > 0 The number of decoded values + * @return 0 Nothing else to decode on this @p buffer + * @return < 0 Error + */ +static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, + uint8_t max_count) +{ + const struct sensor_data_generic_header *header = + (const struct sensor_data_generic_header *)buffer; + int num_samples = 0; + const q31_t *q = + (const q31_t *)(buffer + sizeof(struct sensor_data_generic_header) + + header->num_channels * sizeof(struct sensor_data_generic_channel)); + + if (*fit != 0) { + return -EINVAL; + } + + for (int i = 0; i < header->num_channels; ++i) { + num_samples += SENSOR_CHANNEL_3_AXIS(header->channel_info[i].channel) ? 3 : 1; + } + + int count = 0; + int channel_info_index; + int offset; + + if (compute_channel_info_index(header, *cit, &channel_info_index, &offset) != 0) { + LOG_ERR("Could not compute the channel index"); + return -EINVAL; + } + + for (; *cit < num_samples && count < max_count; ++count) { + enum sensor_channel channel = header->channel_info[channel_info_index].channel; + + channels[count] = channel; + if (SENSOR_CHANNEL_3_AXIS(channel)) { + channels[count] = channel - 3 + offset; + offset++; + if (offset == 3) { + channel_info_index++; + offset = 0; + } + } else { + channel_info_index++; + } + values[count] = q[*cit]; + *cit += 1; + } + + if (*cit >= num_samples) { + *fit += 1; + *cit = 0; + } + return count; +} + +struct sensor_decoder_api __sensor_default_decoder = { + .get_frame_count = get_frame_count, + .get_timestamp = get_timestamp, + .get_shift = get_shift, + .decode = decode, +}; diff --git a/drivers/sensor/sensor_handlers.c b/drivers/sensor/sensor_handlers.c index 16a462c1645dc..ccca6c882612a 100644 --- a/drivers/sensor/sensor_handlers.c +++ b/drivers/sensor/sensor_handlers.c @@ -57,3 +57,33 @@ static inline int z_vrfy_sensor_channel_get(const struct device *dev, (struct sensor_value *)val); } #include + +static inline int z_vrfy_sensor_get_decoder(const struct device *dev, + struct sensor_decoder_api *decoder) +{ + struct sensor_decoder_api k_decoder; + int rc; + + Z_OOPS(Z_SYSCALL_OBJ(dev, K_OBJ_DRIVER_SENSOR)); + Z_OOPS(Z_SYSCALL_MEMORY_READ(decoder, sizeof(struct sensor_decoder_api))); + rc = z_impl_sensor_get_decoder(dev, &k_decoder); + + if (rc == 0) { + z_user_to_copy(decoder, &k_decoder, sizeof(struct sensor_decoder_api)); + } + + return rc; +} +#include + +static inline int z_vrfy_sensor_reconfigure_read_iodev(struct rtio_iodev *iodev, + const struct device *sensor, + const enum sensor_channel *channels, + size_t num_channels) +{ + Z_OOPS(Z_SYSCALL_OBJ(iodev, K_OBJ_RTIO_IODEV)); + Z_OOPS(Z_SYSCALL_OBJ(sensor, K_OBJ_DRIVER_SENSOR)); + Z_OOPS(Z_SYSCALL_MEMORY_READ(channels, sizeof(enum sensor_channel) * num_channels)); + return z_impl_sensor_reconfigure_read_iodev(iodev, sensor, channels, num_channels); +} +#include diff --git a/drivers/sensor/sensor_shell.c b/drivers/sensor/sensor_shell.c index 7366ec816c937..c49c5cd6af380 100644 --- a/drivers/sensor/sensor_shell.c +++ b/drivers/sensor/sensor_shell.c @@ -4,12 +4,15 @@ * SPDX-License-Identifier: Apache-2.0 */ -#include +#include #include #include -#include + #include #include +#include +#include +#include #define SENSOR_GET_HELP \ "Get sensor data. Channel names are optional. All channels are read " \ @@ -113,6 +116,20 @@ enum dynamic_command_context { static enum dynamic_command_context current_cmd_ctx = NONE; +/* Crate a single common config for one-shot reading */ +static enum sensor_channel iodev_sensor_shell_channels[SENSOR_CHAN_ALL]; +static struct sensor_read_config iodev_sensor_shell_read_config = { + .sensor = NULL, + .channels = iodev_sensor_shell_channels, + .num_channels = 0, + .max_channels = ARRAY_SIZE(iodev_sensor_shell_channels), +}; +RTIO_IODEV_DEFINE(iodev_sensor_shell_read, &__sensor_iodev_api, &iodev_sensor_shell_read_config); + +/* Create the RTIO context to service the reading */ +RTIO_EXECUTOR_SIMPLE_DEFINE(sensor_shell_executor); +RTIO_DEFINE_WITH_MEMPOOL(sensor_read_rtio, &sensor_shell_executor, 1, 1, 8, 64, 4); + static int parse_named_int(const char *name, const char *heystack[], size_t count) { char *endptr; @@ -175,43 +192,71 @@ static int parse_sensor_value(const char *val_str, struct sensor_value *out) return 0; } -static int handle_channel_by_name(const struct shell *shell_ptr, const struct device *dev, - const char *channel_name) +struct sensor_shell_processing_context { + const struct device *dev; + const struct shell *sh; +}; + +static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t buf_len, + void *userdata) { - struct sensor_value value[3]; - int err; - const int i = - parse_named_int(channel_name, sensor_channel_name, ARRAY_SIZE(sensor_channel_name)); + struct sensor_shell_processing_context *ctx = userdata; + struct sensor_decoder_api decoder; + sensor_frame_iterator_t fit = {0}; + sensor_channel_iterator_t cit = {0}; + uint64_t timestamp; + enum sensor_channel channel; + q31_t q; + int rc; - if (i < 0) { - shell_error(shell_ptr, "Channel not supported (%s)", channel_name); - return i; + ARG_UNUSED(buf_len); + + if (result != 0) { + shell_error(ctx->sh, "Read failed"); + return; } - err = sensor_channel_get(dev, i, value); - if (err < 0) { - return err; + rc = sensor_get_decoder(ctx->dev, &decoder); + if (rc != 0) { + shell_error(ctx->sh, "Failed to get decoder for '%s'", ctx->dev->name); + return; } - if (i >= ARRAY_SIZE(sensor_channel_name)) { - shell_print(shell_ptr, "channel idx=%d value = %10.6f", i, - sensor_value_to_double(&value[0])); - } else if (i != SENSOR_CHAN_ACCEL_XYZ && i != SENSOR_CHAN_GYRO_XYZ && - i != SENSOR_CHAN_MAGN_XYZ) { - shell_print(shell_ptr, "channel idx=%d %s = %10.6f", i, sensor_channel_name[i], - sensor_value_to_double(&value[0])); - } else { - /* clang-format off */ - shell_print(shell_ptr, - "channel idx=%d %s x = %10.6f y = %10.6f z = %10.6f", - i, sensor_channel_name[i], - sensor_value_to_double(&value[0]), - sensor_value_to_double(&value[1]), - sensor_value_to_double(&value[2])); - /* clang-format on */ + rc = decoder.get_timestamp(buf, ×tamp); + if (rc != 0) { + shell_error(ctx->sh, "Failed to get fetch timestamp for '%s'", ctx->dev->name); + return; } + shell_print(ctx->sh, "Got samples at %" PRIu64 " ns", timestamp); - return 0; + while (decoder.decode(buf, &fit, &cit, &channel, &q, 1) > 0) { + int8_t shift; + + rc = decoder.get_shift(buf, channel, &shift); + if (rc != 0) { + shell_error(ctx->sh, "Failed to get bitshift for channel %d", channel); + continue; + } + + int64_t scaled_value = (int64_t)q << shift; + bool is_negative = scaled_value < 0; + int numerator; + int denominator; + + scaled_value = llabs(scaled_value); + numerator = (int)FIELD_GET(GENMASK64(31 + shift, 31), scaled_value); + denominator = + (int)((FIELD_GET(GENMASK64(30, 0), scaled_value) * 1000000) / INT32_MAX); + + if (channel >= ARRAY_SIZE(sensor_channel_name)) { + shell_print(ctx->sh, "channel idx=%d value=%s%d.%06d", channel, + is_negative ? "-" : "", numerator, denominator); + } else { + shell_print(ctx->sh, "channel idx=%d %s value=%s%d.%06d", channel, + sensor_channel_name[channel], is_negative ? "-" : "", numerator, + denominator); + } + } } static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) @@ -225,27 +270,46 @@ static int cmd_get_sensor(const struct shell *sh, size_t argc, char *argv[]) return -ENODEV; } - err = sensor_sample_fetch(dev); - if (err < 0) { - shell_error(sh, "Failed to read sensor: %d", err); - } - if (argc == 2) { /* read all channels */ - for (int i = 0; i < ARRAY_SIZE(sensor_channel_name); i++) { - if (sensor_channel_name[i]) { - handle_channel_by_name(sh, dev, sensor_channel_name[i]); - } + for (int i = 0; i < ARRAY_SIZE(iodev_sensor_shell_channels); ++i) { + iodev_sensor_shell_channels[i] = i; } + iodev_sensor_shell_read_config.num_channels = + iodev_sensor_shell_read_config.max_channels; } else { - for (int i = 2; i < argc; i++) { - err = handle_channel_by_name(sh, dev, argv[i]); - if (err < 0) { + /* read specific channels */ + iodev_sensor_shell_read_config.num_channels = 0; + for (int i = 2; i < argc; ++i) { + int chan = parse_named_int(argv[i], sensor_channel_name, + ARRAY_SIZE(sensor_channel_name)); + + if (chan < 0) { shell_error(sh, "Failed to read channel (%s)", argv[i]); + continue; } + iodev_sensor_shell_channels[iodev_sensor_shell_read_config.num_channels++] = + chan; } } + /* TODO(yperess) use sensor_reconfigure_read_iodev? */ + if (iodev_sensor_shell_read_config.num_channels == 0) { + shell_error(sh, "No channels to read, bailing"); + return -EINVAL; + } + iodev_sensor_shell_read_config.sensor = dev; + + struct sensor_shell_processing_context ctx = { + .dev = dev, + .sh = sh, + }; + err = sensor_read(&iodev_sensor_shell_read, &sensor_read_rtio, &ctx); + if (err < 0) { + shell_error(sh, "Failed to read sensor: %d", err); + } + z_sensor_processing_loop(&sensor_read_rtio, sensor_shell_processing_callback); + return 0; } diff --git a/drivers/spi/spi_sam.c b/drivers/spi/spi_sam.c index 8673d0a867621..ebc0b9758e413 100644 --- a/drivers/spi/spi_sam.c +++ b/drivers/spi/spi_sam.c @@ -21,7 +21,6 @@ LOG_MODULE_REGISTER(spi_sam); #include #include #include -#include #include #include #include @@ -55,8 +54,8 @@ struct spi_sam_data { #ifdef CONFIG_SPI_RTIO struct rtio *r; /* context for thread calls */ struct rtio_iodev iodev; - struct rtio_iodev_sqe *iodev_sqe; - struct rtio_sqe *sqe; + struct rtio_iodev_sqe *txn_head; + struct rtio_iodev_sqe *txn_curr; struct spi_dt_spec dt_spec; #endif @@ -304,7 +303,7 @@ static void dma_callback(const struct device *dma_dev, void *user_data, struct spi_sam_data *drv_data = dev->data; #ifdef CONFIG_SPI_RTIO - if (drv_data->iodev_sqe != NULL) { + if (drv_data->txn_head != NULL) { spi_sam_iodev_complete(dev, status); return; } @@ -323,7 +322,7 @@ static int spi_sam_dma_txrx(const struct device *dev, const struct spi_sam_config *drv_cfg = dev->config; struct spi_sam_data *drv_data = dev->data; #ifdef CONFIG_SPI_RTIO - bool blocking = drv_data->iodev_sqe == NULL; + bool blocking = drv_data->txn_head == NULL; #else bool blocking = true; #endif @@ -648,12 +647,13 @@ static bool spi_sam_is_regular(const struct spi_buf_set *tx_bufs, #else static void spi_sam_iodev_complete(const struct device *dev, int status); +static void spi_sam_iodev_next(const struct device *dev, bool completion); static void spi_sam_iodev_start(const struct device *dev) { const struct spi_sam_config *cfg = dev->config; struct spi_sam_data *data = dev->data; - struct rtio_sqe *sqe = data->sqe; + struct rtio_sqe *sqe = &data->txn_curr->sqe; int ret = 0; switch (sqe->op) { @@ -671,9 +671,10 @@ static void spi_sam_iodev_start(const struct device *dev) break; default: LOG_ERR("Invalid op code %d for submission %p\n", sqe->op, (void *)sqe); - rtio_iodev_sqe_err(data->iodev_sqe, -EINVAL); - data->iodev_sqe = NULL; - data->sqe = NULL; + struct rtio_iodev_sqe *txn_head = data->txn_head; + + spi_sam_iodev_next(dev, true); + rtio_iodev_sqe_err(txn_head, -EINVAL); ret = 0; } if (ret == 0) { @@ -687,7 +688,7 @@ static void spi_sam_iodev_next(const struct device *dev, bool completion) k_spinlock_key_t key = spi_spin_lock(dev); - if (!completion && data->iodev_sqe != NULL) { + if (!completion && data->txn_curr != NULL) { spi_spin_unlock(dev, key); return; } @@ -697,17 +698,17 @@ static void spi_sam_iodev_next(const struct device *dev, bool completion) if (next != NULL) { struct rtio_iodev_sqe *next_sqe = CONTAINER_OF(next, struct rtio_iodev_sqe, q); - data->iodev_sqe = next_sqe; - data->sqe = (struct rtio_sqe *)next_sqe->sqe; + data->txn_head = next_sqe; + data->txn_curr = next_sqe; } else { - data->iodev_sqe = NULL; - data->sqe = NULL; + data->txn_head = NULL; + data->txn_curr = NULL; } spi_spin_unlock(dev, key); - if (data->iodev_sqe != NULL) { - struct spi_dt_spec *spi_dt_spec = data->sqe->iodev->data; + if (data->txn_curr != NULL) { + struct spi_dt_spec *spi_dt_spec = data->txn_curr->sqe.iodev->data; struct spi_config *spi_cfg = &spi_dt_spec->config; spi_sam_configure(dev, spi_cfg); @@ -720,15 +721,15 @@ static void spi_sam_iodev_complete(const struct device *dev, int status) { struct spi_sam_data *data = dev->data; - if (data->sqe->flags & RTIO_SQE_TRANSACTION) { - data->sqe = rtio_spsc_next(data->iodev_sqe->r->sq, data->sqe); + if (data->txn_curr->sqe.flags & RTIO_SQE_TRANSACTION) { + data->txn_curr = rtio_txn_next(data->txn_curr); spi_sam_iodev_start(dev); } else { - struct rtio_iodev_sqe *iodev_sqe = data->iodev_sqe; + struct rtio_iodev_sqe *txn_head = data->txn_head; spi_context_cs_control(&data->ctx, false); spi_sam_iodev_next(dev, true); - rtio_iodev_sqe_ok(iodev_sqe, status); + rtio_iodev_sqe_ok(txn_head, status); } } @@ -760,20 +761,27 @@ static int spi_sam_transceive(const struct device *dev, dt_spec->config = *config; - sqe = spi_rtio_copy(data->r, &data->iodev, tx_bufs, rx_bufs); - if (sqe == NULL) { - err = -ENOMEM; + int ret = spi_rtio_copy(data->r, &data->iodev, tx_bufs, rx_bufs, &sqe); + + if (ret < 0) { + err = ret; goto done; } /* Submit request and wait */ - rtio_submit(data->r, 1); + rtio_submit(data->r, ret); - cqe = rtio_cqe_consume(data->r); + while (ret > 0) { + cqe = rtio_cqe_consume(data->r); - err = cqe->result; + if (cqe->result < 0) { + err = cqe->result; + } + + rtio_cqe_release(data->r, cqe); - rtio_cqe_release(data->r); + ret--; + } #else const struct spi_sam_config *cfg = dev->config; @@ -905,10 +913,8 @@ static const struct spi_driver_api spi_sam_driver_api = { COND_CODE_1(SPI_SAM_USE_DMA(n), (SPI_DMA_INIT(n)), ()) \ } -#define SPI_SAM_RTIO_DEFINE(n) \ - RTIO_EXECUTOR_SIMPLE_DEFINE(spi_sam_exec_##n); \ - RTIO_DEFINE(spi_sam_rtio_##n, (struct rtio_executor *)&spi_sam_exec_##n, \ - CONFIG_SPI_SAM_RTIO_SQ_SIZE, 1) +#define SPI_SAM_RTIO_DEFINE(n) RTIO_DEFINE(spi_sam_rtio_##n, CONFIG_SPI_SAM_RTIO_SQ_SIZE, \ + CONFIG_SPI_SAM_RTIO_SQ_SIZE) #define SPI_SAM_DEVICE_INIT(n) \ PINCTRL_DT_INST_DEFINE(n); \ diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index e7f5ceba5bfe9..619b69f8fe6b3 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -19,9 +19,13 @@ * @{ */ +#include +#include + #include #include -#include +#include +#include #ifdef __cplusplus extern "C" { @@ -394,12 +398,186 @@ typedef int (*sensor_channel_get_t)(const struct device *dev, enum sensor_channel chan, struct sensor_value *val); +/** + * @typedef sensor_frame_iterator_t + * @brief Used for iterating over the data frames via the :c:struct:`sensor_decoder_api` + * + * Example usage: + * + * @code(.c) + * sensor_frame_iterator_t fit = {0}, fit_last; + * sensor_channel_iterator_t cit = {0}, cit_last; + * + * while (true) { + * int num_decoded_channels; + * enum sensor_channel channel; + * q31_t value; + * + * fit_last = fit; + * num_decoded_channels = decoder->decode(buffer, &fit, &cit, &channel, &value, 1); + * + * if (num_decoded_channels <= 0) { + * printk("Done decoding buffer\n"); + * break; + * } + * + * printk("Decoded channel (%d) with value %s0.%06" PRIi64 "\n", q < 0 ? "-" : "", + * abs(q) * INT64_C(1000000) / (INT64_C(1) << 31)); + * + * if (fit_last != fit) { + * printk("Finished decoding frame\n"); + * } + * } + * @endcode + */ +typedef uint32_t sensor_frame_iterator_t; + +/** + * @typedef sensor_channel_iterator_t + * @brief Used for iterating over data channels in the same frame via :c:struct:`sensor_decoder_api` + */ +typedef uint32_t sensor_channel_iterator_t; + +/** + * @brief Decodes a single raw data buffer + * + * Data buffers are provided on the :c:struct:`rtio` context that's supplied to + * c:func:`sensor_read`. + */ +struct sensor_decoder_api { + /** + * @brief Get the number of frames in the current buffer. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context. + * @param[out] frame_count The number of frames on the buffer (at least 1) + * @return 0 on success + * @return <0 on error + */ + int (*get_frame_count)(const uint8_t *buffer, uint16_t *frame_count); + + /** + * @brief Get the timestamp associated with the first frame. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context. + * @param[out] timestamp_ns The closest timestamp for when the first frame was generated + * as attained by :c:func:`k_uptime_ticks`. + * @return 0 on success + * @return <0 on error + */ + int (*get_timestamp)(const uint8_t *buffer, uint64_t *timestamp_ns); + + /** + * @brief Get the shift count of a particular channel (multiplier) + * + * This value can be used by shifting the q31_t value resulting in the SI unit of the + * reading. It is guaranteed that the shift for a channel will not change between frames. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context. + * @param[in] channel_type The c:enum:`sensor_channel` to query + * @param[out] shift The bit shift of the channel for this data buffer. + * @return 0 on success + * @return -EINVAL if the @p channel_type doesn't exist in the buffer + * @return <0 on error + */ + int (*get_shift)(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift); + + /** + * @brief Decode up to N samples from the buffer + * + * This function will never wrap frames. If 1 channel is available in the current frame and + * @p max_count is 2, only 1 channel will be decoded and the frame iterator will be modified + * so that the next call to decode will begin at the next frame. + * + * @param[in] buffer The buffer provided on the :c:struct:`rtio` context + * @param[in,out] fit The current frame iterator + * @param[in,out] cit The current channel iterator + * @param[out] channels The channels that were decoded + * @param[out] values The scaled data that was decoded + * @param[in] max_count The maximum number of channels to decode. + * @return + */ + int (*decode)(const uint8_t *buffer, sensor_frame_iterator_t *fit, + sensor_channel_iterator_t *cit, enum sensor_channel *channels, q31_t *values, + uint8_t max_count); +}; + +/** + * @typedef sensor_get_decoder_t + * @brief Get the decoder associate with the given device + * + * @see sensor_get_decoder for more details + */ +typedef int (*sensor_get_decoder_t)(const struct device *dev, + struct sensor_decoder_api *api); + +/* + * Internal data structure used to store information about the IODevice for async reading and + * streaming sensor data. + */ +struct sensor_read_config { + const struct device *sensor; + const bool is_streaming; + union { + enum sensor_channel *const channels; + enum sensor_trigger *const triggers; + }; + size_t count; + const size_t max; +}; + +/** + * @brief Define a reading instance of a sensor + * + * Use this macro to generate a :c:struct:`rtio_iodev` for reading specific channels. Example: + * + * @code(.c) + * SENSOR_DT_READ_IODEV(icm42688_accelgyro, DT_NODELABEL(icm42688), + * SENSOR_CHAN_ACCEL_XYZ, SENSOR_CHAN_GYRO_XYZ); + * + * int main(void) { + * sensor_read(&icm42688_accelgyro, &rtio); + * } + * @endcode + */ +#define SENSOR_DT_READ_IODEV(name, dt_node, ...) \ + static enum sensor_channel __channel_array_##name[] = {__VA_ARGS__}; \ + static struct sensor_read_config __sensor_read_config_##name = { \ + .sensor = DEVICE_DT_GET(dt_node), \ + .is_streaming = false, \ + .channels = __channel_array_##name, \ + .count = ARRAY_SIZE(__channel_array_##name), \ + .max = ARRAY_SIZE(__channel_array_##name), \ + }; \ + RTIO_IODEV_DEFINE(name, &__sensor_iodev_api, &__sensor_read_config_##name) + +#define SENSOR_DT_STREAM_IODEV(name, dt_node, ...) \ + static enum sensor_trigger_type __trigger_array_##name[] = {__VA_ARGS__}; \ + static struct sensor_read_config __sensor_read_config_##name = { \ + .sensor = DEVICE_DT_GET(dt_node), \ + .is_streaming = true, \ + .triggers = __trigger_array_##name, \ + .count = ARRAY_SIZE(__trigger_array_##name), \ + .max = ARRAY_SIZE(__trigger_array_##name), \ + }; \ + RTIO_IODEV_DEFINE(name &__sensor_iodev_api, &__sensor_read_config_##name) + +/* Used to submit an RTIO sqe to the sensor's iodev */ +typedef int (*sensor_submit_t)(const struct device *sensor, struct rtio_iodev_sqe *sqe); + +/* The default decoder API */ +extern struct sensor_decoder_api __sensor_default_decoder; + +/* The default sensor iodev API */ +extern struct rtio_iodev_api __sensor_iodev_api; + __subsystem struct sensor_driver_api { sensor_attr_set_t attr_set; sensor_attr_get_t attr_get; sensor_trigger_set_t trigger_set; sensor_sample_fetch_t sample_fetch; sensor_channel_get_t channel_get; + sensor_get_decoder_t get_decoder; + sensor_submit_t submit; }; /** @@ -598,6 +776,158 @@ static inline int z_impl_sensor_channel_get(const struct device *dev, return api->channel_get(dev, chan, val); } +/* + * Generic data structure used for encoding sensor channel information and corresponding scale. + */ +struct __attribute__((__packed__)) sensor_data_generic_channel { + int8_t shift; + enum sensor_channel channel; +}; + +/* + * Generic data structure used for encoding the sample timestamp and number of channels sampled. + */ +struct __attribute__((__packed__)) sensor_data_generic_header { + uint64_t timestamp_ns; + size_t num_channels; + struct sensor_data_generic_channel channel_info[0]; +}; + +/** + * @brief checks if a given channel is a 3-axis channel + * + * @param[in] chan The channel to check + * @return true if @p chan is :c:enum:`SENSOR_CHAN_ACCEL_XYZ` + * @return true if @p chan is :c:enum:`SENSOR_CHAN_GYRO_XYZ` + * @return true if @p chan is :c:enum:`SENSOR_CHAN_MAGN_XYZ` + * @return false otherwise + */ +#define SENSOR_CHANNEL_3_AXIS(chan) \ + ((chan) == SENSOR_CHAN_ACCEL_XYZ || (chan) == SENSOR_CHAN_GYRO_XYZ || \ + (chan) == SENSOR_CHAN_MAGN_XYZ) + +/** + * @brief Get the sensor's decoder API + * + * @param[in] dev The sensor device + * @param[in] decoder Pointer to the decoder which will be set upon success + * @return 0 on success + * @return < 0 on error + */ +__syscall int sensor_get_decoder(const struct device *dev, + struct sensor_decoder_api *decoder); + +static inline int z_impl_sensor_get_decoder(const struct device *dev, + struct sensor_decoder_api *decoder) +{ + const struct sensor_driver_api *api = (const struct sensor_driver_api *)dev->api; + + __ASSERT_NO_MSG(api != NULL); + + if (api->get_decoder == NULL) { + *decoder = __sensor_default_decoder; + return 0; + } + + return api->get_decoder(dev, decoder); +} + +/** + * @brief Reconfigure a reading iodev + * + * Allows a reconfiguration of the iodev associated with reading a sample from a sensor. + * + * Important: If the iodev is currently servicing a read operation, the data will likely be + * invalid. Please be sure the flush or wait for all pending operations to complete before using the + * data after a configuration change. + * + * It is also important that the `data` field of the iodev is a :c:struct:`sensor_read_config`. + * + * @param[in] iodev The iodev to reconfigure + * @param[in] sensor The sensor to read from + * @param[in] channels One or more channels to read + * @param[in] num_channels The number of channels in @p channels + * @return 0 on success + * @return < 0 on error + */ +__syscall int sensor_reconfigure_read_iodev(struct rtio_iodev *iodev, const struct device *sensor, + const enum sensor_channel *channels, + size_t num_channels); + +static inline int z_impl_sensor_reconfigure_read_iodev(struct rtio_iodev *iodev, + const struct device *sensor, + const enum sensor_channel *channels, + size_t num_channels) +{ + struct sensor_read_config *cfg = (struct sensor_read_config *)iodev->data; + + if (cfg->max_channels < num_channels) { + return -ENOMEM; + } + + cfg->sensor = sensor; + memcpy(cfg->channels, channels, num_channels * sizeof(enum sensor_channel)); + cfg->num_channels = num_channels; + return 0; +} + +/** + * @brief Read data from a sensor. + * + * Using @p cfg, read one snapshot of data from the device by using the provided RTIO context + * @p ctx. This call will generate a :c:struct:`rtio_sqe` that will leverage the RTIO's internal + * mempool when the time comes to service the read. + * + * @param[in] iodev The iodev created by :c:macro:`SENSOR_DT_READ_IODEV` + * @param[in] ctx The RTIO context to service the read + * @param[in] userdata Optional userdata that will be available when the read is complete + * @return 0 on success + * @return < 0 on error + */ +static inline int sensor_read(struct rtio_iodev *iodev, struct rtio *ctx, void *userdata) +{ + if (IS_ENABLED(CONFIG_USERSPACE)) { + struct rtio_sqe sqe; + + rtio_sqe_prep_read_with_pool(&sqe, iodev, 0, userdata); + rtio_sqe_copy_in(ctx, &sqe, 1); + } else { + struct rtio_sqe *sqe = rtio_sqe_acquire(ctx); + + if (sqe == NULL) { + return -ENOMEM; + } + rtio_sqe_prep_read_with_pool(sqe, iodev, 0, userdata); + } + rtio_submit(ctx, 0); + return 0; +} + +/** + * @typedef sensor_processing_callback_t + * @brief Callback function used with the helper processing loop :c:func:`z_sensor_processing_loop` + * + * @param[in] result The result code of the read (0 being success) + * @param[in] buf The data buffer holding the sensor data + * @param[in] buf_len The length (in bytes) of the @p buf + * @param[in] userdata The optional userdata passed to :c:func:`sensor_read` + */ +typedef void (*sensor_processing_callback_t)(int result, uint8_t *buf, uint32_t buf_len, + void *userdata); + +/** + * @brief Helper function for common processing of sensor data. + * + * This function can be called in a blocking manner after :c:func:`sensor_read` or in a standalone + * thread dedicated to processing. It will wait for a cqe from the RTIO context, once received, it + * will decode the userdata and call the @p cb. Once the @p cb returns, the buffer will be released + * back into @p ctx's mempool if available. + * + * @param[in] ctx The RTIO context to wait on + * @param[in] cb Callback to call when data is ready for processing + */ +void z_sensor_processing_loop(struct rtio *ctx, sensor_processing_callback_t cb); + /** * @brief The value of gravitational constant in micro m/s^2. */ diff --git a/include/zephyr/drivers/spi.h b/include/zephyr/drivers/spi.h index cfd9aa6e537a6..9e0be4c8c4416 100644 --- a/include/zephyr/drivers/spi.h +++ b/include/zephyr/drivers/spi.h @@ -904,7 +904,7 @@ __deprecated static inline int spi_write_async(const struct device *dev, */ static inline void spi_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) { - const struct spi_dt_spec *dt_spec = iodev_sqe->sqe->iodev->data; + const struct spi_dt_spec *dt_spec = iodev_sqe->sqe.iodev->data; const struct device *dev = dt_spec->bus; const struct spi_driver_api *api = (const struct spi_driver_api *)dev->api; @@ -944,19 +944,22 @@ static inline bool spi_is_ready_iodev(const struct rtio_iodev *spi_iodev) /** * @brief Copy the tx_bufs and rx_bufs into a set of RTIO requests * - * @param r RTIO context - * @param tx_bufs Transmit buffer set - * @param rx_bufs Receive buffer set + * @param r rtio context + * @param iodev iodev to transceive with + * @param tx_bufs transmit buffer set + * @param rx_bufs receive buffer set + * @param sqe[out] Last sqe submitted, NULL if not enough memory * - * @retval sqe Last submission in the queue added - * @retval NULL Not enough memory in the context to copy the requests + * @retval Number of submission queue entries + * @retval -ENOMEM out of memory */ -static inline struct rtio_sqe *spi_rtio_copy(struct rtio *r, - struct rtio_iodev *iodev, - const struct spi_buf_set *tx_bufs, - const struct spi_buf_set *rx_bufs) +static inline int spi_rtio_copy(struct rtio *r, + struct rtio_iodev *iodev, + const struct spi_buf_set *tx_bufs, + const struct spi_buf_set *rx_bufs, + struct rtio_sqe **last_sqe) { - struct rtio_sqe *sqe = NULL; + int ret = 0; size_t tx_count = tx_bufs ? tx_bufs->count : 0; size_t rx_count = rx_bufs ? rx_bufs->count : 0; @@ -964,6 +967,8 @@ static inline struct rtio_sqe *spi_rtio_copy(struct rtio *r, uint32_t rx = 0, rx_len = 0; uint8_t *tx_buf, *rx_buf; + struct rtio_sqe *sqe = NULL; + if (tx < tx_count) { tx_buf = tx_bufs->buffers[tx].buf; tx_len = tx_bufs->buffers[tx].len; @@ -985,10 +990,13 @@ static inline struct rtio_sqe *spi_rtio_copy(struct rtio *r, sqe = rtio_sqe_acquire(r); if (sqe == NULL) { - rtio_spsc_drop_all(r->sq); - return NULL; + ret = -ENOMEM; + rtio_sqe_drop_all(r); + goto out; } + ret++; + /* If tx/rx len are same, we can do a simple transceive */ if (tx_len == rx_len) { if (tx_buf == NULL) { @@ -1084,9 +1092,11 @@ static inline struct rtio_sqe *spi_rtio_copy(struct rtio *r, if (sqe != NULL) { sqe->flags = 0; + *last_sqe = sqe; } - return sqe; +out: + return ret; } #endif /* CONFIG_SPI_RTIO */ diff --git a/include/zephyr/linker/common-ram.ld b/include/zephyr/linker/common-ram.ld index a49288eeed79a..39a49a631b686 100644 --- a/include/zephyr/linker/common-ram.ld +++ b/include/zephyr/linker/common-ram.ld @@ -117,13 +117,13 @@ } GROUP_DATA_LINK_IN(RAMABLE_REGION, ROMABLE_REGION) #endif /* CONFIG_USB_DEVICE_BOS */ - #if defined(CONFIG_RTIO) ITERABLE_SECTION_RAM(rtio, 4) ITERABLE_SECTION_RAM(rtio_iodev, 4) + ITERABLE_SECTION_RAM(rtio_sqe_pool, 4) + ITERABLE_SECTION_RAM(rtio_cqe_pool, 4) #endif /* CONFIG_RTIO */ - #ifdef CONFIG_USERSPACE _static_kernel_objects_end = .; #endif diff --git a/include/zephyr/rtio/rtio.h b/include/zephyr/rtio/rtio.h index d6d21ff648ba6..41ae641d3afc8 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -8,25 +8,19 @@ * @file * @brief Real-Time IO device API for moving bytes with low effort * - * RTIO uses a SPSC lock-free queue pair to enable a DMA and ISR friendly I/O API. + * RTIO is a context for asynchronous batch operations using a submission and completion queue. * - * I/O like operations are setup in a pre-allocated queue with a fixed number of - * submission requests. Each submission request takes the device to operate on - * and an operation. The rest is information needed to perform the operation such - * as a register or mmio address of the device, a source/destination buffers - * to read from or write to, and other pertinent information. + * Asynchronous I/O operation are setup in a submission queue. Each entry in the queue describes + * the operation it wishes to perform with some understood semantics. * * These operations may be chained in a such a way that only when the current - * operation is complete will the next be executed. If the current request fails - * all chained requests will also fail. + * operation is complete the next will be executed. If the current operation fails + * all chained operations will also fail. * - * The completion of these requests are pushed into a fixed size completion - * queue which an application may actively poll for completions. + * Operations may also be submitted as a transaction where a set of operations are considered + * to be one operation. * - * An executor (could use a dma controller!) takes the queues and determines - * how to perform each requested operation. By default there is a software - * executor which does all operations in software using software device - * APIs. + * The completion of these operations typically provide one or more completion queue events. */ #ifndef ZEPHYR_INCLUDE_RTIO_RTIO_H_ @@ -38,10 +32,10 @@ #include #include #include -#include #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -128,6 +122,22 @@ extern "C" { */ #define RTIO_SQE_MEMPOOL_BUFFER BIT(2) +/** + * @brief The SQE should not execute if possible + * + * If possible (not yet executed), the SQE should be canceled by flagging it as failed and returning + * -ECANCELED as the result. + */ +#define RTIO_SQE_CANCELED BIT(3) + +/** + * @brief The SQE should continue producing CQEs until canceled + * + * This flag must exist along :c:macro:`RTIO_SQE_MEMPOOL_BUFFER` and signals that when a read is + * complete. It should be placed back in queue until canceled. + */ +#define RTIO_SQE_MULTISHOT BIT(4) + /** * @} */ @@ -184,6 +194,9 @@ extern "C" { struct rtio; struct rtio_cqe; struct rtio_sqe; +struct rtio_sqe_pool; +struct rtio_cqe_pool; +struct rtio_block_pool; struct rtio_iodev; struct rtio_iodev_sqe; /** @endcond */ @@ -210,8 +223,8 @@ struct rtio_sqe { const struct rtio_iodev *iodev; /**< Device to operation on */ /** - * User provided data which is returned upon operation - * completion. Could be a pointer or integer. + * User provided data which is returned upon operation completion. Could be a pointer or + * integer. * * If unique identification of completions is desired this should be * unique as well. @@ -253,85 +266,17 @@ struct rtio_sqe { BUILD_ASSERT(sizeof(struct rtio_sqe) <= 64); /** @endcond */ - -/** - * @brief Submission queue - * - * This is used for typifying the members of an RTIO queue pair - * but nothing more. - */ -struct rtio_sq { - struct rtio_spsc _spsc; - struct rtio_sqe *const buffer; -}; - /** * @brief A completion queue event */ struct rtio_cqe { + struct rtio_mpsc_node q; + int32_t result; /**< Result from operation */ void *userdata; /**< Associated userdata with operation */ uint32_t flags; /**< Flags associated with the operation */ }; -/** - * @brief Completion queue - * - * This is used for typifying the members of an RTIO queue pair - * but nothing more. - */ -struct rtio_cq { - struct rtio_spsc _spsc; - struct rtio_cqe *const buffer; -}; - - -struct rtio_executor_api { - /** - * @brief Submit the request queue to executor - * - * The executor is responsible for interpreting the submission queue and - * creating concurrent execution chains. - * - * Concurrency is optional and implementation dependent. - */ - int (*submit)(struct rtio *r); - - /** - * @brief SQE completes successfully - */ - void (*ok)(struct rtio_iodev_sqe *iodev_sqe, int result); - - /** - * @brief SQE fails to complete successfully - */ - void (*err)(struct rtio_iodev_sqe *iodev_sqe, int result); -}; - -/** - * @brief An executor does the work of executing the submissions. - * - * This could be a DMA controller backed executor, thread backed, - * or simple in place executor. - * - * A DMA executor might schedule all transfers with priorities - * and use hardware arbitration. - * - * A threaded executor might use a thread pool where each transfer - * chain is executed across the thread pool and the priority of the - * transfer is used as the thread priority. - * - * A simple in place exector might simply loop over and execute each - * transfer in the calling threads context. Priority is entirely - * derived from the calling thread then. - * - * An implementation of the executor must place this struct as - * its first member such that pointer aliasing works. - */ -struct rtio_executor { - const struct rtio_executor_api *api; -}; - /** * Internal state of the mempool sqe map entry. */ @@ -348,23 +293,39 @@ enum rtio_mempool_entry_state { /* Check that we can always fit the state in 2 bits */ BUILD_ASSERT(RTIO_MEMPOOL_ENTRY_STATE_COUNT < 4); +struct rtio_sqe_pool { + struct rtio_mpsc free_q; + const uint16_t pool_size; + uint16_t pool_free; + struct rtio_iodev_sqe *pool; +}; + +struct rtio_cqe_pool { + struct rtio_mpsc free_q; + const uint16_t pool_size; + uint16_t pool_free; + struct rtio_cqe *pool; +}; + +struct rtio_block_pool { + /* Memory pool associated with this RTIO context. */ + struct sys_mem_blocks *mempool; + /* The size (in bytes) of a single block in the mempool */ + const uint32_t blk_size; +}; + /** - * @brief An RTIO queue pair that both the kernel and application work with + * @brief An RTIO context containing what can be viewed as a pair of queues. + * + * A queue for submissions (available and in queue to be produced) as well as a queue + * of completions (available and ready to be consumed). * - * The kernel is the consumer of the submission queue, and producer of the completion queue. - * The application is the consumer of the completion queue and producer of the submission queue. + * The rtio executor along with any objects implementing the rtio_iodev interface are + * the consumers of submissions and producers of completions. * - * Nothing is done until a call is performed to do the work (rtio_execute). + * No work is started until rtio_submit is called. */ struct rtio { - - /* - * An executor which does the job of working through the submission - * queue. - */ - struct rtio_executor *executor; - - #ifdef CONFIG_RTIO_SUBMIT_SEM /* A wait semaphore which may suspend the calling thread * to wait for some number of completions when calling submit @@ -387,18 +348,22 @@ struct rtio { */ atomic_t xcqcnt; - /* Submission queue */ - struct rtio_sq *sq; + /* Submission queue object pool with free list */ + struct rtio_sqe_pool *sqe_pool; - /* Completion queue */ - struct rtio_cq *cq; + /* Complete queue object pool with free list */ + struct rtio_cqe_pool *cqe_pool; #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS - /* Memory pool associated with this RTIO context. */ - struct sys_mem_blocks *mempool; - /* The size (in bytes) of a single block in the mempool */ - uint32_t mempool_blk_size; -#endif /* CONFIG_RTIO_SYS_MEM_BLOCKS */ + /* Mem block pool */ + struct rtio_block_pool *block_pool; +#endif + + /* Submission queue */ + struct rtio_mpsc sq; + + /* Completion queue */ + struct rtio_mpsc cq; }; /** The memory partition associated with all RTIO context information */ @@ -415,22 +380,28 @@ extern struct k_mem_partition rtio_partition; static inline uint16_t __rtio_compute_mempool_block_index(const struct rtio *r, const void *ptr) { uintptr_t addr = (uintptr_t)ptr; - uintptr_t buff = (uintptr_t)r->mempool->buffer; - uint32_t buff_size = r->mempool->num_blocks * r->mempool_blk_size; + struct sys_mem_blocks *mem_pool = r->block_pool->mempool; + uint32_t block_size = r->block_pool->blk_size; + + uintptr_t buff = (uintptr_t)mem_pool->buffer; + uint32_t buff_size = mem_pool->num_blocks * block_size; if (addr < buff || addr >= buff + buff_size) { return UINT16_MAX; } - return (addr - buff) / r->mempool_blk_size; + return (addr - buff) / block_size; } #endif /** * @brief IO device submission queue entry + * + * May be cast safely to and from a rtio_sqe as they occupy the same memory provided by the pool */ struct rtio_iodev_sqe { + struct rtio_sqe sqe; struct rtio_mpsc_node q; - const struct rtio_sqe *sqe; + struct rtio_iodev_sqe *next; struct rtio *r; }; @@ -492,8 +463,8 @@ static inline void rtio_sqe_prep_nop(struct rtio_sqe *sqe, const struct rtio_iodev *iodev, void *userdata) { + memset(sqe, 0, sizeof(struct rtio_sqe)); sqe->op = RTIO_OP_NOP; - sqe->flags = 0; sqe->iodev = iodev; sqe->userdata = userdata; } @@ -508,9 +479,9 @@ static inline void rtio_sqe_prep_read(struct rtio_sqe *sqe, uint32_t len, void *userdata) { + memset(sqe, 0, sizeof(struct rtio_sqe)); sqe->op = RTIO_OP_RX; sqe->prio = prio; - sqe->flags = 0; sqe->iodev = iodev; sqe->buf_len = len; sqe->buf = buf; @@ -530,6 +501,14 @@ static inline void rtio_sqe_prep_read_with_pool(struct rtio_sqe *sqe, sqe->flags = RTIO_SQE_MEMPOOL_BUFFER; } +static inline void rtio_sqe_prep_read_multishot(struct rtio_sqe *sqe, + const struct rtio_iodev *iodev, int8_t prio, + void *userdata) +{ + rtio_sqe_prep_read_with_pool(sqe, iodev, prio, userdata); + sqe->flags |= RTIO_SQE_MULTISHOT; +} + /** * @brief Prepare a write op submission */ @@ -540,9 +519,9 @@ static inline void rtio_sqe_prep_write(struct rtio_sqe *sqe, uint32_t len, void *userdata) { + memset(sqe, 0, sizeof(struct rtio_sqe)); sqe->op = RTIO_OP_TX; sqe->prio = prio; - sqe->flags = 0; sqe->iodev = iodev; sqe->buf_len = len; sqe->buf = buf; @@ -568,9 +547,9 @@ static inline void rtio_sqe_prep_tiny_write(struct rtio_sqe *sqe, { __ASSERT_NO_MSG(tiny_write_len <= sizeof(sqe->tiny_buf)); + memset(sqe, 0, sizeof(struct rtio_sqe)); sqe->op = RTIO_OP_TINY_TX; sqe->prio = prio; - sqe->flags = 0; sqe->iodev = iodev; sqe->tiny_buf_len = tiny_write_len; memcpy(sqe->tiny_buf, tiny_write_data, tiny_write_len); @@ -590,9 +569,9 @@ static inline void rtio_sqe_prep_callback(struct rtio_sqe *sqe, void *arg0, void *userdata) { + memset(sqe, 0, sizeof(struct rtio_sqe)); sqe->op = RTIO_OP_CALLBACK; sqe->prio = 0; - sqe->flags = 0; sqe->iodev = NULL; sqe->callback = callback; sqe->arg0 = arg0; @@ -610,9 +589,9 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, uint32_t buf_len, void *userdata) { + memset(sqe, 0, sizeof(struct rtio_sqe)); sqe->op = RTIO_OP_TXRX; sqe->prio = prio; - sqe->flags = 0; sqe->iodev = iodev; sqe->txrx_buf_len = buf_len; sqe->tx_buf = tx_buf; @@ -620,23 +599,83 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, sqe->userdata = userdata; } -/** - * @brief Statically define and initialize a fixed length submission queue. - * - * @param name Name of the submission queue. - * @param len Queue length, power of 2 required (2, 4, 8). - */ -#define RTIO_SQ_DEFINE(name, len) \ - RTIO_SPSC_DEFINE(name, struct rtio_sqe, len) +static inline struct rtio_iodev_sqe *rtio_sqe_pool_alloc(struct rtio_sqe_pool *pool) +{ + struct rtio_mpsc_node *node = rtio_mpsc_pop(&pool->free_q); -/** - * @brief Statically define and initialize a fixed length completion queue. - * - * @param name Name of the completion queue. - * @param len Queue length, power of 2 required (2, 4, 8). - */ -#define RTIO_CQ_DEFINE(name, len) \ - RTIO_SPSC_DEFINE(name, struct rtio_cqe, len) + if (node == NULL) { + return NULL; + } + + struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q); + + memset(iodev_sqe, 0, sizeof(struct rtio_iodev_sqe)); + + pool->pool_free--; + + return iodev_sqe; +} + +static inline void rtio_sqe_pool_free(struct rtio_sqe_pool *pool, struct rtio_iodev_sqe *iodev_sqe) +{ + rtio_mpsc_push(&pool->free_q, &iodev_sqe->q); + + pool->pool_free++; +} + +static inline struct rtio_cqe *rtio_cqe_pool_alloc(struct rtio_cqe_pool *pool) +{ + struct rtio_mpsc_node *node = rtio_mpsc_pop(&pool->free_q); + + if (node == NULL) { + return NULL; + } + + struct rtio_cqe *cqe = CONTAINER_OF(node, struct rtio_cqe, q); + + memset(cqe, 0, sizeof(struct rtio_cqe)); + + pool->pool_free--; + + return cqe; +} + +static inline void rtio_cqe_pool_free(struct rtio_cqe_pool *pool, struct rtio_cqe *cqe) +{ + rtio_mpsc_push(&pool->free_q, &cqe->q); + + pool->pool_free++; +} + +static inline int rtio_block_pool_alloc(struct rtio_block_pool *pool, size_t min_sz, + size_t max_sz, uint8_t **buf, uint32_t *buf_len) +{ + uint32_t bytes = max_sz; + + do { + size_t num_blks = DIV_ROUND_UP(bytes, pool->blk_size); + int rc = sys_mem_blocks_alloc_contiguous(pool->mempool, num_blks, (void **)buf); + + if (rc == 0) { + *buf_len = num_blks * pool->blk_size; + return 0; + } + + bytes -= pool->blk_size; + } while (bytes >= min_sz); + + return -ENOMEM; +} + +static inline void rtio_block_pool_free(struct rtio_block_pool *pool, void *buf, uint32_t buf_len) +{ + size_t num_blks = buf_len / pool->blk_size; + + sys_mem_blocks_free_contiguous(pool->mempool, buf, num_blks); +} + +/* Do not try and reformat the macros */ +/* clang-format off */ /** * @brief Statically define and initialize an RTIO IODev @@ -645,44 +684,31 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, * @param iodev_api Pointer to struct rtio_iodev_api * @param iodev_data Data pointer */ -#define RTIO_IODEV_DEFINE(name, iodev_api, iodev_data) \ - STRUCT_SECTION_ITERABLE(rtio_iodev, name) = { \ - .api = (iodev_api), \ - .iodev_sq = RTIO_MPSC_INIT((name.iodev_sq)), \ - .data = (iodev_data), \ +#define RTIO_IODEV_DEFINE(name, iodev_api, iodev_data) \ + STRUCT_SECTION_ITERABLE(rtio_iodev, name) = { \ + .api = (iodev_api), \ + .iodev_sq = RTIO_MPSC_INIT((name.iodev_sq)), \ + .data = (iodev_data), \ } -/* clang-format off */ -#define _RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ - IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, \ - (static K_SEM_DEFINE(_submit_sem_##name, 0, K_SEM_MAX_LIMIT))) \ - IF_ENABLED(CONFIG_RTIO_CONSUME_SEM, \ - (static K_SEM_DEFINE(_consume_sem_##name, 0, K_SEM_MAX_LIMIT))) \ - RTIO_SQ_DEFINE(_sq_##name, sq_sz); \ - RTIO_CQ_DEFINE(_cq_##name, cq_sz); \ - STRUCT_SECTION_ITERABLE(rtio, name) = { \ - .executor = (struct rtio_executor *)(exec), \ - IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_sem = &_submit_sem_##name,)) \ - IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_count = 0,)) \ - IF_ENABLED(CONFIG_RTIO_CONSUME_SEM, (.consume_sem = &_consume_sem_##name,)) \ - .xcqcnt = ATOMIC_INIT(0), \ - .sq = (struct rtio_sq *const)&_sq_##name, \ - .cq = (struct rtio_cq *const)&_cq_##name, -/* clang-format on */ +#define Z_RTIO_SQE_POOL_DEFINE(name, sz) \ + static struct rtio_iodev_sqe _sqe_pool_##name[sz]; \ + STRUCT_SECTION_ITERABLE(rtio_sqe_pool, name) = { \ + .free_q = RTIO_MPSC_INIT((name.free_q)), \ + .pool_size = sz, \ + .pool_free = sz, \ + .pool = _sqe_pool_##name, \ + } -/** - * @brief Statically define and initialize an RTIO context - * - * @param name Name of the RTIO - * @param exec Symbol for rtio_executor (pointer) - * @param sq_sz Size of the submission queue, must be power of 2 - * @param cq_sz Size of the completion queue, must be power of 2 - */ -/* clang-format off */ -#define RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ - _RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ + +#define Z_RTIO_CQE_POOL_DEFINE(name, sz) \ + static struct rtio_cqe _cqe_pool_##name[sz]; \ + STRUCT_SECTION_ITERABLE(rtio_cqe_pool, name) = { \ + .free_q = RTIO_MPSC_INIT((name.free_q)), \ + .pool_size = sz, \ + .pool_free = sz, \ + .pool = _cqe_pool_##name, \ } -/* clang-format on */ /** * @brief Allocate to bss if available @@ -706,60 +732,136 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, */ #define RTIO_DMEM COND_CODE_1(CONFIG_USERSPACE, (K_APP_DMEM(rtio_partition) static), (static)) +#define Z_RTIO_BLOCK_POOL_DEFINE(name, blk_sz, blk_cnt, blk_align) \ + RTIO_BMEM uint8_t __aligned(WB_UP(blk_align)) \ + _block_pool_##name[blk_cnt*WB_UP(blk_sz)]; \ + _SYS_MEM_BLOCKS_DEFINE_WITH_EXT_BUF(_sys_blocks_##name, WB_UP(blk_sz), \ + blk_cnt, _block_pool_##name, \ + RTIO_DMEM); \ + static struct rtio_block_pool name = { \ + .mempool = &_sys_blocks_##name, \ + .blk_size = blk_sz, \ + } + +#define Z_RTIO_DEFINE(name, _sqe_pool, _cqe_pool, _block_pool) \ + IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, \ + (static K_SEM_DEFINE(_submit_sem_##name, 0, K_SEM_MAX_LIMIT))) \ + IF_ENABLED(CONFIG_RTIO_CONSUME_SEM, \ + (static K_SEM_DEFINE(_consume_sem_##name, 0, K_SEM_MAX_LIMIT))) \ + STRUCT_SECTION_ITERABLE(rtio, name) = { \ + IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_sem = &_submit_sem_##name,)) \ + IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_count = 0,)) \ + IF_ENABLED(CONFIG_RTIO_CONSUME_SEM, (.consume_sem = &_consume_sem_##name,)) \ + .xcqcnt = ATOMIC_INIT(0), \ + .sqe_pool = _sqe_pool, \ + .cqe_pool = _cqe_pool, \ + IF_ENABLED(CONFIG_RTIO_SYS_MEM_BLOCKS, (.block_pool = _block_pool,)) \ + .sq = RTIO_MPSC_INIT((name.sq)), \ + .cq = RTIO_MPSC_INIT((name.cq)), \ + } + +/** + * @brief Statically define and initialize an RTIO context + * + * @param name Name of the RTIO + * @param sq_sz Size of the submission queue entry pool + * @param cq_sz Size of the completion queue entry pool + */ +#define RTIO_DEFINE(name, sq_sz, cq_sz) \ + Z_RTIO_SQE_POOL_DEFINE(name##_sqe_pool, sq_sz); \ + Z_RTIO_CQE_POOL_DEFINE(name##_cqe_pool, cq_sz); \ + Z_RTIO_DEFINE(name, &name##_sqe_pool, &name##_cqe_pool, NULL) \ + +/* clang-format on */ + /** * @brief Statically define and initialize an RTIO context * * @param name Name of the RTIO - * @param exec Symbol for rtio_executor (pointer) * @param sq_sz Size of the submission queue, must be power of 2 * @param cq_sz Size of the completion queue, must be power of 2 * @param num_blks Number of blocks in the memory pool * @param blk_size The number of bytes in each block * @param balign The block alignment */ -/* clang-format off */ -#define RTIO_DEFINE_WITH_MEMPOOL(name, exec, sq_sz, cq_sz, num_blks, blk_size, balign) \ - RTIO_BMEM uint8_t __aligned(WB_UP(balign)) \ - _mempool_buf_##name[num_blks*WB_UP(blk_size)]; \ - _SYS_MEM_BLOCKS_DEFINE_WITH_EXT_BUF(_mempool_##name, WB_UP(blk_size), num_blks, \ - _mempool_buf_##name, RTIO_DMEM); \ - _RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ - .mempool = &_mempool_##name, \ - .mempool_blk_size = WB_UP(blk_size), \ - } +#define RTIO_DEFINE_WITH_MEMPOOL(name, sq_sz, cq_sz, num_blks, blk_size, balign) \ + Z_RTIO_SQE_POOL_DEFINE(name##_sqe_pool, sq_sz); \ + Z_RTIO_CQE_POOL_DEFINE(name##_cqe_pool, cq_sz); \ + Z_RTIO_BLOCK_POOL_DEFINE(name##_block_pool, blk_size, num_blks, balign); \ + Z_RTIO_DEFINE(name, &name##_sqe_pool, &name##_cqe_pool, &name##_block_pool) + /* clang-format on */ /** - * @brief Set the executor of the rtio context + * @brief Count of acquirable submission queue events + * + * @param r RTIO context + * + * @return Count of acquirable submission queue events */ -static inline void rtio_set_executor(struct rtio *r, struct rtio_executor *exc) +static inline uint32_t rtio_sqe_acquirable(struct rtio *r) { - r->executor = exc; + return r->sqe_pool->pool_free; } /** - * @brief Submit to an iodev a submission to work on + * @brief Count of likely, but not gauranteed, consumable completion queue events * - * Should be called by the executor when it wishes to submit work - * to an iodev. + * @param r RTIO context * - * @param iodev_sqe Submission to work on + * @return Likely count of consumable completion queue events */ -static inline void rtio_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) +static inline uint32_t rtio_cqe_consumable(struct rtio *r) { - iodev_sqe->sqe->iodev->api->submit(iodev_sqe); + return (r->cqe_pool->pool_size - r->cqe_pool->pool_free); } /** - * @brief Count of acquirable submission queue events + * @brief Get the next sqe in the transaction * - * @param r RTIO context + * @param iodev_sqe Submission queue entry * - * @return Count of acquirable submission queue events + * @retval NULL if current sqe is last in transaction + * @retval struct rtio_sqe * if available */ -static inline uint32_t rtio_sqe_acquirable(struct rtio *r) +static inline struct rtio_iodev_sqe *rtio_txn_next(const struct rtio_iodev_sqe *iodev_sqe) { - return rtio_spsc_acquirable(r->sq); + if (iodev_sqe->sqe.flags & RTIO_SQE_TRANSACTION) { + return iodev_sqe->next; + } else { + return NULL; + } +} + + +/** + * @brief Get the next sqe in the chain + * + * @param iodev_sqe Submission queue entry + * + * @retval NULL if current sqe is last in chain + * @retval struct rtio_sqe * if available + */ +static inline struct rtio_iodev_sqe *rtio_chain_next(const struct rtio_iodev_sqe *iodev_sqe) +{ + if (iodev_sqe->sqe.flags & RTIO_SQE_CHAINED) { + return iodev_sqe->next; + } else { + return NULL; + } +} + +/** + * @brief Get the next sqe in the chain or transaction + * + * @param iodev_sqe Submission queue entry + * + * @retval NULL if current sqe is last in chain + * @retval struct rtio_iodev_sqe * if available + */ +static inline struct rtio_iodev_sqe *rtio_iodev_sqe_next(const struct rtio_iodev_sqe *iodev_sqe) +{ + return iodev_sqe->next; } /** @@ -772,30 +874,57 @@ static inline uint32_t rtio_sqe_acquirable(struct rtio *r) */ static inline struct rtio_sqe *rtio_sqe_acquire(struct rtio *r) { - return rtio_spsc_acquire(r->sq); + struct rtio_iodev_sqe *iodev_sqe = rtio_sqe_pool_alloc(r->sqe_pool); + + if (iodev_sqe == NULL) { + return NULL; + } + + rtio_mpsc_push(&r->sq, &iodev_sqe->q); + + return &iodev_sqe->sqe; } /** - * @brief Produce all previously acquired sqe + * @brief Drop all previously acquired sqe * * @param r RTIO context */ -static inline void rtio_sqe_produce_all(struct rtio *r) +static inline void rtio_sqe_drop_all(struct rtio *r) { - rtio_spsc_produce_all(r->sq); -} + struct rtio_iodev_sqe *iodev_sqe; + struct rtio_mpsc_node *node = rtio_mpsc_pop(&r->sq); + while (node != NULL) { + iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q); + rtio_sqe_pool_free(r->sqe_pool, iodev_sqe); + node = rtio_mpsc_pop(&r->sq); + } +} /** - * @brief Drop all previously acquired sqe - * - * @param r RTIO context + * @brief Acquire a complete queue event if available */ -static inline void rtio_sqe_drop_all(struct rtio *r) +static inline struct rtio_cqe *rtio_cqe_acquire(struct rtio *r) { - rtio_spsc_drop_all(r->sq); + struct rtio_cqe *cqe = rtio_cqe_pool_alloc(r->cqe_pool); + + if (cqe == NULL) { + return NULL; + } + + memset(cqe, 0, sizeof(struct rtio_cqe)); + + return cqe; } +/** + * @brief Produce a complete queue event if available + */ +static inline void rtio_cqe_produce(struct rtio *r, struct rtio_cqe *cqe) +{ + rtio_mpsc_push(&r->cq, &cqe->q); +} /** * @brief Consume a single completion queue event if available @@ -810,15 +939,22 @@ static inline void rtio_sqe_drop_all(struct rtio *r) */ static inline struct rtio_cqe *rtio_cqe_consume(struct rtio *r) { + struct rtio_mpsc_node *node; + struct rtio_cqe *cqe = NULL; + #ifdef CONFIG_RTIO_CONSUME_SEM - if (k_sem_take(r->consume_sem, K_NO_WAIT) == 0) { - return rtio_spsc_consume(r->cq); - } else { + if (k_sem_take(r->consume_sem, K_NO_WAIT) != 0) { return NULL; } -#else - return rtio_spsc_consume(r->cq); #endif + + node = rtio_mpsc_pop(&r->cq); + if (node == NULL) { + return NULL; + } + cqe = CONTAINER_OF(node, struct rtio_cqe, q); + + return cqe; } /** @@ -833,20 +969,18 @@ static inline struct rtio_cqe *rtio_cqe_consume(struct rtio *r) */ static inline struct rtio_cqe *rtio_cqe_consume_block(struct rtio *r) { + struct rtio_mpsc_node *node; struct rtio_cqe *cqe; #ifdef CONFIG_RTIO_CONSUME_SEM k_sem_take(r->consume_sem, K_FOREVER); - - cqe = rtio_spsc_consume(r->cq); -#else - cqe = rtio_spsc_consume(r->cq); - - while (cqe == NULL) { - cqe = rtio_spsc_consume(r->cq); - - } #endif + node = rtio_mpsc_pop(&r->cq); + while (node == NULL) { + node = rtio_mpsc_pop(&r->cq); + Z_SPIN_DELAY(1); + } + cqe = CONTAINER_OF(node, struct rtio_cqe, q); return cqe; } @@ -855,24 +989,15 @@ static inline struct rtio_cqe *rtio_cqe_consume_block(struct rtio *r) * @brief Release consumed completion queue event * * @param r RTIO context + * @param cqe Completion queue entry */ -static inline void rtio_cqe_release(struct rtio *r) +static inline void rtio_cqe_release(struct rtio *r, struct rtio_cqe *cqe) { - rtio_spsc_release(r->cq); + rtio_cqe_pool_free(r->cqe_pool, cqe); } /** - * @brief Release all consumed completion queue events - * - * @param r RTIO context - */ -static inline void rtio_cqe_release_all(struct rtio *r) -{ - rtio_spsc_release_all(r->cq); -} - -/** - * @brief Compte the CQE flags from the rtio_iodev_sqe entry + * @brief Compute the CQE flags from the rtio_iodev_sqe entry * * @param iodev_sqe The SQE entry in question. * @return The value that should be set for the CQE's flags field. @@ -884,10 +1009,12 @@ static inline uint32_t rtio_cqe_compute_flags(struct rtio_iodev_sqe *iodev_sqe) ARG_UNUSED(iodev_sqe); #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS - if (iodev_sqe->sqe->op == RTIO_OP_RX && iodev_sqe->sqe->flags & RTIO_SQE_MEMPOOL_BUFFER) { + if (iodev_sqe->sqe.op == RTIO_OP_RX && iodev_sqe->sqe.flags & RTIO_SQE_MEMPOOL_BUFFER) { struct rtio *r = iodev_sqe->r; - int blk_index = (iodev_sqe->sqe->buf - r->mempool->buffer) / r->mempool_blk_size; - int blk_count = iodev_sqe->sqe->buf_len / r->mempool_blk_size; + struct sys_mem_blocks *mem_pool = r->block_pool->mempool; + uint32_t block_size = r->block_pool->blk_size; + int blk_index = (iodev_sqe->sqe.buf - mem_pool->buffer) / block_size; + int blk_count = iodev_sqe->sqe.buf_len / block_size; flags = RTIO_CQE_FLAG_PREP_MEMPOOL(blk_index, blk_count); } @@ -922,11 +1049,12 @@ static inline int z_impl_rtio_cqe_get_mempool_buffer(const struct rtio *r, struc int blk_idx = RTIO_CQE_FLAG_MEMPOOL_GET_BLK_IDX(cqe->flags); int blk_count = RTIO_CQE_FLAG_MEMPOOL_GET_BLK_CNT(cqe->flags); - *buff = r->mempool->buffer + blk_idx * r->mempool_blk_size; - *buff_len = blk_count * r->mempool_blk_size; - __ASSERT_NO_MSG(*buff >= r->mempool->buffer); + *buff = r->block_pool->mempool->buffer + blk_idx * r->block_pool->blk_size; + *buff_len = blk_count * r->block_pool->blk_size; + __ASSERT_NO_MSG(*buff >= r->block_pool->mempool->buffer); __ASSERT_NO_MSG(*buff < - r->mempool->buffer + r->mempool_blk_size * r->mempool->num_blocks); + r->block_pool->mempool->buffer + + r->block_pool->blk_size * r->block_pool->mempool->num_blocks); return 0; } return -EINVAL; @@ -940,6 +1068,9 @@ static inline int z_impl_rtio_cqe_get_mempool_buffer(const struct rtio *r, struc #endif } +void rtio_executor_submit(struct rtio *r); +void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result); +void rtio_executor_err(struct rtio_iodev_sqe *iodev_sqe, int result); /** * @brief Inform the executor of a submission completion with success @@ -951,7 +1082,7 @@ static inline int z_impl_rtio_cqe_get_mempool_buffer(const struct rtio *r, struc */ static inline void rtio_iodev_sqe_ok(struct rtio_iodev_sqe *iodev_sqe, int result) { - iodev_sqe->r->executor->api->ok(iodev_sqe, result); + rtio_executor_ok(iodev_sqe, result); } /** @@ -964,7 +1095,7 @@ static inline void rtio_iodev_sqe_ok(struct rtio_iodev_sqe *iodev_sqe, int resul */ static inline void rtio_iodev_sqe_err(struct rtio_iodev_sqe *iodev_sqe, int result) { - iodev_sqe->r->executor->api->err(iodev_sqe, result); + rtio_executor_err(iodev_sqe, result); } /** @@ -998,7 +1129,7 @@ static inline void rtio_iodev_cancel_all(struct rtio_iodev *iodev) */ static inline void rtio_cqe_submit(struct rtio *r, int result, void *userdata, uint32_t flags) { - struct rtio_cqe *cqe = rtio_spsc_acquire(r->cq); + struct rtio_cqe *cqe = rtio_cqe_acquire(r); if (cqe == NULL) { atomic_inc(&r->xcqcnt); @@ -1006,7 +1137,7 @@ static inline void rtio_cqe_submit(struct rtio *r, int result, void *userdata, u cqe->result = result; cqe->userdata = userdata; cqe->flags = flags; - rtio_spsc_produce(r->cq); + rtio_cqe_produce(r, cqe); } #ifdef CONFIG_RTIO_SUBMIT_SEM if (r->submit_count > 0) { @@ -1038,16 +1169,11 @@ static inline void rtio_cqe_submit(struct rtio *r, int result, void *userdata, u static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32_t min_buf_len, uint32_t max_buf_len, uint8_t **buf, uint32_t *buf_len) { - const struct rtio_sqe *sqe = iodev_sqe->sqe; + struct rtio_sqe *sqe = (struct rtio_sqe *)&iodev_sqe->sqe; #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS if (sqe->op == RTIO_OP_RX && sqe->flags & RTIO_SQE_MEMPOOL_BUFFER) { struct rtio *r = iodev_sqe->r; - uint32_t blk_size = r->mempool_blk_size; - struct sys_mem_blocks *pool = r->mempool; - uint32_t bytes = max_buf_len; - int sqe_index = sqe - r->sq->buffer; - struct rtio_sqe *mutable_sqe = &r->sq->buffer[sqe_index]; if (sqe->buf != NULL) { if (sqe->buf_len < min_buf_len) { @@ -1058,21 +1184,14 @@ static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32 return 0; } - do { - size_t num_blks = DIV_ROUND_UP(bytes, blk_size); - int rc = sys_mem_blocks_alloc_contiguous(pool, num_blks, (void **)buf); + int rc = rtio_block_pool_alloc(r->block_pool, min_buf_len, max_buf_len, + buf, buf_len); + if (rc == 0) { + sqe->buf = *buf; + sqe->buf_len = *buf_len; + return 0; + } - if (rc == 0) { - *buf_len = num_blks * blk_size; - mutable_sqe->buf = *buf; - mutable_sqe->buf_len = *buf_len; - return 0; - } - if (bytes == min_buf_len) { - break; - } - bytes = (bytes + min_buf_len) / 2; - } while (bytes >= min_buf_len); return -ENOMEM; } #endif @@ -1104,12 +1223,11 @@ __syscall void rtio_release_buffer(struct rtio *r, void *buff, uint32_t buff_len static inline void z_impl_rtio_release_buffer(struct rtio *r, void *buff, uint32_t buff_len) { #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS - if (r == NULL || buff == NULL || r->mempool == NULL || buff_len == 0) { + if (r == NULL || buff == NULL || r->block_pool == NULL || buff_len == 0) { return; } - sys_mem_blocks_free_contiguous(r->mempool, buff, - DIV_ROUND_UP(buff_len, r->mempool_blk_size)); + rtio_block_pool_free(r->block_pool, buff, buff_len); #endif } @@ -1130,27 +1248,44 @@ static inline void rtio_access_grant(struct rtio *r, struct k_thread *t) } /** - * @brief Copy an array of SQEs into the queue + * @brief Attempt to cancel an SQE * - * Useful if a batch of submissions is stored in ROM or - * RTIO is used from user mode where a copy must be made. + * If possible (not currently executing), cancel an SQE and generate a failure with -ECANCELED + * result. * - * Partial copying is not done as chained SQEs need to be submitted - * as a whole set. + * @param[in] sqe The SQE to cancel + * @return 0 if the SQE was flagged for cancellation + * @return <0 on error + */ +__syscall int rtio_sqe_cancel(struct rtio_sqe *sqe); + +static inline int z_impl_rtio_sqe_cancel(struct rtio_sqe *sqe) +{ + sqe->flags |= RTIO_SQE_CANCELED; + return 0; +} + +/** + * @brief Copy an array of SQEs into the queue and get resulting handles back * - * @param r RTIO context - * @param sqes Pointer to an array of SQEs - * @param sqe_count Count of sqes in array + * Copies one or more SQEs into the RTIO context and optionally returns their generated SQE handles. + * Handles can be used to cancel events via the :c:func:`rtio_sqe_cancel` call. + * + * @param[in] r RTIO context + * @param[in] sqes Pointer to an array of SQEs + * @param[out] handles Optional array of :c:struct:`rtio_sqe` pointers to store the handles. Use + * NULL to ignore. + * @param[in] sqe_count Count of sqes in array * * @retval 0 success * @retval -ENOMEM not enough room in the queue */ -__syscall int rtio_sqe_copy_in(struct rtio *r, - const struct rtio_sqe *sqes, - size_t sqe_count); -static inline int z_impl_rtio_sqe_copy_in(struct rtio *r, - const struct rtio_sqe *sqes, - size_t sqe_count) +__syscall int rtio_sqe_copy_in_get_handles(struct rtio *r, const struct rtio_sqe *sqes, + struct rtio_sqe **handles, size_t sqe_count); + +static inline int z_impl_rtio_sqe_copy_in_get_handles(struct rtio *r, const struct rtio_sqe *sqes, + struct rtio_sqe **handles, + size_t sqe_count) { struct rtio_sqe *sqe; uint32_t acquirable = rtio_sqe_acquirable(r); @@ -1162,14 +1297,36 @@ static inline int z_impl_rtio_sqe_copy_in(struct rtio *r, for (unsigned long i = 0; i < sqe_count; i++) { sqe = rtio_sqe_acquire(r); __ASSERT_NO_MSG(sqe != NULL); + if (handles != NULL) { + handles[i] = sqe; + } *sqe = sqes[i]; } - rtio_sqe_produce_all(r); - return 0; } +/** + * @brief Copy an array of SQEs into the queue + * + * Useful if a batch of submissions is stored in ROM or + * RTIO is used from user mode where a copy must be made. + * + * Partial copying is not done as chained SQEs need to be submitted + * as a whole set. + * + * @param r RTIO context + * @param sqes Pointer to an array of SQEs + * @param sqe_count Count of sqes in array + * + * @retval 0 success + * @retval -ENOMEM not enough room in the queue + */ +static inline int rtio_sqe_copy_in(struct rtio *r, const struct rtio_sqe *sqes, size_t sqe_count) +{ + return rtio_sqe_copy_in_get_handles(r, sqes, NULL, sqe_count); +} + /** * @brief Copy an array of CQEs from the queue * @@ -1194,19 +1351,25 @@ static inline int z_impl_rtio_cqe_copy_out(struct rtio *r, size_t cqe_count, k_timeout_t timeout) { - size_t copied; + size_t copied = 0; struct rtio_cqe *cqe; + uint64_t end = sys_clock_timeout_end_calc(timeout); - for (copied = 0; copied < cqe_count; copied++) { - cqe = rtio_cqe_consume_block(r); + do { + cqe = K_TIMEOUT_EQ(timeout, K_FOREVER) ? rtio_cqe_consume_block(r) + : rtio_cqe_consume(r); if (cqe == NULL) { - break; +#ifdef CONFIG_BOARD_NATIVE_POSIX + /* Native posix fakes the clock and only moves it forward when sleeping. */ + k_sleep(K_TICKS(1)); +#else + Z_SPIN_DELAY(1); +#endif + continue; } - cqes[copied] = *cqe; - } - - - rtio_cqe_release_all(r); + cqes[copied++] = *cqe; + rtio_cqe_release(r, cqe); + } while (copied < cqe_count && end > k_uptime_ticks()); return copied; } @@ -1228,9 +1391,7 @@ __syscall int rtio_submit(struct rtio *r, uint32_t wait_count); static inline int z_impl_rtio_submit(struct rtio *r, uint32_t wait_count) { - int res; - - __ASSERT(r->executor != NULL, "expected rtio submit context to have an executor"); + int res = 0; #ifdef CONFIG_RTIO_SUBMIT_SEM /* TODO undefined behavior if another thread calls submit of course @@ -1244,16 +1405,11 @@ static inline int z_impl_rtio_submit(struct rtio *r, uint32_t wait_count) } #endif - /* Enqueue all prepared submissions */ - rtio_spsc_produce_all(r->sq); - /* Submit the queue to the executor which consumes submissions * and produces completions through ISR chains or other means. */ - res = r->executor->api->submit(r); - if (res != 0) { - return res; - } + rtio_executor_submit(r); + /* TODO could be nicer if we could suspend the thread and not * wake up on each completion here. @@ -1266,7 +1422,7 @@ static inline int z_impl_rtio_submit(struct rtio *r, uint32_t wait_count) "semaphore was reset or timed out while waiting on completions!"); } #else - while (rtio_spsc_consumable(r->cq) < wait_count) { + while (rtio_cqe_consumable(r) < wait_count) { Z_SPIN_DELAY(10); k_yield(); } diff --git a/include/zephyr/rtio/rtio_executor_concurrent.h b/include/zephyr/rtio/rtio_executor_concurrent.h deleted file mode 100644 index 26bda6bfe0a9e..0000000000000 --- a/include/zephyr/rtio/rtio_executor_concurrent.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (c) 2022 Intel Corporation. - * - * SPDX-License-Identifier: Apache-2.0 - */ -#ifndef ZEPHYR_INCLUDE_RTIO_RTIO_EXECUTOR_CONCURRENT_H_ -#define ZEPHYR_INCLUDE_RTIO_RTIO_EXECUTOR_CONCURRENT_H_ - -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * @brief RTIO Concurrent Executor - * - * Provides a concurrent executor with a pointer overhead per task and a - * 2 word overhead over the simple executor to know the order of tasks (fifo). - * - * @defgroup rtio_executor_concurrent RTIO concurrent Executor - * @ingroup rtio - * @{ - */ - - -/** - * @brief Submit to the concurrent executor - * - * @param r RTIO context to submit - * - * @retval 0 always succeeds - */ -int rtio_concurrent_submit(struct rtio *r); - -/** - * @brief Report a SQE has completed successfully - * - * @param sqe RTIO IODev SQE to report success - * @param result Result of the SQE - */ -void rtio_concurrent_ok(struct rtio_iodev_sqe *sqe, int result); - -/** - * @brief Report a SQE has completed with error - * - * @param sqe RTIO IODev SQE to report success - * @param result Result of the SQE - */ -void rtio_concurrent_err(struct rtio_iodev_sqe *sqe, int result); - -/** - * @brief Concurrent Executor - * - * Notably all values are effectively owned by each task with the exception - * of task_in and task_out. - */ -struct rtio_concurrent_executor { - struct rtio_executor ctx; - - /* Lock around the queues */ - struct k_spinlock lock; - - /* Task ring position and count */ - uint16_t task_in, task_out, task_mask; - - /* First pending sqe to start when a task becomes available */ - struct rtio_sqe *last_sqe; - - /* Array of task statuses */ - uint8_t *task_status; - - /* Array of struct rtio_iodev_sqe *'s one per task' */ - struct rtio_iodev_sqe *task_cur; -}; - -/** - * @cond INTERNAL_HIDDEN - */ -static const struct rtio_executor_api z_rtio_concurrent_api = { - .submit = rtio_concurrent_submit, - .ok = rtio_concurrent_ok, - .err = rtio_concurrent_err -}; - -/** - * @endcond INTERNAL_HIDDEN - */ - - -/** - * @brief Statically define and initialie a concurrent executor - * - * @param name Symbol name, must be unique in the context in which its used - * @param concurrency Allowed concurrency (number of concurrent tasks). - */ -#define RTIO_EXECUTOR_CONCURRENT_DEFINE(name, concurrency) \ - static struct rtio_iodev_sqe _task_cur_##name[(concurrency)]; \ - uint8_t _task_status_##name[(concurrency)]; \ - static struct rtio_concurrent_executor name = { \ - .ctx = { .api = &z_rtio_concurrent_api }, \ - .task_in = 0, \ - .task_out = 0, \ - .task_mask = (concurrency)-1, \ - .last_sqe = NULL, \ - .task_status = _task_status_##name, \ - .task_cur = _task_cur_##name, \ - }; - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - - -#endif /* ZEPHYR_INCLUDE_RTIO_RTIO_EXECUTOR_CONCURRENT_H_ */ diff --git a/include/zephyr/rtio/rtio_executor_simple.h b/include/zephyr/rtio/rtio_executor_simple.h deleted file mode 100644 index 3a394b019e7fd..0000000000000 --- a/include/zephyr/rtio/rtio_executor_simple.h +++ /dev/null @@ -1,91 +0,0 @@ -/* - * Copyright (c) 2022 Intel Corporation. - * - * SPDX-License-Identifier: Apache-2.0 - */ -#ifndef ZEPHYR_INCLUDE_RTIO_RTIO_EXECUTOR_SIMPLE_H_ -#define ZEPHYR_INCLUDE_RTIO_RTIO_EXECUTOR_SIMPLE_H_ - -#include - -#ifdef __cplusplus -extern "C" { -#endif - -/** - * @brief RTIO Simple Executor - * - * Provides the simplest possible executor without any concurrency - * or reinterpretation of requests. - * - * @defgroup rtio_executor_simple RTIO Simple Executor - * @ingroup rtio - * @{ - */ - - -/** - * @brief Submit to the simple executor - * - * @param r RTIO context to submit - * - * @retval 0 always succeeds - */ -int rtio_simple_submit(struct rtio *r); - -/** - * @brief Report a SQE has completed successfully - * - * @param iodev_sqe RTIO IODEV SQE to report success - * @param result Result of the SQE - */ -void rtio_simple_ok(struct rtio_iodev_sqe *iodev_sqe, int result); - -/** - * @brief Report a SQE has completed with error - * - * @param iodev_sqe RTIO IODEV SQE to report success - * @param result Result of the SQE - */ -void rtio_simple_err(struct rtio_iodev_sqe *iodev_sqe, int result); - -/** - * @brief Simple Executor - */ -struct rtio_simple_executor { - struct rtio_executor ctx; - struct rtio_iodev_sqe task; -}; - -/** - * @cond INTERNAL_HIDDEN - */ -static const struct rtio_executor_api z_rtio_simple_api = { - .submit = rtio_simple_submit, - .ok = rtio_simple_ok, - .err = rtio_simple_err -}; - -/** - * @endcond INTERNAL_HIDDEN - */ - - -/** - * @brief Define a simple executor with a given name - * - * @param name Symbol name, must be unique in the context in which its used - */ -#define RTIO_EXECUTOR_SIMPLE_DEFINE(name) \ - struct rtio_simple_executor name = { .ctx = { .api = &z_rtio_simple_api } }; - -/** - * @} - */ - -#ifdef __cplusplus -} -#endif - - -#endif /* ZEPHYR_INCLUDE_RTIO_RTIO_EXECUTOR_SIMPLE_H_ */ diff --git a/include/zephyr/rtio/rtio_mpsc.h b/include/zephyr/rtio/rtio_mpsc.h index c4d81c791a0f6..f129f5712aaad 100644 --- a/include/zephyr/rtio/rtio_mpsc.h +++ b/include/zephyr/rtio/rtio_mpsc.h @@ -5,12 +5,12 @@ * SPDX-License-Identifier: Apache-2.0 */ - #ifndef ZEPHYR_RTIO_MPSC_H_ #define ZEPHYR_RTIO_MPSC_H_ #include #include +#include #include #include @@ -25,6 +25,40 @@ extern "C" { * @{ */ +/* + * On single core systems atomics are unnecessary + * and cause a lot of unnecessary cache invalidation + * + * Using volatile to at least ensure memory is read/written + * by the compiler generated op codes is enough. + * + * On SMP atomics *must* be used to ensure the pointers + * are updated in the correct order and the values are + * updated core caches correctly. + */ +#if defined(CONFIG_SMP) + +typedef atomic_ptr_t mpsc_ptr_t; + +#define mpsc_ptr_get(ptr) atomic_ptr_get(&(ptr)) +#define mpsc_ptr_set(ptr, val) atomic_ptr_set(&(ptr), val) +#define mpsc_ptr_set_get(ptr, val) atomic_ptr_set(&(ptr), val) + +#else + +typedef struct rtio_mpsc_node *mpsc_ptr_t; + +#define mpsc_ptr_get(ptr) ptr +#define mpsc_ptr_set(ptr, val) ptr = val +#define mpsc_ptr_set_get(ptr, val) \ + ({ \ + mpsc_ptr_t tmp = ptr; \ + ptr = val; \ + tmp; \ + }) + +#endif + /** * @file rtio_mpsc.h * @@ -45,19 +79,18 @@ extern "C" { * @brief Queue member */ struct rtio_mpsc_node { - atomic_ptr_t next; + mpsc_ptr_t next; }; /** * @brief MPSC Queue */ struct rtio_mpsc { - atomic_ptr_t head; + mpsc_ptr_t head; struct rtio_mpsc_node *tail; struct rtio_mpsc_node stub; }; - /** * @brief Static initializer for a mpsc queue * @@ -81,9 +114,9 @@ struct rtio_mpsc { */ static inline void rtio_mpsc_init(struct rtio_mpsc *q) { - atomic_ptr_set(&q->head, &q->stub); + mpsc_ptr_set(q->head, &q->stub); q->tail = &q->stub; - atomic_ptr_set(&q->stub.next, NULL); + mpsc_ptr_set(q->stub.next, NULL); } /** @@ -92,16 +125,16 @@ static inline void rtio_mpsc_init(struct rtio_mpsc *q) * @param q Queue to push the node to * @param n Node to push into the queue */ -static inline void rtio_mpsc_push(struct rtio_mpsc *q, struct rtio_mpsc_node *n) +static ALWAYS_INLINE void rtio_mpsc_push(struct rtio_mpsc *q, struct rtio_mpsc_node *n) { struct rtio_mpsc_node *prev; int key; - atomic_ptr_set(&n->next, NULL); + mpsc_ptr_set(n->next, NULL); key = arch_irq_lock(); - prev = (struct rtio_mpsc_node *)atomic_ptr_set(&q->head, n); - atomic_ptr_set(&prev->next, n); + prev = (struct rtio_mpsc_node *)mpsc_ptr_set_get(q->head, n); + mpsc_ptr_set(prev->next, n); arch_irq_unlock(key); } @@ -115,7 +148,7 @@ static inline struct rtio_mpsc_node *rtio_mpsc_pop(struct rtio_mpsc *q) { struct rtio_mpsc_node *head; struct rtio_mpsc_node *tail = q->tail; - struct rtio_mpsc_node *next = (struct rtio_mpsc_node *)atomic_ptr_get(&tail->next); + struct rtio_mpsc_node *next = (struct rtio_mpsc_node *)mpsc_ptr_get(tail->next); /* Skip over the stub/sentinel */ if (tail == &q->stub) { @@ -125,7 +158,7 @@ static inline struct rtio_mpsc_node *rtio_mpsc_pop(struct rtio_mpsc *q) q->tail = next; tail = next; - next = (struct rtio_mpsc_node *)atomic_ptr_get(&next->next); + next = (struct rtio_mpsc_node *)mpsc_ptr_get(next->next); } /* If next is non-NULL then a valid node is found, return it */ @@ -134,7 +167,7 @@ static inline struct rtio_mpsc_node *rtio_mpsc_pop(struct rtio_mpsc *q) return tail; } - head = (struct rtio_mpsc_node *)atomic_ptr_get(&q->head); + head = (struct rtio_mpsc_node *)mpsc_ptr_get(q->head); /* If next is NULL, and the tail != HEAD then the queue has pending * updates that can't yet be accessed. @@ -145,7 +178,7 @@ static inline struct rtio_mpsc_node *rtio_mpsc_pop(struct rtio_mpsc *q) rtio_mpsc_push(q, &q->stub); - next = (struct rtio_mpsc_node *)atomic_ptr_get(&tail->next); + next = (struct rtio_mpsc_node *)mpsc_ptr_get(tail->next); if (next != NULL) { q->tail = next; @@ -163,5 +196,4 @@ static inline struct rtio_mpsc_node *rtio_mpsc_pop(struct rtio_mpsc *q) } #endif - #endif /* ZEPHYR_RTIO_MPSC_H_ */ diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 2a9e6a3422169..9f5cc48698e9e 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -5,3 +5,6 @@ CONFIG_SENSOR_SHELL=y CONFIG_SENSOR_INFO=y CONFIG_LOG=y +CONFIG_ICM42688_TRIGGER_NONE=y +CONFIG_ADC_ADS7052_INIT_PRIORITY=99 +CONFIG_RTIO_CONSUME_SEM=y diff --git a/samples/subsys/rtio/sensor_batch_processing/src/main.c b/samples/subsys/rtio/sensor_batch_processing/src/main.c index fbf2c0c300aac..c9306c68f0157 100644 --- a/samples/subsys/rtio/sensor_batch_processing/src/main.c +++ b/samples/subsys/rtio/sensor_batch_processing/src/main.c @@ -6,7 +6,6 @@ #include #include -#include #include LOG_MODULE_REGISTER(main); @@ -21,9 +20,7 @@ LOG_MODULE_REGISTER(main); #define SAMPLE_SIZE DT_PROP(NODE_ID, sample_size) #define PROCESS_TIME ((M - 1) * SAMPLE_PERIOD) -RTIO_EXECUTOR_SIMPLE_DEFINE(simple_exec); -RTIO_DEFINE_WITH_MEMPOOL(ez_io, (struct rtio_executor *)&simple_exec, SQ_SZ, CQ_SZ, N, SAMPLE_SIZE, - 4); +RTIO_DEFINE_WITH_MEMPOOL(ez_io, SQ_SZ, CQ_SZ, N, SAMPLE_SIZE, 4); int main(void) { @@ -32,10 +29,9 @@ int main(void) /* Fill the entire submission queue. */ for (int n = 0; n < N; n++) { - struct rtio_sqe *sqe = rtio_spsc_acquire(ez_io.sq); + struct rtio_sqe *sqe = rtio_sqe_acquire(&ez_io); rtio_sqe_prep_read_with_pool(sqe, iodev, RTIO_PRIO_HIGH, NULL); - rtio_spsc_produce(ez_io.sq); } while (true) { @@ -51,7 +47,7 @@ int main(void) * an FFT. */ while (m < M) { - struct rtio_cqe *cqe = rtio_spsc_consume(ez_io.cq); + struct rtio_cqe *cqe = rtio_cqe_consume(&ez_io); if (cqe == NULL) { LOG_DBG("No completion events available"); @@ -67,7 +63,7 @@ int main(void) if (rtio_cqe_get_mempool_buffer(&ez_io, cqe, &userdata[m], &data_len[m])) { LOG_ERR("Failed to get mempool buffer info"); } - rtio_spsc_release(ez_io.cq); + rtio_cqe_release(&ez_io, cqe); m++; } @@ -88,11 +84,10 @@ int main(void) * queue. */ for (m = 0; m < M; m++) { - struct rtio_sqe *sqe = rtio_spsc_acquire(ez_io.sq); + struct rtio_sqe *sqe = rtio_sqe_acquire(&ez_io); rtio_release_buffer(&ez_io, userdata[m], data_len[m]); rtio_sqe_prep_read_with_pool(sqe, iodev, RTIO_PRIO_HIGH, NULL); - rtio_spsc_produce(ez_io.sq); } } return 0; diff --git a/samples/subsys/rtio/sensor_batch_processing/src/vnd_sensor.c b/samples/subsys/rtio/sensor_batch_processing/src/vnd_sensor.c index 691fe7103a55e..c00dfd9d421c9 100644 --- a/samples/subsys/rtio/sensor_batch_processing/src/vnd_sensor.c +++ b/samples/subsys/rtio/sensor_batch_processing/src/vnd_sensor.c @@ -55,13 +55,11 @@ static void vnd_sensor_iodev_execute(const struct device *dev, struct rtio_iodev_sqe *iodev_sqe) { const struct vnd_sensor_config *config = dev->config; - const struct rtio_sqe *sqe = iodev_sqe->sqe; uint8_t *buf = NULL; uint32_t buf_len; int result; - if (sqe->op == RTIO_OP_RX) { - + if (iodev_sqe->sqe.op == RTIO_OP_RX) { result = rtio_sqe_rx_buf(iodev_sqe, config->sample_size, config->sample_size, &buf, &buf_len); if (result != 0) { @@ -83,7 +81,7 @@ static void vnd_sensor_iodev_execute(const struct device *dev, static void vnd_sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) { - struct vnd_sensor_data *data = (struct vnd_sensor_data *) iodev_sqe->sqe->iodev; + struct vnd_sensor_data *data = (struct vnd_sensor_data *) iodev_sqe->sqe.iodev; rtio_mpsc_push(&data->iodev.iodev_sq, &iodev_sqe->q); } diff --git a/subsys/rtio/CMakeLists.txt b/subsys/rtio/CMakeLists.txt index 01972b594fa24..41b457b28461d 100644 --- a/subsys/rtio/CMakeLists.txt +++ b/subsys/rtio/CMakeLists.txt @@ -6,24 +6,7 @@ if(CONFIG_RTIO) zephyr_include_directories(${ZEPHYR_BASE}/subsys/rtio) - zephyr_library_sources_ifdef( - CONFIG_USERSPACE - rtio_handlers.c - ) - - zephyr_library_sources_ifdef( - CONFIG_RTIO_EXECUTOR_SIMPLE - rtio_executor_simple.c - ) - - zephyr_library_sources_ifdef( - CONFIG_RTIO_EXECUTOR_CONCURRENT - rtio_executor_concurrent.c - ) - - zephyr_library_sources_ifdef( - CONFIG_USERSPACE - rtio_userspace_init.c - ) - + zephyr_library_sources(rtio_executor.c) + zephyr_library_sources(rtio_init.c) + zephyr_library_sources_ifdef(CONFIG_USERSPACE rtio_handlers.c) endif() diff --git a/subsys/rtio/rtio_executor.c b/subsys/rtio/rtio_executor.c new file mode 100644 index 0000000000000..2dcea8c600fba --- /dev/null +++ b/subsys/rtio/rtio_executor.c @@ -0,0 +1,188 @@ +/* + * Copyright (c) 2022 Intel Corporation. + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include + +#include +LOG_MODULE_REGISTER(rtio_executor, CONFIG_RTIO_LOG_LEVEL); + +/** + * @brief Submit to an iodev a submission to work on + * + * Should be called by the executor when it wishes to submit work + * to an iodev. + * + * @param iodev_sqe Submission to work on + */ +static inline void rtio_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) +{ + if (FIELD_GET(RTIO_SQE_CANCELED, iodev_sqe->sqe.flags)) { + /* Canceled */ + while (iodev_sqe != NULL) { + rtio_iodev_sqe_err(iodev_sqe, -ECANCELED); + iodev_sqe = iodev_sqe->next; + } + return; + } + iodev_sqe->sqe.iodev->api->submit(iodev_sqe); +} + +/** + * @brief Executor handled submissions + */ +static void rtio_executor_op(struct rtio_iodev_sqe *iodev_sqe) +{ + const struct rtio_sqe *sqe = &iodev_sqe->sqe; + + switch (sqe->op) { + case RTIO_OP_CALLBACK: + sqe->callback(iodev_sqe->r, sqe, sqe->arg0); + rtio_iodev_sqe_ok(iodev_sqe, 0); + break; + default: + rtio_iodev_sqe_err(iodev_sqe, -EINVAL); + } +} + +/** + * @brief Submit operations in the queue to iodevs + * + * @param r RTIO context + * + * @retval 0 Always succeeds + */ +void rtio_executor_submit(struct rtio *r) +{ + struct rtio_mpsc_node *node = rtio_mpsc_pop(&r->sq); + + while (node != NULL) { + struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q); + + iodev_sqe->r = r; + + if (iodev_sqe->sqe.iodev == NULL) { + rtio_executor_op(iodev_sqe); + } else { + struct rtio_iodev_sqe *curr = iodev_sqe, *next; + + /* Link up transaction or queue list if needed */ + while (curr->sqe.flags & (RTIO_SQE_TRANSACTION | RTIO_SQE_CHAINED)) { +#ifdef CONFIG_ASSERT + bool transaction = iodev_sqe->sqe.flags & RTIO_SQE_TRANSACTION; + bool chained = iodev_sqe->sqe.flags & RTIO_SQE_CHAINED; + + __ASSERT(transaction != chained, + "Expected chained or transaction flag, not both"); +#endif + node = rtio_mpsc_pop(&iodev_sqe->r->sq); + next = CONTAINER_OF(node, struct rtio_iodev_sqe, q); + curr->next = next; + curr = next; + curr->r = r; + + __ASSERT( + curr != NULL, + "Expected a valid sqe following transaction or chain flag"); + } + + curr->next = NULL; + curr->r = r; + + rtio_iodev_submit(iodev_sqe); + } + + node = rtio_mpsc_pop(&r->sq); + } +} + +/** + * @brief Handle common logic when :c:macro:`RTIO_SQE_MULTISHOT` is set + * + * @param[in] r RTIO context + * @param[in] curr Current IODev SQE that's being marked for finished. + * @param[in] is_canceled Whether or not the SQE is canceled + */ +static inline void rtio_executor_handle_multishot(struct rtio *r, struct rtio_iodev_sqe *curr, + bool is_canceled) +{ + /* Reset the mempool if needed */ + if (curr->sqe.op == RTIO_OP_RX && FIELD_GET(RTIO_SQE_MEMPOOL_BUFFER, curr->sqe.flags)) { + if (is_canceled) { + /* Free the memory first since no CQE will be generated */ + LOG_DBG("Releasing memory @%p size=%u", (void *)curr->sqe.buf, + curr->sqe.buf_len); + rtio_release_buffer(r, curr->sqe.buf, curr->sqe.buf_len); + } + /* Reset the buffer info so the next request can get a new one */ + curr->sqe.buf = NULL; + curr->sqe.buf_len = 0; + } + if (!is_canceled) { + /* Request was not canceled, put the SQE back in the queue */ + rtio_mpsc_push(&r->sq, &curr->q); + rtio_executor_submit(r); + } +} + +static inline void rtio_executor_done(struct rtio_iodev_sqe *iodev_sqe, int result, bool is_ok) +{ + const bool is_multishot = FIELD_GET(RTIO_SQE_MULTISHOT, iodev_sqe->sqe.flags) == 1; + const bool is_canceled = FIELD_GET(RTIO_SQE_CANCELED, iodev_sqe->sqe.flags) == 1; + struct rtio *r = iodev_sqe->r; + struct rtio_iodev_sqe *curr = iodev_sqe, *next; + void *userdata; + uint32_t sqe_flags, cqe_flags; + + do { + userdata = curr->sqe.userdata; + sqe_flags = curr->sqe.flags; + cqe_flags = rtio_cqe_compute_flags(iodev_sqe); + + next = rtio_iodev_sqe_next(curr); + if (is_multishot) { + rtio_executor_handle_multishot(r, curr, is_canceled); + } + if (!is_multishot || is_canceled) { + /* SQE is no longer needed, release it */ + rtio_sqe_pool_free(r->sqe_pool, curr); + } + if (!is_canceled) { + /* Request was not canceled, generate a CQE */ + rtio_cqe_submit(r, result, userdata, cqe_flags); + } + curr = next; + if (!is_ok) { + /* This is an error path, so cancel any chained SQEs */ + result = -ECANCELED; + } + } while (sqe_flags & RTIO_SQE_TRANSACTION); + + /* Curr should now be the last sqe in the transaction if that is what completed */ + if (sqe_flags & RTIO_SQE_CHAINED) { + rtio_iodev_submit(curr); + } +} + +/** + * @brief Callback from an iodev describing success + */ +void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) +{ + rtio_executor_done(iodev_sqe, result, true); +} + +/** + * @brief Callback from an iodev describing error + * + * Some assumptions are made and should have been validated on rtio_submit + * - a sqe marked as chained or transaction has a next sqe + * - a sqe is marked either chained or transaction but not both + */ +void rtio_executor_err(struct rtio_iodev_sqe *iodev_sqe, int result) +{ + rtio_executor_done(iodev_sqe, result, false); +} diff --git a/subsys/rtio/rtio_executor_common.h b/subsys/rtio/rtio_executor_common.h deleted file mode 100644 index 78fa8416b5160..0000000000000 --- a/subsys/rtio/rtio_executor_common.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (c) 2023 Intel Corporation. - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include - -/** - * @brief Executor handled submissions - */ -static void rtio_executor_submit_self(struct rtio_iodev_sqe *iodev_sqe) -{ - const struct rtio_sqe *sqe = iodev_sqe->sqe; - - switch (sqe->op) { - case RTIO_OP_CALLBACK: - sqe->callback(iodev_sqe->r, sqe, sqe->arg0); - rtio_iodev_sqe_ok(iodev_sqe, 0); - break; - default: - rtio_iodev_sqe_err(iodev_sqe, -EINVAL); - } -} - -/** - * @brief Common executor handling of submit - * - * Some submissions may have NULL for the iodev pointer which implies - * an executor related operation such as a callback or flush. This - * common codepath avoids duplicating efforts for dealing with this. - */ -static void rtio_executor_submit(struct rtio_iodev_sqe *iodev_sqe) -{ - if (iodev_sqe->sqe->iodev == NULL) { - rtio_executor_submit_self(iodev_sqe); - } else { - rtio_iodev_submit(iodev_sqe); - } -} diff --git a/subsys/rtio/rtio_executor_concurrent.c b/subsys/rtio/rtio_executor_concurrent.c deleted file mode 100644 index e58309a84633f..0000000000000 --- a/subsys/rtio/rtio_executor_concurrent.c +++ /dev/null @@ -1,301 +0,0 @@ -/* - * Copyright (c) 2022 Intel Corporation. - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include -#include -#include -#include - -#include -LOG_MODULE_REGISTER(rtio_executor_concurrent, CONFIG_RTIO_LOG_LEVEL); - -#include "rtio_executor_common.h" - -#define CONEX_TASK_COMPLETE BIT(0) -#define CONEX_TASK_SUSPENDED BIT(1) - -/** - * @file - * @brief Concurrent RTIO Executor - * - * The concurrent executor provides fixed amounts of concurrency - * using minimal overhead but assumes a small number of concurrent tasks. - * - * Many of the task lookup and management functions in here are O(N) over N - * tasks. This is fine when the task set is *small*. Task lookup could be - * improved in the future with a binary search at the expense of code size. - * - * The assumption here is that perhaps only 8-16 concurrent tasks are likely - * such that simple short for loops over task array are reasonably fast. - * - * A maximum of 65K submissions queue entries are possible. - */ - -/** - * check if there is a free task available - */ -static bool conex_task_free(struct rtio_concurrent_executor *exc) -{ - return (exc->task_in - exc->task_out) < (exc->task_mask + 1); -} - -/** - * get the next free available task index - */ -static uint16_t conex_task_next(struct rtio_concurrent_executor *exc) -{ - uint16_t task_id = exc->task_in; - - exc->task_in++; - return task_id & exc->task_mask; -} - -static inline uint16_t conex_task_id(struct rtio_concurrent_executor *exc, - const struct rtio_iodev_sqe *iodev_sqe) -{ - __ASSERT_NO_MSG(iodev_sqe <= &exc->task_cur[exc->task_mask] && - iodev_sqe >= &exc->task_cur[0]); - return iodev_sqe - &exc->task_cur[0]; -} - -static void conex_sweep_task(struct rtio *r, struct rtio_concurrent_executor *exc) -{ - struct rtio_sqe *sqe = rtio_spsc_consume(r->sq); - - while (sqe != NULL && (sqe->flags & (RTIO_SQE_CHAINED | RTIO_SQE_TRANSACTION))) { - rtio_spsc_release(r->sq); - sqe = rtio_spsc_consume(r->sq); - } - - rtio_spsc_release(r->sq); - - if (sqe == exc->last_sqe) { - exc->last_sqe = NULL; - } -} - -/** - * @brief Sweep like a GC of sorts old tasks that are completed in order - * - * Will only sweep tasks in the order they arrived in the submission queue. - * Meaning there might be completed tasks that could be freed but are not yet - * because something before it has not yet completed. - */ -static void conex_sweep(struct rtio *r, struct rtio_concurrent_executor *exc) -{ - /* In order sweep up */ - for (uint16_t task_id = exc->task_out; task_id < exc->task_in; task_id++) { - if (exc->task_status[task_id & exc->task_mask] & CONEX_TASK_COMPLETE) { - LOG_DBG("sweeping oldest task %d", task_id); - conex_sweep_task(r, exc); - exc->task_out++; - } else { - break; - } - } -} - -/** - * @brief Prepare tasks to run by iterating through the submission queue - * - * For each submission in the queue that begins a chain or transaction - * start a task if possible. Concurrency is limited by the allocated concurrency - * per executor instance. - */ -static void conex_prepare(struct rtio *r, struct rtio_concurrent_executor *exc) -{ - struct rtio_sqe *sqe, *last_sqe; - - /* If never submitted before peek at the first item - * otherwise start back up where the last submit call - * left off - */ - if (exc->last_sqe == NULL) { - last_sqe = NULL; - sqe = rtio_spsc_peek(r->sq); - } else { - last_sqe = exc->last_sqe; - sqe = rtio_spsc_next(r->sq, last_sqe); - } - - LOG_DBG("starting at sqe %p, last %p", sqe, exc->last_sqe); - - while (sqe != NULL && conex_task_free(exc)) { - /* Get the next free task id */ - uint16_t task_idx = conex_task_next(exc); - - LOG_DBG("preparing task %d, sqe %p", task_idx, sqe); - - /* Setup task */ - exc->task_cur[task_idx].sqe = sqe; - exc->task_cur[task_idx].r = r; - exc->task_status[task_idx] = CONEX_TASK_SUSPENDED; - - /* Go to the next sqe not in the current chain or transaction */ - while (sqe->flags & (RTIO_SQE_CHAINED | RTIO_SQE_TRANSACTION)) { - sqe = rtio_spsc_next(r->sq, sqe); - } - - /* SQE is the end of the previous chain or transaction so skip it */ - last_sqe = sqe; - sqe = rtio_spsc_next(r->sq, sqe); - } - - /* Out of available tasks so remember where we left off to begin again once tasks free up */ - exc->last_sqe = last_sqe; -} - -/** - * @brief Resume tasks that are suspended - * - * All tasks begin as suspended tasks. This kicks them off to the submissions - * associated iodev. - */ -static void conex_resume(struct rtio *r, struct rtio_concurrent_executor *exc) -{ - /* In order resume tasks */ - for (uint16_t task_id = exc->task_out; task_id < exc->task_in; task_id++) { - if (exc->task_status[task_id & exc->task_mask] & CONEX_TASK_SUSPENDED) { - LOG_DBG("resuming suspended task %d", task_id); - exc->task_status[task_id & exc->task_mask] &= ~CONEX_TASK_SUSPENDED; - rtio_executor_submit(&exc->task_cur[task_id & exc->task_mask]); - } - } -} - -/** - * @brief Submit submissions to concurrent executor - * - * @param r RTIO context - * - * @retval 0 Always succeeds - */ -int rtio_concurrent_submit(struct rtio *r) -{ - - struct rtio_concurrent_executor *exc = (struct rtio_concurrent_executor *)r->executor; - k_spinlock_key_t key; - - key = k_spin_lock(&exc->lock); - - /* Prepare tasks to run, they start in a suspended state */ - conex_prepare(r, exc); - - /* Resume all suspended tasks */ - conex_resume(r, exc); - - k_spin_unlock(&exc->lock, key); - - return 0; -} - -/** - * @brief Callback from an iodev describing success - */ -void rtio_concurrent_ok(struct rtio_iodev_sqe *iodev_sqe, int result) -{ - struct rtio *r = iodev_sqe->r; - const struct rtio_sqe *sqe = iodev_sqe->sqe; - struct rtio_concurrent_executor *exc = (struct rtio_concurrent_executor *)r->executor; - const struct rtio_sqe *next_sqe; - k_spinlock_key_t key; - - /* Interrupt may occur in spsc_acquire, breaking the contract - * so spin around it effectively preventing another interrupt on - * this core, and another core trying to concurrently work in here. - */ - key = k_spin_lock(&exc->lock); - - LOG_DBG("completed sqe %p", sqe); - - /* Determine the task id by memory offset O(1) */ - uint16_t task_id = conex_task_id(exc, iodev_sqe); - - if (sqe->flags & RTIO_SQE_CHAINED) { - next_sqe = rtio_spsc_next(r->sq, sqe); - - exc->task_cur[task_id].sqe = next_sqe; - rtio_executor_submit(&exc->task_cur[task_id]); - } else { - exc->task_status[task_id] |= CONEX_TASK_COMPLETE; - } - - bool transaction; - - do { - /* Capture the sqe information */ - void *userdata = sqe->userdata; - uint32_t flags = rtio_cqe_compute_flags(iodev_sqe); - - transaction = sqe->flags & RTIO_SQE_TRANSACTION; - - /* Release the sqe */ - conex_sweep(r, exc); - - /* Submit the completion event */ - rtio_cqe_submit(r, result, userdata, flags); - - if (transaction) { - /* sqe was a transaction, get the next one */ - sqe = rtio_spsc_next(r->sq, sqe); - __ASSERT_NO_MSG(sqe != NULL); - } - } while (transaction); - conex_prepare(r, exc); - conex_resume(r, exc); - - k_spin_unlock(&exc->lock, key); -} - -/** - * @brief Callback from an iodev describing error - */ -void rtio_concurrent_err(struct rtio_iodev_sqe *iodev_sqe, int result) -{ - k_spinlock_key_t key; - struct rtio *r = iodev_sqe->r; - const struct rtio_sqe *sqe = iodev_sqe->sqe; - struct rtio_concurrent_executor *exc = (struct rtio_concurrent_executor *)r->executor; - void *userdata = sqe->userdata; - uint32_t flags = rtio_cqe_compute_flags(iodev_sqe); - bool chained = sqe->flags & RTIO_SQE_CHAINED; - bool transaction = sqe->flags & RTIO_SQE_TRANSACTION; - uint16_t task_id = conex_task_id(exc, iodev_sqe); - - /* Another interrupt (and sqe complete) may occur in spsc_acquire, - * breaking the contract so spin around it effectively preventing another - * interrupt on this core, and another core trying to concurrently work - * in here. - */ - key = k_spin_lock(&exc->lock); - - if (!transaction) { - rtio_cqe_submit(r, result, userdata, flags); - } - - /* While the last sqe was marked as chained or transactional, do more work */ - while (chained | transaction) { - sqe = rtio_spsc_next(r->sq, sqe); - chained = sqe->flags & RTIO_SQE_CHAINED; - transaction = sqe->flags & RTIO_SQE_TRANSACTION; - userdata = sqe->userdata; - - if (!transaction) { - rtio_cqe_submit(r, result, userdata, flags); - } else { - rtio_cqe_submit(r, -ECANCELED, userdata, flags); - } - } - - /* Determine the task id : O(1) */ - exc->task_status[task_id] |= CONEX_TASK_COMPLETE; - - conex_sweep(r, exc); - conex_prepare(r, exc); - conex_resume(r, exc); - - k_spin_unlock(&exc->lock, key); -} diff --git a/subsys/rtio/rtio_executor_simple.c b/subsys/rtio/rtio_executor_simple.c deleted file mode 100644 index 8231d45362d36..0000000000000 --- a/subsys/rtio/rtio_executor_simple.c +++ /dev/null @@ -1,157 +0,0 @@ -/* - * Copyright (c) 2022 Intel Corporation. - * - * SPDX-License-Identifier: Apache-2.0 - */ - -#include "rtio_executor_common.h" -#include -#include -#include - -#include -LOG_MODULE_REGISTER(rtio_executor_simple, CONFIG_RTIO_LOG_LEVEL); - - -/** - * @brief Submit submissions to simple executor - * - * The simple executor provides no concurrency instead - * execution each submission chain one after the next. - * - * @param r RTIO context - * - * @retval 0 Always succeeds - */ -int rtio_simple_submit(struct rtio *r) -{ - struct rtio_simple_executor *exc = (struct rtio_simple_executor *)r->executor; - - /* Task is already running */ - if (exc->task.sqe != NULL) { - return 0; - } - - struct rtio_sqe *sqe = rtio_spsc_consume(r->sq); - - if (sqe == NULL) { - return 0; - } - - /* Some validation on the sqe to ensure no programming errors were - * made so assumptions in ok and err are valid. - */ -#ifdef CONFIG_ASSERT - __ASSERT(sqe != NULL, "Expected a valid sqe on submit call"); - - bool transaction = sqe->flags & RTIO_SQE_TRANSACTION; - bool chained = sqe->flags & RTIO_SQE_CHAINED; - - if (transaction || chained) { - struct rtio_sqe *next = rtio_spsc_next(r->sq, sqe); - - __ASSERT(next != NULL, - "sqe %p flagged as transaction (%d) or chained (%d) without subsequent sqe in queue", - sqe, transaction, chained); - } - __ASSERT(!(chained && transaction), - "sqe %p flagged as both being transaction and chained, only one is allowed", - sqe); -#endif - - exc->task.sqe = sqe; - exc->task.r = r; - - rtio_executor_submit(&exc->task); - - return 0; -} - -/** - * @brief Callback from an iodev describing success - */ -void rtio_simple_ok(struct rtio_iodev_sqe *iodev_sqe, int result) -{ - struct rtio *r = iodev_sqe->r; - const struct rtio_sqe *sqe = iodev_sqe->sqe; - bool transaction; - -#ifdef CONFIG_ASSERT - struct rtio_simple_executor *exc = - (struct rtio_simple_executor *)r->executor; - - __ASSERT_NO_MSG(iodev_sqe == &exc->task); -#endif - - do { - /* Capture the sqe information */ - void *userdata = sqe->userdata; - uint32_t flags = rtio_cqe_compute_flags(iodev_sqe); - - transaction = sqe->flags & RTIO_SQE_TRANSACTION; - - /* Release the sqe */ - rtio_spsc_release(r->sq); - - /* Submit the completion event */ - rtio_cqe_submit(r, result, userdata, flags); - - if (transaction) { - /* sqe was a transaction, get the next one */ - sqe = rtio_spsc_consume(r->sq); - __ASSERT_NO_MSG(sqe != NULL); - } - - } while (transaction); - - iodev_sqe->sqe = NULL; - rtio_simple_submit(r); -} - -/** - * @brief Callback from an iodev describing error - * - * Some assumptions are made and should have been validated on rtio_submit - * - a sqe marked as chained or transaction has a next sqe - * - a sqe is marked either chained or transaction but not both - */ -void rtio_simple_err(struct rtio_iodev_sqe *iodev_sqe, int result) -{ - const struct rtio_sqe *sqe = iodev_sqe->sqe; - struct rtio *r = iodev_sqe->r; - void *userdata = sqe->userdata; - uint32_t flags = rtio_cqe_compute_flags(iodev_sqe); - bool chained = sqe->flags & RTIO_SQE_CHAINED; - bool transaction = sqe->flags & RTIO_SQE_TRANSACTION; - -#ifdef CONFIG_ASSERT - struct rtio_simple_executor *exc = - (struct rtio_simple_executor *)r->executor; - - __ASSERT_NO_MSG(iodev_sqe == &exc->task); -#endif - - rtio_spsc_release(r->sq); - iodev_sqe->sqe = NULL; - if (!transaction) { - rtio_cqe_submit(r, result, userdata, flags); - } - while (chained | transaction) { - sqe = rtio_spsc_consume(r->sq); - chained = sqe->flags & RTIO_SQE_CHAINED; - transaction = sqe->flags & RTIO_SQE_TRANSACTION; - userdata = sqe->userdata; - rtio_spsc_release(r->sq); - - if (!transaction) { - rtio_cqe_submit(r, result, userdata, flags); - } else { - rtio_cqe_submit(r, -ECANCELED, userdata, flags); - } - } - - iodev_sqe->sqe = rtio_spsc_consume(r->sq); - if (iodev_sqe->sqe != NULL) { - rtio_executor_submit(iodev_sqe); - } -} diff --git a/subsys/rtio/rtio_handlers.c b/subsys/rtio/rtio_handlers.c index 1fb0dac964276..24555ab8f39c7 100644 --- a/subsys/rtio/rtio_handlers.c +++ b/subsys/rtio/rtio_handlers.c @@ -4,6 +4,7 @@ * SPDX-License-Identifier: Apache-2.0 */ +#include "zephyr/kernel.h" #include #include #include @@ -69,13 +70,12 @@ static inline int z_vrfy_rtio_cqe_get_mempool_buffer(const struct rtio *r, struc } #include -static inline int z_vrfy_rtio_sqe_copy_in(struct rtio *r, - const struct rtio_sqe *sqes, - size_t sqe_count) +static inline int z_vrfy_rtio_sqe_copy_in_get_handles(struct rtio *r, const struct rtio_sqe *sqes, + struct rtio_sqe **handles, size_t sqe_count) { Z_OOPS(Z_SYSCALL_OBJ(r, K_OBJ_RTIO)); - Z_OOPS(Z_SYSCALL_MEMORY(sqes, sqe_count, false)); + Z_OOPS(Z_SYSCALL_MEMORY_ARRAY_READ(sqes, sqe_count, sizeof(struct rtio_sqe))); struct rtio_sqe *sqe; uint32_t acquirable = rtio_sqe_acquirable(r); @@ -86,6 +86,9 @@ static inline int z_vrfy_rtio_sqe_copy_in(struct rtio *r, for (int i = 0; i < sqe_count; i++) { sqe = rtio_sqe_acquire(r); __ASSERT_NO_MSG(sqe != NULL); + if (handles != NULL) { + handles[i] = sqe; + } *sqe = sqes[i]; if (!rtio_vrfy_sqe(sqe)) { @@ -94,10 +97,8 @@ static inline int z_vrfy_rtio_sqe_copy_in(struct rtio *r, } } - rtio_sqe_produce_all(r); - /* Already copied *and* verified, no need to redo */ - return z_impl_rtio_sqe_copy_in(r, NULL, 0); + return z_impl_rtio_sqe_copy_in_get_handles(r, NULL, NULL, 0); } #include @@ -108,7 +109,7 @@ static inline int z_vrfy_rtio_cqe_copy_out(struct rtio *r, { Z_OOPS(Z_SYSCALL_OBJ(r, K_OBJ_RTIO)); - Z_OOPS(Z_SYSCALL_MEMORY(cqes, cqe_count, true)); + Z_OOPS(Z_SYSCALL_MEMORY_ARRAY_WRITE(cqes, cqe_count, sizeof(struct rtio_cqe))); return z_impl_rtio_cqe_copy_out(r, cqes, cqe_count, timeout); } diff --git a/subsys/rtio/rtio_init.c b/subsys/rtio/rtio_init.c new file mode 100644 index 0000000000000..6c95986936c17 --- /dev/null +++ b/subsys/rtio/rtio_init.c @@ -0,0 +1,31 @@ +/* + * Copyright (c) 2023 Google LLC + * SPDX-License-Identifier: Apache-2.0 + */ + +#include +#include +#include + +#ifdef CONFIG_USERSPACE +K_APPMEM_PARTITION_DEFINE(rtio_partition); +#endif + +int rtio_init(void) +{ + STRUCT_SECTION_FOREACH(rtio_sqe_pool, sqe_pool) { + for (int i = 0; i < sqe_pool->pool_size; i++) { + rtio_mpsc_push(&sqe_pool->free_q, &sqe_pool->pool[i].q); + } + } + + STRUCT_SECTION_FOREACH(rtio_cqe_pool, cqe_pool) { + for (int i = 0; i < cqe_pool->pool_size; i++) { + rtio_mpsc_push(&cqe_pool->free_q, &cqe_pool->pool[i].q); + } + } + + return 0; +} + +SYS_INIT(rtio_init, POST_KERNEL, 0); diff --git a/subsys/rtio/rtio_userspace_init.c b/subsys/rtio/rtio_userspace_init.c deleted file mode 100644 index 6df54d1d2364c..0000000000000 --- a/subsys/rtio/rtio_userspace_init.c +++ /dev/null @@ -1,8 +0,0 @@ -/* - * Copyright (c) 2023 Google LLC - * SPDX-License-Identifier: Apache-2.0 - */ - -#include - -K_APPMEM_PARTITION_DEFINE(rtio_partition); diff --git a/tests/drivers/spi/spi_loopback/src/spi_rtio.c b/tests/drivers/spi/spi_loopback/src/spi_rtio.c index 5498ad0571c4c..5c1bb4a356dac 100644 --- a/tests/drivers/spi/spi_loopback/src/spi_rtio.c +++ b/tests/drivers/spi/spi_loopback/src/spi_rtio.c @@ -16,7 +16,6 @@ LOG_MODULE_REGISTER(spi_rtio_loopback); #include #include -#include #include #define SPI_FAST_DEV DT_COMPAT_GET_ANY_STATUS_OKAY(test_spi_loopback_fast) @@ -34,8 +33,7 @@ LOG_MODULE_REGISTER(spi_rtio_loopback); static SPI_DT_IODEV_DEFINE(spi_fast, SPI_FAST_DEV, SPI_OP, 0); static SPI_DT_IODEV_DEFINE(spi_slow, SPI_FAST_DEV, SPI_OP, 0); -static RTIO_EXECUTOR_SIMPLE_DEFINE(rexec); -RTIO_DEFINE(r, (struct rtio_executor *)&rexec, 8, 8); +RTIO_DEFINE(r, 8, 8); /* to run this test, connect MOSI pin to the MISO of the SPI */ @@ -99,7 +97,7 @@ static int spi_complete_multiple(struct rtio_iodev *spi_iodev) rtio_submit(&r, 1); cqe = rtio_cqe_consume(&r); ret = cqe->result; - rtio_cqe_release(&r); + rtio_cqe_release(&r, cqe); if (ret) { LOG_ERR("Code %d", ret); @@ -145,7 +143,7 @@ static int spi_complete_loop(struct rtio_iodev *spi_iodev) rtio_submit(&r, 1); cqe = rtio_cqe_consume(&r); ret = cqe->result; - rtio_cqe_release(&r); + rtio_cqe_release(&r, cqe); if (ret) { LOG_ERR("Code %d", ret); @@ -187,7 +185,7 @@ static int spi_null_tx_buf(struct rtio_iodev *spi_iodev) rtio_submit(&r, 1); cqe = rtio_cqe_consume(&r); ret = cqe->result; - rtio_cqe_release(&r); + rtio_cqe_release(&r, cqe); if (ret) { LOG_ERR("Code %d", ret); @@ -231,7 +229,7 @@ static int spi_rx_half_start(struct rtio_iodev *spi_iodev) rtio_submit(&r, 1); cqe = rtio_cqe_consume(&r); ret = cqe->result; - rtio_cqe_release(&r); + rtio_cqe_release(&r, cqe); if (ret) { LOG_ERR("Code %d", ret); @@ -286,7 +284,7 @@ static int spi_rx_half_end(struct rtio_iodev *spi_iodev) rtio_submit(&r, 1); cqe = rtio_cqe_consume(&r); ret = cqe->result; - rtio_cqe_release(&r); + rtio_cqe_release(&r, cqe); if (ret) { LOG_ERR("Code %d", ret); @@ -353,7 +351,7 @@ static int spi_rx_every_4(struct rtio_iodev *spi_iodev) rtio_submit(&r, 1); cqe = rtio_cqe_consume(&r); ret = cqe->result; - rtio_cqe_release(&r); + rtio_cqe_release(&r, cqe); if (ret) { LOG_ERR("Code %d", ret); diff --git a/tests/lib/cpp/cxx/src/main.cpp b/tests/lib/cpp/cxx/src/main.cpp index 0f9447337d39f..02f7f767efe41 100644 --- a/tests/lib/cpp/cxx/src/main.cpp +++ b/tests/lib/cpp/cxx/src/main.cpp @@ -83,8 +83,6 @@ #include #include #include -#include -#include #include diff --git a/tests/subsys/rtio/rtio_api/prj.conf b/tests/subsys/rtio/rtio_api/prj.conf index baded798fd45a..aa4a5fcfadbf5 100644 --- a/tests/subsys/rtio/rtio_api/prj.conf +++ b/tests/subsys/rtio/rtio_api/prj.conf @@ -3,3 +3,4 @@ CONFIG_ZTEST_NEW_API=y CONFIG_LOG=y CONFIG_RTIO=y CONFIG_RTIO_SYS_MEM_BLOCKS=y +CONFIG_TIMING_FUNCTIONS=y diff --git a/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h b/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h index eef85e5490735..67a412d819ee4 100644 --- a/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h +++ b/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h @@ -17,8 +17,8 @@ struct rtio_iodev_test_data { struct k_timer timer; /* Currently executing sqe */ - struct rtio_iodev_sqe *iodev_sqe; - const struct rtio_sqe *sqe; + struct rtio_iodev_sqe *txn_head; + struct rtio_iodev_sqe *txn_curr; /* Count of submit calls */ atomic_t submit_count; @@ -34,7 +34,7 @@ static void rtio_iodev_test_next(struct rtio_iodev *iodev) /* The next section must be serialized to ensure single consumer semantics */ k_spinlock_key_t key = k_spin_lock(&data->lock); - if (data->iodev_sqe != NULL) { + if (data->txn_head != NULL) { goto out; } @@ -43,9 +43,13 @@ static void rtio_iodev_test_next(struct rtio_iodev *iodev) if (next != NULL) { struct rtio_iodev_sqe *next_sqe = CONTAINER_OF(next, struct rtio_iodev_sqe, q); - data->iodev_sqe = next_sqe; - data->sqe = next_sqe->sqe; + TC_PRINT("next task in queue %p\n", (void *)next_sqe); + + data->txn_head = next_sqe; + data->txn_curr = next_sqe; k_timer_start(&data->timer, K_MSEC(10), K_NO_WAIT); + } else { + TC_PRINT("no more tasks in the queue\n"); } out: @@ -54,41 +58,61 @@ static void rtio_iodev_test_next(struct rtio_iodev *iodev) static void rtio_iodev_timer_fn(struct k_timer *tm) { + static struct rtio_iodev_sqe *last_iodev_sqe; + static int consecutive_sqes; + struct rtio_iodev_test_data *data = CONTAINER_OF(tm, struct rtio_iodev_test_data, timer); - struct rtio_iodev_sqe *iodev_sqe = data->iodev_sqe; - struct rtio_iodev *iodev = (struct rtio_iodev *)iodev_sqe->sqe->iodev; + struct rtio_iodev_sqe *iodev_sqe = data->txn_curr; + struct rtio_iodev *iodev = (struct rtio_iodev *)data->txn_head->sqe.iodev; + + if (iodev_sqe == last_iodev_sqe) { + consecutive_sqes++; + } else { + consecutive_sqes = 0; + } + last_iodev_sqe = iodev_sqe; - if (data->sqe->op == RTIO_OP_RX) { + if (iodev_sqe->sqe.op == RTIO_OP_RX) { uint8_t *buf; uint32_t buf_len; int rc = rtio_sqe_rx_buf(iodev_sqe, 16, 16, &buf, &buf_len); if (rc != 0) { + iodev_sqe = data->txn_head; + data->txn_head = NULL; + data->txn_curr = NULL; rtio_iodev_sqe_err(iodev_sqe, rc); + rtio_iodev_test_next(iodev); return; } for (int i = 0; i < 16; ++i) { - buf[i] = ((uint8_t *)iodev_sqe->sqe->userdata)[i]; + buf[i] = ((uint8_t *)iodev_sqe->sqe.userdata)[i]; } } - if (data->sqe->flags & RTIO_SQE_TRANSACTION) { - data->sqe = rtio_spsc_next(data->iodev_sqe->r->sq, data->sqe); - k_timer_start(&data->timer, K_MSEC(10), K_NO_WAIT); + if (iodev_sqe->sqe.flags & RTIO_SQE_TRANSACTION) { + data->txn_curr = rtio_txn_next(data->txn_curr); + TC_PRINT("iodev_sqe %p marked transaction, next %p\n", iodev_sqe, data->txn_curr); + k_timer_start(tm, K_MSEC(10), K_NO_WAIT); return; } - data->iodev_sqe = NULL; - data->sqe = NULL; - rtio_iodev_sqe_ok(iodev_sqe, 0); + iodev_sqe = data->txn_head; + data->txn_head = NULL; + data->txn_curr = NULL; rtio_iodev_test_next(iodev); + if (consecutive_sqes == 0) { + rtio_iodev_sqe_ok(iodev_sqe, 0); + } else { + rtio_iodev_sqe_err(iodev_sqe, consecutive_sqes); + } } static void rtio_iodev_test_submit(struct rtio_iodev_sqe *iodev_sqe) { - struct rtio_iodev *iodev = (struct rtio_iodev *)iodev_sqe->sqe->iodev; + struct rtio_iodev *iodev = (struct rtio_iodev *)iodev_sqe->sqe.iodev; struct rtio_iodev_test_data *data = iodev->data; atomic_inc(&data->submit_count); @@ -108,7 +132,8 @@ void rtio_iodev_test_init(struct rtio_iodev *test) struct rtio_iodev_test_data *data = test->data; rtio_mpsc_init(&test->iodev_sq); - data->iodev_sqe = NULL; + data->txn_head = NULL; + data->txn_curr = NULL; k_timer_init(&data->timer, rtio_iodev_timer_fn, NULL); } diff --git a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c index 7fee16fad5155..3ed9e16beef11 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c @@ -13,11 +13,8 @@ #include #include #include -#include -#include +#include #include -#include -#include #include "rtio_iodev_test.h" @@ -28,18 +25,12 @@ #define MEM_BLK_SIZE 16 #define MEM_BLK_ALIGN 4 -RTIO_EXECUTOR_SIMPLE_DEFINE(simple_exec_simp); /* * Purposefully double the block count and half the block size. This leaves the same size mempool, * but ensures that allocation is done in larger blocks because the tests assume a larger block * size. */ -RTIO_DEFINE_WITH_MEMPOOL(r_simple_simp, (struct rtio_executor *)&simple_exec_simp, 4, 4, - MEM_BLK_COUNT * 2, MEM_BLK_SIZE / 2, MEM_BLK_ALIGN); - -RTIO_EXECUTOR_CONCURRENT_DEFINE(simple_exec_con, 1); -RTIO_DEFINE_WITH_MEMPOOL(r_simple_con, (struct rtio_executor *)&simple_exec_con, 4, 4, - MEM_BLK_COUNT * 2, MEM_BLK_SIZE / 2, MEM_BLK_ALIGN); +RTIO_DEFINE_WITH_MEMPOOL(r_simple, 4, 4, MEM_BLK_COUNT * 2, MEM_BLK_SIZE / 2, MEM_BLK_ALIGN); RTIO_IODEV_TEST_DEFINE(iodev_test_simple); @@ -59,7 +50,7 @@ void test_rtio_simple_(struct rtio *r) rtio_iodev_test_init(&iodev_test_simple); TC_PRINT("setting up single no-op\n"); - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, (struct rtio_iodev *)&iodev_test_simple, &userdata[0]); @@ -67,30 +58,22 @@ void test_rtio_simple_(struct rtio *r) res = rtio_submit(r, 1); zassert_ok(res, "Should return ok from rtio_execute"); - cqe = rtio_spsc_consume(r->cq); + cqe = rtio_cqe_consume(r); zassert_not_null(cqe, "Expected a valid cqe"); zassert_ok(cqe->result, "Result should be ok"); zassert_equal_ptr(cqe->userdata, &userdata[0], "Expected userdata back"); - rtio_spsc_release(r->cq); + rtio_cqe_release(r, cqe); } ZTEST(rtio_api, test_rtio_simple) { TC_PRINT("rtio simple simple\n"); for (int i = 0; i < TEST_REPEATS; i++) { - test_rtio_simple_(&r_simple_simp); - } - TC_PRINT("rtio simple concurrent\n"); - for (int i = 0; i < TEST_REPEATS; i++) { - test_rtio_simple_(&r_simple_con); + test_rtio_simple_(&r_simple); } } -RTIO_EXECUTOR_SIMPLE_DEFINE(chain_exec_simp); -RTIO_DEFINE(r_chain_simp, (struct rtio_executor *)&chain_exec_simp, 4, 4); - -RTIO_EXECUTOR_CONCURRENT_DEFINE(chain_exec_con, 1); -RTIO_DEFINE(r_chain_con, (struct rtio_executor *)&chain_exec_con, 4, 4); +RTIO_DEFINE(r_chain, 4, 4); RTIO_IODEV_TEST_DEFINE(iodev_test_chain0); RTIO_IODEV_TEST_DEFINE(iodev_test_chain1); @@ -111,7 +94,7 @@ void test_rtio_chain_(struct rtio *r) struct rtio_cqe *cqe; for (int i = 0; i < 4; i++) { - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, iodev_test_chain[i % 2], &userdata[i]); @@ -126,16 +109,16 @@ void test_rtio_chain_(struct rtio *r) res = rtio_submit(r, 4); TC_PRINT("checking cq\n"); zassert_ok(res, "Should return ok from rtio_execute"); - zassert_equal(rtio_spsc_consumable(r->cq), 4, "Should have 4 pending completions"); + zassert_equal(rtio_cqe_consumable(r), 4, "Should have 4 pending completions"); for (int i = 0; i < 4; i++) { - cqe = rtio_spsc_consume(r->cq); + cqe = rtio_cqe_consume(r); zassert_not_null(cqe, "Expected a valid cqe"); TC_PRINT("consume %d, cqe %p, userdata %d\n", i, cqe, *(uint32_t *)cqe->userdata); zassert_ok(cqe->result, "Result should be ok"); zassert_equal_ptr(cqe->userdata, &userdata[i], "Expected in order completions"); - rtio_spsc_release(r->cq); + rtio_cqe_release(r, cqe); } } @@ -149,19 +132,11 @@ ZTEST(rtio_api, test_rtio_chain) TC_PRINT("rtio chain simple\n"); for (int i = 0; i < TEST_REPEATS; i++) { - test_rtio_chain_(&r_chain_simp); - } - TC_PRINT("rtio chain concurrent\n"); - for (int i = 0; i < TEST_REPEATS; i++) { - test_rtio_chain_(&r_chain_con); + test_rtio_chain_(&r_chain); } } -RTIO_EXECUTOR_SIMPLE_DEFINE(multi_exec_simp); -RTIO_DEFINE(r_multi_simp, (struct rtio_executor *)&multi_exec_simp, 4, 4); - -RTIO_EXECUTOR_CONCURRENT_DEFINE(multi_exec_con, 2); -RTIO_DEFINE(r_multi_con, (struct rtio_executor *)&multi_exec_con, 4, 4); +RTIO_DEFINE(r_multi_chain, 4, 4); RTIO_IODEV_TEST_DEFINE(iodev_test_multi0); RTIO_IODEV_TEST_DEFINE(iodev_test_multi1); @@ -175,11 +150,11 @@ void test_rtio_multiple_chains_(struct rtio *r) int res; uintptr_t userdata[4] = {0, 1, 2, 3}; struct rtio_sqe *sqe; - struct rtio_cqe *cqe; + struct rtio_cqe *cqe = NULL; for (int i = 0; i < 2; i++) { for (int j = 0; j < 2; j++) { - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, iodev_test_multi[i], (void *)userdata[i*2 + j]); @@ -200,16 +175,17 @@ void test_rtio_multiple_chains_(struct rtio *r) TC_PRINT("waiting for 4 completions\n"); for (int i = 0; i < 4; i++) { TC_PRINT("waiting on completion %d\n", i); - cqe = rtio_spsc_consume(r->cq); + cqe = rtio_cqe_consume(r); while (cqe == NULL) { k_sleep(K_MSEC(1)); - cqe = rtio_spsc_consume(r->cq); + cqe = rtio_cqe_consume(r); } + TC_PRINT("consumed cqe %p, result, %d, userdata %lu\n", cqe, + cqe->result, (uintptr_t)cqe->userdata); + zassert_not_null(cqe, "Expected a valid cqe"); - TC_PRINT("result %d, would block is %d, inval is %d\n", - cqe->result, -EWOULDBLOCK, -EINVAL); zassert_ok(cqe->result, "Result should be ok"); seen[(uintptr_t)cqe->userdata] = true; if (seen[1]) { @@ -218,7 +194,7 @@ void test_rtio_multiple_chains_(struct rtio *r) if (seen[3]) { zassert_true(seen[2], "Should see 2 before 3"); } - rtio_spsc_release(r->cq); + rtio_cqe_release(r, cqe); } } @@ -228,10 +204,8 @@ ZTEST(rtio_api, test_rtio_multiple_chains) rtio_iodev_test_init(iodev_test_multi[i]); } - TC_PRINT("rtio multiple simple\n"); - test_rtio_multiple_chains_(&r_multi_simp); - TC_PRINT("rtio_multiple concurrent\n"); - test_rtio_multiple_chains_(&r_multi_con); + TC_PRINT("rtio multiple chains\n"); + test_rtio_multiple_chains_(&r_multi_chain); } #ifdef CONFIG_USERSPACE @@ -240,14 +214,13 @@ struct k_mem_domain rtio_domain; RTIO_BMEM uint8_t syscall_bufs[4]; -RTIO_EXECUTOR_SIMPLE_DEFINE(syscall_simple); -RTIO_DEFINE(r_syscall, (struct rtio_executor *)&syscall_simple, 4, 4); +RTIO_DEFINE(r_syscall, 4, 4); RTIO_IODEV_TEST_DEFINE(iodev_test_syscall); void rtio_syscall_test(void *p1, void *p2, void *p3) { int res; - struct rtio_sqe sqe; + struct rtio_sqe sqe = {0}; struct rtio_cqe cqe = {0}; struct rtio *r = &r_syscall; @@ -297,8 +270,8 @@ RTIO_BMEM uint8_t mempool_data[MEM_BLK_SIZE]; static void test_rtio_simple_mempool_(struct rtio *r, int run_count) { int res; - struct rtio_sqe sqe; - struct rtio_cqe cqe; + struct rtio_sqe sqe = {0}; + struct rtio_cqe cqe = {0}; for (int i = 0; i < MEM_BLK_SIZE; ++i) { mempool_data[i] = i + run_count; @@ -312,11 +285,12 @@ static void test_rtio_simple_mempool_(struct rtio *r, int run_count) zassert_ok(res); TC_PRINT("submit with wait\n"); - res = rtio_submit(r, 1); + res = rtio_submit(r, 0); zassert_ok(res, "Should return ok from rtio_execute"); TC_PRINT("Calling rtio_cqe_copy_out\n"); zassert_equal(1, rtio_cqe_copy_out(r, &cqe, 1, K_FOREVER)); + TC_PRINT("cqe result %d, userdata %p\n", cqe.result, cqe.userdata); zassert_ok(cqe.result, "Result should be ok"); zassert_equal_ptr(cqe.userdata, mempool_data, "Expected userdata back"); @@ -339,14 +313,11 @@ static void rtio_simple_mempool_test(void *a, void *b, void *c) ARG_UNUSED(b); ARG_UNUSED(c); - TC_PRINT("rtio simple mempool simple\n"); - for (int i = 0; i < TEST_REPEATS * 2; i++) { - test_rtio_simple_mempool_(&r_simple_simp, i); - } - TC_PRINT("rtio simple mempool concurrent\n"); + TC_PRINT("rtio simple mempool from thread %p\n", k_current_get()); for (int i = 0; i < TEST_REPEATS * 2; i++) { - test_rtio_simple_mempool_(&r_simple_con, i); + test_rtio_simple_mempool_(&r_simple, i); } + } ZTEST(rtio_api, test_rtio_simple_mempool) @@ -354,8 +325,7 @@ ZTEST(rtio_api, test_rtio_simple_mempool) rtio_iodev_test_init(&iodev_test_simple); #ifdef CONFIG_USERSPACE k_mem_domain_add_thread(&rtio_domain, k_current_get()); - rtio_access_grant(&r_simple_simp, k_current_get()); - rtio_access_grant(&r_simple_con, k_current_get()); + rtio_access_grant(&r_simple, k_current_get()); k_object_access_grant(&iodev_test_simple, k_current_get()); k_thread_user_mode_enter(rtio_simple_mempool_test, NULL, NULL, NULL); #else @@ -363,6 +333,129 @@ ZTEST(rtio_api, test_rtio_simple_mempool) #endif } +static void test_rtio_simple_cancel_(struct rtio *r, int i) +{ + struct rtio_sqe sqe; + struct rtio_cqe cqe; + struct rtio_sqe *handle[1]; + + rtio_sqe_prep_nop(&sqe, (struct rtio_iodev *)&iodev_test_simple, NULL); + rtio_sqe_copy_in_get_handles(r, &sqe, handle, 1); + rtio_sqe_cancel(handle[0]); + TC_PRINT("Submitting 1 to RTIO\n"); + rtio_submit(r, 0); + + TC_PRINT("waiting for CQE\n"); + zassert_equal(0, rtio_cqe_copy_out(r, &cqe, 1, K_MSEC(15))); + TC_PRINT("Checking SQE pool size\n"); + zassert_equal(r->sqe_pool->pool_free, r->sqe_pool->pool_size); +} + +static void rtio_simple_cancel_test(void *a, void *b, void *c) +{ + ARG_UNUSED(a); + ARG_UNUSED(b); + ARG_UNUSED(c); + + TC_PRINT("rtio simple cancel\n"); + for (int i = 0; i < TEST_REPEATS; i++) { + test_rtio_simple_cancel_(&r_simple, i); + } +} + +ZTEST(rtio_api, test_rtio_simple_cancel) +{ + rtio_iodev_test_init(&iodev_test_simple); +#ifdef CONFIG_USERSPACE + k_mem_domain_add_thread(&rtio_domain, k_current_get()); + rtio_access_grant(&r_simple_simp, k_current_get()); + rtio_access_grant(&r_simple_con, k_current_get()); + k_object_access_grant(&iodev_test_simple, k_current_get()); + k_thread_user_mode_enter(rtio_simple_cancel_test, NULL, NULL, NULL); +#else + rtio_simple_cancel_test(NULL, NULL, NULL); +#endif +} + +static inline void test_rtio_simple_multishot_(struct rtio *r, int idx) +{ + int res; + struct rtio_sqe sqe; + struct rtio_cqe cqe; + struct rtio_sqe *handles[1]; + + for (int i = 0; i < MEM_BLK_SIZE; ++i) { + mempool_data[i] = i + idx; + } + + TC_PRINT("setting up single mempool read\n"); + rtio_sqe_prep_read_multishot(&sqe, (struct rtio_iodev *)&iodev_test_simple, 0, + mempool_data); + TC_PRINT("Calling rtio_sqe_copy_in()\n"); + res = rtio_sqe_copy_in_get_handles(r, &sqe, handles, 1); + zassert_ok(res); + + TC_PRINT("submit with wait, handle=%p\n", handles[0]); + res = rtio_submit(r, 1); + zassert_ok(res, "Should return ok from rtio_execute"); + + TC_PRINT("Calling rtio_cqe_copy_out\n"); + zassert_equal(1, rtio_cqe_copy_out(r, &cqe, 1, K_FOREVER)); + zassert_ok(cqe.result, "Result should be ok but got %d", cqe.result); + zassert_equal_ptr(cqe.userdata, mempool_data, "Expected userdata back"); + + uint8_t *buffer = NULL; + uint32_t buffer_len = 0; + + TC_PRINT("Calling rtio_cqe_get_mempool_buffer\n"); + zassert_ok(rtio_cqe_get_mempool_buffer(r, &cqe, &buffer, &buffer_len)); + + zassert_not_null(buffer, "Expected an allocated mempool buffer"); + zassert_equal(buffer_len, MEM_BLK_SIZE); + zassert_mem_equal(buffer, mempool_data, MEM_BLK_SIZE, "Data expected to be the same"); + TC_PRINT("Calling rtio_release_buffer\n"); + rtio_release_buffer(r, buffer, buffer_len); + + TC_PRINT("Waiting for next cqe\n"); + zassert_equal(1, rtio_cqe_copy_out(r, &cqe, 1, K_FOREVER)); + zassert_equal(1, cqe.result, "Result should be ok but got %d", cqe.result); + zassert_equal_ptr(cqe.userdata, mempool_data, "Expected userdata back"); + + TC_PRINT("Canceling %p\n", handles[0]); + rtio_sqe_cancel(handles[0]); + /* Flush any pending CQEs */ + while (rtio_cqe_consumable(r)) { + rtio_cqe_copy_out(r, &cqe, 1, K_NO_WAIT); + } +} + +static void rtio_simple_multishot_test(void *a, void *b, void *c) +{ + ARG_UNUSED(a); + ARG_UNUSED(b); + ARG_UNUSED(c); + + TC_PRINT("rtio simple multishot\n"); + for (int i = 0; i < TEST_REPEATS; i++) { + TC_PRINT("/***** Iteration %d *****/\n", i); + test_rtio_simple_multishot_(&r_simple, i); + } +} + +ZTEST(rtio_api, test_rtio_simple_multishot) +{ + rtio_iodev_test_init(&iodev_test_simple); +#ifdef CONFIG_USERSPACE + k_mem_domain_add_thread(&rtio_domain, k_current_get()); + rtio_access_grant(&r_simple_simp, k_current_get()); + rtio_access_grant(&r_simple_con, k_current_get()); + k_object_access_grant(&iodev_test_simple, k_current_get()); + k_thread_user_mode_enter(rtio_simple_multishot_test, NULL, NULL, NULL); +#else + rtio_simple_multishot_test(NULL, NULL, NULL); +#endif +} + ZTEST(rtio_api, test_rtio_syscalls) { @@ -374,11 +467,7 @@ ZTEST(rtio_api, test_rtio_syscalls) } } -RTIO_EXECUTOR_SIMPLE_DEFINE(transaction_exec_simp); -RTIO_DEFINE(r_transaction_simp, (struct rtio_executor *)&transaction_exec_simp, 4, 4); - -RTIO_EXECUTOR_CONCURRENT_DEFINE(transaction_exec_con, 2); -RTIO_DEFINE(r_transaction_con, (struct rtio_executor *)&transaction_exec_con, 4, 4); +RTIO_DEFINE(r_transaction, 4, 4); RTIO_IODEV_TEST_DEFINE(iodev_test_transaction0); RTIO_IODEV_TEST_DEFINE(iodev_test_transaction1); @@ -399,41 +488,41 @@ void test_rtio_transaction_(struct rtio *r) struct rtio_cqe *cqe; bool seen[2] = { 0 }; - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, &iodev_test_transaction0, NULL); sqe->flags |= RTIO_SQE_TRANSACTION; - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, NULL, &userdata[0]); - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, &iodev_test_transaction1, NULL); sqe->flags |= RTIO_SQE_TRANSACTION; - sqe = rtio_spsc_acquire(r->sq); + sqe = rtio_sqe_acquire(r); zassert_not_null(sqe, "Expected a valid sqe"); rtio_sqe_prep_nop(sqe, NULL, &userdata[1]); TC_PRINT("submitting userdata 0 %p, userdata 1 %p\n", &userdata[0], &userdata[1]); res = rtio_submit(r, 4); - TC_PRINT("checking cq, completions available %lu\n", rtio_spsc_consumable(r->cq)); + TC_PRINT("checking cq, completions available %u\n", rtio_cqe_consumable(r)); zassert_ok(res, "Should return ok from rtio_execute"); - zassert_equal(rtio_spsc_consumable(r->cq), 4, "Should have 4 pending completions"); + zassert_equal(rtio_cqe_consumable(r), 4, "Should have 4 pending completions"); for (int i = 0; i < 4; i++) { TC_PRINT("consume %d\n", i); - cqe = rtio_spsc_consume(r->cq); + cqe = rtio_cqe_consume(r); zassert_not_null(cqe, "Expected a valid cqe"); zassert_ok(cqe->result, "Result should be ok"); if (i % 2 == 0) { zassert_is_null(cqe->userdata); - rtio_spsc_release(r->cq); + rtio_cqe_release(r, cqe); continue; } uintptr_t idx = *(uintptr_t *)cqe->userdata; @@ -441,7 +530,7 @@ void test_rtio_transaction_(struct rtio *r) TC_PRINT("userdata is %p, value %" PRIuPTR "\n", cqe->userdata, idx); zassert(idx == 0 || idx == 1, "idx should be 0 or 1"); seen[idx] = true; - rtio_spsc_release(r->cq); + rtio_cqe_release(r, cqe); } zassert_true(seen[0], "Should have seen transaction 0"); @@ -458,14 +547,49 @@ ZTEST(rtio_api, test_rtio_transaction) TC_PRINT("rtio transaction simple\n"); for (int i = 0; i < TEST_REPEATS; i++) { - test_rtio_transaction_(&r_transaction_simp); - } - TC_PRINT("rtio transaction concurrent\n"); - for (int i = 0; i < TEST_REPEATS; i++) { - test_rtio_transaction_(&r_transaction_con); + test_rtio_transaction_(&r_transaction); } } +//#define THROUGHPUT_ITERS 100000 +//RTIO_DEFINE(r_throughput, 4, 4); +// +//void _test_rtio_throughput(struct rtio *r) +//{ +// timing_t start_time, end_time; +// struct rtio_cqe *cqe; +// struct rtio_sqe *sqe; +// +// timing_init(); +// timing_start(); +// +// start_time = timing_counter_get(); +// +// for (uint32_t i = 0; i < THROUGHPUT_ITERS; i++) { +// sqe = rtio_sqe_acquire(r); +// rtio_sqe_prep_nop(sqe, NULL, NULL); +// rtio_submit(r, 0); +// cqe = rtio_cqe_consume(r); +// rtio_cqe_release(r, cqe); +// } +// +// end_time = timing_counter_get(); +// +// uint64_t cycles = timing_cycles_get(&start_time, &end_time); +// uint64_t ns = timing_cycles_to_ns(cycles); +// +// zassert_true(ns < UINT64_C(1000000000), "Should take less than a second"); +// TC_PRINT("%llu ns for %d iterations, %llu ns per op\n", +// ns, THROUGHPUT_ITERS, ns/THROUGHPUT_ITERS); +//} +// +// +//ZTEST(rtio_api, test_rtio_throughput) +//{ +// _test_rtio_throughput(&r_throughput); +//} + + static void *rtio_api_setup(void) { #ifdef CONFIG_USERSPACE diff --git a/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c b/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c index 5083d08785952..cc1ad6bad9fb4 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -25,14 +26,15 @@ static struct rtio_mpsc_node push_pop_nodes[2]; ZTEST(rtio_mpsc, test_push_pop) { - struct rtio_mpsc_node *node, *head, *stub, *next, *tail; + mpsc_ptr_t node, head; + struct rtio_mpsc_node *stub, *next, *tail; rtio_mpsc_init(&push_pop_q); - head = atomic_ptr_get(&push_pop_q.head); + head = mpsc_ptr_get(push_pop_q.head); tail = push_pop_q.tail; stub = &push_pop_q.stub; - next = atomic_ptr_get(&stub->next); + next = stub->next; zassert_equal(head, stub, "Head should point at stub"); zassert_equal(tail, stub, "Tail should point at stub"); @@ -43,12 +45,12 @@ ZTEST(rtio_mpsc, test_push_pop) rtio_mpsc_push(&push_pop_q, &push_pop_nodes[0]); - head = atomic_ptr_get(&push_pop_q.head); + head = mpsc_ptr_get(push_pop_q.head); zassert_equal(head, &push_pop_nodes[0], "Queue head should point at push_pop_node"); - next = atomic_ptr_get(&push_pop_nodes[0].next); + next = mpsc_ptr_get(push_pop_nodes[0].next); zassert_is_null(next, NULL, "push_pop_node next should point at null"); - next = atomic_ptr_get(&push_pop_q.stub.next); + next = mpsc_ptr_get(push_pop_q.stub.next); zassert_equal(next, &push_pop_nodes[0], "Queue stub should point at push_pop_node"); tail = push_pop_q.tail; stub = &push_pop_q.stub; @@ -182,4 +184,33 @@ ZTEST(rtio_mpsc, test_mpsc_threaded) } } +#define THROUGHPUT_ITERS 100000 + +ZTEST(rtio_mpsc, test_mpsc_throughput) +{ + struct rtio_mpsc_node node; + timing_t start_time, end_time; + + rtio_mpsc_init(&mpsc_q); + timing_init(); + timing_start(); + + start_time = timing_counter_get(); + + for (int i = 0; i < THROUGHPUT_ITERS; i++) { + rtio_mpsc_push(&mpsc_q, &node); + + rtio_mpsc_pop(&mpsc_q); + } + + end_time = timing_counter_get(); + + uint64_t cycles = timing_cycles_get(&start_time, &end_time); + uint64_t ns = timing_cycles_to_ns(cycles); + + zassert_true(ns < UINT64_C(1000000000), "Should take less than a second"); + TC_PRINT("%llu ns for %d iterations, %llu ns per op\n", ns, + THROUGHPUT_ITERS, ns/THROUGHPUT_ITERS); +} + ZTEST_SUITE(rtio_mpsc, NULL, NULL, NULL, NULL, NULL); diff --git a/tests/subsys/rtio/rtio_api/src/test_rtio_spsc.c b/tests/subsys/rtio/rtio_api/src/test_rtio_spsc.c index 9dda0960dbcec..1f2d101fd3b3c 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_spsc.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_spsc.c @@ -5,6 +5,7 @@ */ #include +#include #include #include "rtio_api.h" @@ -215,4 +216,39 @@ ZTEST(rtio_spsc, test_spsc_threaded) k_thread_join(tinfo[0].tid, K_FOREVER); } +#define THROUGHPUT_ITERS 100000 + +ZTEST(rtio_spsc, test_spsc_throughput) +{ + timing_t start_time, end_time; + + timing_init(); + timing_start(); + + start_time = timing_counter_get(); + + uint32_t *x, *y; + + for (int i = 0; i < THROUGHPUT_ITERS; i++) { + x = rtio_spsc_acquire(&spsc); + *x = i; + rtio_spsc_produce(&spsc); + + y = rtio_spsc_consume(&spsc); + rtio_spsc_release(&spsc); + } + + end_time = timing_counter_get(); + + uint64_t cycles = timing_cycles_get(&start_time, &end_time); + uint64_t ns = timing_cycles_to_ns(cycles); + + zassert_true(ns < UINT64_C(1000000000), "Should take less than a second"); + + TC_PRINT("%llu ns for %d iterations, %llu ns per op\n", ns, + THROUGHPUT_ITERS, ns/THROUGHPUT_ITERS); + +} + + ZTEST_SUITE(rtio_spsc, NULL, NULL, NULL, NULL, NULL);