rtio.h

Go to the documentation of this file.

/*
 * Copyright (c) 2022 Intel Corporation
 *
 * SPDX-License-Identifier: Apache-2.0
 */

 
#ifndef ZEPHYR_INCLUDE_RTIO_RTIO_H_
#define ZEPHYR_INCLUDE_RTIO_RTIO_H_
 
#include <string.h>
 
#include <zephyr/app_memory/app_memdomain.h>
#include <zephyr/device.h>
#include <zephyr/kernel.h>
#include <zephyr/kernel_structs.h>
#include <zephyr/sys/__assert.h>
#include <zephyr/sys/atomic.h>
#include <zephyr/sys/mem_blocks.h>
#include <zephyr/sys/util.h>
#include <zephyr/sys/iterable_sections.h>
#include <zephyr/sys/mpsc_lockfree.h>
 
#ifdef __cplusplus
extern "C" {
#endif
 



#define RTIO_PRIO_LOW 0U

#define RTIO_PRIO_NORM 127U

#define RTIO_PRIO_HIGH 255U

 


#define RTIO_SQE_CHAINED BIT(0)

#define RTIO_SQE_TRANSACTION BIT(1)
 

#define RTIO_SQE_MEMPOOL_BUFFER BIT(2)

#define RTIO_SQE_CANCELED BIT(3)

#define RTIO_SQE_MULTISHOT BIT(4)

#define RTIO_SQE_NO_RESPONSE BIT(5)



#define RTIO_CQE_FLAG_MEMPOOL_BUFFER BIT(0)
 
#define RTIO_CQE_FLAG_GET(flags) FIELD_GET(GENMASK(7, 0), (flags))

#define RTIO_CQE_FLAG_MEMPOOL_GET_BLK_IDX(flags) FIELD_GET(GENMASK(19, 8), (flags))

#define RTIO_CQE_FLAG_MEMPOOL_GET_BLK_CNT(flags) FIELD_GET(GENMASK(31, 20), (flags))

#define RTIO_CQE_FLAG_PREP_MEMPOOL(blk_idx, blk_cnt)                                               \
        (FIELD_PREP(GENMASK(7, 0), RTIO_CQE_FLAG_MEMPOOL_BUFFER) |                                 \
         FIELD_PREP(GENMASK(19, 8), blk_idx) | FIELD_PREP(GENMASK(31, 20), blk_cnt))


#define RTIO_IODEV_I2C_STOP BIT(1)

#define RTIO_IODEV_I2C_RESTART BIT(2)

#define RTIO_IODEV_I2C_10_BITS BIT(3)

#define RTIO_IODEV_I3C_STOP BIT(1)

#define RTIO_IODEV_I3C_RESTART BIT(2)

#define RTIO_IODEV_I3C_HDR BIT(3)

#define RTIO_IODEV_I3C_NBCH BIT(4)

#define RTIO_IODEV_I3C_HDR_MODE_MASK GENMASK(15, 8)

#define RTIO_IODEV_I3C_HDR_MODE_SET(flags) \
        FIELD_PREP(RTIO_IODEV_I3C_HDR_MODE_MASK, flags)

#define RTIO_IODEV_I3C_HDR_MODE_GET(flags) \
        FIELD_GET(RTIO_IODEV_I3C_HDR_MODE_MASK, flags)

#define RTIO_IODEV_I3C_HDR_CMD_CODE_MASK GENMASK(22, 16)

#define RTIO_IODEV_I3C_HDR_CMD_CODE_SET(flags) \
        FIELD_PREP(RTIO_IODEV_I3C_HDR_CMD_CODE_MASK, flags)

#define RTIO_IODEV_I3C_HDR_CMD_CODE_GET(flags) \
        FIELD_GET(RTIO_IODEV_I3C_HDR_CMD_CODE_MASK, flags)

struct rtio;
struct rtio_cqe;
struct rtio_sqe;
struct rtio_sqe_pool;
struct rtio_cqe_pool;
struct rtio_iodev;
struct rtio_iodev_sqe;

typedef void (*rtio_callback_t)(struct rtio *r, const struct rtio_sqe *sqe, void *arg0);

typedef void (*rtio_signaled_t)(struct rtio_iodev_sqe *iodev_sqe, void *userdata);

struct rtio_sqe {
        uint8_t op; 
 
        uint8_t prio; 
 
        uint16_t flags; 
 
        uint32_t iodev_flags; 
 
        const struct rtio_iodev *iodev; 

        void *userdata;
 
        union {

                struct {
                        uint32_t buf_len; 
                        const uint8_t *buf; 
                } tx;

                struct {
                        uint32_t buf_len; 
                        uint8_t *buf; 
                } rx;

                struct {
                        uint8_t buf_len; 
                        uint8_t buf[7]; 
                } tiny_tx;

                struct {
                        rtio_callback_t callback;
                        void *arg0; 
                } callback;

                struct {
                        uint32_t buf_len; 
                        const uint8_t *tx_buf; 
                        uint8_t *rx_buf; 
                } txrx;

                struct {
                        k_timeout_t timeout; 
                        struct _timeout to; 
                } delay;

                uint32_t i2c_config;

                struct {
                        /* enum i3c_config_type type; */
                        int type;
                        void *config;
                } i3c_config;

                /* struct i3c_ccc_payload *ccc_payload; */
                void *ccc_payload;

                struct {
                        atomic_t ok;
                        rtio_signaled_t callback;
                        void *userdata;
                } await;
        };
};

/* Ensure the rtio_sqe never grows beyond a common cacheline size of 64 bytes */
BUILD_ASSERT(sizeof(struct rtio_sqe) <= 64);

struct rtio_cqe {
        struct mpsc_node q;
 
        int32_t result; 
        void *userdata; 
        uint32_t flags; 
};
 
struct rtio_sqe_pool {
        struct mpsc free_q;
        const uint16_t pool_size;
        uint16_t pool_free;
        struct rtio_iodev_sqe *pool;
};
 
struct rtio_cqe_pool {
        struct mpsc free_q;
        const uint16_t pool_size;
        uint16_t pool_free;
        struct rtio_cqe *pool;
};

struct rtio {
#ifdef CONFIG_RTIO_SUBMIT_SEM
        /* A wait semaphore which may suspend the calling thread
         * to wait for some number of completions when calling submit
         */
        struct k_sem *submit_sem;
 
        uint32_t submit_count;
#endif
 
#ifdef CONFIG_RTIO_CONSUME_SEM
        /* A wait semaphore which may suspend the calling thread
         * to wait for some number of completions while consuming
         * them from the completion queue
         */
        struct k_sem *consume_sem;
#endif
 
        /* Total number of completions */
        atomic_t cq_count;
 
        /* Number of completions that were unable to be submitted with results
         * due to the cq spsc being full
         */
        atomic_t xcqcnt;
 
        /* Submission queue object pool with free list */
        struct rtio_sqe_pool *sqe_pool;
 
        /* Complete queue object pool with free list */
        struct rtio_cqe_pool *cqe_pool;
 
#ifdef CONFIG_RTIO_SYS_MEM_BLOCKS
        /* Mem block pool */
        struct sys_mem_blocks *block_pool;
#endif
 
        /* Submission queue */
        struct mpsc sq;
 
        /* Completion queue */
        struct mpsc cq;
};

extern struct k_mem_partition rtio_partition;

static inline size_t rtio_mempool_block_size(const struct rtio *r)
{
#ifndef CONFIG_RTIO_SYS_MEM_BLOCKS
        ARG_UNUSED(r);
        return 0;
#else
        if (r == NULL || r->block_pool == NULL) {
                return 0;
        }
        return BIT(r->block_pool->info.blk_sz_shift);
#endif
}

#ifdef CONFIG_RTIO_SYS_MEM_BLOCKS
static inline uint16_t __rtio_compute_mempool_block_index(const struct rtio *r, const void *ptr)
{
        uintptr_t addr = (uintptr_t)ptr;
        struct sys_mem_blocks *mem_pool = r->block_pool;
        uint32_t block_size = rtio_mempool_block_size(r);
 
        uintptr_t buff = (uintptr_t)mem_pool->buffer;
        uint32_t buff_size = mem_pool->info.num_blocks * block_size;
 
        if (addr < buff || addr >= buff + buff_size) {
                return UINT16_MAX;
        }
        return (addr - buff) / block_size;
}
#endif

struct rtio_iodev_sqe {
        struct rtio_sqe sqe;
        struct mpsc_node q;
        struct rtio_iodev_sqe *next;
        struct rtio *r;
};

struct rtio_iodev_api {
        void (*submit)(struct rtio_iodev_sqe *iodev_sqe);
};

struct rtio_iodev {
        /* Function pointer table */
        const struct rtio_iodev_api *api;
 
        /* Data associated with this iodev */
        void *data;
};

#define RTIO_OP_NOP 0

#define RTIO_OP_RX (RTIO_OP_NOP+1)

#define RTIO_OP_TX (RTIO_OP_RX+1)

#define RTIO_OP_TINY_TX (RTIO_OP_TX+1)

#define RTIO_OP_CALLBACK (RTIO_OP_TINY_TX+1)

#define RTIO_OP_TXRX (RTIO_OP_CALLBACK+1)

#define RTIO_OP_DELAY (RTIO_OP_TXRX+1)

#define RTIO_OP_I2C_RECOVER (RTIO_OP_DELAY+1)

#define RTIO_OP_I2C_CONFIGURE (RTIO_OP_I2C_RECOVER+1)

#define RTIO_OP_I3C_RECOVER (RTIO_OP_I2C_CONFIGURE+1)

#define RTIO_OP_I3C_CONFIGURE (RTIO_OP_I3C_RECOVER+1)

#define RTIO_OP_I3C_CCC (RTIO_OP_I3C_CONFIGURE+1)

#define RTIO_OP_AWAIT (RTIO_OP_I3C_CCC+1)

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;
}

