1 /*
2 * Copyright (c) 2024 Croxel Inc.
3 * Copyright (c) 2025 Croxel Inc.
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <zephyr/rtio/work.h>
9 #include <zephyr/kernel.h>
10
11 K_MEM_SLAB_DEFINE_STATIC(rtio_work_items_slab,
12 sizeof(struct rtio_work_req),
13 CONFIG_RTIO_WORKQ_POOL_ITEMS,
14 4);
15 static K_THREAD_STACK_ARRAY_DEFINE(rtio_workq_threads_stack,
16 CONFIG_RTIO_WORKQ_THREADS_POOL,
17 CONFIG_RTIO_WORKQ_THREADS_POOL_STACK_SIZE);
18 static struct k_thread rtio_work_threads[CONFIG_RTIO_WORKQ_THREADS_POOL];
19 static K_QUEUE_DEFINE(rtio_workq);
20
rtio_work_req_alloc(void)21 struct rtio_work_req *rtio_work_req_alloc(void)
22 {
23 struct rtio_work_req *req;
24 int err;
25
26 err = k_mem_slab_alloc(&rtio_work_items_slab, (void **)&req, K_NO_WAIT);
27 if (err) {
28 return NULL;
29 }
30
31 return req;
32 }
33
rtio_work_req_submit(struct rtio_work_req * req,struct rtio_iodev_sqe * iodev_sqe,rtio_work_submit_t handler)34 void rtio_work_req_submit(struct rtio_work_req *req,
35 struct rtio_iodev_sqe *iodev_sqe,
36 rtio_work_submit_t handler)
37 {
38 if (!req) {
39 return;
40 }
41
42 if (!iodev_sqe || !handler) {
43 k_mem_slab_free(&rtio_work_items_slab, req);
44 return;
45 }
46
47 req->iodev_sqe = iodev_sqe;
48 req->handler = handler;
49
50 /** For now we're simply treating this as a FIFO queue. It may be
51 * desirable to expand this to handle queue ordering based on RTIO
52 * SQE priority.
53 */
54 k_queue_append(&rtio_workq, req);
55 }
56
rtio_work_req_used_count_get(void)57 uint32_t rtio_work_req_used_count_get(void)
58 {
59 return k_mem_slab_num_used_get(&rtio_work_items_slab);
60 }
61
rtio_workq_thread_fn(void * arg1,void * arg2,void * arg3)62 static void rtio_workq_thread_fn(void *arg1, void *arg2, void *arg3)
63 {
64 ARG_UNUSED(arg1);
65 ARG_UNUSED(arg2);
66 ARG_UNUSED(arg3);
67
68 while (true) {
69 struct rtio_work_req *req = k_queue_get(&rtio_workq, K_FOREVER);
70
71 if (req != NULL) {
72 req->handler(req->iodev_sqe);
73
74 k_mem_slab_free(&rtio_work_items_slab, req);
75 }
76 }
77 }
78
static_init(void)79 static int static_init(void)
80 {
81 for (size_t i = 0 ; i < ARRAY_SIZE(rtio_work_threads) ; i++) {
82 k_thread_create(&rtio_work_threads[i],
83 rtio_workq_threads_stack[i],
84 CONFIG_RTIO_WORKQ_THREADS_POOL_STACK_SIZE,
85 rtio_workq_thread_fn,
86 NULL, NULL, NULL,
87 CONFIG_RTIO_WORKQ_THREADS_POOL_PRIO,
88 0,
89 K_NO_WAIT);
90 }
91
92 return 0;
93 }
94
95 SYS_INIT(static_init, POST_KERNEL, 1);
96
