1
#include "internal.h"
2
3
void idle_entry(UNUSED void *arg) {
4
for (;;) {
5
if (sched_cpu_id() == 0) {
6
sched_reap();
7
}8
9
arch_cpu_wait();
10
}11
}12
13
static uintptr_t build_initial_stack(sched_thread_t *thread) {
14
return arch_build_kernel_stack(thread, (uintptr_t)thread_trampoline);
15
}16
17
static uintptr_t build_user_stack(sched_thread_t *thread, uintptr_t entry, uintptr_t user_stack_top) {
18
uintptr_t sp = (uintptr_t)thread->stack + thread->stack_size;
19
sp = ALIGN_DOWN(sp, 16);
20
21
sp -= sizeof(arch_int_state_t);
22
arch_int_state_t *frame = (arch_int_state_t *)sp;
23
memset(frame, 0, sizeof(*frame));
24
25
arch_word_t user_sp = (arch_word_t)ALIGN_DOWN(user_stack_top, 16);
26
arch_state_set_user_entry(frame, (arch_word_t)entry, user_sp);
27
28
return sp;
29
}30
31
static uintptr_t build_fork_stack(sched_thread_t *thread, arch_int_state_t *state) {
32
if (!thread || !state) {
33
return 0;
34
}35
36
uintptr_t sp = (uintptr_t)thread->stack + thread->stack_size;
37
sp = ALIGN_DOWN(sp, 16);
38
39
sp -= sizeof(*state);
40
memcpy((void *)sp, state, sizeof(*state));
41
42
arch_int_state_t *child_state = (arch_int_state_t *)sp;
43
44
arch_state_set_return(child_state, 0);
45
46
return sp;
47
}48
49
static pid_t _fork_fail(const char *reason, int error) {
50
if (reason && reason[0]) {
51
sched_thread_t *thread = sched_local_current();
52
log_warn(
53
"fork failed for pid=%ld (%s): %s",
54
thread ? (long)thread->pid : 0L,
55
thread ? thread->name : "unknown",
56
reason
57
);
58
}59
60
return error > 0 ? -error : -ENOMEM;
61
}62
63
void thread_prepare_user(sched_thread_t *thread, uintptr_t entry, uintptr_t user_stack_top) {
64
if (!thread) {
65
return;
66
}67
68
thread->context = build_user_stack(thread, entry, user_stack_top);
69
}70
71
bool wake_cpu(size_t cpu_id) {
72
if (cpu_id >= MAX_CORES) {
73
return false;
74
}75
76
if (!sched_mark_need_resched_cpu(cpu_id)) {
77
return false;
78
}79
80
if (cpu_id == sched_cpu_id()) {
81
return false;
82
}83
84
if (!cpu_needs_ipi(cpu_id)) {
85
return false;
86
}87
88
if (!arch_resched_cpu(cpu_id)) {
89
return false;
90
}91
92
return true;
93
}94
95
static size_t sched_wake_cpu_count(void) {
96
size_t ncpu = core_count;
97
98
if (ncpu > MAX_CORES) {
99
ncpu = MAX_CORES;
100
}101
if (ncpu > 64) {
102
ncpu = 64;
103
}104
if (!ncpu) {
105
ncpu = 1;
106
}107
108
return ncpu;
109
}110
111
static u64 sched_wake_allowed_mask(const sched_thread_t *thread, u64 online) {
112
u64 allowed = thread ? thread->allowed_cpu_mask : 0;
113
114
if (!allowed) {
115
allowed = online;
116
}117
118
allowed &= online;
119
if (!allowed) {
120
allowed = online ? online : 1ULL;
121
}122
123
return allowed;
124
}125
126
static size_t first_cpu_in_mask(u64 mask, size_t ncpu) {
127
for (size_t cpu = 0; cpu < ncpu; cpu++) {
128
if (mask & (1ULL << cpu)) {
129
return cpu;
130
}131
}132
133
return 0;
134
}135
136
static size_t pick_idle_cpu(u64 idle_mask, size_t base_cpu, size_t ncpu) {
137
size_t best_cpu = MAX_CORES;
138
size_t best_distance = (size_t)-1;
139
140
for (size_t cpu = 0; cpu < ncpu; cpu++) {
141
if (!(idle_mask & (1ULL << cpu))) {
142
continue;
143
}144
145
size_t distance = sched_cpu_distance(base_cpu, cpu, ncpu);
146
bool better = best_cpu >= MAX_CORES || distance < best_distance ||
147
(distance == best_distance && cpu < best_cpu);
148
149
if (better) {
150
best_cpu = cpu;
151
best_distance = distance;
152
}153
}154
155
return best_cpu;
156
}157
158
static size_t pick_min_cpu(u64 min_mask, size_t ncpu) {
159
u32 start = __atomic_fetch_add(&sched_state.cpus.wake_rr_cursor, 1, __ATOMIC_RELAXED);
160
161
for (size_t step = 0; step < ncpu; step++) {
162
size_t cpu = (start + step) % ncpu;
163
164
if (min_mask & (1ULL << cpu)) {
165
return cpu;
166
}167
}168
169
return 0;
170
}171
172
static bool wake_can_prefer(const sched_thread_t *thread, size_t ncpu, u64 allowed) {
173
if (!thread) {
174
return false;
175
}176
177
if (thread->last_cpu >= ncpu) {
178
return false;
179
}180
181
return allowed & (1ULL << thread->last_cpu);
182
}183
184
static size_t sched_pick_target_cpu(const sched_thread_t *thread) {
185
size_t ncpu = sched_wake_cpu_count();
186
u64 online = sched_online_cpu_mask();
187
u64 allowed = sched_wake_allowed_mask(thread, online);
188
189
size_t min_load = (size_t)-1;
190
bool found = false;
191
u64 idle_mask = 0;
192
u64 min_mask = 0;
193
194
for (size_t cpu = 0; cpu < ncpu; cpu++) {
195
if (!(allowed & (1ULL << cpu))) {
196
continue;
197
}198
199
size_t load = sched_cpu_load(cpu);
200
if (!load) {
201
idle_mask |= (1ULL << cpu);
202
}203
204
if (!found || load < min_load) {
205
min_load = load;
206
min_mask = (1ULL << cpu);
207
found = true;
208
} else if (load == min_load) {
209
min_mask |= (1ULL << cpu);
210
}211
}212
213
if (!found) {
214
return 0;
215
}216
217
size_t preferred_cpu = MAX_CORES;
218
219
if (wake_can_prefer(thread, ncpu, allowed)) {
220
preferred_cpu = thread->last_cpu;
221
}222
223
if (idle_mask) {
224
size_t base_cpu = preferred_cpu;
225
226
if (base_cpu >= ncpu || !(allowed & (1ULL << base_cpu))) {
227
base_cpu = first_cpu_in_mask(min_mask, ncpu);
228
}229
230
size_t best_idle = pick_idle_cpu(idle_mask, base_cpu, ncpu);
231
if (best_idle < MAX_CORES) {
232
return best_idle;
233
}234
}235
236
if (preferred_cpu < ncpu) {
237
size_t preferred_load = sched_cpu_load(preferred_cpu);
238
239
if (preferred_load <= min_load + SCHED_WAKE_LOAD_SLOP) {
240
return preferred_cpu;
241
}242
}243
244
return min_mask ? pick_min_cpu(min_mask, ncpu) : 0;
245
}246
247
void enqueue_ipi(sched_thread_t *thread, bool allow_remote_ipi) {
248
if (!thread || thread == sched_local_idle()) {
249
return;
250
}251
252
if (!thread->context) {
253
return;
254
}255
256
if (thread_get_state(thread) != THREAD_READY) {
257
return;
258
}259
260
if (thread->pid == 0) {
261
return;
262
}263
264
size_t target_cpu = sched_pick_target_cpu(thread);
265
size_t prev_cpu = thread->last_cpu;
266
267
if (thread->on_rq && prev_cpu != target_cpu) {
268
rq_remove_thread(thread);
269
}270
271
rq_enqueue_cpu(thread, target_cpu);
272
273
if (prev_cpu != target_cpu) {
274
__atomic_fetch_add(&sched_state.metrics.migrations, 1, __ATOMIC_RELAXED);
275
}276
277
size_t self_cpu = sched_cpu_id();
278
279
if (target_cpu != self_cpu) {
280
if (allow_remote_ipi && wake_cpu(target_cpu)) {
281
__atomic_fetch_add(&sched_state.metrics.wake_ipi, 1, __ATOMIC_RELAXED);
282
} else {
283
sched_set_need_resched_cpu(target_cpu, true);
284
}285
} else {
286
sched_request_resched_local();
287
}288
}289
290
void enqueue_thread(sched_thread_t *thread) {
291
enqueue_ipi(thread, true);
292
}293
294
void rq_remove(sched_thread_t *thread) {
295
if (!thread) {
296
return;
297
}298
299
rq_remove_thread(thread);
300
}301
302
sched_thread_t *create_thread(
303
const char *name,
304
thread_entry_t entry,
305
void *arg,
306
bool enqueue,
307
bool user_thread,
308
sched_pid_class_t pid_class
309
) {
310
sched_thread_t *thread = calloc(1, sizeof(*thread));
311
if (!thread) {
312
log_warn("failed to allocate scheduler thread object");
313
return NULL;
314
}315
316
thread_set_name(thread, name);
317
318
thread->entry = entry;
319
thread->arg = arg;
320
thread_set_state(thread, THREAD_READY);
321
thread->affinity_core = MAX_CORES;
322
thread->last_cpu = sched_cpu_id();
323
thread->allowed_cpu_mask = sched_online_cpu_mask();
324
thread->affinity_user_set = false;
325
thread->vruntime_ns = 0;
326
thread->exec_start_ns = 0;
327
thread->sum_exec_ns = 0;
328
thread->refcount = 1;
329
thread->lifecycle_flags = 0;
330
thread->user_thread = user_thread;
331
thread->pid = sched_next_pid(pid_class);
332
thread->ppid = 0;
333
thread->rq_index = UINT32_MAX;
334
thread_set_cpu(thread, -1);
335
336
sched_thread_t *parent = sched_local_current();
337
338
if (user_thread) {
339
if (parent && parent->user_thread && parent->pid > 0) {
340
thread->pgid = parent->pgid;
341
thread->sid = parent->sid;
342
thread->allowed_cpu_mask = parent->allowed_cpu_mask;
343
thread->affinity_user_set = parent->affinity_user_set;
344
thread->vruntime_ns = parent->vruntime_ns;
345
} else {
346
thread->pgid = thread->pid;
347
thread->sid = thread->pid;
348
}349
} else {
350
thread->pgid = 0;
351
thread->sid = 0;
352
}353
354
thread->uid = parent ? parent->uid : 0;
355
thread->gid = parent ? parent->gid : 0;
356
thread->group_count = parent ? parent->group_count : 0;
357
if (parent && parent->group_count) {
358
memcpy(thread->groups, parent->groups, sizeof(thread->groups));
359
}360
thread->umask = parent ? parent->umask : 0022;
361
thread->stack_size = SCHED_STACK_SIZE;
362
thread->tty_index = parent ? parent->tty_index : -1;
363
thread->sleep_queued = false;
364
thread->sleep_index = 0;
365
thread->wait_deadline_tick = 0;
366
thread->wait_flags = 0;
367
thread->wait_result = (u8)SCHED_WAIT_ABORTED;
368
thread->wait_cookie = 0;
369
370
if (!arch_kernel_stack_alloc(thread)) {
371
log_warn("failed to allocate scheduler thread stack");
372
free(thread);
373
return NULL;
374
}375
376
thread->cwd[0] = '/';
377
thread->cwd[1] = '\0';
378
379
if (!user_thread) {
380
thread->context = build_initial_stack(thread);
381
}382
383
if (user_thread) {
384
thread->vm_space = arch_vm_create_user();
385
if (!thread->vm_space) {
386
log_warn("failed to allocate user VM space");
387
arch_kernel_stack_free(thread);
388
free(thread);
389
return NULL;
390
}391
} else {
392
thread->vm_space = sched_state.core.kernel_vm;
393
}394
395
spinlock_init(&thread->vm_lock);
396
397
sched_wait_queue_init(&thread->wait_queue);
398
sched_signal_init_thread(thread);
399
400
arch_fpu_init(thread->fpu_state);
401
thread->fpu_initialized = true;
402
403
thread_add(thread);
404
405
if (thread->pid > 0) {
406
procfs_register_pid(thread->pid);
407
}408
409
if (enqueue) {
410
unsigned long flags = sched_lock_save();
411
enqueue_thread(thread);
412
sched_lock_restore(flags);
413
}414
415
return thread;
416
}417
418
sched_thread_t *sched_current(void) {
419
return sched_local_current();
420
}421
422
sched_thread_t *sched_current_core(size_t core_id) {
423
if (core_id >= MAX_CORES) {
424
return NULL;
425
}426
427
return __atomic_load_n(&sched_state.cpus.cpu[core_id].current, __ATOMIC_ACQUIRE);
428
}429
430
sched_thread_t *sched_find_thread(pid_t pid) {
431
unsigned long flags = sched_lock_save();
432
sched_thread_t *thread = find_thread(pid);
433
434
if (thread && thread->in_all_list && thread->pid == pid) {
435
thread_get(thread);
436
} else {
437
thread = NULL;
438
}439
440
sched_lock_restore(flags);
441
442
return thread;
443
}444
445
sched_thread_t *sched_create_kernel_thread(const char *name, thread_entry_t entry, void *arg) {
446
return create_thread(name, entry, arg, true, false, SCHED_PID_KERNEL);
447
}448
449
sched_thread_t *sched_create_user_thread(const char *name) {
450
return create_thread(name, NULL, NULL, false, true, SCHED_PID_USER);
451
}452
453
static void copy_fork_state(sched_thread_t *child, sched_thread_t *parent) {
454
child->ppid = parent->pid;
455
child->pgid = parent->pgid;
456
child->sid = parent->sid;
457
child->umask = parent->umask;
458
child->user_stack_base = parent->user_stack_base;
459
child->user_stack_size = parent->user_stack_size;
460
461
memcpy(child->cwd, parent->cwd, sizeof(parent->cwd));
462
memcpy(child->signal_handlers, parent->signal_handlers, sizeof(child->signal_handlers));
463
464
child->signal_mask = parent->signal_mask;
465
child->signal_trampoline = parent->signal_trampoline;
466
child->signal_pending = 0;
467
__atomic_store_n(&child->signal_saved_valid, 0, __ATOMIC_RELEASE);
468
__atomic_store_n(&child->current_signal, 0, __ATOMIC_RELEASE);
469
child->tty_index = parent->tty_index;
470
471
if (parent->fpu_initialized) {
472
memcpy(child->fpu_state, parent->fpu_state, sizeof(child->fpu_state));
473
child->fpu_initialized = true;
474
}475
}476
477
pid_t sched_fork(arch_int_state_t *state) {
478
sched_thread_t *parent = sched_local_current();
479
480
if (!parent || !parent->user_thread || !state) {
481
return _fork_fail("invalid parent or missing trap state", EINVAL);
482
}483
484
sched_thread_t *child = sched_create_user_thread(parent->name);
485
if (!child) {
486
return _fork_fail("failed to create child thread", ENOMEM);
487
}488
489
copy_fork_state(child, parent);
490
491
if (!sched_fd_clone_table(child, parent)) {
492
sched_discard_thread(child);
493
return _fork_fail("failed to clone file descriptor table", ENOMEM);
494
}495
496
bool cow_enabled = pmm_ref_ready();
497
498
bool parent_tlb_needs_flush = false;
499
unsigned long vm_flags = spin_lock_irqsave(&parent->vm_lock);
500
501
sched_user_region_t *region = parent->regions;
502
503
while (region) {
504
size_t pages = region->pages;
505
void *root = arch_vm_root(child->vm_space);
506
507
if (!root) {
508
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
509
sched_discard_thread(child);
510
return _fork_fail("child VM space missing root page table", ENOMEM);
511
}512
513
if (!cow_enabled) {
514
bool region_overflows =
515
(pages > (uintptr_t)-1 / PAGE_4KIB || region->vaddr > (uintptr_t)-1 - (pages * PAGE_4KIB));
516
517
if (region_overflows) {
518
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
519
sched_discard_thread(child);
520
return _fork_fail("copied user region is out of range", ENOMEM);
521
}522
523
size_t size = pages * PAGE_4KIB;
524
uintptr_t new_paddr = (uintptr_t)arch_alloc_frames_user(pages);
525
if (!new_paddr) {
526
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
527
sched_discard_thread(child);
528
return _fork_fail("failed to allocate copied user pages", ENOMEM);
529
}530
531
arch_map_region(root, pages, region->vaddr, new_paddr, region->flags);
532
if (!sched_add_user_region(child, region->vaddr, new_paddr, pages, region->flags)) {
533
for (size_t i = 0; i < pages; i++) {
534
uintptr_t vaddr = region->vaddr + i * PAGE_4KIB;
535
unmap_page((page_t *)root, vaddr);
536
arch_tlb_flush(vaddr);
537
}538
539
arch_free_frames((void *)new_paddr, pages);
540
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
541
sched_discard_thread(child);
542
return _fork_fail("failed to record copied user region", ENOMEM);
543
}544
545
void *dst = arch_phys_map(new_paddr, size, 0);
546
if (!dst) {
547
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
548
sched_discard_thread(child);
549
return _fork_fail("failed to map copied user pages", ENOMEM);
550
}551
552
memcpy(dst, (void *)region->vaddr, size);
553
arch_phys_unmap(dst, size);
554
555
region = region->next;
556
continue;
557
}558
559
u64 region_flags = region->flags;
560
bool writable = (region_flags & PT_WRITE) != 0;
561
562
if (writable) {
563
region_flags |= SCHED_REGION_COW;
564
}565
566
region->flags = region_flags;
567
568
u64 map_flags = region_flags;
569
if (writable) {
570
map_flags &= ~PT_WRITE;
571
}572
573
arch_map_region(root, pages, region->vaddr, region->paddr, map_flags);
574
if (!sched_add_user_region(child, region->vaddr, region->paddr, pages, region_flags)) {
575
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
576
sched_discard_thread(child);
577
return _fork_fail("failed to record COW user region", ENOMEM);
578
}579
580
pmm_ref_hold((void *)(uintptr_t)region->paddr, pages);
581
582
if (writable) {
583
if (sched_user_region_mark_cow(parent, region)) {
584
parent_tlb_needs_flush = true;
585
}586
}587
588
region = region->next;
589
}590
591
if (parent_tlb_needs_flush) {
592
arch_vm_switch(parent->vm_space);
593
}594
595
spin_unlock_irqrestore(&parent->vm_lock, vm_flags);
596
597
child->context = build_fork_stack(child, state);
598
599
// let freshly forked children compete at current rq baseline instead of
600
// inheriting a potentially stale/high vruntime from interactive parents
601
child->vruntime_ns = 0;
602
603
enqueue_thread(child);
604
605
return child->pid;
606
}