static inline void rtio_sqe_prep_read(struct rtio_sqe *sqe,
                                      const struct rtio_iodev *iodev,
                                      int8_t prio,
                                      uint8_t *buf,
                                      uint32_t len,
                                      void *userdata)
{
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_RX;
        sqe->prio = prio;
        sqe->iodev = iodev;
        sqe->rx.buf_len = len;
        sqe->rx.buf = buf;
        sqe->userdata = userdata;
}

static inline void rtio_sqe_prep_read_with_pool(struct rtio_sqe *sqe,
                                                const struct rtio_iodev *iodev, int8_t prio,
                                                void *userdata)
{
        rtio_sqe_prep_read(sqe, iodev, prio, NULL, 0, userdata);
        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;
}

static inline void rtio_sqe_prep_write(struct rtio_sqe *sqe,
                                       const struct rtio_iodev *iodev,
                                       int8_t prio,
                                       const uint8_t *buf,
                                       uint32_t len,
                                       void *userdata)
{
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_TX;
        sqe->prio = prio;
        sqe->iodev = iodev;
        sqe->tx.buf_len = len;
        sqe->tx.buf = buf;
        sqe->userdata = userdata;
}

static inline void rtio_sqe_prep_tiny_write(struct rtio_sqe *sqe,
                                            const struct rtio_iodev *iodev,
                                            int8_t prio,
                                            const uint8_t *tiny_write_data,
                                            uint8_t tiny_write_len,
                                            void *userdata)
{
        __ASSERT_NO_MSG(tiny_write_len <= sizeof(sqe->tiny_tx.buf));
 
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_TINY_TX;
        sqe->prio = prio;
        sqe->iodev = iodev;
        sqe->tiny_tx.buf_len = tiny_write_len;
        memcpy(sqe->tiny_tx.buf, tiny_write_data, tiny_write_len);
        sqe->userdata = userdata;
}

