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