1 /*
2 * Copyright (c) 2024 Croxel Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/rtio/work.h>
8 #include <zephyr/kernel.h>
9
10 #define RTIO_WORKQ_PRIO_MED CONFIG_RTIO_WORKQ_PRIO_MED
11 #define RTIO_WORKQ_PRIO_HIGH RTIO_WORKQ_PRIO_MED - 1
12 #define RTIO_WORKQ_PRIO_LOW RTIO_WORKQ_PRIO_MED + 1
13
14 K_MEM_SLAB_DEFINE_STATIC(rtio_work_items_slab,
15 sizeof(struct rtio_work_req),
16 CONFIG_RTIO_WORKQ_POOL_ITEMS,
17 4);
18
rtio_work_req_done_handler(struct k_p4wq_work * work)19 static void rtio_work_req_done_handler(struct k_p4wq_work *work)
20 {
21 struct rtio_work_req *req = CONTAINER_OF(work,
22 struct rtio_work_req,
23 work);
24 k_mem_slab_free(&rtio_work_items_slab, req);
25 }
26
27 K_P4WQ_DEFINE_WITH_DONE_HANDLER(rtio_workq,
28 CONFIG_RTIO_WORKQ_THREADS_POOL,
29 CONFIG_RTIO_WORKQ_STACK_SIZE,
30 rtio_work_req_done_handler);
31
rtio_work_handler(struct k_p4wq_work * work)32 static void rtio_work_handler(struct k_p4wq_work *work)
33 {
34 struct rtio_work_req *req = CONTAINER_OF(work,
35 struct rtio_work_req,
36 work);
37 struct rtio_iodev_sqe *iodev_sqe = req->iodev_sqe;
38
39 req->handler(iodev_sqe);
40 }
41
rtio_work_req_alloc(void)42 struct rtio_work_req *rtio_work_req_alloc(void)
43 {
44 struct rtio_work_req *req;
45 int err;
46
47 err = k_mem_slab_alloc(&rtio_work_items_slab, (void **)&req, K_NO_WAIT);
48 if (err) {
49 return NULL;
50 }
51
52 /** Initialize work item before using it as it comes
53 * from a Memory slab (no-init region).
54 */
55 req->work.thread = NULL;
56 (void)k_sem_init(&req->work.done_sem, 1, 1);
57
58 return req;
59 }
60
rtio_work_req_submit(struct rtio_work_req * req,struct rtio_iodev_sqe * iodev_sqe,rtio_work_submit_t handler)61 void rtio_work_req_submit(struct rtio_work_req *req,
62 struct rtio_iodev_sqe *iodev_sqe,
63 rtio_work_submit_t handler)
64 {
65 if (!req) {
66 return;
67 }
68
69 if (!iodev_sqe || !handler) {
70 k_mem_slab_free(&rtio_work_items_slab, req);
71 return;
72 }
73
74 struct k_p4wq_work *work = &req->work;
75 struct rtio_sqe *sqe = &iodev_sqe->sqe;
76
77 /** Link the relevant info so that we can get it on the k_p4wq_work work item.
78 */
79 req->iodev_sqe = iodev_sqe;
80 req->handler = handler;
81
82 /** Set the required information to handle the action */
83 work->handler = rtio_work_handler;
84 work->deadline = 0;
85
86 if (sqe->prio == RTIO_PRIO_LOW) {
87 work->priority = RTIO_WORKQ_PRIO_LOW;
88 } else if (sqe->prio == RTIO_PRIO_HIGH) {
89 work->priority = RTIO_WORKQ_PRIO_HIGH;
90 } else {
91 work->priority = RTIO_WORKQ_PRIO_MED;
92 }
93
94 /** Decoupling action: Let the P4WQ execute the action. */
95 k_p4wq_submit(&rtio_workq, work);
96 }
97
rtio_work_req_used_count_get(void)98 uint32_t rtio_work_req_used_count_get(void)
99 {
100 return k_mem_slab_num_used_get(&rtio_work_items_slab);
101 }
102