static inline void rtio_sqe_prep_callback(struct rtio_sqe *sqe,
                                          rtio_callback_t callback,
                                          void *arg0,
                                          void *userdata)
{
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_CALLBACK;
        sqe->prio = 0;
        sqe->iodev = NULL;
        sqe->callback.callback = callback;
        sqe->callback.arg0 = arg0;
        sqe->userdata = userdata;
}

static inline void rtio_sqe_prep_callback_no_cqe(struct rtio_sqe *sqe,
                                                 rtio_callback_t callback,
                                                 void *arg0,
                                                 void *userdata)
{
        rtio_sqe_prep_callback(sqe, callback, arg0, userdata);
        sqe->flags |= RTIO_SQE_NO_RESPONSE;
}

static inline void rtio_sqe_prep_transceive(struct rtio_sqe *sqe,
                                            const struct rtio_iodev *iodev,
                                            int8_t prio,
                                            const uint8_t *tx_buf,
                                            uint8_t *rx_buf,
                                            uint32_t buf_len,
                                            void *userdata)
{
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_TXRX;
        sqe->prio = prio;
        sqe->iodev = iodev;
        sqe->txrx.buf_len = buf_len;
        sqe->txrx.tx_buf = tx_buf;
        sqe->txrx.rx_buf = rx_buf;
        sqe->userdata = userdata;
}
 
static inline void rtio_sqe_prep_await(struct rtio_sqe *sqe,
                                       const struct rtio_iodev *iodev,
                                       int8_t prio,
                                       void *userdata)
{
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_AWAIT;
        sqe->prio = prio;
        sqe->iodev = iodev;
        sqe->userdata = userdata;
}
 
static inline void rtio_sqe_prep_delay(struct rtio_sqe *sqe,
                                       k_timeout_t timeout,
                                       void *userdata)
{
        memset(sqe, 0, sizeof(struct rtio_sqe));
        sqe->op = RTIO_OP_DELAY;
        sqe->prio = 0;
        sqe->iodev = NULL;
        sqe->delay.timeout = timeout;
        sqe->userdata = userdata;
}
 
