1
#include "internal.h"
2
3
static inline bool rq_less(const sched_thread_t *a, const sched_thread_t *b) {
4
if (!a) {
5
return false;
6
}7
8
if (!b) {
9
return true;
10
}11
12
if (a->vruntime_ns != b->vruntime_ns) {
13
return a->vruntime_ns < b->vruntime_ns;
14
}15
16
if (a->sum_exec_ns != b->sum_exec_ns) {
17
return a->sum_exec_ns < b->sum_exec_ns;
18
}19
20
return a->pid < b->pid;
21
}22
23
static bool rq_runnable(const sched_thread_t *thread) {
24
if (!thread || thread_get_state(thread) != THREAD_READY) {
25
return false;
26
}27
28
return thread->context && thread->pid != 0;
29
}30
31
static bool rq_worse_than(const sched_thread_t *thread, const sched_thread_t *best) {
32
if (!best) {
33
return true;
34
}35
36
if (thread->vruntime_ns != best->vruntime_ns) {
37
return thread->vruntime_ns > best->vruntime_ns;
38
}39
40
return thread->pid > best->pid;
41
}42
43
static inline void rq_swap(sched_rq_t *rq, u32 left, u32 right) {
44
sched_thread_t *tmp = rq->heap[left];
45
rq->heap[left] = rq->heap[right];
46
rq->heap[right] = tmp;
47
48
if (rq->heap[left]) {
49
rq->heap[left]->rq_index = left;
50
}51
52
if (rq->heap[right]) {
53
rq->heap[right]->rq_index = right;
54
}55
}56
57
static void rq_sift_up(sched_rq_t *rq, u32 index) {
58
while (index > 0) {
59
u32 parent = (index - 1U) / 2U;
60
61
if (!rq_less(rq->heap[index], rq->heap[parent])) {
62
break;
63
}64
65
rq_swap(rq, index, parent);
66
index = parent;
67
}68
}69
70
static void rq_sift_down(sched_rq_t *rq, u32 index) {
71
for (;;) {
72
u32 left = index * 2U + 1U;
73
u32 right = left + 1U;
74
u32 best = index;
75
76
if ((size_t)left < rq->nr_running && rq_less(rq->heap[left], rq->heap[best])) {
77
best = left;
78
}79
80
if ((size_t)right < rq->nr_running && rq_less(rq->heap[right], rq->heap[best])) {
81
best = right;
82
}83
84
if (best == index) {
85
return;
86
}87
88
rq_swap(rq, index, best);
89
index = best;
90
}91
}92
93
static bool rq_insert(sched_rq_t *rq, sched_thread_t *thread) {
94
if (!rq || !thread || !rq->heap || rq->nr_running >= rq->capacity) {
95
return false;
96
}97
98
u32 index = (u32)rq->nr_running;
99
100
rq->heap[index] = thread;
101
thread->rq_index = index;
102
rq->nr_running++;
103
104
rq_sift_up(rq, index);
105
106
return true;
107
}108
109
static int rq_find_index(const sched_rq_t *rq, const sched_thread_t *thread) {
110
if (!rq || !thread || !rq->heap) {
111
return -1;
112
}113
114
for (size_t i = 0; i < rq->nr_running; i++) {
115
if (rq->heap[i] == thread) {
116
return (int)i;
117
}118
}119
120
return -1;
121
}122
123
bool rq_remove_index(sched_rq_t *rq, u32 index) {
124
if (!rq || !rq->heap || (size_t)index >= rq->nr_running) {
125
return false;
126
}127
128
size_t last = rq->nr_running - 1U;
129
130
sched_thread_t *removed = rq->heap[index];
131
if (!removed) {
132
return false;
133
}134
135
if ((size_t)index != last) {
136
rq->heap[index] = rq->heap[last];
137
138
if (rq->heap[index]) {
139
rq->heap[index]->rq_index = index;
140
}141
}142
143
rq->heap[last] = NULL;
144
rq->nr_running = last;
145
146
removed->rq_index = UINT32_MAX;
147
removed->on_rq = false;
148
removed->in_run_queue = false;
149
150
if ((size_t)index < rq->nr_running) {
151
if (index > 0U) {
152
u32 parent = (index - 1U) / 2U;
153
154
if (rq_less(rq->heap[index], rq->heap[parent])) {
155
rq_sift_up(rq, index);
156
157
if (rq->nr_running > 0 && rq->heap[0]) {
158
rq->min_vruntime = rq->heap[0]->vruntime_ns;
159
}160
161
return true;
162
}163
}164
165
rq_sift_down(rq, index);
166
}167
168
if (rq->nr_running > 0 && rq->heap[0]) {
169
rq->min_vruntime = rq->heap[0]->vruntime_ns;
170
}171
172
return true;
173
}174
175
void rq_enqueue_cpu(sched_thread_t *thread, size_t cpu_id) {
176
if (!thread || cpu_id >= MAX_CORES) {
177
return;
178
}179
180
if (!sched_cpu_allowed(thread, cpu_id) || !cores_local[cpu_id].online) {
181
size_t allowed_cpu = pick_cpu(thread, cpu_id);
182
bool bad_cpu =
183
(allowed_cpu >= MAX_CORES || !cores_local[allowed_cpu].online || !sched_cpu_allowed(thread, allowed_cpu));
184
185
if (bad_cpu) {
186
return;
187
}188
189
cpu_id = allowed_cpu;
190
}191
192
if (!rq_runnable(thread)) {
193
return;
194
}195
196
if (thread->on_rq && thread->last_cpu != cpu_id) {
197
(void)rq_remove_thread(thread);
198
}199
200
if (thread_cpu(thread) >= 0) {
201
bool running_elsewhere = (thread_is_owned(thread) || thread_get_state(thread) == THREAD_RUNNING);
202
203
if (running_elsewhere) {
204
return;
205
}206
207
// recover stale running_cpu metadata only for non running states
208
thread_set_cpu(thread, -1);
209
}210
211
sched_rq_t *rq = &sched_state.cpus.runqueues[cpu_id];
212
unsigned long flags = spin_lock_irqsave(&rq->lock);
213
214
if (thread->on_rq || thread->rq_index != UINT32_MAX) {
215
bool already_here =
216
(thread->last_cpu == cpu_id && thread->rq_index < rq->nr_running && rq->heap[thread->rq_index] == thread);
217
218
if (already_here) {
219
spin_unlock_irqrestore(&rq->lock, flags);
220
return;
221
}222
223
int found = rq_find_index(rq, thread);
224
225
if (found >= 0) {
226
thread->rq_index = (u32)found;
227
thread->on_rq = true;
228
thread->in_run_queue = true;
229
thread->last_cpu = cpu_id;
230
spin_unlock_irqrestore(&rq->lock, flags);
231
return;
232
}233
234
thread->on_rq = false;
235
thread->in_run_queue = false;
236
thread->rq_index = UINT32_MAX;
237
}238
239
if (!thread->user_thread && thread->vruntime_ns < rq->min_vruntime) {
240
thread->vruntime_ns = rq->min_vruntime;
241
}242
243
if (!rq_insert(rq, thread)) {
244
spin_unlock_irqrestore(&rq->lock, flags);
245
return;
246
}247
248
thread->on_rq = true;
249
thread->in_run_queue = true;
250
thread->last_cpu = cpu_id;
251
thread->affinity_core = cpu_id;
252
253
if (rq->nr_running > 0 && rq->heap[0]) {
254
rq->min_vruntime = rq->heap[0]->vruntime_ns;
255
}256
257
size_t depth = rq->nr_running;
258
spin_unlock_irqrestore(&rq->lock, flags);
259
260
rq_note_depth(depth);
261
}262
263
static bool rq_remove_cpu(sched_rq_t *rq, sched_thread_t *thread) {
264
if (!rq || !thread || !thread->on_rq) {
265
return false;
266
}267
268
if (!rq->heap) {
269
thread->on_rq = false;
270
thread->in_run_queue = false;
271
thread->rq_index = UINT32_MAX;
272
return false;
273
}274
275
bool stale_index =
276
(thread->rq_index == UINT32_MAX || thread->rq_index >= rq->nr_running || rq->heap[thread->rq_index] != thread);
277
278
if (stale_index) {
279
int found = rq_find_index(rq, thread);
280
281
if (found < 0) {
282
thread->on_rq = false;
283
thread->in_run_queue = false;
284
thread->rq_index = UINT32_MAX;
285
286
return false;
287
}288
289
thread->rq_index = (u32)found;
290
}291
292
return rq_remove_index(rq, thread->rq_index);
293
}294
295
bool rq_remove_thread(sched_thread_t *thread) {
296
if (!thread || !thread->on_rq) {
297
return false;
298
}299
300
sched_rq_t *rq = &sched_state.cpus.runqueues[thread->last_cpu];
301
302
unsigned long flags = spin_lock_irqsave(&rq->lock);
303
bool removed = rq_remove_cpu(rq, thread);
304
spin_unlock_irqrestore(&rq->lock, flags);
305
306
return removed;
307
}308
309
sched_thread_t *rq_pop_best_allowed(size_t cpu_id) {
310
if (cpu_id >= MAX_CORES) {
311
return NULL;
312
}313
314
sched_rq_t *rq = &sched_state.cpus.runqueues[cpu_id];
315
unsigned long flags = spin_lock_irqsave(&rq->lock);
316
317
if (rq->nr_running) {
318
sched_thread_t *root = rq->heap[0];
319
320
if (root && sched_cpu_allowed(root, cpu_id)) {
321
rq_remove_index(rq, 0);
322
spin_unlock_irqrestore(&rq->lock, flags);
323
324
return root;
325
}326
}327
328
u32 best_index = UINT32_MAX;
329
sched_thread_t *best = NULL;
330
331
for (u32 i = 0; (size_t)i < rq->nr_running; i++) {
332
sched_thread_t *thread = rq->heap[i];
333
334
if (!thread || !sched_cpu_allowed(thread, cpu_id)) {
335
continue;
336
}337
338
if (!best || rq_less(thread, best)) {
339
best = thread;
340
best_index = i;
341
}342
}343
344
if (best && best_index != UINT32_MAX) {
345
rq_remove_index(rq, best_index);
346
spin_unlock_irqrestore(&rq->lock, flags);
347
348
return best;
349
}350
351
spin_unlock_irqrestore(&rq->lock, flags);
352
return NULL;
353
}354
355
sched_thread_t *rq_peek_best(size_t cpu_id) {
356
if (cpu_id >= MAX_CORES) {
357
return NULL;
358
}359
360
sched_rq_t *rq = &sched_state.cpus.runqueues[cpu_id];
361
unsigned long flags = spin_lock_irqsave(&rq->lock);
362
sched_thread_t *thread = NULL;
363
364
if (rq->nr_running) {
365
sched_thread_t *root = rq->heap[0];
366
367
if (root && sched_cpu_allowed(root, cpu_id)) {
368
spin_unlock_irqrestore(&rq->lock, flags);
369
return root;
370
}371
}372
373
for (u32 i = 0; (size_t)i < rq->nr_running; i++) {
374
sched_thread_t *candidate = rq->heap[i];
375
376
if (!candidate || !sched_cpu_allowed(candidate, cpu_id)) {
377
continue;
378
}379
380
if (!thread || rq_less(candidate, thread)) {
381
thread = candidate;
382
}383
}384
385
spin_unlock_irqrestore(&rq->lock, flags);
386
387
return thread;
388
}389
390
sched_thread_t *rq_pop_worst_allowed_from_cpu(size_t source_cpu, size_t target_cpu) {
391
if (source_cpu >= MAX_CORES || target_cpu >= MAX_CORES) {
392
return NULL;
393
}394
395
sched_rq_t *rq = &sched_state.cpus.runqueues[source_cpu];
396
unsigned long flags = spin_lock_irqsave(&rq->lock);
397
398
sched_thread_t *candidate = NULL;
399
u32 candidate_index = UINT32_MAX;
400
401
for (u32 i = 0; (size_t)i < rq->nr_running; i++) {
402
sched_thread_t *thread = rq->heap[i];
403
404
bool wrong_cpu = (!rq_runnable(thread) || !sched_cpu_allowed(thread, target_cpu));
405
406
if (wrong_cpu) {
407
continue;
408
}409
410
if (rq_worse_than(thread, candidate)) {
411
candidate = thread;
412
candidate_index = i;
413
}414
}415
416
if (candidate && candidate_index != UINT32_MAX) {
417
rq_remove_index(rq, candidate_index);
418
} else {
419
candidate = NULL;
420
}421
422
spin_unlock_irqrestore(&rq->lock, flags);
423
424
return candidate;
425
}426
427
sched_thread_t *rq_pop_disallowed_from_cpu(size_t source_cpu, size_t disallowed_cpu) {
428
if (source_cpu >= MAX_CORES || disallowed_cpu >= MAX_CORES) {
429
return NULL;
430
}431
432
sched_rq_t *rq = &sched_state.cpus.runqueues[source_cpu];
433
unsigned long flags = spin_lock_irqsave(&rq->lock);
434
435
sched_thread_t *candidate = NULL;
436
u32 candidate_index = UINT32_MAX;
437
438
for (u32 i = 0; (size_t)i < rq->nr_running; i++) {
439
sched_thread_t *thread = rq->heap[i];
440
441
bool still_allowed = (!rq_runnable(thread) || sched_cpu_allowed(thread, disallowed_cpu));
442
443
if (still_allowed) {
444
continue;
445
}446
447
if (rq_worse_than(thread, candidate)) {
448
candidate = thread;
449
candidate_index = i;
450
}451
}452
453
if (candidate && candidate_index != UINT32_MAX) {
454
rq_remove_index(rq, candidate_index);
455
} else {
456
candidate = NULL;
457
}458
459
spin_unlock_irqrestore(&rq->lock, flags);
460
return candidate;
461
}