From 2fd6dc5f68f6d6459060566738bbf919f3fa470f Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Fri, 28 Apr 2023 15:52:16 -0500 Subject: [PATCH 01/15] rtio: Use mpsc for submission and completion queue Rather than the rings, which weren't shared between userspace and kernel space in Zephyr like they are in Linux with io_uring, use atomic mpsc queues for submission and completion queues. Most importantly this removes a potential head of line blocker in the submission queue as the sqe would be held until a task is completed. As additional bonuses this avoids some additional locks and restrictions about what can be submitted and where. It also removes the need for two executors as all chains/transactions are done concurrently. Lastly this opens up the possibility for a common pool of sqe's to allocate from potentially saving lots of memory. Signed-off-by: Tom Burdick --- drivers/spi/spi_sam.c | 68 +-- include/zephyr/drivers/spi.h | 38 +- include/zephyr/linker/common-ram.ld | 2 + include/zephyr/rtio/rtio.h | 425 +++++++++--------- .../zephyr/rtio/rtio_executor_concurrent.h | 120 ----- include/zephyr/rtio/rtio_executor_simple.h | 91 ---- .../rtio/sensor_batch_processing/src/main.c | 15 +- .../sensor_batch_processing/src/vnd_sensor.c | 6 +- subsys/rtio/CMakeLists.txt | 23 +- subsys/rtio/rtio_executor.c | 155 +++++++ subsys/rtio/rtio_executor_common.h | 40 -- subsys/rtio/rtio_executor_concurrent.c | 301 ------------- subsys/rtio/rtio_executor_simple.c | 157 ------- subsys/rtio/rtio_handlers.c | 2 - subsys/rtio/rtio_init.c | 29 ++ subsys/rtio/rtio_userspace_init.c | 8 - tests/drivers/spi/spi_loopback/src/spi_rtio.c | 16 +- tests/lib/cpp/cxx/src/main.cpp | 2 - .../rtio/rtio_api/src/rtio_iodev_test.h | 45 +- .../subsys/rtio/rtio_api/src/test_rtio_api.c | 125 ++---- 20 files changed, 545 insertions(+), 1123 deletions(-) delete mode 100644 include/zephyr/rtio/rtio_executor_concurrent.h delete mode 100644 include/zephyr/rtio/rtio_executor_simple.h create mode 100644 subsys/rtio/rtio_executor.c delete mode 100644 subsys/rtio/rtio_executor_common.h delete mode 100644 subsys/rtio/rtio_executor_concurrent.c delete mode 100644 subsys/rtio/rtio_executor_simple.c create mode 100644 subsys/rtio/rtio_init.c delete mode 100644 subsys/rtio/rtio_userspace_init.c diff --git a/drivers/spi/spi_sam.c b/drivers/spi/spi_sam.c index 8673d0a867621f..ebc0b9758e4131 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/spi.h b/include/zephyr/drivers/spi.h index cfd9aa6e537a6a..9e0be4c8c4416f 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 a49288eeed79a6..f31c3bafd8f072 100644 --- a/include/zephyr/linker/common-ram.ld +++ b/include/zephyr/linker/common-ram.ld @@ -121,6 +121,8 @@ #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 */ diff --git a/include/zephyr/rtio/rtio.h b/include/zephyr/rtio/rtio.h index d6d21ff648ba6f..8ce312679e3646 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -8,25 +8,21 @@ * @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. * - * The completion of these requests are pushed into a fixed size completion - * queue which an application may actively poll for completions. + * They may 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. + * + * An executor takes the queues and determines how to perform each requested operation. */ #ifndef ZEPHYR_INCLUDE_RTIO_RTIO_H_ @@ -38,10 +34,10 @@ #include #include #include -#include #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -210,8 +206,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 +249,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. */ @@ -349,22 +277,17 @@ enum rtio_mempool_entry_state { BUILD_ASSERT(RTIO_MEMPOOL_ENTRY_STATE_COUNT < 4); /** - * @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. * - * 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. + * A queue for submissions (available and in queue to be produced) as well as a queue + * of completions (available and ready to be consumed). * - * Nothing is done until a call is performed to do the work (rtio_execute). + * The rtio executor along with any objects implementing the rtio_iodev interface are + * the consumers of submissions and producers of completions. + * + * 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,11 +310,23 @@ struct rtio { */ atomic_t xcqcnt; + /* Submission queue object pool with free list */ + struct rtio_iodev_sqe *sqe_pool; + struct rtio_mpsc sq_free; + const uint16_t sqe_pool_sz; + uint16_t sqe_pool_free; + + /* Completion queue object pool with free list */ + struct rtio_cqe *cqe_pool; + struct rtio_mpsc cq_free; + const uint16_t cqe_pool_sz; + uint16_t cqe_pool_used; + /* Submission queue */ - struct rtio_sq *sq; + struct rtio_mpsc sq; /* Completion queue */ - struct rtio_cq *cq; + struct rtio_mpsc cq; #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS /* Memory pool associated with this RTIO context. */ @@ -427,10 +362,13 @@ static inline uint16_t __rtio_compute_mempool_block_index(const struct rtio *r, /** * @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; }; @@ -493,7 +431,6 @@ static inline void rtio_sqe_prep_nop(struct rtio_sqe *sqe, void *userdata) { sqe->op = RTIO_OP_NOP; - sqe->flags = 0; sqe->iodev = iodev; sqe->userdata = userdata; } @@ -510,7 +447,6 @@ static inline void rtio_sqe_prep_read(struct rtio_sqe *sqe, { sqe->op = RTIO_OP_RX; sqe->prio = prio; - sqe->flags = 0; sqe->iodev = iodev; sqe->buf_len = len; sqe->buf = buf; @@ -542,7 +478,6 @@ static inline void rtio_sqe_prep_write(struct rtio_sqe *sqe, { sqe->op = RTIO_OP_TX; sqe->prio = prio; - sqe->flags = 0; sqe->iodev = iodev; sqe->buf_len = len; sqe->buf = buf; @@ -570,7 +505,6 @@ static inline void rtio_sqe_prep_tiny_write(struct rtio_sqe *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); @@ -592,7 +526,6 @@ static inline void rtio_sqe_prep_callback(struct rtio_sqe *sqe, { sqe->op = RTIO_OP_CALLBACK; sqe->prio = 0; - sqe->flags = 0; sqe->iodev = NULL; sqe->callback = callback; sqe->arg0 = arg0; @@ -612,7 +545,6 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *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,24 +552,6 @@ 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) - -/** - * @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) - /** * @brief Statically define and initialize an RTIO IODev * @@ -653,34 +567,40 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, } /* clang-format off */ -#define _RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ +#define _RTIO_DEFINE(name, 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); \ + static struct rtio_iodev_sqe _sqe_pool_##name[sq_sz]; \ + static struct rtio_cqe _cqe_pool_##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, + .sqe_pool = _sqe_pool_##name, \ + .sq_free = RTIO_MPSC_INIT((name.sq_free)), \ + .sqe_pool_sz = sq_sz, \ + .sqe_pool_free = 0, \ + .cqe_pool = _cqe_pool_##name, \ + .cq_free = RTIO_MPSC_INIT((name.cq_free)), \ + .cqe_pool_sz = cq_sz, \ + .cqe_pool_used = 0, \ + .sq = RTIO_MPSC_INIT((name.sq)), \ + .cq = RTIO_MPSC_INIT((name.cq)), /* 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 sq_sz Size of the submission queue entry pool + * @param cq_sz Size of the completion queue entry pool */ /* clang-format off */ -#define RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ - _RTIO_DEFINE(name, exec, sq_sz, cq_sz) \ +#define RTIO_DEFINE(name, sq_sz, cq_sz) \ + _RTIO_DEFINE(name, sq_sz, cq_sz) \ } /* clang-format on */ @@ -710,7 +630,6 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, * @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 @@ -718,48 +637,87 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, * @param balign The block alignment */ /* clang-format off */ -#define RTIO_DEFINE_WITH_MEMPOOL(name, exec, sq_sz, cq_sz, num_blks, blk_size, balign) \ +#define RTIO_DEFINE_WITH_MEMPOOL(name, 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) \ + _RTIO_DEFINE(name, sq_sz, cq_sz) \ .mempool = &_mempool_##name, \ .mempool_blk_size = WB_UP(blk_size), \ } /* 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_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_used; } /** - * @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) +{ + 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 rtio_spsc_acquirable(r->sq); + return iodev_sqe->next; } /** @@ -772,30 +730,67 @@ 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; + struct rtio_mpsc_node *node = rtio_mpsc_pop(&r->sq_free); + + if (node == NULL) { + return NULL; + } + + iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q); + + memset(iodev_sqe, 0, sizeof(struct rtio_iodev_sqe)); + + rtio_mpsc_push(&r->sq, &iodev_sqe->q); + + r->sqe_pool_free--; + + 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_mpsc_push(&r->sq_free, &iodev_sqe->q); + node = rtio_mpsc_pop(&r->sq); + r->sqe_pool_free++; + } +} /** - * @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_mpsc_node *node = rtio_mpsc_pop(&r->cq_free); + + if (node == NULL) { + return NULL; + } + + struct rtio_cqe *cqe = CONTAINER_OF(node, struct rtio_cqe, q); + + 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 +805,27 @@ 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 { - return NULL; + node = rtio_mpsc_pop(&r->cq); + if (node == NULL) { + return NULL; + } + cqe = CONTAINER_OF(node, struct rtio_cqe, q); } #else - return rtio_spsc_consume(r->cq); + node = rtio_mpsc_pop(&r->cq); + if (node == NULL) { + return NULL; + } + cqe = CONTAINER_OF(node, struct rtio_cqe, q); #endif + r->cqe_pool_used--; + + return cqe; } /** @@ -833,20 +840,26 @@ 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); + node = rtio_mpsc_pop(&r->cq); + if (node == NULL) { + return NULL; + } + cqe = CONTAINER_OF(node, struct rtio_cqe, q); #else - cqe = rtio_spsc_consume(r->cq); - - while (cqe == NULL) { - cqe = rtio_spsc_consume(r->cq); - + 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); #endif + r->cqe_pool_used--; return cqe; } @@ -855,24 +868,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) -{ - rtio_spsc_release(r->cq); -} - -/** - * @brief Release all consumed completion queue events - * - * @param r RTIO context - */ -static inline void rtio_cqe_release_all(struct rtio *r) +static inline void rtio_cqe_release(struct rtio *r, struct rtio_cqe *cqe) { - rtio_spsc_release_all(r->cq); + rtio_mpsc_push(&r->cq_free, &cqe->q); } /** - * @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 +888,10 @@ 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; + 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; flags = RTIO_CQE_FLAG_PREP_MEMPOOL(blk_index, blk_count); } @@ -940,6 +944,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 +958,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 +971,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 +1005,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 +1013,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) { @@ -1018,6 +1025,8 @@ static inline void rtio_cqe_submit(struct rtio *r, int result, void *userdata, u #endif #ifdef CONFIG_RTIO_CONSUME_SEM k_sem_give(r->consume_sem); +#else + r->cqe_pool_used++; #endif } @@ -1038,7 +1047,7 @@ 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) { @@ -1046,8 +1055,6 @@ static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32 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) { @@ -1064,8 +1071,8 @@ static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32 if (rc == 0) { *buf_len = num_blks * blk_size; - mutable_sqe->buf = *buf; - mutable_sqe->buf_len = *buf_len; + sqe->buf = *buf; + sqe->buf_len = *buf_len; return 0; } if (bytes == min_buf_len) { @@ -1108,8 +1115,7 @@ static inline void z_impl_rtio_release_buffer(struct rtio *r, void *buff, uint32 return; } - sys_mem_blocks_free_contiguous(r->mempool, buff, - DIV_ROUND_UP(buff_len, r->mempool_blk_size)); + sys_mem_blocks_free_contiguous(r->mempool, buff, buff_len); #endif } @@ -1165,8 +1171,6 @@ static inline int z_impl_rtio_sqe_copy_in(struct rtio *r, *sqe = sqes[i]; } - rtio_sqe_produce_all(r); - return 0; } @@ -1203,11 +1207,9 @@ static inline int z_impl_rtio_cqe_copy_out(struct rtio *r, break; } cqes[copied] = *cqe; + rtio_cqe_release(r, cqe); } - - rtio_cqe_release_all(r); - return copied; } @@ -1228,9 +1230,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 +1244,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 +1261,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 (r->cqe_pool_used < 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 26bda6bfe0a9ed..00000000000000 --- 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 3a394b019e7fdd..00000000000000 --- 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/samples/subsys/rtio/sensor_batch_processing/src/main.c b/samples/subsys/rtio/sensor_batch_processing/src/main.c index fbf2c0c300aac7..c9306c68f01570 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 691fe7103a55e2..c00dfd9d421c9a 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 01972b594fa246..41b457b28461d3 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 00000000000000..06171849fd58b1 --- /dev/null +++ b/subsys/rtio/rtio_executor.c @@ -0,0 +1,155 @@ +/* + * 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) +{ + 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 Callback from an iodev describing success + */ +void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) +{ + struct rtio_iodev_sqe *curr = iodev_sqe, *next; + struct rtio *r = curr->r; + 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); + rtio_mpsc_push(&r->sq_free, &curr->q); + r->sqe_pool_free++; + rtio_cqe_submit(r, result, userdata, cqe_flags); + curr = next; + } 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 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) +{ + struct rtio *r = iodev_sqe->r; + struct rtio_iodev_sqe *curr = iodev_sqe, *next; + void *userdata = curr->sqe.userdata; + uint32_t sqe_flags = iodev_sqe->sqe.flags; + uint32_t cqe_flags = rtio_cqe_compute_flags(curr); + + while (sqe_flags & (RTIO_SQE_CHAINED | RTIO_SQE_TRANSACTION)) { + userdata = curr->sqe.userdata; + sqe_flags = curr->sqe.flags; + cqe_flags = rtio_cqe_compute_flags(curr); + + next = rtio_iodev_sqe_next(curr); + rtio_mpsc_push(&r->sq_free, &curr->q); + r->sqe_pool_free++; + rtio_cqe_submit(r, result, userdata, cqe_flags); + curr = next; + result = -ECANCELED; + } + + rtio_mpsc_push(&r->sq_free, &curr->q); + r->sqe_pool_free++; + + rtio_cqe_submit(r, result, userdata, cqe_flags); +} diff --git a/subsys/rtio/rtio_executor_common.h b/subsys/rtio/rtio_executor_common.h deleted file mode 100644 index 78fa8416b51604..00000000000000 --- 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 e58309a84633f1..00000000000000 --- 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 8231d45362d36a..00000000000000 --- 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 1fb0dac9642766..16d6745fb2ab00 100644 --- a/subsys/rtio/rtio_handlers.c +++ b/subsys/rtio/rtio_handlers.c @@ -94,8 +94,6 @@ 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); } diff --git a/subsys/rtio/rtio_init.c b/subsys/rtio/rtio_init.c new file mode 100644 index 00000000000000..ce27b9290dfaac --- /dev/null +++ b/subsys/rtio/rtio_init.c @@ -0,0 +1,29 @@ +/* + * 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, r) { + for (int i = 0; i < r->sqe_pool_sz; i++) { + rtio_mpsc_push(&r->sq_free, &r->sqe_pool[i].q); + } + for (int i = 0; i < r->cqe_pool_sz; i++) { + rtio_mpsc_push(&r->cq_free, &r->cqe_pool[i].q); + } + r->sqe_pool_free = r->sqe_pool_sz; + r->cqe_pool_used = 0; + } + 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 6df54d1d2364ca..00000000000000 --- 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 5498ad0571c4c9..5c1bb4a356dac7 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 0f9447337d39f5..02f7f767efe412 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/src/rtio_iodev_test.h b/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h index eef85e5490735f..966f379c67d1d1 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: @@ -55,40 +59,46 @@ static void rtio_iodev_test_next(struct rtio_iodev *iodev) static void rtio_iodev_timer_fn(struct k_timer *tm) { 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 (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); + rtio_iodev_sqe_ok(iodev_sqe, 0); } 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 +118,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 7fee16fad51554..fbe0616814d77c 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c @@ -13,33 +13,24 @@ #include #include #include -#include #include #include -#include -#include #include "rtio_iodev_test.h" /* Repeat tests to ensure they are repeatable */ -#define TEST_REPEATS 4 +#define TEST_REPEATS 2 #define MEM_BLK_COUNT 4 #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,16 @@ 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 +193,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 +203,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 +213,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 +269,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 +284,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 +312,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\n"); 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 +324,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 @@ -374,11 +343,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 +364,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 +406,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,11 +423,7 @@ 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); } } From d1dd93b1ad93ec94a393def12599e3716704af72 Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Sat, 29 Apr 2023 15:45:41 -0500 Subject: [PATCH 02/15] rtio: Add throughput test Test throughput of submit and consume pair for rtio Signed-off-by: Tom Burdick --- tests/subsys/rtio/rtio_api/prj.conf | 1 + .../subsys/rtio/rtio_api/src/test_rtio_api.c | 46 +++++++++++++++++-- .../subsys/rtio/rtio_api/src/test_rtio_mpsc.c | 30 ++++++++++++ .../subsys/rtio/rtio_api/src/test_rtio_spsc.c | 36 +++++++++++++++ 4 files changed, 110 insertions(+), 3 deletions(-) diff --git a/tests/subsys/rtio/rtio_api/prj.conf b/tests/subsys/rtio/rtio_api/prj.conf index baded798fd45a1..aa4a5fcfadbf56 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/test_rtio_api.c b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c index fbe0616814d77c..3ab39eb569667a 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c @@ -13,13 +13,13 @@ #include #include #include -#include +#include #include #include "rtio_iodev_test.h" /* Repeat tests to ensure they are repeatable */ -#define TEST_REPEATS 2 +#define TEST_REPEATS 4 #define MEM_BLK_COUNT 4 #define MEM_BLK_SIZE 16 @@ -182,7 +182,8 @@ void test_rtio_multiple_chains_(struct rtio *r) cqe = rtio_cqe_consume(r); } - TC_PRINT("consumed cqe %p, result, %d, userdata %lu\n", cqe, cqe-> result, (uintptr_t)cqe->userdata); + 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"); zassert_ok(cqe->result, "Result should be ok"); @@ -427,6 +428,45 @@ ZTEST(rtio_api, test_rtio_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 5083d087859527..879b9649f7d5cd 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 @@ -182,4 +183,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 9dda0960dbcecc..1f2d101fd3b3c4 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); From 362173ebf70761ee9d976f8e41a3513568226160 Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Sat, 29 Apr 2023 16:37:06 -0500 Subject: [PATCH 03/15] rtio: Make MPSC faster on non-smp systems On non-smp systems where multiple cores aren't in play atomics aren't really necessary and volatile can be used in stead. Additionally marks the push function as ALWAYS_INLINE as I saw at times it was not being inlined. MPSC operation speed is crucial to the performance of rtio, these changes provided a 30% throughput improvmement in the throughput test. Signed-off-by: Tom Burdick --- include/zephyr/rtio/rtio_mpsc.h | 62 ++++++++++++++----- .../subsys/rtio/rtio_api/src/test_rtio_mpsc.c | 13 ++-- 2 files changed, 54 insertions(+), 21 deletions(-) diff --git a/include/zephyr/rtio/rtio_mpsc.h b/include/zephyr/rtio/rtio_mpsc.h index c4d81c791a0f64..f129f5712aaadf 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/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c b/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c index 879b9649f7d5cd..cc1ad6bad9fb48 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_mpsc.c @@ -26,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"); @@ -44,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; From 784689dae56f7fa1f43503a40cbb6afa4ef85a27 Mon Sep 17 00:00:00 2001 From: Tom Burdick Date: Sun, 30 Apr 2023 17:02:39 -0500 Subject: [PATCH 04/15] rtio: Cleanup the various define macros Reworks the zephyr macros and pools to be objects in their own right. Each pool can be statically defined with a Z_ private macro. The objects can then be initialized with an rtio instance statically. This cleans up a lot of code that was otherwise doing little bits of management around allocation/freeing and reduces the scope those functions has to the data it needs. This should enable sharing the pools of sqe, cqe, and mem blocks among rtio instances in a future improvement easily. Signed-off-by: Tom Burdick --- include/zephyr/linker/common-ram.ld | 2 - include/zephyr/rtio/rtio.h | 360 +++++++++++------- subsys/rtio/rtio_executor.c | 10 +- subsys/rtio/rtio_handlers.c | 5 +- subsys/rtio/rtio_init.c | 16 +- .../subsys/rtio/rtio_api/src/test_rtio_api.c | 2 +- 6 files changed, 237 insertions(+), 158 deletions(-) diff --git a/include/zephyr/linker/common-ram.ld b/include/zephyr/linker/common-ram.ld index f31c3bafd8f072..39a49a631b686f 100644 --- a/include/zephyr/linker/common-ram.ld +++ b/include/zephyr/linker/common-ram.ld @@ -117,7 +117,6 @@ } 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) @@ -125,7 +124,6 @@ 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 8ce312679e3646..7702d26ddefd93 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -14,15 +14,13 @@ * 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. * - * They may be submitted as a transaction where a set of operations are considered to be one - * operation. + * Operations may also be submitted as a transaction where a set of operations are considered + * to be one operation. * * The completion of these operations typically provide one or more completion queue events. - * - * An executor takes the queues and determines how to perform each requested operation. */ #ifndef ZEPHYR_INCLUDE_RTIO_RTIO_H_ @@ -180,6 +178,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 */ @@ -276,6 +277,27 @@ 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 context containing what can be viewed as a pair of queues. * @@ -311,29 +333,21 @@ struct rtio { atomic_t xcqcnt; /* Submission queue object pool with free list */ - struct rtio_iodev_sqe *sqe_pool; - struct rtio_mpsc sq_free; - const uint16_t sqe_pool_sz; - uint16_t sqe_pool_free; + struct rtio_sqe_pool *sqe_pool; - /* Completion queue object pool with free list */ - struct rtio_cqe *cqe_pool; - struct rtio_mpsc cq_free; - const uint16_t cqe_pool_sz; - uint16_t cqe_pool_used; + /* Complete queue object pool with free list */ + struct rtio_cqe_pool *cqe_pool; + +#ifdef 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; - -#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 */ }; /** The memory partition associated with all RTIO context information */ @@ -350,13 +364,16 @@ 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 @@ -552,6 +569,84 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, sqe->userdata = userdata; } +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); + + 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 * @@ -559,50 +654,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, 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))) \ - static struct rtio_iodev_sqe _sqe_pool_##name[sq_sz]; \ - static struct rtio_cqe _cqe_pool_##name[cq_sz]; \ - 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_##name, \ - .sq_free = RTIO_MPSC_INIT((name.sq_free)), \ - .sqe_pool_sz = sq_sz, \ - .sqe_pool_free = 0, \ - .cqe_pool = _cqe_pool_##name, \ - .cq_free = RTIO_MPSC_INIT((name.cq_free)), \ - .cqe_pool_sz = cq_sz, \ - .cqe_pool_used = 0, \ - .sq = RTIO_MPSC_INIT((name.sq)), \ - .cq = RTIO_MPSC_INIT((name.cq)), -/* 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 sq_sz Size of the submission queue entry pool - * @param cq_sz Size of the completion queue entry pool - */ -/* clang-format off */ -#define RTIO_DEFINE(name, sq_sz, cq_sz) \ - _RTIO_DEFINE(name, 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 @@ -626,6 +702,48 @@ 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 * @@ -636,16 +754,12 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, * @param blk_size The number of bytes in each block * @param balign The block alignment */ -/* clang-format off */ -#define RTIO_DEFINE_WITH_MEMPOOL(name, 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, 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 */ /** @@ -657,7 +771,7 @@ static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe, */ static inline uint32_t rtio_sqe_acquirable(struct rtio *r) { - return r->sqe_pool_free; + return r->sqe_pool->pool_free; } /** @@ -669,7 +783,7 @@ static inline uint32_t rtio_sqe_acquirable(struct rtio *r) */ static inline uint32_t rtio_cqe_consumable(struct rtio *r) { - return r->cqe_pool_used; + return (r->cqe_pool->pool_size - r->cqe_pool->pool_free); } /** @@ -730,21 +844,14 @@ static inline struct rtio_iodev_sqe *rtio_iodev_sqe_next(const struct rtio_iodev */ static inline struct rtio_sqe *rtio_sqe_acquire(struct rtio *r) { - struct rtio_iodev_sqe *iodev_sqe; - struct rtio_mpsc_node *node = rtio_mpsc_pop(&r->sq_free); + struct rtio_iodev_sqe *iodev_sqe = rtio_sqe_pool_alloc(r->sqe_pool); - if (node == NULL) { + if (iodev_sqe == NULL) { return NULL; } - iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q); - - memset(iodev_sqe, 0, sizeof(struct rtio_iodev_sqe)); - rtio_mpsc_push(&r->sq, &iodev_sqe->q); - r->sqe_pool_free--; - return &iodev_sqe->sqe; } @@ -760,9 +867,8 @@ static inline void rtio_sqe_drop_all(struct rtio *r) while (node != NULL) { iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q); - rtio_mpsc_push(&r->sq_free, &iodev_sqe->q); + rtio_sqe_pool_free(r->sqe_pool, iodev_sqe); node = rtio_mpsc_pop(&r->sq); - r->sqe_pool_free++; } } @@ -771,14 +877,12 @@ static inline void rtio_sqe_drop_all(struct rtio *r) */ static inline struct rtio_cqe *rtio_cqe_acquire(struct rtio *r) { - struct rtio_mpsc_node *node = rtio_mpsc_pop(&r->cq_free); + struct rtio_cqe *cqe = rtio_cqe_pool_alloc(r->cqe_pool); - if (node == NULL) { + if (cqe == NULL) { return NULL; } - struct rtio_cqe *cqe = CONTAINER_OF(node, struct rtio_cqe, q); - memset(cqe, 0, sizeof(struct rtio_cqe)); return cqe; @@ -809,21 +913,16 @@ static inline struct rtio_cqe *rtio_cqe_consume(struct rtio *r) struct rtio_cqe *cqe = NULL; #ifdef CONFIG_RTIO_CONSUME_SEM - if (k_sem_take(r->consume_sem, K_NO_WAIT) == 0) { - node = rtio_mpsc_pop(&r->cq); - if (node == NULL) { - return NULL; - } - cqe = CONTAINER_OF(node, struct rtio_cqe, q); + if (k_sem_take(r->consume_sem, K_NO_WAIT) != 0) { + return NULL; } -#else +#endif + node = rtio_mpsc_pop(&r->cq); if (node == NULL) { return NULL; } cqe = CONTAINER_OF(node, struct rtio_cqe, q); -#endif - r->cqe_pool_used--; return cqe; } @@ -845,21 +944,13 @@ static inline struct rtio_cqe *rtio_cqe_consume_block(struct rtio *r) #ifdef CONFIG_RTIO_CONSUME_SEM k_sem_take(r->consume_sem, K_FOREVER); - - node = rtio_mpsc_pop(&r->cq); - if (node == NULL) { - return NULL; - } - cqe = CONTAINER_OF(node, struct rtio_cqe, q); -#else +#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); -#endif - r->cqe_pool_used--; return cqe; } @@ -872,7 +963,7 @@ static inline struct rtio_cqe *rtio_cqe_consume_block(struct rtio *r) */ static inline void rtio_cqe_release(struct rtio *r, struct rtio_cqe *cqe) { - rtio_mpsc_push(&r->cq_free, &cqe->q); + rtio_cqe_pool_free(r->cqe_pool, cqe); } /** @@ -890,8 +981,10 @@ static inline uint32_t rtio_cqe_compute_flags(struct rtio_iodev_sqe *iodev_sqe) #ifdef CONFIG_RTIO_SYS_MEM_BLOCKS 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); } @@ -926,11 +1019,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; @@ -1025,8 +1119,6 @@ static inline void rtio_cqe_submit(struct rtio *r, int result, void *userdata, u #endif #ifdef CONFIG_RTIO_CONSUME_SEM k_sem_give(r->consume_sem); -#else - r->cqe_pool_used++; #endif } @@ -1052,9 +1144,6 @@ static inline int rtio_sqe_rx_buf(const struct rtio_iodev_sqe *iodev_sqe, uint32 #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; if (sqe->buf != NULL) { if (sqe->buf_len < min_buf_len) { @@ -1065,21 +1154,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; - sqe->buf = *buf; - 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 @@ -1111,11 +1193,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, buff_len); + rtio_block_pool_free(r->block_pool, buff, buff_len); #endif } @@ -1261,7 +1343,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 (r->cqe_pool_used < wait_count) { + while (rtio_cqe_consumable(r) < wait_count) { Z_SPIN_DELAY(10); k_yield(); } diff --git a/subsys/rtio/rtio_executor.c b/subsys/rtio/rtio_executor.c index 06171849fd58b1..a9f9177d939df2 100644 --- a/subsys/rtio/rtio_executor.c +++ b/subsys/rtio/rtio_executor.c @@ -108,8 +108,7 @@ void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) cqe_flags = rtio_cqe_compute_flags(iodev_sqe); next = rtio_iodev_sqe_next(curr); - rtio_mpsc_push(&r->sq_free, &curr->q); - r->sqe_pool_free++; + rtio_sqe_pool_free(r->sqe_pool, curr); rtio_cqe_submit(r, result, userdata, cqe_flags); curr = next; } while (sqe_flags & RTIO_SQE_TRANSACTION); @@ -141,15 +140,12 @@ void rtio_executor_err(struct rtio_iodev_sqe *iodev_sqe, int result) cqe_flags = rtio_cqe_compute_flags(curr); next = rtio_iodev_sqe_next(curr); - rtio_mpsc_push(&r->sq_free, &curr->q); - r->sqe_pool_free++; + rtio_sqe_pool_free(r->sqe_pool, curr); rtio_cqe_submit(r, result, userdata, cqe_flags); curr = next; result = -ECANCELED; } - rtio_mpsc_push(&r->sq_free, &curr->q); - r->sqe_pool_free++; - + rtio_sqe_pool_free(r->sqe_pool, curr); rtio_cqe_submit(r, result, userdata, cqe_flags); } diff --git a/subsys/rtio/rtio_handlers.c b/subsys/rtio/rtio_handlers.c index 16d6745fb2ab00..fe06a14ed9af23 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 @@ -75,7 +76,7 @@ static inline int z_vrfy_rtio_sqe_copy_in(struct rtio *r, { 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); @@ -106,7 +107,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 index ce27b9290dfaac..6c95986936c173 100644 --- a/subsys/rtio/rtio_init.c +++ b/subsys/rtio/rtio_init.c @@ -13,16 +13,18 @@ K_APPMEM_PARTITION_DEFINE(rtio_partition); int rtio_init(void) { - STRUCT_SECTION_FOREACH(rtio, r) { - for (int i = 0; i < r->sqe_pool_sz; i++) { - rtio_mpsc_push(&r->sq_free, &r->sqe_pool[i].q); + 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); } - for (int i = 0; i < r->cqe_pool_sz; i++) { - rtio_mpsc_push(&r->cq_free, &r->cqe_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); } - r->sqe_pool_free = r->sqe_pool_sz; - r->cqe_pool_used = 0; } + return 0; } 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 3ab39eb569667a..cdb3ad216e8686 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c @@ -313,7 +313,7 @@ static void rtio_simple_mempool_test(void *a, void *b, void *c) ARG_UNUSED(b); ARG_UNUSED(c); - TC_PRINT("rtio simple mempool\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, i); } From d2d00ac51254ce7e398d2fd851bc07f1e929b1e6 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 3 May 2023 12:32:40 -0600 Subject: [PATCH 05/15] DNM, local IDE required change --- .clang-format | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.clang-format b/.clang-format index fdcd735fe283f3..dc561d739b9c0f 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 From c6240bff4080e806621ab04c57d9e84bfc52deb6 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 3 May 2023 12:32:04 -0600 Subject: [PATCH 06/15] rtio: Clear sqe data before prep Add a memset to clear out the sqe prior to running any of the rtio_sqe_prep_* functions. Signed-off-by: Yuval Peress topic#rtio_multishot --- include/zephyr/rtio/rtio.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/include/zephyr/rtio/rtio.h b/include/zephyr/rtio/rtio.h index 7702d26ddefd93..0725108cef7903 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -447,6 +447,7 @@ 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->iodev = iodev; sqe->userdata = userdata; @@ -462,6 +463,7 @@ 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->iodev = iodev; @@ -493,6 +495,7 @@ 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->iodev = iodev; @@ -520,6 +523,7 @@ 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->iodev = iodev; @@ -541,6 +545,7 @@ 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->iodev = NULL; @@ -560,6 +565,7 @@ 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->iodev = iodev; From 431bd87dbeda78ef9f70c910c870fa14a5b73a57 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Fri, 28 Apr 2023 16:47:21 -0600 Subject: [PATCH 07/15] rtio: add cancel support - Add a new API `rtio_sqe_cancel` to attempt canceling a queued SQE - Add a new syscall `rtio_sqe_copy_in_get_handles` which allows getting back the SQE handles generated by the copy_in operation so that they can be canceled. Signed-off-by: Yuval Peress topic#rtio_multishot --- include/zephyr/rtio/rtio.h | 97 +++++++++++++++---- subsys/rtio/rtio_executor.c | 64 ++++++------ subsys/rtio/rtio_handlers.c | 10 +- .../subsys/rtio/rtio_api/src/test_rtio_api.c | 44 +++++++++ 4 files changed, 161 insertions(+), 54 deletions(-) diff --git a/include/zephyr/rtio/rtio.h b/include/zephyr/rtio/rtio.h index 0725108cef7903..aa96b1ebe74959 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -122,6 +122,14 @@ 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) + /** * @} */ @@ -1224,27 +1232,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); @@ -1256,12 +1281,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]; } 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 * @@ -1286,17 +1335,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; + cqes[copied++] = *cqe; rtio_cqe_release(r, cqe); - } + } while (copied < cqe_count && end > k_uptime_ticks()); return copied; } diff --git a/subsys/rtio/rtio_executor.c b/subsys/rtio/rtio_executor.c index a9f9177d939df2..f4203068ef6064 100644 --- a/subsys/rtio/rtio_executor.c +++ b/subsys/rtio/rtio_executor.c @@ -20,11 +20,17 @@ LOG_MODULE_REGISTER(rtio_executor, CONFIG_RTIO_LOG_LEVEL); */ 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 */ @@ -78,8 +84,9 @@ void rtio_executor_submit(struct rtio *r) curr = next; curr->r = r; - __ASSERT(curr != NULL, - "Expected a valid sqe following transaction or chain flag"); + __ASSERT( + curr != NULL, + "Expected a valid sqe following transaction or chain flag"); } curr->next = NULL; @@ -92,13 +99,11 @@ void rtio_executor_submit(struct rtio *r) } } -/** - * @brief Callback from an iodev describing success - */ -void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) +static inline void rtio_executor_done(struct rtio_iodev_sqe *iodev_sqe, int result, bool is_ok) { + 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; - struct rtio *r = curr->r; void *userdata; uint32_t sqe_flags, cqe_flags; @@ -108,9 +113,19 @@ void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) cqe_flags = rtio_cqe_compute_flags(iodev_sqe); next = rtio_iodev_sqe_next(curr); + + /* SQE is no longer needed, release it */ rtio_sqe_pool_free(r->sqe_pool, curr); - rtio_cqe_submit(r, result, userdata, cqe_flags); + + 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 */ @@ -119,6 +134,14 @@ void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) } } +/** + * @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 * @@ -128,24 +151,5 @@ void rtio_executor_ok(struct rtio_iodev_sqe *iodev_sqe, int result) */ void rtio_executor_err(struct rtio_iodev_sqe *iodev_sqe, int result) { - struct rtio *r = iodev_sqe->r; - struct rtio_iodev_sqe *curr = iodev_sqe, *next; - void *userdata = curr->sqe.userdata; - uint32_t sqe_flags = iodev_sqe->sqe.flags; - uint32_t cqe_flags = rtio_cqe_compute_flags(curr); - - while (sqe_flags & (RTIO_SQE_CHAINED | RTIO_SQE_TRANSACTION)) { - userdata = curr->sqe.userdata; - sqe_flags = curr->sqe.flags; - cqe_flags = rtio_cqe_compute_flags(curr); - - next = rtio_iodev_sqe_next(curr); - rtio_sqe_pool_free(r->sqe_pool, curr); - rtio_cqe_submit(r, result, userdata, cqe_flags); - curr = next; - result = -ECANCELED; - } - - rtio_sqe_pool_free(r->sqe_pool, curr); - rtio_cqe_submit(r, result, userdata, cqe_flags); + rtio_executor_done(iodev_sqe, result, false); } diff --git a/subsys/rtio/rtio_handlers.c b/subsys/rtio/rtio_handlers.c index fe06a14ed9af23..24555ab8f39c7d 100644 --- a/subsys/rtio/rtio_handlers.c +++ b/subsys/rtio/rtio_handlers.c @@ -70,9 +70,8 @@ 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)); @@ -87,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)) { @@ -96,7 +98,7 @@ static inline int z_vrfy_rtio_sqe_copy_in(struct rtio *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 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 cdb3ad216e8686..0ee3ad7dc29fce 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c @@ -333,6 +333,50 @@ 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 +} + ZTEST(rtio_api, test_rtio_syscalls) { From ddb5ee842ed5f31ee87bfa67cd5967cf90a1d95f Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 3 May 2023 22:44:56 -0600 Subject: [PATCH 08/15] rtio: Add support for multishot reads - Introduce multishot reads which remain on the SQ until canceled Signed-off-by: Yuval Peress topic#rtio_multishot --- include/zephyr/rtio/rtio.h | 16 ++ subsys/rtio/rtio_executor.c | 41 ++++- .../rtio/rtio_api/src/rtio_iodev_test.h | 16 +- .../subsys/rtio/rtio_api/src/test_rtio_api.c | 153 +++++++++++++----- 4 files changed, 184 insertions(+), 42 deletions(-) diff --git a/include/zephyr/rtio/rtio.h b/include/zephyr/rtio/rtio.h index aa96b1ebe74959..41ae641d3afc89 100644 --- a/include/zephyr/rtio/rtio.h +++ b/include/zephyr/rtio/rtio.h @@ -130,6 +130,14 @@ extern "C" { */ #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) + /** * @} */ @@ -493,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 */ diff --git a/subsys/rtio/rtio_executor.c b/subsys/rtio/rtio_executor.c index f4203068ef6064..2dcea8c600fba6 100644 --- a/subsys/rtio/rtio_executor.c +++ b/subsys/rtio/rtio_executor.c @@ -99,8 +99,38 @@ void rtio_executor_submit(struct rtio *r) } } +/** + * @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; @@ -113,10 +143,13 @@ static inline void rtio_executor_done(struct rtio_iodev_sqe *iodev_sqe, int resu cqe_flags = rtio_cqe_compute_flags(iodev_sqe); next = rtio_iodev_sqe_next(curr); - - /* SQE is no longer needed, release it */ - rtio_sqe_pool_free(r->sqe_pool, 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); 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 966f379c67d1d1..67a412d819ee4b 100644 --- a/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h +++ b/tests/subsys/rtio/rtio_api/src/rtio_iodev_test.h @@ -58,10 +58,20 @@ 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->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 (iodev_sqe->sqe.op == RTIO_OP_RX) { uint8_t *buf; uint32_t buf_len; @@ -93,7 +103,11 @@ static void rtio_iodev_timer_fn(struct k_timer *tm) data->txn_head = NULL; data->txn_curr = NULL; rtio_iodev_test_next(iodev); - rtio_iodev_sqe_ok(iodev_sqe, 0); + 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) 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 0ee3ad7dc29fce..3ed9e16beef111 100644 --- a/tests/subsys/rtio/rtio_api/src/test_rtio_api.c +++ b/tests/subsys/rtio/rtio_api/src/test_rtio_api.c @@ -377,6 +377,85 @@ ZTEST(rtio_api, test_rtio_simple_cancel) #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) { @@ -472,43 +551,43 @@ ZTEST(rtio_api, test_rtio_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); -} +//#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) From 19a0aaceed16ed2a5ac53f5f422d013277ab79dd Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Tue, 25 Apr 2023 23:17:46 -0600 Subject: [PATCH 09/15] sensors: Add new async one-shot reading API Add a new async API based on the RTIO subsystem. This new API allows: 1. Users to create sampling configs (telling the sensor which channels they want to sample together). 2. Sample data in an asynchronous manner which provides greater control over the data processing priority. 3. Fully backwards compatible API with no driver changes needed for functionality (they are needed to improve performance). 4. Helper functions for processing loop. Signed-off-by: Yuval Peress --- drivers/sensor/CMakeLists.txt | 7 +- drivers/sensor/Kconfig | 2 + drivers/sensor/default_rtio_sensor.c | 371 +++++++++++++++++++++++++++ drivers/sensor/sensor_handlers.c | 30 +++ include/zephyr/drivers/sensor.h | 316 ++++++++++++++++++++++- 5 files changed, 719 insertions(+), 7 deletions(-) create mode 100644 drivers/sensor/default_rtio_sensor.c diff --git a/drivers/sensor/CMakeLists.txt b/drivers/sensor/CMakeLists.txt index b1ac7a12873b48..ed909f3659b1d8 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 4a2349accb5e90..2b3f8cf1c30219 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/default_rtio_sensor.c b/drivers/sensor/default_rtio_sensor.c new file mode 100644 index 00000000000000..feee040cce685d --- /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 { + 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 16a462c1645dc1..ccca6c882612a6 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/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index e7f5ceba5bfe91..399bd549696fbe 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,170 @@ 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; + enum sensor_channel *const channels; + size_t num_channels; + const size_t max_channels; +}; + +/** + * @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), \ + .channels = __channel_array_##name, \ + .num_channels = ARRAY_SIZE(__channel_array_##name), \ + .max_channels = ARRAY_SIZE(__channel_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 +760,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. */ From d8b6e730831d1b5603b69c354cf563051f69dc06 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 26 Apr 2023 01:13:33 -0600 Subject: [PATCH 10/15] sensor_shell: Update to new sensor_read API Update the sensor shell logic to use the new sensor_read() APIs Signed-off-by: Yuval Peress --- drivers/sensor/sensor_shell.c | 150 +++++++++++++++++++-------- samples/sensor/sensor_shell/prj.conf | 1 + 2 files changed, 108 insertions(+), 43 deletions(-) diff --git a/drivers/sensor/sensor_shell.c b/drivers/sensor/sensor_shell.c index 7366ec816c9376..deca1ed229ec15 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 " ticks", 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/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index 2a9e6a34221698..d3189e39aeac76 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -5,3 +5,4 @@ CONFIG_SENSOR_SHELL=y CONFIG_SENSOR_INFO=y CONFIG_LOG=y +CONFIG_RTIO_CONSUME_SEM=y From c7b0f9d574f5991f9256b1c0444a988a43cfc647 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Thu, 27 Apr 2023 02:02:25 -0600 Subject: [PATCH 11/15] DNM: Temporary fix for the sensor_shell app Once #57130 merges this will not be needed. Signed-off-by: Yuval Peress --- drivers/sensor/sensor_shell.c | 2 +- samples/sensor/sensor_shell/prj.conf | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/drivers/sensor/sensor_shell.c b/drivers/sensor/sensor_shell.c index deca1ed229ec15..c49c5cd6af3806 100644 --- a/drivers/sensor/sensor_shell.c +++ b/drivers/sensor/sensor_shell.c @@ -227,7 +227,7 @@ static void sensor_shell_processing_callback(int result, uint8_t *buf, uint32_t shell_error(ctx->sh, "Failed to get fetch timestamp for '%s'", ctx->dev->name); return; } - shell_print(ctx->sh, "Got samples at %" PRIu64 " ticks", timestamp); + shell_print(ctx->sh, "Got samples at %" PRIu64 " ns", timestamp); while (decoder.decode(buf, &fit, &cit, &channel, &q, 1) > 0) { int8_t shift; diff --git a/samples/sensor/sensor_shell/prj.conf b/samples/sensor/sensor_shell/prj.conf index d3189e39aeac76..9f5cc48698e9e4 100644 --- a/samples/sensor/sensor_shell/prj.conf +++ b/samples/sensor/sensor_shell/prj.conf @@ -5,4 +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 From 74b71f213887c37cf1024638aee0cd8d28e69ed4 Mon Sep 17 00:00:00 2001 From: Yuval Peress Date: Wed, 3 May 2023 23:26:09 -0600 Subject: [PATCH 12/15] WIP: streaming sensor API Signed-off-by: Yuval Peress --- drivers/sensor/default_rtio_sensor.c | 2 +- include/zephyr/drivers/sensor.h | 26 +++++++++++++++++++++----- 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/drivers/sensor/default_rtio_sensor.c b/drivers/sensor/default_rtio_sensor.c index feee040cce685d..183489723ea5c2 100644 --- a/drivers/sensor/default_rtio_sensor.c +++ b/drivers/sensor/default_rtio_sensor.c @@ -22,7 +22,7 @@ static void sensor_iodev_submit(struct rtio_iodev_sqe *iodev_sqe) if (api->submit != NULL) { api->submit(dev, iodev_sqe); - } else { + } else if (!cfg->is_streaming) { sensor_submit_fallback(dev, iodev_sqe); } } diff --git a/include/zephyr/drivers/sensor.h b/include/zephyr/drivers/sensor.h index 399bd549696fbe..619b69f8fe6b3b 100644 --- a/include/zephyr/drivers/sensor.h +++ b/include/zephyr/drivers/sensor.h @@ -516,9 +516,13 @@ typedef int (*sensor_get_decoder_t)(const struct device *dev, */ struct sensor_read_config { const struct device *sensor; - enum sensor_channel *const channels; - size_t num_channels; - const size_t max_channels; + const bool is_streaming; + union { + enum sensor_channel *const channels; + enum sensor_trigger *const triggers; + }; + size_t count; + const size_t max; }; /** @@ -539,12 +543,24 @@ struct sensor_read_config { 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, \ - .num_channels = ARRAY_SIZE(__channel_array_##name), \ - .max_channels = ARRAY_SIZE(__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); From 7dc6d0efb5fc153ed192bd7e541e9e9456671f6e Mon Sep 17 00:00:00 2001 From: Tristan Honscheid Date: Fri, 5 May 2023 14:56:00 -0600 Subject: [PATCH 13/15] WIP AKM09918C RTIO implementation --- drivers/sensor/akm09918c/akm09918c.c | 224 ++++++++++++++++++++++++++- drivers/sensor/akm09918c/akm09918c.h | 1 + 2 files changed, 222 insertions(+), 3 deletions(-) diff --git a/drivers/sensor/akm09918c/akm09918c.c b/drivers/sensor/akm09918c/akm09918c.c index 759c00c91f4846..a9b2ba861ea448 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,201 @@ 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, +}; + +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) +{ + /* TODO */ +} + +static int get_frame_count(const uint8_t *buffer, uint16_t *frame_count) +{ + *frame_count = 1; + return 0; +} + +static int get_timestamp(const uint8_t *buffer, uint64_t *timestamp_ns) +{ + *timestamp_ns = ((struct sensor_data_generic_header *)buffer)->timestamp_ns; + return 0; +} + +static int get_shift(const uint8_t *buffer, enum sensor_channel channel_type, int8_t *shift) +{ + /* TODO */ +} + +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 9bb1b6db40621c..a0088d8d7566c3 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; From 0f26f7033113fed9e53a86146f2aa8bedd1650f9 Mon Sep 17 00:00:00 2001 From: Tristan Honscheid Date: Sat, 6 May 2023 11:02:55 -0600 Subject: [PATCH 14/15] Add get_shift, get_timestamp, get_frame_count implementations --- drivers/sensor/akm09918c/akm09918c.c | 38 +++++++++++++++++++++++++++- 1 file changed, 37 insertions(+), 1 deletion(-) diff --git a/drivers/sensor/akm09918c/akm09918c.c b/drivers/sensor/akm09918c/akm09918c.c index a9b2ba861ea448..946ad77bcd8ab9 100644 --- a/drivers/sensor/akm09918c/akm09918c.c +++ b/drivers/sensor/akm09918c/akm09918c.c @@ -391,21 +391,57 @@ static int decode(const uint8_t *buffer, sensor_frame_iterator_t *fit, /* TODO */ } +/** + * @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) { - /* TODO */ + 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 = { From 37091f2e6dd074dbad3e70a9b58bdf3f1698dde2 Mon Sep 17 00:00:00 2001 From: Tristan Honscheid Date: Sat, 6 May 2023 19:54:34 -0600 Subject: [PATCH 15/15] WIP of decoder --- drivers/sensor/akm09918c/akm09918c.c | 79 +++++++++++++++++++++++++++- 1 file changed, 78 insertions(+), 1 deletion(-) diff --git a/drivers/sensor/akm09918c/akm09918c.c b/drivers/sensor/akm09918c/akm09918c.c index 946ad77bcd8ab9..708619cd35da8f 100644 --- a/drivers/sensor/akm09918c/akm09918c.c +++ b/drivers/sensor/akm09918c/akm09918c.c @@ -384,11 +384,88 @@ 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) { - /* TODO */ + 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; } /**