static inline struct rtio_iodev_sqe *rtio_sqe_pool_alloc(struct rtio_sqe_pool *pool)
{
        struct mpsc_node *node = mpsc_pop(&pool->free_q);
 
        if (node == NULL) {
                return NULL;
        }
 
        struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q);
 
        pool->pool_free--;
 
        return iodev_sqe;
}
 
static inline void rtio_sqe_pool_free(struct rtio_sqe_pool *pool, struct rtio_iodev_sqe *iodev_sqe)
{
        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 mpsc_node *node = 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)
{
        mpsc_push(&pool->free_q, &cqe->q);
 
        pool->pool_free++;
}
 
static inline int rtio_block_pool_alloc(struct rtio *r, size_t min_sz,
                                          size_t max_sz, uint8_t **buf, uint32_t *buf_len)
{
#ifndef CONFIG_RTIO_SYS_MEM_BLOCKS
        ARG_UNUSED(r);
        ARG_UNUSED(min_sz);
        ARG_UNUSED(max_sz);
        ARG_UNUSED(buf);
        ARG_UNUSED(buf_len);
        return -ENOTSUP;
#else
        const uint32_t block_size = rtio_mempool_block_size(r);
        uint32_t bytes = max_sz;
 
        /* Not every context has a block pool and the block size may return 0 in
         * that case
         */
        if (block_size == 0) {
                return -ENOMEM;
        }
 
        do {
                size_t num_blks = DIV_ROUND_UP(bytes, block_size);
                int rc = sys_mem_blocks_alloc_contiguous(r->block_pool, num_blks, (void **)buf);
 
                if (rc == 0) {
                        *buf_len = num_blks * block_size;
                        return 0;
                }
 
                if (bytes <= block_size) {
                        break;
                }
 
                bytes -= block_size;
        } while (bytes >= min_sz);
 
        return -ENOMEM;
#endif
}
 
static inline void rtio_block_pool_free(struct rtio *r, void *buf, uint32_t buf_len)
{
#ifndef CONFIG_RTIO_SYS_MEM_BLOCKS
        ARG_UNUSED(r);
        ARG_UNUSED(buf);
        ARG_UNUSED(buf_len);
#else
        size_t num_blks = buf_len >> r->block_pool->info.blk_sz_shift;
 
        sys_mem_blocks_free_contiguous(r->block_pool, buf, num_blks);
#endif
}
 
/* Do not try and reformat the macros */
/* clang-format off */

#define RTIO_IODEV_DEFINE(name, iodev_api, iodev_data)          \
        STRUCT_SECTION_ITERABLE(rtio_iodev, name) = {           \
                .api = (iodev_api),                             \
                .data = (iodev_data),                           \
        }
 
#define Z_RTIO_SQE_POOL_DEFINE(name, sz)                        \
        static struct rtio_iodev_sqe CONCAT(_sqe_pool_, name)[sz];      \
        STRUCT_SECTION_ITERABLE(rtio_sqe_pool, name) = {        \
                .free_q = MPSC_INIT((name.free_q)),     \
                .pool_size = sz,                                \
                .pool_free = sz,                                \
                .pool = CONCAT(_sqe_pool_, name),               \
        }
 
 
#define Z_RTIO_CQE_POOL_DEFINE(name, sz)                        \
        static struct rtio_cqe CONCAT(_cqe_pool_, name)[sz];    \
        STRUCT_SECTION_ITERABLE(rtio_cqe_pool, name) = {        \
                .free_q = MPSC_INIT((name.free_q)),     \
                .pool_size = sz,                                \
                .pool_free = sz,                                \
                .pool = CONCAT(_cqe_pool_, name),               \
        }

#define RTIO_BMEM COND_CODE_1(CONFIG_USERSPACE, (K_APP_BMEM(rtio_partition) static), (static))

#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))                                              \
        CONCAT(_block_pool_, name)[blk_cnt*WB_UP(blk_sz)];                                         \
        _SYS_MEM_BLOCKS_DEFINE_WITH_EXT_BUF(name, WB_UP(blk_sz), blk_cnt,                          \
                                            CONCAT(_block_pool_, name), RTIO_DMEM)
 
