Branch data Line data Source code
1 : : /*
2 : : * Copyright (c) 2024 Intel Corporation
3 : : *
4 : : * SPDX-License-Identifier: Apache-2.0
5 : : */
6 : :
7 : : #ifndef ZEPHYR_KERNEL_INCLUDE_PRIORITY_Q_H_
8 : : #define ZEPHYR_KERNEL_INCLUDE_PRIORITY_Q_H_
9 : :
10 : : #include <zephyr/sys/math_extras.h>
11 : : #include <zephyr/sys/dlist.h>
12 : :
13 : : /* Dumb Scheduling */
14 : : #if defined(CONFIG_SCHED_SIMPLE)
15 : : #define _priq_run_init z_priq_simple_init
16 : : #define _priq_run_add z_priq_simple_add
17 : : #define _priq_run_remove z_priq_simple_remove
18 : : #define _priq_run_yield z_priq_simple_yield
19 : : # if defined(CONFIG_SCHED_CPU_MASK)
20 : : # define _priq_run_best z_priq_simple_mask_best
21 : : # else
22 : : # define _priq_run_best z_priq_simple_best
23 : : # endif /* CONFIG_SCHED_CPU_MASK */
24 : : /* Scalable Scheduling */
25 : : #elif defined(CONFIG_SCHED_SCALABLE)
26 : : #define _priq_run_init z_priq_rb_init
27 : : #define _priq_run_add z_priq_rb_add
28 : : #define _priq_run_remove z_priq_rb_remove
29 : : #define _priq_run_yield z_priq_rb_yield
30 : : #define _priq_run_best z_priq_rb_best
31 : : /* Multi Queue Scheduling */
32 : : #elif defined(CONFIG_SCHED_MULTIQ)
33 : : #define _priq_run_init z_priq_mq_init
34 : : #define _priq_run_add z_priq_mq_add
35 : : #define _priq_run_remove z_priq_mq_remove
36 : : #define _priq_run_yield z_priq_mq_yield
37 : : #define _priq_run_best z_priq_mq_best
38 : : #endif
39 : :
40 : : /* Scalable Wait Queue */
41 : : #if defined(CONFIG_WAITQ_SCALABLE)
42 : : #define _priq_wait_add z_priq_rb_add
43 : : #define _priq_wait_remove z_priq_rb_remove
44 : : #define _priq_wait_best z_priq_rb_best
45 : : /* Dumb Wait Queue */
46 : : #elif defined(CONFIG_WAITQ_SIMPLE)
47 : : #define _priq_wait_add z_priq_simple_add
48 : : #define _priq_wait_remove z_priq_simple_remove
49 : : #define _priq_wait_best z_priq_simple_best
50 : : #endif
51 : :
52 : : #if defined(CONFIG_64BIT)
53 : : #define NBITS 64
54 : : #define TRAILING_ZEROS u64_count_trailing_zeros
55 : : #else
56 : : #define NBITS 32
57 : : #define TRAILING_ZEROS u32_count_trailing_zeros
58 : : #endif /* CONFIG_64BIT */
59 : :
60 : : #ifdef IAR_SUPPRESS_ALWAYS_INLINE_WARNING_FLAG
61 : : TOOLCHAIN_DISABLE_WARNING(TOOLCHAIN_WARNING_ALWAYS_INLINE)
62 : : #endif
63 : 1 : static ALWAYS_INLINE void z_priq_simple_init(sys_dlist_t *pq)
64 : : {
65 : 1 : sys_dlist_init(pq);
66 : 1 : }
67 : :
68 : : /*
69 : : * Return value same as e.g. memcmp
70 : : * > 0 -> thread 1 priority > thread 2 priority
71 : : * = 0 -> thread 1 priority == thread 2 priority
72 : : * < 0 -> thread 1 priority < thread 2 priority
73 : : * Do not rely on the actual value returned aside from the above.
74 : : * (Again, like memcmp.)
75 : : */
76 : 58 : static ALWAYS_INLINE int32_t z_sched_prio_cmp(struct k_thread *thread_1, struct k_thread *thread_2)
77 : : {
78 : : /* `prio` is <32b, so the below cannot overflow. */
79 : 58 : int32_t b1 = thread_1->base.prio;
80 : 58 : int32_t b2 = thread_2->base.prio;
81 : :
82 [ + + ]: 58 : if (b1 != b2) {
83 : 52 : return b2 - b1;
84 : : }
85 : :
86 : : #ifdef CONFIG_SCHED_DEADLINE
87 : : /* If we assume all deadlines live within the same "half" of
88 : : * the 32 bit modulus space (this is a documented API rule),
89 : : * then the latest deadline in the queue minus the earliest is
90 : : * guaranteed to be (2's complement) non-negative. We can
91 : : * leverage that to compare the values without having to check
92 : : * the current time.
93 : : */
94 : : uint32_t d1 = thread_1->base.prio_deadline;
95 : : uint32_t d2 = thread_2->base.prio_deadline;
96 : :
97 : : if (d1 != d2) {
98 : : /* Sooner deadline means higher effective priority.
99 : : * Doing the calculation with unsigned types and casting
100 : : * to signed isn't perfect, but at least reduces this
101 : : * from UB on overflow to impdef.
102 : : */
103 : : return (int32_t)(d2 - d1);
104 : : }
105 : : #endif /* CONFIG_SCHED_DEADLINE */
106 : : return 0;
107 : : }
108 : :
109 : 67 : static ALWAYS_INLINE void z_priq_simple_add(sys_dlist_t *pq, struct k_thread *thread)
110 : : {
111 : 67 : struct k_thread *t;
112 : :
113 [ + + + + ]: 154 : SYS_DLIST_FOR_EACH_CONTAINER(pq, t, base.qnode_dlist) {
114 [ + + ]: 58 : if (z_sched_prio_cmp(thread, t) > 0) {
115 : 44 : sys_dlist_insert(&t->base.qnode_dlist, &thread->base.qnode_dlist);
116 : 44 : return;
117 : : }
118 : : }
119 : :
120 : 23 : sys_dlist_append(pq, &thread->base.qnode_dlist);
121 : : }
122 : :
123 : 66 : static ALWAYS_INLINE void z_priq_simple_remove(sys_dlist_t *pq, struct k_thread *thread)
124 : : {
125 : 66 : ARG_UNUSED(pq);
126 : :
127 : 66 : sys_dlist_remove(&thread->base.qnode_dlist);
128 : 66 : }
129 : :
130 : 0 : static ALWAYS_INLINE void z_priq_simple_yield(sys_dlist_t *pq)
131 : : {
132 : : #ifndef CONFIG_SMP
133 : 0 : sys_dnode_t *n;
134 : :
135 : 0 : n = sys_dlist_peek_next_no_check(pq, &_current->base.qnode_dlist);
136 : :
137 : 0 : sys_dlist_dequeue(&_current->base.qnode_dlist);
138 : :
139 : 0 : struct k_thread *t;
140 : :
141 : : /*
142 : : * As it is possible that the current thread was not at the head of
143 : : * the run queue, start searching from the present position for where
144 : : * to re-insert it.
145 : : */
146 : :
147 [ # # ]: 0 : while (n != NULL) {
148 : 0 : t = CONTAINER_OF(n, struct k_thread, base.qnode_dlist);
149 [ # # ]: 0 : if (z_sched_prio_cmp(_current, t) > 0) {
150 : 0 : sys_dlist_insert(&t->base.qnode_dlist,
151 : : &_current->base.qnode_dlist);
152 : 0 : return;
153 : : }
154 : 0 : n = sys_dlist_peek_next_no_check(pq, n);
155 : : }
156 : :
157 : 0 : sys_dlist_append(pq, &_current->base.qnode_dlist);
158 : : #endif
159 : : }
160 : :
161 : 6 : static ALWAYS_INLINE struct k_thread *z_priq_simple_best(sys_dlist_t *pq)
162 : : {
163 : 6 : struct k_thread *thread = NULL;
164 : 6 : sys_dnode_t *n = sys_dlist_peek_head(pq);
165 : :
166 : 6 : if (n != NULL) {
167 : : thread = CONTAINER_OF(n, struct k_thread, base.qnode_dlist);
168 : : }
169 : 6 : return thread;
170 : : }
171 : :
172 : : #ifdef CONFIG_SCHED_CPU_MASK
173 : 114 : static ALWAYS_INLINE struct k_thread *z_priq_simple_mask_best(sys_dlist_t *pq)
174 : : {
175 : : /* With masks enabled we need to be prepared to walk the list
176 : : * looking for one we can run
177 : : */
178 : 114 : struct k_thread *thread;
179 : :
180 [ - - + - ]: 228 : SYS_DLIST_FOR_EACH_CONTAINER(pq, thread, base.qnode_dlist) {
181 [ + - ]: 114 : if ((thread->base.cpu_mask & BIT(_current_cpu->id)) != 0) {
182 : 114 : return thread;
183 : : }
184 : : }
185 : : return NULL;
186 : : }
187 : : #endif /* CONFIG_SCHED_CPU_MASK */
188 : :
189 : : #if defined(CONFIG_SCHED_SCALABLE) || defined(CONFIG_WAITQ_SCALABLE)
190 : : static ALWAYS_INLINE void z_priq_rb_init(struct _priq_rb *pq)
191 : : {
192 : : bool z_priq_rb_lessthan(struct rbnode *a, struct rbnode *b);
193 : :
194 : : *pq = (struct _priq_rb) {
195 : : .tree = {
196 : : .lessthan_fn = z_priq_rb_lessthan,
197 : : }
198 : : };
199 : : }
200 : :
201 : : static ALWAYS_INLINE void z_priq_rb_add(struct _priq_rb *pq, struct k_thread *thread)
202 : : {
203 : : struct k_thread *t;
204 : :
205 : : thread->base.order_key = pq->next_order_key;
206 : : ++pq->next_order_key;
207 : :
208 : : /* Renumber at wraparound. This is tiny code, and in practice
209 : : * will almost never be hit on real systems. BUT on very
210 : : * long-running systems where a priq never completely empties
211 : : * AND that contains very large numbers of threads, it can be
212 : : * a latency glitch to loop over all the threads like this.
213 : : */
214 : : if (!pq->next_order_key) {
215 : : RB_FOR_EACH_CONTAINER(&pq->tree, t, base.qnode_rb) {
216 : : t->base.order_key = pq->next_order_key;
217 : : ++pq->next_order_key;
218 : : }
219 : : }
220 : :
221 : : rb_insert(&pq->tree, &thread->base.qnode_rb);
222 : : }
223 : :
224 : : static ALWAYS_INLINE void z_priq_rb_remove(struct _priq_rb *pq, struct k_thread *thread)
225 : : {
226 : : rb_remove(&pq->tree, &thread->base.qnode_rb);
227 : :
228 : : if (!pq->tree.root) {
229 : : pq->next_order_key = 0;
230 : : }
231 : : }
232 : :
233 : : static ALWAYS_INLINE void z_priq_rb_yield(struct _priq_rb *pq)
234 : : {
235 : : #ifndef CONFIG_SMP
236 : : z_priq_rb_remove(pq, _current);
237 : : z_priq_rb_add(pq, _current);
238 : : #endif
239 : : }
240 : :
241 : : static ALWAYS_INLINE struct k_thread *z_priq_rb_best(struct _priq_rb *pq)
242 : : {
243 : : struct k_thread *thread = NULL;
244 : : struct rbnode *n = rb_get_min(&pq->tree);
245 : :
246 : : if (n != NULL) {
247 : : thread = CONTAINER_OF(n, struct k_thread, base.qnode_rb);
248 : : }
249 : : return thread;
250 : : }
251 : : #endif
252 : :
253 : : struct prio_info {
254 : : uint8_t offset_prio;
255 : : uint8_t idx;
256 : : uint8_t bit;
257 : : };
258 : :
259 : : static ALWAYS_INLINE struct prio_info get_prio_info(int8_t old_prio)
260 : : {
261 : : struct prio_info ret;
262 : :
263 : : ret.offset_prio = old_prio - K_HIGHEST_THREAD_PRIO;
264 : : ret.idx = ret.offset_prio / NBITS;
265 : : ret.bit = ret.offset_prio % NBITS;
266 : :
267 : : return ret;
268 : : }
269 : :
270 : : static ALWAYS_INLINE unsigned int z_priq_mq_best_queue_index(struct _priq_mq *pq)
271 : : {
272 : : unsigned int i = 0;
273 : :
274 : : do {
275 : : if (likely(pq->bitmask[i])) {
276 : : return i * NBITS + TRAILING_ZEROS(pq->bitmask[i]);
277 : : }
278 : : i++;
279 : : } while (i < PRIQ_BITMAP_SIZE);
280 : :
281 : : return K_NUM_THREAD_PRIO - 1;
282 : : }
283 : :
284 : : static ALWAYS_INLINE void z_priq_mq_init(struct _priq_mq *q)
285 : : {
286 : : for (size_t i = 0; i < ARRAY_SIZE(q->queues); i++) {
287 : : sys_dlist_init(&q->queues[i]);
288 : : }
289 : :
290 : : #ifndef CONFIG_SMP
291 : : q->cached_queue_index = K_NUM_THREAD_PRIO - 1;
292 : : #endif
293 : : }
294 : :
295 : : static ALWAYS_INLINE void z_priq_mq_add(struct _priq_mq *pq,
296 : : struct k_thread *thread)
297 : : {
298 : : struct prio_info pos = get_prio_info(thread->base.prio);
299 : :
300 : : sys_dlist_append(&pq->queues[pos.offset_prio], &thread->base.qnode_dlist);
301 : : pq->bitmask[pos.idx] |= BIT(pos.bit);
302 : :
303 : : #ifndef CONFIG_SMP
304 : : if (pos.offset_prio < pq->cached_queue_index) {
305 : : pq->cached_queue_index = pos.offset_prio;
306 : : }
307 : : #endif
308 : : }
309 : :
310 : : static ALWAYS_INLINE void z_priq_mq_remove(struct _priq_mq *pq,
311 : : struct k_thread *thread)
312 : : {
313 : : struct prio_info pos = get_prio_info(thread->base.prio);
314 : :
315 : : sys_dlist_dequeue(&thread->base.qnode_dlist);
316 : : if (unlikely(sys_dlist_is_empty(&pq->queues[pos.offset_prio]))) {
317 : : pq->bitmask[pos.idx] &= ~BIT(pos.bit);
318 : : #ifndef CONFIG_SMP
319 : : pq->cached_queue_index = z_priq_mq_best_queue_index(pq);
320 : : #endif
321 : : }
322 : : }
323 : :
324 : : static ALWAYS_INLINE void z_priq_mq_yield(struct _priq_mq *pq)
325 : : {
326 : : #ifndef CONFIG_SMP
327 : : struct prio_info pos = get_prio_info(_current->base.prio);
328 : :
329 : : sys_dlist_dequeue(&_current->base.qnode_dlist);
330 : : sys_dlist_append(&pq->queues[pos.offset_prio],
331 : : &_current->base.qnode_dlist);
332 : : #endif
333 : : }
334 : :
335 : : static ALWAYS_INLINE struct k_thread *z_priq_mq_best(struct _priq_mq *pq)
336 : : {
337 : : #ifdef CONFIG_SMP
338 : : unsigned int index = z_priq_mq_best_queue_index(pq);
339 : : #else
340 : : unsigned int index = pq->cached_queue_index;
341 : : #endif
342 : :
343 : : sys_dnode_t *n = sys_dlist_peek_head(&pq->queues[index]);
344 : :
345 : : if (likely(n != NULL)) {
346 : : return CONTAINER_OF(n, struct k_thread, base.qnode_dlist);
347 : : }
348 : :
349 : : return NULL;
350 : : }
351 : : #ifdef IAR_SUPPRESS_ALWAYS_INLINE_WARNING_FLAG
352 : : TOOLCHAIN_ENABLE_WARNING(TOOLCHAIN_WARNING_ALWAYS_INLINE)
353 : : #endif
354 : :
355 : : #endif /* ZEPHYR_KERNEL_INCLUDE_PRIORITY_Q_H_ */
|
