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(&ret->lock);
49 return ret;
50}
51
52/* Fails only on OOM */
53static bool add_to_rbt_or_set_cpu_mask(struct rt_scheduler_static *rts,
54 rt_domain_id_t id, struct core *core) {
55 struct rbt_node *node = rbt_search(tree: &rts->mappings_internal, data: id);
56 struct rt_scheduler_mapping *mapping;
57 if (node) {
58 mapping = container_of(node, struct rt_scheduler_mapping, tree_node);
59 } else {
60 mapping = create_mapping(rts, id);
61 }
62
63 if (!mapping)
64 return false;
65
66 cpu_mask_set(m: &mapping->members, cpu: core->id);
67 return true;
68}
69
70static void destroy_rt_mappings(struct rt_scheduler_static *rts) {
71 struct rbt_node *iter, *tmp;
72 rbt_for_each_safe(iter, tmp, &rts->mappings_internal) {
73 rbt_delete(tree: &rts->mappings_internal, z: iter);
74
75 struct rt_scheduler_mapping *m =
76 container_of(iter, struct rt_scheduler_mapping, tree_node);
77
78 kfree(m);
79 }
80}
81
82/* Lock the mapping for this CPU, and return it. */
83struct rt_scheduler_mapping *rt_lookup_mapping(struct rt_scheduler_static *rts,
84 struct core *c) {
85 rt_domain_id_t id = rts->ops.domain_id_for_cpu(c);
86 struct rbt_node *found = rbt_search(tree: &rts->mappings_internal, data: id);
87 if (!found)
88 panic("CPU %zu does not have a mapping in this scheduler", c->id);
89
90 struct rt_scheduler_mapping *mapping =
91 container_of(found, struct rt_scheduler_mapping, tree_node);
92
93 /* Must be set */
94 kassert(cpu_mask_test(&mapping->members, c->id));
95
96 return mapping;
97}
98
99/* We build the mapping ONCE at the very start, and then it becomes RO */
100enum rt_scheduler_error rt_build_mapping(struct rt_scheduler_static *rts) {
101 struct rt_scheduler_ops *ops = &rts->ops;
102 struct core *iter;
103
104 kassert(rts->mappings_internal.root == NULL);
105
106 /* Let's go through every CPU on the system and then make a node
107 * for all of them, or add them to an existing node's bitmap */
108 for_each_cpu_struct(iter) {
109 rt_domain_id_t id = ops->domain_id_for_cpu(iter);
110 if (!add_to_rbt_or_set_cpu_mask(rts, id, core: iter)) {
111 destroy_rt_mappings(rts);
112 return RT_SCHEDULER_ERR_OOM;
113 }
114 }
115
116 return RT_SCHEDULER_ERR_OK;
117}
118
119static size_t rt_mapping_get_data(struct rbt_node *n) {
120 return container_of(n, struct rt_scheduler_mapping, tree_node)->id;
121}
122
123static int32_t rt_mapping_cmp(const struct rbt_node *a,
124 const struct rbt_node *b) {
125 int32_t l = rt_mapping_get_data(n: (void *) a);
126 int32_t r = rt_mapping_get_data(n: (void *) b);
127 return l - r;
128}
129
130static void reset_summary(struct rt_thread_summary *sum) {
131 sum->status = RT_SCHEDULER_STATUS_OK;
132 sum->weight = 0;
133 sum->urgency = FX(0.0);
134}
135
136static void reset_shed_request(struct rt_thread_shed_request *rtsr) {
137 rtsr->urgency = FX(0.0);
138 rtsr->threads_available = 0;
139 kassert(list_empty(&rtsr->threads));
140 INIT_LIST_HEAD(list: &rtsr->threads);
141 rtsr->on = false;
142}
143
144static void reset_scheduler(struct rt_scheduler *rts) {
145 kassert(rts->thread_count == 0);
146 rts->mapping_source = NULL;
147 reset_summary(sum: &rts->summary);
148 reset_shed_request(rtsr: &rts->shed_request);
149}
150
151enum rt_scheduler_error
152rt_load_scheduler_static(struct rt_scheduler_static *rts) {
153 enum rt_scheduler_error err = RT_SCHEDULER_ERR_INVALID;
154 enum irql outer = spin_lock(&rt_global.static_list.lock);
155 enum irql irql = spin_lock(&rts->state_change_lock);
156
157 if (rt_scheduler_static_get_state(rts) != RT_SCHEDULER_STATIC_UNLOADED)
158 goto done;
159
160 rbt_init(t: &rts->mappings_internal, get_data: rt_mapping_get_data, compare: rt_mapping_cmp);
161 refcount_init(rc: &rts->refcount, val: 1);
162 INIT_LIST_HEAD(list: &rts->list_internal);
163 list_add_tail(new: &rts->list_internal, head: &rt_global.static_list.list);
164 rt_global.static_list.num_elems++;
165
166 if (!(rts->active_mask_internal = cpu_mask_create())) {
167 err = RT_SCHEDULER_ERR_OOM;
168 goto done;
169 }
170
171 if (!cpu_mask_init(m: rts->active_mask_internal, nbits: global.core_count)) {
172 err = RT_SCHEDULER_ERR_OOM;
173 goto done;
174 }
175
176 if ((err = rt_slots_init_for_scheduler(rts)) != RT_SCHEDULER_ERR_OK)
177 goto done;
178
179 if ((err = rt_build_mapping(rts)) != RT_SCHEDULER_ERR_OK)
180 goto done;
181
182 rt_scheduler_static_work_init(rts);
183 err = rts->ops.on_load(rts);
184
185done:
186 if (err == RT_SCHEDULER_ERR_OK)
187 rt_scheduler_static_set_state(rts, new: RT_SCHEDULER_STATIC_LOADED);
188
189 spin_unlock(&rts->state_change_lock, irql);
190 spin_unlock(&rt_global.static_list.lock, outer);
191 return err;
192}
193
194enum rt_scheduler_error
195rt_unload_scheduler_static(struct rt_scheduler_static *rts) {
196 /* In here, we want to validate that we are looking at a loaded
197 * scheduler_static, and if we are, we can safely drop the initial
198 * ref. Otherwise, return the error (unloading an unloaded scheduler) */
199 enum rt_scheduler_error err = RT_SCHEDULER_ERR_INVALID;
200 enum irql irql = spin_lock(&rts->state_change_lock);
201
202 if (rts->state != RT_SCHEDULER_STATIC_LOADED)
203 goto done;
204
205 rt_scheduler_static_set_state(rts, new: RT_SCHEDULER_STATIC_DESTROYING);
206 rt_scheduler_static_put(rts);
207 err = RT_SCHEDULER_ERR_OK;
208
209done:
210
211 spin_unlock(&rts->state_change_lock, irql);
212 return err;
213}
214
215static void rt_scheduler_destroy_internal(struct rt_scheduler_static *rts) {
216 /* Assert that the refs are gone, lock, unload, teardown */
217 enum irql outer = spin_lock(&rt_global.static_list.lock);
218 enum irql irql = spin_lock(&rts->state_change_lock);
219 kassert(refcount_read(&rts->refcount) == 0);
220 rt_scheduler_static_set_state(rts, new: RT_SCHEDULER_STATIC_UNLOADED);
221
222 rts->ops.on_unload(rts);
223
224 list_del_init(entry: &rts->list_internal);
225 rt_global.static_list.num_elems--;
226 rt_slots_dealloc_for_scheduler(rts);
227 cpu_mask_deinit(m: rts->active_mask_internal);
228 cpu_mask_free(m: rts->active_mask_internal);
229 rts->active_mask_internal = NULL;
230 destroy_rt_mappings(rts);
231
232 spin_unlock(&rts->state_change_lock, irql);
233 spin_unlock(&rt_global.static_list.lock, outer);
234}
235
236static void destroy_work(void *a, void *b) {
237 (void) b;
238 rt_sched_trace("Destroying realtime scheduler %p", a);
239 struct rt_scheduler_static *rts = a;
240 rt_scheduler_destroy_internal(rts);
241}
242
243/* Just mask the bits and see if anything goes through. If it does,
244 * there is a compatibility between the two */
245static inline bool is_compatible(struct rt_scheduler_static *rts,
246 struct thread *t) {
247 return rts->capabilities & t->accepted_rt_caps;
248}
249
250static inline bool needs_migrate(struct rt_scheduler_static *rts,
251 struct thread *t) {
252 return !is_compatible(rts, t);
253}
254
255static ssize_t find_migration_target(struct thread *t) {
256 size_t iter;
257 kassert(cpu_mask_popcount(&t->allowed_cpus));
258 cpu_mask_for_each(iter, t->allowed_cpus) {
259 struct rt_scheduler_percpu *pcpu = global.schedulers[iter]->rt;
260 struct rt_scheduler_static *rts = pcpu->active_mapping->static_bptr;
261 if (is_compatible(rts, t))
262 return iter;
263 }
264
265 return -1;
266}
267
268static inline bool has_migration_target(struct thread *t) {
269 return find_migration_target(t) != -1;
270}
271
272static void send_to_compatible_cpu(struct thread *t) {
273 size_t go_to = find_migration_target(t);
274 struct rt_scheduler *next =
275 global.schedulers[go_to]->rt->active_mapping->rts;
276 enum irql irql = spin_lock_irq_disable(&next->lock);
277
278 /* TODO: Wrap around thread add/remove to better work with counters and
279 * stuff.
280 *
281 * THIS IS TEMPORARY */
282 kassert(is_compatible(next->mapping_source->static_bptr, t));
283 next->mapping_source->static_bptr->ops.add_thread(next, t);
284
285 spin_unlock(&next->lock, irql);
286}
287
288static bool try_migrate_all_before_switch(struct rt_scheduler *rts,
289 struct rt_scheduler_static *st,
290 struct list_head *thread_list) {
291 /* All switch events are protected under our beloved rt_global.switch_lock.
292 *
293 * This means that we are free to read the rt_schedulers of other CPUs
294 * without a fear that the rug will be pulled from under us, and this
295 * allows us to take our own rt_scheduler lock, and perform proper
296 * lock ordering after we move all the threads off of its runqueues.
297 */
298 enum irql irql = spin_lock_irq_disable(&rts->lock);
299
300 /* Tell the `st` to give us all of its threads so we can take a gander */
301 /* TODO: Write a wrapper macro around any operation call */
302 st->ops.return_all_threads(rts, thread_list);
303 size_t old = rts->thread_count;
304 rts->thread_count = 0; /* So as to not confuse the rt_scheduler */
305
306 /* Drop the lock now, we have ownership of all threads (hopefully!!!) */
307 spin_unlock(&rts->lock, irql);
308
309 if (list_empty(head: thread_list)) {
310 /* TODO: Find a better way to wrap around assertions made
311 * after a RT scheduler op so we don't crash the kernel */
312 kassert(old == 0);
313 return true; /* No threads, no one can be unhoused! */
314 }
315
316 /* Our strategy for this works as follows:
317 *
318 * First, we do one pass to check and see if all threads have a home
319 * after the migration to the new scheduler_static. If some threads
320 * do NOT have a home, then we abort the switch, and bail.
321 *
322 * Otherwise, we will keep the threads that do not need to be
323 * migrated on the list, and for every thread that needs to be
324 * migrated, we will take it off and away onto another CPU.
325 *
326 */
327
328 struct thread *iter, *tmp;
329 list_for_each_entry_safe(iter, tmp, thread_list, rt_list_node) {
330 /* Yikes, it needs to be migrated off and can go nowhere */
331 if (needs_migrate(rts: st, t: iter) && !has_migration_target(t: iter))
332 return false;
333 }
334
335 /* OK - now we have verified that everyone has somewhere to go */
336 list_for_each_entry_safe(iter, tmp, thread_list, rt_list_node) {
337 /* Keep it */
338 if (!needs_migrate(rts: st, t: iter))
339 continue;
340
341 /* Bye bye */
342 list_del_init(entry: &iter->rt_list_node);
343 send_to_compatible_cpu(t: iter);
344 }
345
346 return true;
347}
348
349static void re_enqueue_threads(struct rt_scheduler *rts,
350 struct list_head *threads) {
351 struct thread *iter, *tmp;
352
353 list_for_each_entry_safe(iter, tmp, threads, rt_list_node) {
354 list_del_init(entry: &iter->rt_list_node);
355 enum irql irql = spin_lock_irq_disable(&rts->lock);
356
357 /* TODO: remember to wrap these! */
358 rts->mapping_source->static_bptr->ops.add_thread(rts, iter);
359
360 spin_unlock(&rts->lock, irql);
361 }
362}
363
364static void setup_new_rt_scheduler(struct rt_scheduler *rts,
365 struct rt_scheduler_mapping *rtm) {
366 kassert(rts->thread_count == 0);
367 kassert(!rts->failed_internal);
368 rts->mapping_source = rtm;
369 log_trace(rts->log_site, &rts->log_handle, "rts %p setting up by %zu", rts,
370 smp_id(TOPC_IRQL));
371}
372
373/* On NUMA systems, we'll iterate to the next closest node
374 * if we can't find a struct rt_scheduler for our system.
375 *
376 * Otherwise, we just scan from 0 to max
377 *
378 * If we fail to find a struct rt_scheduler, something
379 * has gone very very very wrong... */
380static struct rt_scheduler *get_new_rt_scheduler(domain_id_t domain) {
381 struct list_head *got = NULL;
382 if ((got = locked_list_pop_front(ll: &rt_global.sch_pool[domain])))
383 goto out;
384
385 if (global.numa_node_count > 1) {
386 struct numa_node *node = &global.numa_nodes[domain];
387 for (size_t i = 0; i < global.numa_node_count; i++) {
388 uint8_t next = node->nodes_by_distance[i];
389 if ((got = locked_list_pop_front(ll: &rt_global.sch_pool[next])))
390 goto out;
391 }
392 } else {
393 for (size_t i = 0; i < global.domain_count; i++) {
394 if ((got = locked_list_pop_front(ll: &rt_global.sch_pool[i])))
395 goto out;
396 }
397 }
398
399out:
400 kassert(got);
401 return container_of(got, struct rt_scheduler, list);
402}
403
404static inline bool check_active(struct rt_scheduler_mapping *rtm) {
405 return cpu_mask_test_atomic(m: &rtm->active, cpu: smp_id(cond: TOPC_IRQL));
406}
407
408static inline void mark_active(struct rt_scheduler_mapping *rtm) {
409 cpu_mask_set_atomic(m: &rtm->active, cpu: smp_id(cond: TOPC_IRQL));
410}
411
412static inline void unmark_active(struct rt_scheduler_mapping *rtm) {
413 cpu_mask_clear_atomic(m: &rtm->active, cpu: smp_id(cond: TOPC_IRQL));
414}
415
416static inline void clear_switch_and_post(struct rt_scheduler_percpu *rts,
417 enum rt_scheduler_error err) {
418 atomic_store_explicit(&rts->switch_code, err, memory_order_release);
419 atomic_store_explicit(&rts->switch_into, NULL, memory_order_release);
420 semaphore_post(s: &rts->switch_semaphore);
421}
422
423void rt_scheduler_switch() {
424 struct rt_scheduler_percpu *pcpu = smp_core_scheduler()->rt;
425 struct rt_scheduler_static *into =
426 atomic_load_explicit(&pcpu->switch_into, memory_order_acquire);
427
428 /* Nothing to do */
429 if (!into)
430 return;
431
432 if (!rt_scheduler_static_get(obj: into))
433 return clear_switch_and_post(rts: pcpu, err: RT_SCHEDULER_ERR_NOT_FOUND);
434
435 struct rt_scheduler_static *from = pcpu->active_mapping->static_bptr;
436 enum irql girql = spin_lock_irq_disable(&rt_global.switch_lock);
437 enum rt_scheduler_error err = RT_SCHEDULER_ERR_OK;
438 bool put_into = false;
439
440 struct rt_scheduler_mapping *curr = pcpu->active_mapping;
441 struct rt_scheduler_mapping *next =
442 rt_lookup_mapping(rts: into, c: smp_core(cond: TOPC_IRQL));
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_id(TOPC_IRQL), 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_id(TOPC_IRQL));
453
454 clear_switch_and_post(rts: pcpu, err);
455 spin_unlock(&rt_global.switch_lock, 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_id(TOPC_IRQL)));
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(c: TOPC_IRQL)],
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(c: TOPC_IRQL));
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(&rt_global.switch_lock, 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