#define Z_RTIO_DEFINE(name, _sqe_pool, _cqe_pool, _block_pool)                                     \
        IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM,                                                         \
                   (static K_SEM_DEFINE(CONCAT(_submit_sem_, name), 0, K_SEM_MAX_LIMIT)))          \
        IF_ENABLED(CONFIG_RTIO_CONSUME_SEM,                                                        \
                   (static K_SEM_DEFINE(CONCAT(_consume_sem_, name), 0, K_SEM_MAX_LIMIT)))         \
        STRUCT_SECTION_ITERABLE(rtio, name) = {                                                    \
                IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_sem = &CONCAT(_submit_sem_, name),))   \
                IF_ENABLED(CONFIG_RTIO_SUBMIT_SEM, (.submit_count = 0,))                           \
                IF_ENABLED(CONFIG_RTIO_CONSUME_SEM, (.consume_sem = &CONCAT(_consume_sem_, name),))\
                .cq_count = ATOMIC_INIT(0),                                                        \
                .xcqcnt = ATOMIC_INIT(0),                                                          \
                .sqe_pool = _sqe_pool,                                                             \
                .cqe_pool = _cqe_pool,                                                             \
                IF_ENABLED(CONFIG_RTIO_SYS_MEM_BLOCKS, (.block_pool = _block_pool,))               \
                .sq = MPSC_INIT((name.sq)),                                                        \
                .cq = MPSC_INIT((name.cq)),                                                        \
        }

#define RTIO_DEFINE(name, sq_sz, cq_sz)                                         \
        Z_RTIO_SQE_POOL_DEFINE(CONCAT(name, _sqe_pool), sq_sz);                 \
        Z_RTIO_CQE_POOL_DEFINE(CONCAT(name, _cqe_pool), cq_sz);                 \
        Z_RTIO_DEFINE(name, &CONCAT(name, _sqe_pool),                           \
                      &CONCAT(name, _cqe_pool), NULL)
 
/* clang-format on */

#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 */

static inline uint32_t rtio_sqe_acquirable(struct rtio *r)
{
        return r->sqe_pool->pool_free;
}

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;
        }
}
 

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;
        }
}

static inline struct rtio_iodev_sqe *rtio_iodev_sqe_next(const struct rtio_iodev_sqe *iodev_sqe)
{
        return iodev_sqe->next;
}

static inline struct rtio_sqe *rtio_sqe_acquire(struct rtio *r)
{
        struct rtio_iodev_sqe *iodev_sqe = rtio_sqe_pool_alloc(r->sqe_pool);
 
        if (iodev_sqe == NULL) {
                return NULL;
        }
 
        mpsc_push(&r->sq, &iodev_sqe->q);
 
        return &iodev_sqe->sqe;
}

static inline void rtio_sqe_drop_all(struct rtio *r)
{
        struct rtio_iodev_sqe *iodev_sqe;
        struct mpsc_node *node = mpsc_pop(&r->sq);
 
        while (node != NULL) {
                iodev_sqe = CONTAINER_OF(node, struct rtio_iodev_sqe, q);
                rtio_sqe_pool_free(r->sqe_pool, iodev_sqe);
                node = mpsc_pop(&r->sq);
        }
}

static inline struct rtio_cqe *rtio_cqe_acquire(struct rtio *r)
{
        struct rtio_cqe *cqe = rtio_cqe_pool_alloc(r->cqe_pool);
 
        if (cqe == NULL) {
                return NULL;
        }
 
        memset(cqe, 0, sizeof(struct rtio_cqe));
 
        return cqe;
}

static inline void rtio_cqe_produce(struct rtio *r, struct rtio_cqe *cqe)
{
        mpsc_push(&r->cq, &cqe->q);
}

static inline struct rtio_cqe *rtio_cqe_consume(struct rtio *r)
{
        struct 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 NULL;
        }
#endif
 
        node = mpsc_pop(&r->cq);
        if (node == NULL) {
                return NULL;
        }
        cqe = CONTAINER_OF(node, struct rtio_cqe, q);
 
        return cqe;
}

static inline struct rtio_cqe *rtio_cqe_consume_block(struct rtio *r)
{
        struct mpsc_node *node;
        struct rtio_cqe *cqe;
 
#ifdef CONFIG_RTIO_CONSUME_SEM
        k_sem_take(r->consume_sem, K_FOREVER);
#endif
        node = mpsc_pop(&r->cq);
        while (node == NULL) {
                Z_SPIN_DELAY(1);
                node = mpsc_pop(&r->cq);
        }
        cqe = CONTAINER_OF(node, struct rtio_cqe, q);
 
        return cqe;
}

static inline void rtio_cqe_release(struct rtio *r, struct rtio_cqe *cqe)
{
        rtio_cqe_pool_free(r->cqe_pool, cqe);
}

static inline int rtio_flush_completion_queue(struct rtio *r)
{
        struct rtio_cqe *cqe;
        int res = 0;
 
        do {
                cqe = rtio_cqe_consume(r);
                if (cqe != NULL) {
                        if ((cqe->result < 0) && (res == 0)) {
                                res = cqe->result;
                        }
                        rtio_cqe_release(r, cqe);
                }
        } while (cqe != NULL);
 
        return res;
}

