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