1#include <math/fixed.h>
2#include <mem/alloc.h>
3#include <mem/numa.h>
4#include <sch/rt_sched.h>
5#include <sch/sched.h>
6#include <smp/topology.h>
7#include <sync/spinlock.h>
8
9#include "internal.h"
10#include "sch/internal.h"
11
12struct rt_global rt_global = {0};
13struct workqueue *rt_wq = NULL;
14static void destroy_work(void *a, void *b);
15
16void rt_scheduler_static_destroy_work_enqueue(struct rt_scheduler_static *rts) {
17 struct work *this_work = &rts->teardown_work;
18 workqueue_enqueue(queue: rt_wq, work: this_work);
19}
20
21void rt_scheduler_static_work_init(struct rt_scheduler_static *rts) {
22 struct work *this_work = &rts->teardown_work;
23 work_init(work: this_work, fn: destroy_work, WORK_ARGS(rts, NULL));
24}
25
26static struct rt_scheduler_mapping *
27create_mapping(struct rt_scheduler_static *rts, rt_domain_id_t id) {
28 struct rt_scheduler_mapping *ret =
29 kmalloc(sizeof(struct rt_scheduler_mapping), ALLOC_FLAGS_ZERO);
30 if (!ret)
31 return NULL;
32
33 if (!cpu_mask_init(m: &ret->members, nbits: global.core_count)) {
34 kfree(ret);
35 return NULL;
36 }
37
38 if (!cpu_mask_init(m: &ret->active, nbits: global.core_count)) {
39 cpu_mask_deinit(m: &ret->members);
40 kfree(ret);
41 return NULL;
42 }
43
44 ret->id = id;
45 ret->static_bptr = rts;
46 rbt_init_node(n: &ret->tree_node);
47 rbt_insert(tree: &rts->mappings_internal, new_node: &ret->tree_node);
48 spinlock_init(lock: &ret->lock);
49
50 return NULL;
51}
52
53/* Fails only on OOM */
54static bool add_to_rbt_or_set_cpu_mask(struct rt_scheduler_static *rts,
55 rt_domain_id_t id, struct core *core) {
56 struct rbt_node *node = rbt_search(tree: &rts->mappings_internal, data: id);
57 struct rt_scheduler_mapping *mapping;
58 if (node) {
59 mapping = container_of(node, struct rt_scheduler_mapping, tree_node);
60 } else {
61 mapping = create_mapping(rts, id);
62 }
63
64 if (!mapping)
65 return false;
66
67 cpu_mask_set(m: &mapping->members, cpu: core->id);
68 return true;
69}
70
71static void destroy_rt_mappings(struct rt_scheduler_static *rts) {
72 struct rbt_node *iter, *tmp;
73 rbt_for_each_safe(iter, tmp, &rts->mappings_internal) {
74 rbt_delete(tree: &rts->mappings_internal, z: iter);
75
76 struct rt_scheduler_mapping *m =
77 container_of(iter, struct rt_scheduler_mapping, tree_node);
78
79 kfree(m);
80 }
81}
82
83/* Lock the mapping for this CPU, and return it. */
84struct rt_scheduler_mapping *rt_lookup_mapping(struct rt_scheduler_static *rts,
85 struct core *c) {
86 rt_domain_id_t id = rts->ops.domain_id_for_cpu(c);
87 struct rbt_node *found = rbt_search(tree: &rts->mappings_internal, data: id);
88 if (!found)
89 panic("CPU %zu does not have a mapping in this scheduler", c->id);
90
91 struct rt_scheduler_mapping *mapping =
92 container_of(found, struct rt_scheduler_mapping, tree_node);
93
94 /* Must be set */
95 kassert(cpu_mask_test(&mapping->members, c->id));
96
97 return mapping;
98}
99
100/* We build the mapping ONCE at the very start, and then it becomes RO */
101enum rt_scheduler_error rt_build_mapping(struct rt_scheduler_static *rts) {
102 struct rt_scheduler_ops *ops = &rts->ops;
103 struct core *iter;
104
105 kassert(rts->mappings_internal.root == NULL);
106
107 /* Let's go through every CPU on the system and then make a node
108 * for all of them, or add them to an existing node's bitmap */
109 for_each_cpu_struct(iter) {
110 rt_domain_id_t id = ops->domain_id_for_cpu(iter);
111 if (!add_to_rbt_or_set_cpu_mask(rts, id, core: iter)) {
112 destroy_rt_mappings(rts);
113 return RT_SCHEDULER_ERR_OOM;
114 }
115 }
116
117 return RT_SCHEDULER_ERR_OK;
118}
119
120static size_t rt_mapping_get_data(struct rbt_node *n) {
121 return container_of(n, struct rt_scheduler_mapping, tree_node)->id;
122}
123
124static int32_t rt_mapping_cmp(const struct rbt_node *a,
125 const struct rbt_node *b) {
126 int32_t l = rt_mapping_get_data(n: (void *) a);
127 int32_t r = rt_mapping_get_data(n: (void *) b);
128 return l - r;
129}
130
131static void reset_summary(struct rt_thread_summary *sum) {
132 sum->status = RT_SCHEDULER_STATUS_OK;
133 sum->weight = 0;
134 sum->urgency = FX(0.0);
135}
136
137static void reset_shed_request(struct rt_thread_shed_request *rtsr) {
138 rtsr->urgency = FX(0.0);
139 rtsr->threads_available = 0;
140 kassert(list_empty(&rtsr->threads));
141 INIT_LIST_HEAD(list: &rtsr->threads);
142 rtsr->on = false;
143}
144
145static void reset_scheduler(struct rt_scheduler *rts) {
146 kassert(rts->thread_count == 0);
147 rts->mapping_source = NULL;
148 reset_summary(sum: &rts->summary);
149 reset_shed_request(rtsr: &rts->shed_request);
150}
151
152enum rt_scheduler_error
153rt_load_scheduler_static(struct rt_scheduler_static *rts) {
154 enum rt_scheduler_error err = RT_SCHEDULER_ERR_INVALID;
155 enum irql outer = spin_lock(lock: &rt_global.static_list.lock);
156 enum irql irql = spin_lock(lock: &rts->state_change_lock);
157
158 if (rt_scheduler_static_get_state(rts) != RT_SCHEDULER_STATIC_UNLOADED)
159 goto done;
160
161 rbt_init(t: &rts->mappings_internal, get_data: rt_mapping_get_data, compare: rt_mapping_cmp);
162 refcount_init(rc: &rts->refcount, val: 1);
163 INIT_LIST_HEAD(list: &rts->list_internal);
164 list_add_tail(new: &rts->list_internal, head: &rt_global.static_list.list);
165 rt_global.static_list.num_elems++;
166
167 if (!(rts->active_mask_internal = cpu_mask_create())) {
168 err = RT_SCHEDULER_ERR_OOM;
169 goto done;
170 }
171
172 if (!cpu_mask_init(m: rts->active_mask_internal, nbits: global.core_count)) {
173 err = RT_SCHEDULER_ERR_OOM;
174 goto done;
175 }
176
177 if ((err = rt_slots_init_for_scheduler(rts)) != RT_SCHEDULER_ERR_OK)
178 goto done;
179
180 if ((err = rt_build_mapping(rts)) != RT_SCHEDULER_ERR_OK)
181 goto done;
182
183 rt_scheduler_static_work_init(rts);
184 err = rts->ops.on_load(rts);
185
186done:
187 if (err == RT_SCHEDULER_ERR_OK)
188 rt_scheduler_static_set_state(rts, new: RT_SCHEDULER_STATIC_LOADED);
189
190 spin_unlock(lock: &rts->state_change_lock, old: irql);
191 spin_unlock(lock: &rt_global.static_list.lock, old: outer);
192 return err;
193}
194
195enum rt_scheduler_error
196rt_unload_scheduler_static(struct rt_scheduler_static *rts) {
197 /* In here, we want to validate that we are looking at a loaded
198 * scheduler_static, and if we are, we can safely drop the initial
199 * ref. Otherwise, return the error (unloading an unloaded scheduler) */
200 enum rt_scheduler_error err = RT_SCHEDULER_ERR_INVALID;
201 enum irql irql = spin_lock(lock: &rts->state_change_lock);
202
203 if (rts->state != RT_SCHEDULER_STATIC_LOADED)
204 goto done;
205
206 rt_scheduler_static_set_state(rts, new: RT_SCHEDULER_STATIC_DESTROYING);
207 rt_scheduler_static_put(rts);
208 err = RT_SCHEDULER_ERR_OK;
209
210done:
211
212 spin_unlock(lock: &rts->state_change_lock, old: irql);
213 return err;
214}
215
216static void rt_scheduler_destroy_internal(struct rt_scheduler_static *rts) {
217 /* Assert that the refs are gone, lock, unload, teardown */
218 enum irql outer = spin_lock(lock: &rt_global.static_list.lock);
219 enum irql irql = spin_lock(lock: &rts->state_change_lock);
220 kassert(refcount_read(&rts->refcount) == 0);
221 rt_scheduler_static_set_state(rts, new: RT_SCHEDULER_STATIC_UNLOADED);
222
223 rts->ops.on_unload(rts);
224
225 list_del_init(entry: &rts->list_internal);
226 rt_global.static_list.num_elems--;
227 rt_slots_dealloc_for_scheduler(rts);
228 cpu_mask_deinit(m: rts->active_mask_internal);
229 cpu_mask_free(m: rts->active_mask_internal);
230 rts->active_mask_internal = NULL;
231 destroy_rt_mappings(rts);
232
233 spin_unlock(lock: &rts->state_change_lock, old: irql);
234 spin_unlock(lock: &rt_global.static_list.lock, old: outer);
235}
236
237static void destroy_work(void *a, void *b) {
238 (void) b;
239 rt_sched_trace("Destroying realtime scheduler %p", a);
240 struct rt_scheduler_static *rts = a;
241 rt_scheduler_destroy_internal(rts);
242}
243
244/* Just mask the bits and see if anything goes through. If it does,
245 * there is a compatibility between the two */
246static inline bool is_compatible(struct rt_scheduler_static *rts,
247 struct thread *t) {
248 return rts->capabilities & t->accepted_rt_caps;
249}
250
251static inline bool needs_migrate(struct rt_scheduler_static *rts,
252 struct thread *t) {
253 return !is_compatible(rts, t);
254}
255
256static ssize_t find_migration_target(struct thread *t) {
257 size_t iter;
258 kassert(cpu_mask_popcount(&t->allowed_cpus));
259 cpu_mask_for_each(iter, t->allowed_cpus) {
260 struct rt_scheduler_percpu *pcpu = global.schedulers[iter]->rt;
261 struct rt_scheduler_static *rts = pcpu->active_mapping->static_bptr;
262 if (is_compatible(rts, t))
263 return iter;
264 }
265
266 return -1;
267}
268
269static inline bool has_migration_target(struct thread *t) {
270 return find_migration_target(t) != -1;
271}
272
273static void send_to_compatible_cpu(struct thread *t) {
274 size_t go_to = find_migration_target(t);
275 struct rt_scheduler *next =
276 global.schedulers[go_to]->rt->active_mapping->rts;
277 enum irql irql = spin_lock_irq_disable(lock: &next->lock);
278
279 /* TODO: Wrap around thread add/remove to better work with counters and
280 * stuff.
281 *
282 * THIS IS TEMPORARY */
283 kassert(is_compatible(next->mapping_source->static_bptr, t));
284 next->mapping_source->static_bptr->ops.add_thread(next, t);
285
286 spin_unlock(lock: &next->lock, old: irql);
287}
288
289static bool try_migrate_all_before_switch(struct rt_scheduler *rts,
290 struct rt_scheduler_static *st,
291 struct list_head *thread_list) {
292 /* All switch events are protected under our beloved rt_global.switch_lock.
293 *
294 * This means that we are free to read the rt_schedulers of other CPUs
295 * without a fear that the rug will be pulled from under us, and this
296 * allows us to take our own rt_scheduler lock, and perform proper
297 * lock ordering after we move all the threads off of its runqueues.
298 */
299 enum irql irql = spin_lock_irq_disable(lock: &rts->lock);
300
301 /* Tell the `st` to give us all of its threads so we can take a gander */
302 /* TODO: Write a wrapper macro around any operation call */
303 st->ops.return_all_threads(rts, thread_list);
304 size_t old = rts->thread_count;
305 rts->thread_count = 0; /* So as to not confuse the rt_scheduler */
306
307 /* Drop the lock now, we have ownership of all threads (hopefully!!!) */
308 spin_unlock(lock: &rts->lock, old: irql);
309
310 if (list_empty(head: thread_list)) {
311 /* TODO: Find a better way to wrap around assertions made
312 * after a RT scheduler op so we don't crash the kernel */
313 kassert(old == 0);
314 return true; /* No threads, no one can be unhoused! */
315 }
316
317 /* Our strategy for this works as follows:
318 *
319 * First, we do one pass to check and see if all threads have a home
320 * after the migration to the new scheduler_static. If some threads
321 * do NOT have a home, then we abort the switch, and bail.
322 *
323 * Otherwise, we will keep the threads that do not need to be
324 * migrated on the list, and for every thread that needs to be
325 * migrated, we will take it off and away onto another CPU.
326 *
327 */
328
329 struct thread *iter, *tmp;
330 list_for_each_entry_safe(iter, tmp, thread_list, rt_list_node) {
331 /* Yikes, it needs to be migrated off and can go nowhere */
332 if (needs_migrate(rts: st, t: iter) && !has_migration_target(t: iter))
333 return false;
334 }
335
336 /* OK - now we have verified that everyone has somewhere to go */
337 list_for_each_entry_safe(iter, tmp, thread_list, rt_list_node) {
338 /* Keep it */
339 if (!needs_migrate(rts: st, t: iter))
340 continue;
341
342 /* Bye bye */
343 list_del_init(entry: &iter->rt_list_node);
344 send_to_compatible_cpu(t: iter);
345 }
346
347 return true;
348}
349
350static void re_enqueue_threads(struct rt_scheduler *rts,
351 struct list_head *threads) {
352 struct thread *iter, *tmp;
353
354 list_for_each_entry_safe(iter, tmp, threads, rt_list_node) {
355 list_del_init(entry: &iter->rt_list_node);
356 enum irql irql = spin_lock_irq_disable(lock: &rts->lock);
357
358 /* TODO: remember to wrap these! */
359 rts->mapping_source->static_bptr->ops.add_thread(rts, iter);
360
361 spin_unlock(lock: &rts->lock, old: irql);
362 }
363}
364
365static void setup_new_rt_scheduler(struct rt_scheduler *rts,
366 struct rt_scheduler_mapping *rtm) {
367 kassert(rts->thread_count == 0);
368 kassert(!rts->failed_internal);
369 rts->mapping_source = rtm;
370 log_trace(rts->log_site, &rts->log_handle, "rts %p setting up by %zu", rts,
371 smp_core_id());
372}
373
374/* On NUMA systems, we'll iterate to the next closest node
375 * if we can't find a struct rt_scheduler for our system.
376 *
377 * Otherwise, we just scan from 0 to max
378 *
379 * If we fail to find a struct rt_scheduler, something
380 * has gone very very very wrong... */
381static struct rt_scheduler *get_new_rt_scheduler(size_t domain) {
382 struct list_head *got = NULL;
383 if ((got = locked_list_pop_front(ll: &rt_global.sch_pool[domain])))
384 goto out;
385
386 if (global.numa_node_count > 1) {
387 struct numa_node *node = &global.numa_nodes[domain];
388 for (size_t i = 0; i < global.numa_node_count; i++) {
389 uint8_t next = node->nodes_by_distance[i];
390 if ((got = locked_list_pop_front(ll: &rt_global.sch_pool[next])))
391 goto out;
392 }
393 } else {
394 for (size_t i = 0; i < global.domain_count; i++) {
395 if ((got = locked_list_pop_front(ll: &rt_global.sch_pool[i])))
396 goto out;
397 }
398 }
399
400out:
401 kassert(got);
402 return container_of(got, struct rt_scheduler, list);
403}
404
405static inline bool check_active(struct rt_scheduler_mapping *rtm) {
406 return cpu_mask_test(m: &rtm->active, cpu: smp_core_id());
407}
408
409static inline void mark_active(struct rt_scheduler_mapping *rtm) {
410 cpu_mask_set(m: &rtm->active, cpu: smp_core_id());
411}
412
413static inline void unmark_active(struct rt_scheduler_mapping *rtm) {
414 cpu_mask_clear(m: &rtm->active, cpu: smp_core_id());
415}
416
417static inline void clear_switch_and_post(struct rt_scheduler_percpu *rts,
418 enum rt_scheduler_error err) {
419 atomic_store_explicit(&rts->switch_code, err, memory_order_release);
420 atomic_store_explicit(&rts->switch_into, NULL, memory_order_release);
421 semaphore_post(s: &rts->switch_semaphore);
422}
423
424void rt_scheduler_switch() {
425 struct rt_scheduler_percpu *pcpu = smp_core_scheduler()->rt;
426 struct rt_scheduler_static *into =
427 atomic_load_explicit(&pcpu->switch_into, memory_order_acquire);
428
429 /* Nothing to do */
430 if (!into)
431 return;
432
433 if (!rt_scheduler_static_get(obj: into))
434 return clear_switch_and_post(rts: pcpu, err: RT_SCHEDULER_ERR_NOT_FOUND);
435
436 struct rt_scheduler_static *from = pcpu->active_mapping->static_bptr;
437 enum irql girql = spin_lock_irq_disable(lock: &rt_global.switch_lock);
438 enum rt_scheduler_error err = RT_SCHEDULER_ERR_OK;
439 bool put_into = false;
440
441 struct rt_scheduler_mapping *curr = pcpu->active_mapping;
442 struct rt_scheduler_mapping *next = rt_lookup_mapping(rts: into, c: smp_core());
443 bool next_exists = next->rts;
444 rt_sched_trace(
445 "CPU %zu wants to switch from mapping %zu to mapping %zu (exists: %d)",
446 smp_core_id(), curr->id, next->id, next_exists);
447
448 if (curr == next) {
449 /* No switch needed, just signal the waiting thread and return */
450 rt_sched_trace(
451 "No switch needed for CPU %zu, already on the right mapping",
452 smp_core_id());
453
454 clear_switch_and_post(rts: pcpu, err);
455 spin_unlock(lock: &rt_global.switch_lock, old: girql);
456 rt_scheduler_static_put(rts: into);
457 return;
458 }
459
460 enum irql irql_curr, irql_next;
461 rt_scheduler_acquire_two_mappings(a: curr, b: next, out_a: &irql_curr, out_b: &irql_next);
462
463 /* First we check if we even *can* switch out. If we can't
464 * then we leave and return IMPOSSIBLE
465 *
466 * To do this, we use the following strategy:
467 *
468 * Check if we are the ONLY CPU for our mapping. If we
469 * are NOT, we **always can** switch out.
470 *
471 * If we ARE the only CPU for our mapping, then we iterate
472 * through all the threads, and see if they will all have
473 * a safe place to get migrated to. If they DO NOT, then
474 * we FAIL the migration with IMPOSSIBLE
475 */
476 kassert(cpu_mask_test(&curr->active, smp_core_id()));
477 bool only_cpu = cpu_mask_popcount(m: &curr->active) == 1;
478
479 bool can_switch = true;
480
481 struct list_head thread_list;
482 INIT_LIST_HEAD(list: &thread_list);
483 if (only_cpu)
484 can_switch =
485 try_migrate_all_before_switch(rts: curr->rts, st: into, thread_list: &thread_list);
486
487 if (!can_switch) {
488 put_into = true;
489 err = RT_SCHEDULER_ERR_SWITCH_IMPOSSIBLE;
490 goto out;
491 }
492
493 /* We know we can switch now. All the threads are on our current
494 * thread_list.
495 *
496 * Our approach becomes the following:
497 *
498 * If we are not the only CPU left, we do not need to reset the scheduler,
499 * because someone else is using it. In this case, we just remove ourselves
500 * from the active mask of the scheduler. Otherwise, we need to go
501 * through the whole process of resetting, etc. etc.
502 *
503 */
504
505 kassert(check_active(curr));
506 unmark_active(rtm: curr);
507 if (only_cpu)
508 reset_scheduler(rts: curr->rts);
509
510 /* The logic for deciding whether or not we need a new rt_scheduler is...
511 *
512 * If nothing exists for the next mapping (we need to provide an
513 * rt_scheduler), AND we are NOT the only CPU for the current
514 * mapping, then we will need to get a scheduler from the pool
515 */
516
517 /* We are the only CPU and the next mapping has an owner already */
518 bool donate = only_cpu && next_exists;
519 bool need_new = !only_cpu && !next_exists;
520 struct rt_scheduler *next_rts = NULL;
521
522 if (donate)
523 locked_list_add(ll: &rt_global.sch_pool[domain_local_id()],
524 lh: &curr->rts->list);
525
526 /* Switch out is completed */
527 if (need_new) {
528 /* Get a new one, nothing exists for this mapping */
529 next_rts = get_new_rt_scheduler(domain: domain_local_id());
530 } else {
531 next_rts = next->rts;
532 }
533
534 /* We are now using the new rt_scheduler */
535 pcpu->active_mapping = next;
536
537 if (need_new) {
538 /* This list should not have anything because we should
539 * not have taken anything from our rt_scheduler. need_new
540 * requires us to NOT be the only CPU, so we do not drain
541 * the runqueue of our mapping here */
542 kassert(list_empty(&thread_list));
543 setup_new_rt_scheduler(rts: next_rts, rtm: next);
544 } else {
545 re_enqueue_threads(rts: next_rts, threads: &thread_list);
546 }
547
548 mark_active(rtm: next);
549
550 /* Drop the old ref, only reachable via success path */
551 rt_scheduler_static_put(rts: from);
552out:
553 /* Post to the thread that sent us this */
554 clear_switch_and_post(rts: pcpu, err);
555 rt_scheduler_release_two_mappings(a: curr, b: next, out_a: irql_curr, out_b: irql_next);
556 spin_unlock(lock: &rt_global.switch_lock, old: girql);
557 if (put_into)
558 rt_scheduler_static_put(rts: into);
559}
560
561enum rt_scheduler_error
562rt_scheduler_switch_cpu(size_t cpu, struct rt_scheduler_static *into) {
563 struct rt_scheduler_percpu *pcpu = global.schedulers[cpu]->rt;
564 semaphore_wait(s: &pcpu->switch_semaphore);
565
566 kassert(!atomic_exchange(&pcpu->switch_into, into));
567 scheduler_force_resched(sched: global.schedulers[cpu]);
568
569 /* It will signal us now */
570 semaphore_wait(s: &pcpu->switch_semaphore);
571
572 enum rt_scheduler_error ret =
573 atomic_load_explicit(&pcpu->switch_code, memory_order_relaxed);
574
575 /* Signal the waiting thread */
576 semaphore_post(s: &pcpu->switch_semaphore);
577
578 return ret;
579}
580