static inline uint32_t rtio_cqe_compute_flags(struct rtio_iodev_sqe *iodev_sqe)
{
        uint32_t flags = 0;
 
#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;
                struct sys_mem_blocks *mem_pool = r->block_pool;
                unsigned int blk_index = 0;
                unsigned int blk_count = 0;
 
                if (iodev_sqe->sqe.rx.buf) {
                        blk_index = (iodev_sqe->sqe.rx.buf - mem_pool->buffer) >>
                                    mem_pool->info.blk_sz_shift;
                        blk_count = iodev_sqe->sqe.rx.buf_len >> mem_pool->info.blk_sz_shift;
                }
                flags = RTIO_CQE_FLAG_PREP_MEMPOOL(blk_index, blk_count);
        }
#else
        ARG_UNUSED(iodev_sqe);
#endif
 
        return flags;
}

__syscall int rtio_cqe_get_mempool_buffer(const struct rtio *r, struct rtio_cqe *cqe,
                                          uint8_t **buff, uint32_t *buff_len);
 
static inline int z_impl_rtio_cqe_get_mempool_buffer(const struct rtio *r, struct rtio_cqe *cqe,
                                                     uint8_t **buff, uint32_t *buff_len)
{
#ifdef CONFIG_RTIO_SYS_MEM_BLOCKS
        if (RTIO_CQE_FLAG_GET(cqe->flags) == RTIO_CQE_FLAG_MEMPOOL_BUFFER) {
                unsigned int blk_idx = RTIO_CQE_FLAG_MEMPOOL_GET_BLK_IDX(cqe->flags);
                unsigned int blk_count = RTIO_CQE_FLAG_MEMPOOL_GET_BLK_CNT(cqe->flags);
                uint32_t blk_size = rtio_mempool_block_size(r);
 
                *buff_len = blk_count * blk_size;
 
                if (blk_count > 0) {
                        *buff = r->block_pool->buffer + blk_idx * blk_size;
 
                        __ASSERT_NO_MSG(*buff >= r->block_pool->buffer);
                        __ASSERT_NO_MSG(*buff <
                                r->block_pool->buffer + blk_size * r->block_pool->info.num_blocks);
                } else {
                        *buff = NULL;
                }
                return 0;
        }
        return -EINVAL;
#else
        ARG_UNUSED(r);
        ARG_UNUSED(cqe);
        ARG_UNUSED(buff);
        ARG_UNUSED(buff_len);
 
        return -ENOTSUP;
#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);

static inline void rtio_iodev_sqe_ok(struct rtio_iodev_sqe *iodev_sqe, int result)
{
        rtio_executor_ok(iodev_sqe, result);
}

static inline void rtio_iodev_sqe_err(struct rtio_iodev_sqe *iodev_sqe, int result)
{
        rtio_executor_err(iodev_sqe, result);
}

static inline void rtio_cqe_submit(struct rtio *r, int result, void *userdata, uint32_t flags)
{
        struct rtio_cqe *cqe = rtio_cqe_acquire(r);
 
        if (cqe == NULL) {
                atomic_inc(&r->xcqcnt);
        } else {
                cqe->result = result;
                cqe->userdata = userdata;
                cqe->flags = flags;
                rtio_cqe_produce(r, cqe);
#ifdef CONFIG_RTIO_CONSUME_SEM
                k_sem_give(r->consume_sem);
#endif
        }
 
        /* atomic_t isn't guaranteed to wrap correctly as it could be signed, so
         * we must resort to a cas loop.
         */
        atomic_t val, new_val;
 
        do {
                val = atomic_get(&r->cq_count);
                new_val = (atomic_t)((uintptr_t)val + 1);
        } while (!atomic_cas(&r->cq_count, val, new_val));
 
#ifdef CONFIG_RTIO_SUBMIT_SEM
        if (r->submit_count > 0) {
                r->submit_count--;
                if (r->submit_count == 0) {
                        k_sem_give(r->submit_sem);
                }
        }
#endif
}
 
#define __RTIO_MEMPOOL_GET_NUM_BLKS(num_bytes, blk_size) (((num_bytes) + (blk_size)-1) / (blk_size))

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)
{
        struct rtio_sqe *sqe = (struct rtio_sqe *)&iodev_sqe->sqe;
 
#ifdef CONFIG_RTIO_SYS_MEM_BLOCKS
        if (sqe->op == RTIO_OP_RX && sqe->flags & RTIO_SQE_MEMPOOL_BUFFER) {
                struct rtio *r = iodev_sqe->r;
 
                if (sqe->rx.buf != NULL) {
                        if (sqe->rx.buf_len < min_buf_len) {
                                return -ENOMEM;
                        }
                        *buf = sqe->rx.buf;
                        *buf_len = sqe->rx.buf_len;
                        return 0;
                }
 
                int rc = rtio_block_pool_alloc(r, min_buf_len, max_buf_len, buf, buf_len);
                if (rc == 0) {
                        sqe->rx.buf = *buf;
                        sqe->rx.buf_len = *buf_len;
                        return 0;
                }
 
                return -ENOMEM;
        }
#else
        ARG_UNUSED(max_buf_len);
#endif
 
        if (sqe->rx.buf_len < min_buf_len) {
                return -ENOMEM;
        }
 
        *buf = sqe->rx.buf;
        *buf_len = sqe->rx.buf_len;
        return 0;
}

__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->block_pool == NULL || buff_len == 0) {
                return;
        }
 
        rtio_block_pool_free(r, buff, buff_len);
#else
        ARG_UNUSED(r);
        ARG_UNUSED(buff);
        ARG_UNUSED(buff_len);
#endif
}

static inline void rtio_access_grant(struct rtio *r, struct k_thread *t)
{
        k_object_access_grant(r, t);
 
#ifdef CONFIG_RTIO_SUBMIT_SEM
        k_object_access_grant(r->submit_sem, t);
#endif
 
#ifdef CONFIG_RTIO_CONSUME_SEM
        k_object_access_grant(r->consume_sem, t);
#endif
}
 

static inline void rtio_access_revoke(struct rtio *r, struct k_thread *t)
{
        k_object_access_revoke(r, t);
 
#ifdef CONFIG_RTIO_SUBMIT_SEM
        k_object_access_revoke(r->submit_sem, t);
#endif
 
#ifdef CONFIG_RTIO_CONSUME_SEM
        k_object_access_revoke(r->consume_sem, t);
#endif
}

__syscall int rtio_sqe_cancel(struct rtio_sqe *sqe);
 
static inline int z_impl_rtio_sqe_cancel(struct rtio_sqe *sqe)
{
        struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(sqe, struct rtio_iodev_sqe, sqe);
 
        do {
                iodev_sqe->sqe.flags |= RTIO_SQE_CANCELED;
                iodev_sqe = rtio_iodev_sqe_next(iodev_sqe);
        } while (iodev_sqe != NULL);
 
        return 0;
}

__syscall void rtio_sqe_signal(struct rtio_sqe *sqe);
 
static inline void z_impl_rtio_sqe_signal(struct rtio_sqe *sqe)
{
        struct rtio_iodev_sqe *iodev_sqe = CONTAINER_OF(sqe, struct rtio_iodev_sqe, sqe);
 
        if (!atomic_cas(&iodev_sqe->sqe.await.ok, 0, 1)) {
                iodev_sqe->sqe.await.callback(iodev_sqe, iodev_sqe->sqe.await.userdata);
        }
}

static inline void rtio_iodev_sqe_await_signal(struct rtio_iodev_sqe *iodev_sqe,
                                               rtio_signaled_t callback,
                                               void *userdata)
{
        iodev_sqe->sqe.await.callback = callback;
        iodev_sqe->sqe.await.userdata = userdata;
 
        if (!atomic_cas(&iodev_sqe->sqe.await.ok, 0, 1)) {
                callback(iodev_sqe, userdata);
        }
}

__syscall int rtio_sqe_copy_in_get_handles(struct rtio *r, const struct rtio_sqe *sqes,
                                           struct rtio_sqe **handle, 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 **handle,
                                                      size_t sqe_count)
{
        struct rtio_sqe *sqe;
        uint32_t acquirable = rtio_sqe_acquirable(r);
 
        if (acquirable < sqe_count) {
                return -ENOMEM;
        }
 
        for (unsigned long i = 0; i < sqe_count; i++) {
                sqe = rtio_sqe_acquire(r);
                __ASSERT_NO_MSG(sqe != NULL);
                if (handle != NULL && i == 0) {
                        *handle = sqe;
                }
                *sqe = sqes[i];
        }
 
        return 0;
}

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);
}

__syscall int rtio_cqe_copy_out(struct rtio *r,
                                struct rtio_cqe *cqes,
                                size_t cqe_count,
                                k_timeout_t timeout);
static inline int z_impl_rtio_cqe_copy_out(struct rtio *r,
                                           struct rtio_cqe *cqes,
                                           size_t cqe_count,
                                           k_timeout_t timeout)
{
        size_t copied = 0;
        struct rtio_cqe *cqe;
        k_timepoint_t end = sys_timepoint_calc(timeout);
 
        do {
                cqe = K_TIMEOUT_EQ(timeout, K_FOREVER) ? rtio_cqe_consume_block(r)
                                                       : rtio_cqe_consume(r);
                if (cqe == NULL) {
                        Z_SPIN_DELAY(25);
                        continue;
                }
                cqes[copied++] = *cqe;
                rtio_cqe_release(r, cqe);
        } while (copied < cqe_count && !sys_timepoint_expired(end));
 
        return copied;
}

__syscall int rtio_submit(struct rtio *r, uint32_t wait_count);
 
#ifdef CONFIG_RTIO_SUBMIT_SEM
static inline int z_impl_rtio_submit(struct rtio *r, uint32_t wait_count)
{
        int res = 0;
 
        if (wait_count > 0) {
                __ASSERT(!k_is_in_isr(),
                         "expected rtio submit with wait count to be called from a thread");
 
                k_sem_reset(r->submit_sem);
                r->submit_count = wait_count;
        }
 
        rtio_executor_submit(r);
 
        if (wait_count > 0) {
                res = k_sem_take(r->submit_sem, K_FOREVER);
                __ASSERT(res == 0,
                         "semaphore was reset or timed out while waiting on completions!");
        }
 
        return res;
}
#else
static inline int z_impl_rtio_submit(struct rtio *r, uint32_t wait_count)
{
 
        int res = 0;
        uintptr_t cq_count = (uintptr_t)atomic_get(&r->cq_count);
        uintptr_t cq_complete_count = cq_count + wait_count;
        bool wraps = cq_complete_count < cq_count;
 
        rtio_executor_submit(r);
 
        if (wraps) {
                while ((uintptr_t)atomic_get(&r->cq_count) >= cq_count) {
                        Z_SPIN_DELAY(10);
                        k_yield();
                }
        }
 
        while ((uintptr_t)atomic_get(&r->cq_count) < cq_complete_count) {
                Z_SPIN_DELAY(10);
                k_yield();
        }
 
        return res;
}
#endif /* CONFIG_RTIO_SUBMIT_SEM */

struct rtio_pool {
        size_t pool_size;

        struct rtio **contexts;

        atomic_t *used;
};

__syscall struct rtio *rtio_pool_acquire(struct rtio_pool *pool);
 
static inline struct rtio *z_impl_rtio_pool_acquire(struct rtio_pool *pool)
{
        struct rtio *r = NULL;
 
        for (size_t i = 0; i < pool->pool_size; i++) {
                if (atomic_test_and_set_bit(pool->used, i) == 0) {
                        r = pool->contexts[i];
                        break;
                }
        }
 
        if (r != NULL) {
                rtio_access_grant(r, k_current_get());
        }
 
        return r;
}

__syscall void rtio_pool_release(struct rtio_pool *pool, struct rtio *r);
 
static inline void z_impl_rtio_pool_release(struct rtio_pool *pool, struct rtio *r)
{
 
        if (k_is_user_context()) {
                rtio_access_revoke(r, k_current_get());
        }
 
        for (size_t i = 0; i < pool->pool_size; i++) {
                if (pool->contexts[i] == r) {
                        atomic_clear_bit(pool->used, i);
                        break;
                }
        }
}
 
/* clang-format off */

 
#define Z_RTIO_POOL_NAME_N(n, name)                                             \
        name##_##n
 
#define Z_RTIO_POOL_DEFINE_N(n, name, sq_sz, cq_sz)                             \
        RTIO_DEFINE(Z_RTIO_POOL_NAME_N(n, name), sq_sz, cq_sz)
 
#define Z_RTIO_POOL_REF_N(n, name)                                              \
        &Z_RTIO_POOL_NAME_N(n, name)


#define RTIO_POOL_DEFINE(name, pool_sz, sq_sz, cq_sz)                           \
        LISTIFY(pool_sz, Z_RTIO_POOL_DEFINE_N, (;), name, sq_sz, cq_sz);        \
        static struct rtio *name##_contexts[] = {                               \
                LISTIFY(pool_sz, Z_RTIO_POOL_REF_N, (,), name)                  \
        };                                                                      \
        ATOMIC_DEFINE(name##_used, pool_sz);                                    \
        STRUCT_SECTION_ITERABLE(rtio_pool, name) = {                            \
                .pool_size = pool_sz,                                           \
                .contexts = name##_contexts,                                    \
                .used = name##_used,                                            \
        }
 
/* clang-format on */

 
#ifdef __cplusplus
}
#endif
 
#include <zephyr/syscalls/rtio.h>
 
#endif /* ZEPHYR_INCLUDE_RTIO_RTIO_H_ */