summaryrefslogtreecommitdiffstats
path: root/firmware/kernel/thread.c
blob: bbd610122b3e88306bdf327a4b9c1557fc96b269 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
1270
1271
1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282
1283
1284
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298
1299
1300
1301
1302
1303
1304
1305
1306
1307
1308
1309
1310
1311
1312
1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
1340
1341
1342
1343
1344
1345
1346
1347
1348
1349
1350
1351
1352
1353
1354
1355
1356
1357
1358
1359
1360
1361
1362
1363
1364
1365
1366
1367
1368
1369
1370
1371
1372
1373
1374
1375
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388
1389
1390
1391
1392
1393
1394
1395
1396
1397
1398
1399
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412
1413
1414
1415
1416
1417
1418
1419
1420
1421
1422
1423
1424
1425
1426
1427
1428
1429
1430
1431
1432
1433
1434
1435
1436
1437
1438
1439
1440
1441
1442
1443
1444
1445
1446
1447
1448
1449
1450
1451
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461
1462
1463
1464
1465
1466
1467
1468
1469
1470
1471
1472
1473
1474
1475
1476
1477
1478
1479
1480
1481
1482
1483
1484
1485
1486
1487
1488
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498
1499
1500
1501
1502
1503
1504
1505
1506
1507
1508
1509
1510
1511
1512
1513
1514
1515
1516
1517
1518
1519
1520
1521
1522
1523
1524
1525
1526
1527
1528
1529
1530
1531
1532
1533
1534
1535
1536
1537
1538
1539
1540
1541
1542
1543
1544
1545
1546
1547
1548
1549
1550
1551
1552
1553
1554
1555
1556
1557
1558
1559
1560
1561
1562
1563
1564
1565
1566
1567
1568
1569
1570
1571
1572
1573
1574
1575
1576
1577
1578
1579
1580
1581
1582
1583
1584
1585
1586
1587
1588
1589
1590
1591
1592
1593
1594
/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 * $Id$
 *
 * Copyright (C) 2002 by Ulf Ralberg
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/
#include "config.h"

#ifdef HAVE_SIGALTSTACK_THREADS
/*
 * The sp check in glibc __longjmp_chk() will cause
 * a fatal error when switching threads via longjmp().
 */
#undef _FORTIFY_SOURCE
#endif

#include "thread-internal.h"
#include "kernel.h"
#include "cpu.h"
#include "string.h"
#ifdef RB_PROFILE
#include <profile.h>
#endif
#include "core_alloc.h"

/* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */
#ifdef DEBUG
#define THREAD_EXTRA_CHECKS 1 /* Always 1 for DEBUG */
#else
#define THREAD_EXTRA_CHECKS 0
#endif

/****************************************************************************
 *                              ATTENTION!!                                 *
 *    See notes below on implementing processor-specific portions!          *
 ****************************************************************************
 *
 * General locking order to guarantee progress. Order must be observed but
 * all stages are not nescessarily obligatory. Going from 1) to 3) is
 * perfectly legal.
 *
 * 1) IRQ
 * This is first because of the likelyhood of having an interrupt occur that
 * also accesses one of the objects farther down the list. Any non-blocking
 * synchronization done may already have a lock on something during normal
 * execution and if an interrupt handler running on the same processor as
 * the one that has the resource locked were to attempt to access the
 * resource, the interrupt handler would wait forever waiting for an unlock
 * that will never happen. There is no danger if the interrupt occurs on
 * a different processor because the one that has the lock will eventually
 * unlock and the other processor's handler may proceed at that time. Not
 * nescessary when the resource in question is definitely not available to
 * interrupt handlers.
 *
 * 2) Kernel Object
 * 1) May be needed beforehand if the kernel object allows dual-use such as
 * event queues. The kernel object must have a scheme to protect itself from
 * access by another processor and is responsible for serializing the calls
 * to block_thread  and wakeup_thread both to themselves and to each other.
 * Objects' queues are also protected here.
 *
 * 3) Thread Slot
 * This locks access to the thread's slot such that its state cannot be
 * altered by another processor when a state change is in progress such as
 * when it is in the process of going on a blocked list. An attempt to wake
 * a thread while it is still blocking will likely desync its state with
 * the other resources used for that state.
 *
 * 4) Core Lists
 * These lists are specific to a particular processor core and are accessible
 * by all processor cores and interrupt handlers. The running (rtr) list is
 * the prime example where a thread may be added by any means.
 */

/*---------------------------------------------------------------------------
 * Processor specific: core_sleep/core_wake/misc. notes
 *
 * ARM notes:
 * FIQ is not dealt with by the scheduler code and is simply restored if it
 * must by masked for some reason - because threading modifies a register
 * that FIQ may also modify and there's no way to accomplish it atomically.
 * s3c2440 is such a case.
 *
 * Audio interrupts are generally treated at a higher priority than others
 * usage of scheduler code with interrupts higher than HIGHEST_IRQ_LEVEL
 * are not in general safe. Special cases may be constructed on a per-
 * source basis and blocking operations are not available.
 *
 * core_sleep procedure to implement for any CPU to ensure an asychronous
 * wakup never results in requiring a wait until the next tick (up to
 * 10000uS!). May require assembly and careful instruction ordering.
 *
 * 1) On multicore, stay awake if directed to do so by another. If so, goto
 *    step 4.
 * 2) If processor requires, atomically reenable interrupts and perform step
 *    3.
 * 3) Sleep the CPU core. If wakeup itself enables interrupts (stop #0x2000
 *    on Coldfire) goto step 5.
 * 4) Enable interrupts.
 * 5) Exit procedure.
 *
 * core_wake and multprocessor notes for sleep/wake coordination:
 * If possible, to wake up another processor, the forcing of an interrupt on
 * the woken core by the waker core is the easiest way to ensure a non-
 * delayed wake and immediate execution of any woken threads. If that isn't
 * available then some careful non-blocking synchonization is needed (as on
 * PP targets at the moment).
 *---------------------------------------------------------------------------
 *
 *
 *---------------------------------------------------------------------------
 * Priority distribution structure (one category for each possible priority):
 *
 *       +----+----+----+ ... +------+
 * hist: | F0 | F1 | F2 |     | Fn-1 |
 *       +----+----+----+ ... +------+
 * mask: | b0 | b1 | b2 |     | bn-1 |
 *       +----+----+----+ ... +------+
 *
 * F = count of threads at priority category n (frequency)
 * b = bitmask of non-zero priority categories (occupancy)
 *
 *        / if H[n] != 0 : 1
 * b[n] = |
 *        \ else         : 0
 *
 *---------------------------------------------------------------------------
 * Basic priority inheritance priotocol (PIP):
 *
 * Mn = mutex n, Tn = thread n
 *
 * A lower priority thread inherits the priority of the highest priority
 * thread blocked waiting for it to complete an action (such as release a
 * mutex or respond to a message via queue_send):
 *
 * 1) T2->M1->T1
 *
 * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher
 * priority than T1 then T1 inherits the priority of T2.
 *
 * 2) T3
 *    \/
 *    T2->M1->T1
 *
 * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so
 * T1 inherits the higher of T2 and T3.
 *
 * 3) T3->M2->T2->M1->T1
 *
 * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2,
 * then T1 inherits the priority of T3 through T2.
 *
 * Blocking chains can grow arbitrarily complex (though it's best that they
 * not form at all very often :) and build-up from these units.
 *---------------------------------------------------------------------------
 */
static FORCE_INLINE void core_sleep(IF_COP_VOID(unsigned int core));
static FORCE_INLINE void store_context(void* addr);
static FORCE_INLINE void load_context(const void* addr);

/****************************************************************************
 * Processor/OS-specific section - include necessary core support
 */

#include "asm/thread.c"

#if defined (CPU_PP)
#include "thread-pp.c"
#endif /* CPU_PP */

/*
 * End Processor-specific section
 ***************************************************************************/

static NO_INLINE NORETURN_ATTR
  void thread_panicf(const char *msg, struct thread_entry *thread)
{
    IF_COP( const unsigned int core = thread->core; )
    static char name[sizeof (((struct thread_debug_info *)0)->name)];
    format_thread_name(name, sizeof (name), thread);
    panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core));
    while (1);
}

static NO_INLINE void thread_stkov(struct thread_entry *thread)
{
    thread_panicf("Stkov", thread);
}

#if THREAD_EXTRA_CHECKS
#define THREAD_PANICF(msg, thread) \
    thread_panicf(msg, thread)
#define THREAD_ASSERT(exp, msg, thread) \
    ({ if (!({ exp; })) thread_panicf((msg), (thread)); })
#else
#define THREAD_PANICF(msg, thread) \
    do {} while (1)
#define THREAD_ASSERT(exp, msg, thread) \
    do {} while (0)
#endif /* THREAD_EXTRA_CHECKS */

/* Thread locking */
#if NUM_CORES > 1
#define LOCK_THREAD(thread) \
    ({ corelock_lock(&(thread)->slot_cl); })
#define TRY_LOCK_THREAD(thread) \
    ({ corelock_try_lock(&(thread)->slot_cl); })
#define UNLOCK_THREAD(thread) \
    ({ corelock_unlock(&(thread)->slot_cl); })
#else /* NUM_CORES == 1*/
#define LOCK_THREAD(thread) \
    ({ (void)(thread); })
#define TRY_LOCK_THREAD(thread) \
    ({ (void)(thread); })
#define UNLOCK_THREAD(thread) \
    ({ (void)(thread); })
#endif /* NUM_CORES */

/* RTR list */
#define RTR_LOCK(corep) \
    corelock_lock(&(corep)->rtr_cl)
#define RTR_UNLOCK(corep) \
    corelock_unlock(&(corep)->rtr_cl)

#ifdef HAVE_PRIORITY_SCHEDULING
#define rtr_add_entry(corep, priority) \
    prio_add_entry(&(corep)->rtr_dist, (priority))
#define rtr_subtract_entry(corep, priority) \
    prio_subtract_entry(&(corep)->rtr_dist, (priority))
#define rtr_move_entry(corep, from, to) \
    prio_move_entry(&(corep)->rtr_dist, (from), (to))
#else /* !HAVE_PRIORITY_SCHEDULING */
#define rtr_add_entry(corep, priority) \
    do {} while (0)
#define rtr_subtract_entry(corep, priority) \
    do {} while (0)
#define rtr_move_entry(corep, from, to) \
    do {} while (0)
#endif /* HAVE_PRIORITY_SCHEDULING */

static FORCE_INLINE void thread_store_context(struct thread_entry *thread)
{
#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
    thread->__errno = errno;
#endif
    store_context(&thread->context);
}

static FORCE_INLINE void thread_load_context(struct thread_entry *thread)
{
    load_context(&thread->context);
#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
    errno = thread->__errno;
#endif
}

static FORCE_INLINE unsigned int
should_switch_tasks(struct thread_entry *thread)
{
#ifdef HAVE_PRIORITY_SCHEDULING
    const unsigned int core = CURRENT_CORE;
#if NUM_CORES > 1
    /* Forget about it if different CPU */
    if (thread->core != core)
        return THREAD_OK;
#endif
    /* Just woke something therefore a thread is on the run queue */
    struct thread_entry *current =
        RTR_THREAD_FIRST(&__core_id_entry(core)->rtr);
    if (LIKELY(thread->priority >= current->priority))
        return THREAD_OK;

    /* There is a thread ready to run of higher priority on the same
     * core as the current one; recommend a task switch. */
    return THREAD_OK | THREAD_SWITCH;
#else
    return THREAD_OK;
    (void)thread;
#endif /* HAVE_PRIORITY_SCHEDULING */
}

#ifdef HAVE_PRIORITY_SCHEDULING

/*---------------------------------------------------------------------------
 * Increment frequency at category "priority"
 *---------------------------------------------------------------------------
 */
static inline unsigned int prio_add_entry(
    struct priority_distribution *pd, int priority)
{
    unsigned int count = ++pd->hist[priority];
    if (count == 1)
        priobit_set_bit(&pd->mask, priority);
    return count;
}

/*---------------------------------------------------------------------------
 * Decrement frequency at category "priority"
 *---------------------------------------------------------------------------
 */
static inline unsigned int prio_subtract_entry(
    struct priority_distribution *pd, int priority)
{
    unsigned int count = --pd->hist[priority];
    if (count == 0)
        priobit_clear_bit(&pd->mask, priority);
    return count;
}

/*---------------------------------------------------------------------------
 * Remove from one category and add to another
 *---------------------------------------------------------------------------
 */
static inline void prio_move_entry(
    struct priority_distribution *pd, int from, int to)
{
    if (--pd->hist[from] == 0)
        priobit_clear_bit(&pd->mask, from);

    if (++pd->hist[to] == 1)
        priobit_set_bit(&pd->mask, to);
}

#endif /* HAVE_PRIORITY_SCHEDULING */

/*---------------------------------------------------------------------------
 * Common init for new thread basic info
 *---------------------------------------------------------------------------
 */
static void new_thread_base_init(struct thread_entry *thread,
                                 void **stackp, size_t *stack_sizep,
                                 const char *name IF_PRIO(, int priority)
                                 IF_COP(, unsigned int core))
{
    ALIGN_BUFFER(*stackp, *stack_sizep, MIN_STACK_ALIGN);
    thread->stack = *stackp;
    thread->stack_size = *stack_sizep;

    thread->name = name;
    wait_queue_init(&thread->queue);
    thread->wqp = NULL;
    tmo_set_dequeued(thread);
#ifdef HAVE_PRIORITY_SCHEDULING
    thread->skip_count    = 0;
    thread->blocker       = NULL;
    thread->base_priority = priority;
    thread->priority      = priority;
    memset(&thread->pdist, 0, sizeof(thread->pdist));
    prio_add_entry(&thread->pdist, priority);
#endif
#if NUM_CORES > 1
    thread->core = core;
#endif
#ifdef HAVE_SCHEDULER_BOOSTCTRL
    thread->cpu_boost = 0;
#endif
#ifdef HAVE_IO_PRIORITY
    /* Default to high (foreground) priority */
    thread->io_priority = IO_PRIORITY_IMMEDIATE;
#endif
}

/*---------------------------------------------------------------------------
 * Move a thread onto the core's run queue and promote it
 *---------------------------------------------------------------------------
 */
static inline void core_rtr_add(struct core_entry *corep,
                                struct thread_entry *thread)
{
    RTR_LOCK(corep);
    rtr_queue_add(&corep->rtr, thread);
    rtr_add_entry(corep, thread->priority);
#ifdef HAVE_PRIORITY_SCHEDULING
    thread->skip_count = thread->base_priority;
#endif
    thread->state = STATE_RUNNING;
    RTR_UNLOCK(corep);
}

/*---------------------------------------------------------------------------
 * Remove a thread from the core's run queue
 *---------------------------------------------------------------------------
 */
static inline void core_rtr_remove(struct core_entry *corep,
                                   struct thread_entry *thread)
{
    RTR_LOCK(corep);
    rtr_queue_remove(&corep->rtr, thread);
    rtr_subtract_entry(corep, thread->priority);
    /* Does not demote state */
    RTR_UNLOCK(corep);
}

/*---------------------------------------------------------------------------
 * Move a thread back to a running state on its core
 *---------------------------------------------------------------------------
 */
static NO_INLINE void core_schedule_wakeup(struct thread_entry *thread)
{
    const unsigned int core = IF_COP_CORE(thread->core);
    struct core_entry *corep = __core_id_entry(core);
    core_rtr_add(corep, thread);
#if NUM_CORES > 1
    if (core != CURRENT_CORE)
        core_wake(core);
#endif
}

#ifdef HAVE_PRIORITY_SCHEDULING
/*---------------------------------------------------------------------------
 * Locks the thread registered as the owner of the block and makes sure it
 * didn't change in the meantime
 *---------------------------------------------------------------------------
 */
#if NUM_CORES == 1
static inline struct thread_entry * lock_blocker_thread(struct blocker *bl)
{
    return bl->thread;
}
#else /* NUM_CORES > 1 */
static struct thread_entry * lock_blocker_thread(struct blocker *bl)
{
    /* The blocker thread may change during the process of trying to
       capture it */
    while (1)
    {
        struct thread_entry *t = bl->thread;

        /* TRY, or else deadlocks are possible */
        if (!t)
        {
            struct blocker_splay *blsplay = (struct blocker_splay *)bl;
            if (corelock_try_lock(&blsplay->cl))
            {
                if (!bl->thread)
                    return NULL;    /* Still multi */

                corelock_unlock(&blsplay->cl);
            }
        }
        else
        {
            if (TRY_LOCK_THREAD(t))
            {
                if (bl->thread == t)
                    return t;

                UNLOCK_THREAD(t);
            }
        }
    }
}
#endif /* NUM_CORES */

static inline void unlock_blocker_thread(struct blocker *bl)
{
#if NUM_CORES > 1
    struct thread_entry *blt = bl->thread;
    if (blt)
        UNLOCK_THREAD(blt);
    else
        corelock_unlock(&((struct blocker_splay *)bl)->cl);
#endif /* NUM_CORES > 1*/
    (void)bl;
}

/*---------------------------------------------------------------------------
 * Change the priority and rtr entry for a running thread
 *---------------------------------------------------------------------------
 */
static inline void set_rtr_thread_priority(
    struct thread_entry *thread, int priority)
{
    const unsigned int core = IF_COP_CORE(thread->core);
    struct core_entry *corep = __core_id_entry(core);
    RTR_LOCK(corep);
    rtr_move_entry(corep, thread->priority, priority);
    thread->priority = priority;
    RTR_UNLOCK(corep);
}

/*---------------------------------------------------------------------------
 * Finds the highest priority thread in a list of threads. If the list is
 * empty, the PRIORITY_IDLE is returned.
 *
 * It is possible to use the struct priority_distribution within an object
 * instead of scanning the remaining threads in the list but as a compromise,
 * the resulting per-object memory overhead is saved at a slight speed
 * penalty under high contention.
 *---------------------------------------------------------------------------
 */
static int wait_queue_find_priority(struct __wait_queue *wqp)
{
    int highest_priority = PRIORITY_IDLE;
    struct thread_entry *thread = WQ_THREAD_FIRST(wqp);

    while (thread != NULL)
    {
        int priority = thread->priority;
        if (priority < highest_priority)
            highest_priority = priority;

        thread = WQ_THREAD_NEXT(thread);
    }

    return highest_priority;
}

/*---------------------------------------------------------------------------
 * Register priority with blocking system and bubble it down the chain if
 * any until we reach the end or something is already equal or higher.
 *
 * NOTE: A simultaneous circular wait could spin deadlock on multiprocessor
 * targets but that same action also guarantees a circular block anyway and
 * those are prevented, right? :-)
 *---------------------------------------------------------------------------
 */
static void inherit_priority(
    struct blocker * const blocker0, struct blocker *bl,
    struct thread_entry *blt, int newblpr)
{
    int oldblpr = bl->priority;

    while (1)
    {
        if (blt == NULL)
        {
            /* Multiple owners */
            struct blocker_splay *blsplay = (struct blocker_splay *)bl;

            /* Recurse down the all the branches of this; it's the only way.
               We might meet the same queue several times if more than one of
               these threads is waiting the same queue. That isn't a problem
               for us since we early-terminate, just notable. */
            FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum)
            {
                bl->priority = oldblpr; /* To see the change each time */
                blt = __thread_slot_entry(slotnum);
                LOCK_THREAD(blt);
                inherit_priority(blocker0, bl, blt, newblpr);
            }

            corelock_unlock(&blsplay->cl);
            return;
        }

        bl->priority = newblpr;

        /* Update blocker thread inheritance record */
        if (newblpr < PRIORITY_IDLE)
            prio_add_entry(&blt->pdist, newblpr);

        if (oldblpr < PRIORITY_IDLE)
            prio_subtract_entry(&blt->pdist, oldblpr);

        int oldpr = blt->priority;
        int newpr = priobit_ffs(&blt->pdist.mask);
        if (newpr == oldpr)
            break; /* No blocker thread priority change */

        if (blt->state == STATE_RUNNING)
        {
            set_rtr_thread_priority(blt, newpr);
            break; /* Running: last in chain */
        }

        /* Blocker is blocked */
        blt->priority = newpr;

        bl = blt->blocker;
        if (LIKELY(bl == NULL))
            break; /* Block doesn't support PIP */

        if (UNLIKELY(bl == blocker0))
            break; /* Full circle - deadlock! */

        /* Blocker becomes current thread and the process repeats */
        struct __wait_queue *wqp = wait_queue_ptr(blt);
        struct thread_entry *t = blt;
        blt = lock_blocker_thread(bl);

        UNLOCK_THREAD(t);

        /* Adjust this wait queue */
        oldblpr = bl->priority;
        if (newpr <= oldblpr)
            newblpr = newpr;
        else if (oldpr <= oldblpr)
            newblpr = wait_queue_find_priority(wqp);

        if (newblpr == oldblpr)
            break; /* Queue priority not changing */
    }

    UNLOCK_THREAD(blt);
}

/*---------------------------------------------------------------------------
 * Quick-inherit of priority elevation. 'thread' must be not runnable
 *---------------------------------------------------------------------------
 */
static void priority_inherit_internal_inner(struct thread_entry *thread,
                                            int blpr)
{
    if (prio_add_entry(&thread->pdist, blpr) == 1 && blpr < thread->priority)
        thread->priority = blpr;
}

static inline void priority_inherit_internal(struct thread_entry *thread,
                                             int blpr)
{
    if (blpr < PRIORITY_IDLE)
        priority_inherit_internal_inner(thread, blpr);
}

/*---------------------------------------------------------------------------
 * Quick-disinherit of priority elevation. 'thread' must current
 *---------------------------------------------------------------------------
 */
static void priority_disinherit_internal_inner(struct thread_entry *thread,
                                               int blpr)
{
    if (prio_subtract_entry(&thread->pdist, blpr) == 0 &&
        blpr <= thread->priority)
    {
        int priority = priobit_ffs(&thread->pdist.mask);
        if (priority != thread->priority)
            set_rtr_thread_priority(thread, priority);
    }
}

static inline void priority_disinherit_internal(struct thread_entry *thread,
                                                int blpr)
{
    if (blpr < PRIORITY_IDLE)
        priority_disinherit_internal_inner(thread, blpr);
}

void priority_disinherit(struct thread_entry *thread, struct blocker *bl)
{
    LOCK_THREAD(thread);
    priority_disinherit_internal(thread, bl->priority);
    UNLOCK_THREAD(thread);
}

/*---------------------------------------------------------------------------
 * Transfer ownership from a single owner to a multi-owner splay from a wait
 * queue
 *---------------------------------------------------------------------------
 */
static void wakeup_thread_queue_multi_transfer(struct thread_entry *thread)
{
    /* All threads will have the same blocker and queue; only we are changing
       it now */
    struct __wait_queue *wqp = wait_queue_ptr(thread);
    struct blocker *bl = thread->blocker;
    struct blocker_splay *blsplay = (struct blocker_splay *)bl;
    struct thread_entry *blt = bl->thread;

    /* The first thread is already locked and is assumed tagged "multi" */
    int count = 1;

    /* Multiple versions of the wait queue may be seen if doing more than
       one thread; queue removal isn't destructive to the pointers of the node
       being removed; this may lead to the blocker priority being wrong for a
       time but it gets fixed up below after getting exclusive access to the
       queue */
    while (1)
    {
        thread->blocker = NULL;
        wait_queue_remove(thread);

        unsigned int slotnum = THREAD_ID_SLOT(thread->id);
        threadbit_set_bit(&blsplay->mask, slotnum);

        struct thread_entry *tnext = WQ_THREAD_NEXT(thread);
        if (tnext == NULL || tnext->retval == 0)
            break;

        UNLOCK_THREAD(thread);

        count++;
        thread = tnext;

        LOCK_THREAD(thread);
    }

    /* Locking order reverses here since the threads are no longer on the
       queued side */
    if (count > 1)
        corelock_lock(&blsplay->cl);

    LOCK_THREAD(blt);

    int blpr = bl->priority;
    priority_disinherit_internal(blt, blpr);

    if (count > 1)
    {
        blsplay->blocker.thread = NULL;

        blpr = wait_queue_find_priority(wqp);

        FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum)
        {
            UNLOCK_THREAD(thread);
            thread = __thread_slot_entry(slotnum);
            LOCK_THREAD(thread);
            priority_inherit_internal(thread, blpr);
            core_schedule_wakeup(thread);
        }
    }
    else
    {
        /* Becomes a simple, direct transfer */
        blsplay->blocker.thread = thread;

        if (thread->priority <= blpr)
            blpr = wait_queue_find_priority(wqp);

        priority_inherit_internal(thread, blpr);
        core_schedule_wakeup(thread);
    }

    UNLOCK_THREAD(thread);

    bl->priority = blpr;

    UNLOCK_THREAD(blt);

    if (count > 1)
        corelock_unlock(&blsplay->cl);

    blt->retval = count;
}

/*---------------------------------------------------------------------------
 * Transfer ownership to a thread waiting for an objects and transfer
 * inherited priority boost from other waiters. This algorithm knows that
 * blocking chains may only unblock from the very end.
 *
 * Only the owning thread itself may call this and so the assumption that
 * it is the running thread is made.
 *---------------------------------------------------------------------------
 */
static void wakeup_thread_transfer(struct thread_entry *thread)
{
    /* Waking thread inherits priority boost from object owner (blt) */
    struct blocker *bl = thread->blocker;
    struct thread_entry *blt = bl->thread;

    THREAD_ASSERT(__running_self_entry() == blt,
                  "UPPT->wrong thread", __running_self_entry());

    LOCK_THREAD(blt);

    thread->blocker = NULL;
    struct __wait_queue *wqp = wait_queue_remove(thread);

    int blpr = bl->priority;

    /* Remove the object's boost from the owning thread */
    priority_disinherit_internal_inner(blt, blpr);

    struct thread_entry *tnext = WQ_THREAD_FIRST(wqp);
    if (LIKELY(tnext == NULL))
    {
        /* Expected shortcut - no more waiters */
        blpr = PRIORITY_IDLE;
    }
    else
    {
        /* If thread is at the blocker priority, its removal may drop it */
        if (thread->priority <= blpr)
            blpr = wait_queue_find_priority(wqp);

        priority_inherit_internal_inner(thread, blpr);
    }

    bl->thread = thread; /* This thread pwns */

    core_schedule_wakeup(thread);
    UNLOCK_THREAD(thread);

    bl->priority = blpr; /* Save highest blocked priority */

    UNLOCK_THREAD(blt);
}

/*---------------------------------------------------------------------------
 * Readjust priorities when waking a thread blocked waiting for another
 * in essence "releasing" the thread's effect on the object owner. Can be
 * performed from any context.
 *---------------------------------------------------------------------------
 */
static void wakeup_thread_release(struct thread_entry *thread)
{
    struct blocker *bl = thread->blocker;
    struct thread_entry *blt = lock_blocker_thread(bl);

    thread->blocker = NULL;
    struct __wait_queue *wqp = wait_queue_remove(thread);

    /* Off to see the wizard... */
    core_schedule_wakeup(thread);

    if (thread->priority > bl->priority)
    {
        /* Queue priority won't change */
        UNLOCK_THREAD(thread);
        unlock_blocker_thread(bl);
        return;
    }

    UNLOCK_THREAD(thread);

    int newblpr = wait_queue_find_priority(wqp);
    if (newblpr == bl->priority)
    {
        /* Blocker priority won't change */
        unlock_blocker_thread(bl);
        return;
    }

    inherit_priority(bl, bl, blt, newblpr);
}

#endif /* HAVE_PRIORITY_SCHEDULING */


/*---------------------------------------------------------------------------
 * Explicitly wakeup a thread on a blocking queue. Only effects threads of
 * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
 *
 * INTERNAL: Intended for use by kernel and not programs.
 *---------------------------------------------------------------------------
 */
unsigned int wakeup_thread_(struct thread_entry *thread
                            IF_PRIO(, enum wakeup_thread_protocol proto))
{
    LOCK_THREAD(thread);

    /* Determine thread's current state. */
    switch (thread->state)
    {
    case STATE_BLOCKED:
    case STATE_BLOCKED_W_TMO:
#ifdef HAVE_PRIORITY_SCHEDULING
        /* Threads with PIP blockers cannot specify "WAKEUP_DEFAULT" */
        if (thread->blocker != NULL)
        {
            static void (* const funcs[])(struct thread_entry *thread)
                ICONST_ATTR =
            {
                [WAKEUP_DEFAULT]        = NULL,
                [WAKEUP_TRANSFER]       = wakeup_thread_transfer,
                [WAKEUP_RELEASE]        = wakeup_thread_release,
                [WAKEUP_TRANSFER_MULTI] = wakeup_thread_queue_multi_transfer,
            };

            /* Call the specified unblocking PIP (does the rest) */
            funcs[proto](thread);
        }
        else
#endif /* HAVE_PRIORITY_SCHEDULING */
        {
            wait_queue_remove(thread);
            core_schedule_wakeup(thread);
            UNLOCK_THREAD(thread);
        }

        return should_switch_tasks(thread);

    case STATE_RUNNING:
        if (wait_queue_try_remove(thread))
        {
            UNLOCK_THREAD(thread);
            return THREAD_OK; /* timed out */
        }

    default:
        UNLOCK_THREAD(thread);
        return THREAD_NONE;
    }
}

/*---------------------------------------------------------------------------
 * Check the core's timeout list when at least one thread is due to wake.
 * Filtering for the condition is done before making the call. Resets the
 * tick when the next check will occur.
 *---------------------------------------------------------------------------
 */
static NO_INLINE void check_tmo_expired_inner(struct core_entry *corep)
{
    const long tick = current_tick; /* snapshot the current tick */
    long next_tmo_check = tick + 60*HZ; /* minimum duration: once/minute */
    struct thread_entry *prev = NULL;
    struct thread_entry *thread = TMO_THREAD_FIRST(&corep->tmo);

    /* If there are no processes waiting for a timeout, just keep the check
       tick from falling into the past. */

    /* Break the loop once we have walked through the list of all
     * sleeping processes or have removed them all. */
    while (thread != NULL)
    {
        /* Check sleeping threads. Allow interrupts between checks. */
        enable_irq();

        struct thread_entry *next = TMO_THREAD_NEXT(thread);

        /* Lock thread slot against explicit wakeup */
        disable_irq();
        LOCK_THREAD(thread);

        unsigned int state = thread->state;

        if (LIKELY(state >= TIMEOUT_STATE_FIRST &&
                   TIME_BEFORE(tick, thread->tmo_tick)))
        {
            /* Timeout still pending - this will be the usual case */
            if (TIME_BEFORE(thread->tmo_tick, next_tmo_check))
            {
                /* Move the next check up to its time */
                next_tmo_check = thread->tmo_tick;
            }

            prev = thread;
        }
        else
        {
            /* TODO: there are no priority-inheriting timeout blocks
               right now but the procedure should be established */

            /* Sleep timeout has been reached / garbage collect stale list
               items */
            tmo_queue_expire(&corep->tmo, prev, thread);

            if (state >= TIMEOUT_STATE_FIRST)
                core_rtr_add(corep, thread);

            /* removed this one - prev doesn't change */
        }

        UNLOCK_THREAD(thread);

        thread = next;
    }

    corep->next_tmo_check = next_tmo_check;
}

static FORCE_INLINE void check_tmo_expired(struct core_entry *corep)
{
    if (!TIME_BEFORE(current_tick, corep->next_tmo_check))
        check_tmo_expired_inner(corep);
}

/*---------------------------------------------------------------------------
 * Prepares a the current thread to sleep forever or for the given duration.
 *---------------------------------------------------------------------------
 */
static FORCE_INLINE void prepare_block(struct thread_entry *current,
                                       unsigned int state, int timeout)
{
    const unsigned int core = IF_COP_CORE(current->core);

    /* Remove the thread from the list of running threads. */
    struct core_entry *corep = __core_id_entry(core);
    core_rtr_remove(corep, current);

    if (timeout >= 0)
    {
        /* Sleep may expire. */
        long tmo_tick = current_tick + timeout;
        current->tmo_tick = tmo_tick;

        if (TIME_BEFORE(tmo_tick, corep->next_tmo_check))
            corep->next_tmo_check = tmo_tick;

        tmo_queue_register(&corep->tmo, current);

        if (state == STATE_BLOCKED)
            state = STATE_BLOCKED_W_TMO;
    }

    /* Report new state. */
    current->state = state;
}

/*---------------------------------------------------------------------------
 * Switch thread in round robin fashion for any given priority. Any thread
 * that removed itself from the running list first must specify itself in
 * the paramter.
 *
 * INTERNAL: Intended for use by kernel and not programs.
 *---------------------------------------------------------------------------
 */
void switch_thread(void)
{
    const unsigned int core = CURRENT_CORE;
    struct core_entry *corep = __core_id_entry(core);
    struct thread_entry *thread = corep->running;

    if (thread)
    {
#ifdef RB_PROFILE
        profile_thread_stopped(THREAD_ID_SLOT(thread->id));
#endif
#ifdef DEBUG
        /* Check core_ctx buflib integrity */
        core_check_valid();
#endif
        thread_store_context(thread);

        /* Check if the current thread stack is overflown */
        if (UNLIKELY(thread->stack[0] != DEADBEEF) && thread->stack_size > 0)
            thread_stkov(thread);
    }

    /* TODO: make a real idle task */
    for (;;)
    {
        disable_irq();

        /* Check for expired timeouts */
        check_tmo_expired(corep);

        RTR_LOCK(corep);

        if (!RTR_EMPTY(&corep->rtr))
            break;

        thread = NULL;
        
        /* Enter sleep mode to reduce power usage */
        RTR_UNLOCK(corep);
        core_sleep(IF_COP(core));

        /* Awakened by interrupt or other CPU */
    }

    thread = (thread && thread->state == STATE_RUNNING) ?
        RTR_THREAD_NEXT(thread) : RTR_THREAD_FIRST(&corep->rtr);

#ifdef HAVE_PRIORITY_SCHEDULING
    /* Select the new task based on priorities and the last time a
     * process got CPU time relative to the highest priority runnable
     * task. If priority is not a feature, then FCFS is used (above). */
    int max = priobit_ffs(&corep->rtr_dist.mask);

    for (;;)
    {
        int priority = thread->priority;
        int diff;

        /* This ridiculously simple method of aging seems to work
         * suspiciously well. It does tend to reward CPU hogs (under
         * yielding) but that's generally not desirable at all. On
         * the plus side, it, relatively to other threads, penalizes
         * excess yielding which is good if some high priority thread
         * is performing no useful work such as polling for a device
         * to be ready. Of course, aging is only employed when higher
         * and lower priority threads are runnable. The highest
         * priority runnable thread(s) are never skipped unless a
         * lower-priority process has aged sufficiently. Priorities
         * of REALTIME class are run strictly according to priority
         * thus are not subject to switchout due to lower-priority
         * processes aging; they must give up the processor by going
         * off the run list. */
        if (LIKELY(priority <= max) ||
            (priority > PRIORITY_REALTIME &&
             (diff = priority - max, ++thread->skip_count > diff*diff)))
        {
            break;
        }

        thread = RTR_THREAD_NEXT(thread);
    }

    thread->skip_count = 0; /* Reset aging counter */
#endif /* HAVE_PRIORITY_SCHEDULING */

    rtr_queue_make_first(&corep->rtr, thread);
    corep->running = thread;

    RTR_UNLOCK(corep);
    enable_irq();

    /* And finally, give control to the next thread. */
    thread_load_context(thread);

#ifdef RB_PROFILE
    profile_thread_started(THREAD_ID_SLOT(thread->id));
#endif
}

/*---------------------------------------------------------------------------
 * Sleeps a thread for at least a specified number of ticks with zero being
 * a wait until the next tick.
 *
 * INTERNAL: Intended for use by kernel and not programs.
 *---------------------------------------------------------------------------
 */
void sleep_thread(int ticks)
{
    struct thread_entry *current = __running_self_entry();
    LOCK_THREAD(current);
    prepare_block(current, STATE_SLEEPING, MAX(ticks, 0) + 1);
    UNLOCK_THREAD(current);
}

/*---------------------------------------------------------------------------
 * Block a thread on a blocking queue for explicit wakeup. If timeout is
 * negative, the block is infinite.
 *
 * INTERNAL: Intended for use by kernel and not programs.
 *---------------------------------------------------------------------------
 */
void block_thread_(struct thread_entry *current, int timeout)
{
    LOCK_THREAD(current);

#ifdef HAVE_PRIORITY_SCHEDULING
    struct blocker *bl = current->blocker;
    struct thread_entry *blt = NULL;
    if (bl != NULL)
    {
        current->blocker = bl;
        blt = lock_blocker_thread(bl);
    }
#endif /* HAVE_PRIORITY_SCHEDULING */

    wait_queue_register(current);
    prepare_block(current, STATE_BLOCKED, timeout);

#ifdef HAVE_PRIORITY_SCHEDULING
    if (bl != NULL)
    {
        int newblpr = current->priority;
        UNLOCK_THREAD(current);

        if (newblpr < bl->priority)
            inherit_priority(bl, bl, blt, newblpr);
        else
            unlock_blocker_thread(bl); /* Queue priority won't change */
    }
    else
#endif /* HAVE_PRIORITY_SCHEDULING */
    {
        UNLOCK_THREAD(current);
    }
}

/*---------------------------------------------------------------------------
 * Place the current core in idle mode - woken up on interrupt or wake
 * request from another core.
 *---------------------------------------------------------------------------
 */
void core_idle(void)
{
    disable_irq();
    core_sleep(IF_COP(CURRENT_CORE));
}

/*---------------------------------------------------------------------------
 * Create a thread. If using a dual core architecture, specify which core to
 * start the thread on.
 *
 * Return ID if context area could be allocated, else NULL.
 *---------------------------------------------------------------------------
 */
unsigned int create_thread(void (*function)(void),
                           void* stack, size_t stack_size,
                           unsigned flags, const char *name
                           IF_PRIO(, int priority)
                           IF_COP(, unsigned int core))
{
    struct thread_entry *thread = thread_alloc();
    if (thread == NULL)
        return 0;

    new_thread_base_init(thread, &stack, &stack_size, name
                         IF_PRIO(, priority) IF_COP(, core));

    unsigned int stack_words = stack_size / sizeof (uintptr_t);
    if (stack_words == 0)
        return 0;

    /* Munge the stack to make it easy to spot stack overflows */
    for (unsigned int i = 0; i < stack_words; i++)
        ((uintptr_t *)stack)[i] = DEADBEEF;

#if NUM_CORES > 1
    /* Writeback stack munging or anything else before starting */
    if (core != CURRENT_CORE)
        commit_dcache();
#endif

    thread->context.sp = (typeof (thread->context.sp))(stack + stack_size);
    THREAD_STARTUP_INIT(core, thread, function);

    int oldlevel = disable_irq_save();
    LOCK_THREAD(thread);

    thread->state = STATE_FROZEN;

    if (!(flags & CREATE_THREAD_FROZEN))
        core_schedule_wakeup(thread);

    unsigned int id = thread->id; /* Snapshot while locked */

    UNLOCK_THREAD(thread);
    restore_irq(oldlevel);

    return id;
}

/*---------------------------------------------------------------------------
 * Block the current thread until another thread terminates. A thread may
 * wait on itself to terminate but that will deadlock
 *.
 * Parameter is the ID as returned from create_thread().
 *---------------------------------------------------------------------------
 */
void thread_wait(unsigned int thread_id)
{
    struct thread_entry *current = __running_self_entry();
    struct thread_entry *thread = __thread_id_entry(thread_id);

    corelock_lock(&thread->waiter_cl);

    if (thread->id == thread_id && thread->state != STATE_KILLED)
    {
        disable_irq();
        block_thread(current, TIMEOUT_BLOCK, &thread->queue, NULL);

        corelock_unlock(&thread->waiter_cl);

        switch_thread();
        return;
    }

    corelock_unlock(&thread->waiter_cl);
}

/*---------------------------------------------------------------------------
 * Exit the current thread
 *---------------------------------------------------------------------------
 */
static USED_ATTR NORETURN_ATTR
void thread_exit_final(struct thread_entry *current)
{
    /* Slot is no longer this thread */
    new_thread_id(current);
    current->name = NULL;

    /* No longer using resources from creator */
    wait_queue_wake(&current->queue);

    UNLOCK_THREAD(current);
    corelock_unlock(&current->waiter_cl);

    thread_free(current);

    switch_thread();

    /* This should never and must never be reached - if it is, the
     * state is corrupted */
    THREAD_PANICF("thread_exit->K:*R", current);
}

void thread_exit(void)
{
    struct core_entry *corep = __core_id_entry(CURRENT_CORE);
    register struct thread_entry *current = corep->running;

    /* Cancel CPU boost if any */
    cancel_cpu_boost();

    disable_irq();

    corelock_lock(&current->waiter_cl);
    LOCK_THREAD(current);

#ifdef HAVE_PRIORITY_SCHEDULING
    /* Only one bit in the mask should be set with a frequency on 1 which
     * represents the thread's own base priority otherwise threads are waiting
     * on an abandoned object */
    if (priobit_popcount(&current->pdist.mask) != 1 ||
        current->pdist.hist[priobit_ffs(&current->pdist.mask)] > 1)
        thread_panicf("abandon ship!", current);
#endif /* HAVE_PRIORITY_SCHEDULING */

    /* Remove from scheduler lists */
    tmo_queue_remove(&corep->tmo, current);
    prepare_block(current, STATE_KILLED, -1);
    corep->running = NULL; /* No switch_thread context save */

#ifdef RB_PROFILE
    profile_thread_stopped(THREAD_ID_SLOT(current->id));
#endif

    /* Do final release of resources and remove the thread */
#if NUM_CORES > 1
    thread_exit_finalize(current->core, current);
#else
    thread_exit_final(current);
#endif
}

#ifdef HAVE_PRIORITY_SCHEDULING
/*---------------------------------------------------------------------------
 * Sets the thread's relative base priority for the core it runs on. Any
 * needed inheritance changes also may happen.
 *---------------------------------------------------------------------------
 */
int thread_set_priority(unsigned int thread_id, int priority)
{
    if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
        return -1; /* Invalid priority argument */

    int old_base_priority = -1;
    struct thread_entry *thread = __thread_id_entry(thread_id);

    const int oldlevel = disable_irq_save();
    LOCK_THREAD(thread);

    if (thread->id != thread_id || thread->state == STATE_KILLED)
        goto done; /* Invalid thread */

    old_base_priority = thread->base_priority;
    if (priority == old_base_priority)
        goto done; /* No base priority change */

    thread->base_priority = priority;

    /* Adjust the thread's priority influence on itself */
    prio_move_entry(&thread->pdist, old_base_priority, priority);

    int old_priority = thread->priority;
    int new_priority = priobit_ffs(&thread->pdist.mask);

    if (old_priority == new_priority)
        goto done; /* No running priority change */

    if (thread->state == STATE_RUNNING)
    {
        /* This thread is running - just change location on the run queue.
           Also sets thread->priority. */
        set_rtr_thread_priority(thread, new_priority);
        goto done;
    }

    /* Thread is blocked */
    struct blocker *bl = thread->blocker;
    if (bl == NULL)
    {
        thread->priority = new_priority;
        goto done; /* End of transitive blocks */
    }

    struct thread_entry *blt = lock_blocker_thread(bl);
    struct __wait_queue *wqp = wait_queue_ptr(thread);

    thread->priority = new_priority;

    UNLOCK_THREAD(thread);
    thread = NULL;

    int oldblpr = bl->priority;
    int newblpr = oldblpr;
    if (new_priority < oldblpr)
        newblpr = new_priority;
    else if (old_priority <= oldblpr)
        newblpr = wait_queue_find_priority(wqp);

    if (newblpr == oldblpr)
    {
        unlock_blocker_thread(bl);
        goto done;
    }

    inherit_priority(bl, bl, blt, newblpr);
done:
    if (thread)
        UNLOCK_THREAD(thread);
    restore_irq(oldlevel);
    return old_base_priority;
}

/*---------------------------------------------------------------------------
 * Returns the current base priority for a thread.
 *---------------------------------------------------------------------------
 */
int thread_get_priority(unsigned int thread_id)
{
    struct thread_entry *thread = __thread_id_entry(thread_id);
    int base_priority = thread->base_priority;

    /* Simply check without locking slot. It may or may not be valid by the
     * time the function returns anyway. If all tests pass, it is the
     * correct value for when it was valid. */
    if (thread->id != thread_id || thread->state == STATE_KILLED)
        base_priority = -1;

    return base_priority;
}
#endif /* HAVE_PRIORITY_SCHEDULING */

#ifdef HAVE_IO_PRIORITY
int thread_get_io_priority(unsigned int thread_id)
{
    struct thread_entry *thread = __thread_id_entry(thread_id);
    return thread->io_priority;
}

void thread_set_io_priority(unsigned int thread_id,int io_priority)
{
    struct thread_entry *thread = __thread_id_entry(thread_id);
    thread->io_priority = io_priority;
}
#endif

/*---------------------------------------------------------------------------
 * Starts a frozen thread - similar semantics to wakeup_thread except that
 * the thread is on no scheduler or wakeup queue at all. It exists simply by
 * virtue of the slot having a state of STATE_FROZEN.
 *---------------------------------------------------------------------------
 */
void thread_thaw(unsigned int thread_id)
{
    struct thread_entry *thread = __thread_id_entry(thread_id);
    int oldlevel = disable_irq_save();

    LOCK_THREAD(thread);

    /* If thread is the current one, it cannot be frozen, therefore
     * there is no need to check that. */
    if (thread->id == thread_id && thread->state == STATE_FROZEN)
        core_schedule_wakeup(thread);

    UNLOCK_THREAD(thread);
    restore_irq(oldlevel);
}

#if NUM_CORES > 1
/*---------------------------------------------------------------------------
 * Switch the processor that the currently executing thread runs on.
 *---------------------------------------------------------------------------
 */
static USED_ATTR NORETURN_ATTR
void switch_core_final(unsigned int old_core, struct thread_entry *current)
{
    /* Old core won't be using slot resources at this point */
    core_schedule_wakeup(current);
    UNLOCK_THREAD(current);
#ifdef RB_PROFILE
    profile_thread_stopped(THREAD_ID_SLOT(current->id));
#endif
    switch_thread();
    /* not reached */
    THREAD_PANICF("switch_core_final->same core!", current);
    (void)old_core;
}

unsigned int switch_core(unsigned int new_core)
{
    const unsigned int old_core = CURRENT_CORE;
    if (old_core == new_core)
        return old_core; /* No change */

    struct core_entry *corep = __core_id_entry(old_core);
    struct thread_entry *current = corep->running;

    disable_irq();
    LOCK_THREAD(current);

    /* Remove us from old core lists */
    tmo_queue_remove(&corep->tmo, current);
    core_rtr_remove(corep, current);
    corep->running = NULL; /* No switch_thread context save */

    /* Do the actual migration */
    current->core = new_core;
    switch_thread_core(old_core, current);

    /* Executing on new core */
    return old_core;
}
#endif /* NUM_CORES > 1 */

#ifdef HAVE_SCHEDULER_BOOSTCTRL
/*---------------------------------------------------------------------------
 * Change the boost state of a thread boosting or unboosting the CPU
 * as required.
 *---------------------------------------------------------------------------
 */
static inline void boost_thread(struct thread_entry *thread, bool boost)
{
    if ((thread->cpu_boost != 0) != boost)
    {
        thread->cpu_boost = boost;
        cpu_boost(boost);
    }
}

void trigger_cpu_boost(void)
{
    boost_thread(__running_self_entry(), true);
}

void cancel_cpu_boost(void)
{
    boost_thread(__running_self_entry(), false);
}
#endif /* HAVE_SCHEDULER_BOOSTCTRL */

/*---------------------------------------------------------------------------
 * Initialize threading API. This assumes interrupts are not yet enabled. On
 * multicore setups, no core is allowed to proceed until create_thread calls
 * are safe to perform.
 *---------------------------------------------------------------------------
 */
void INIT_ATTR init_threads(void)
{
    const unsigned int core = CURRENT_CORE;

    if (core == CPU)
    {
        thread_alloc_init(); /* before using cores! */

        /* Create main thread */
        struct thread_entry *thread = thread_alloc();
        if (thread == NULL)
        {
            /* WTF? There really must be a slot available at this stage.
             * This can fail if, for example, .bss isn't zero'ed out by the
             * loader or threads is in the wrong section. */
            THREAD_PANICF("init_threads->no slot", NULL);
        }

        size_t stack_size;
        void *stack = __get_main_stack(&stack_size);
        new_thread_base_init(thread, &stack, &stack_size, __main_thread_name
                             IF_PRIO(, PRIORITY_MAIN_THREAD) IF_COP(, core));

        struct core_entry *corep = __core_id_entry(core);
        core_rtr_add(corep, thread);
        corep->running = thread;

#ifdef INIT_MAIN_THREAD
        init_main_thread(&thread->context);
#endif
    }

#if NUM_CORES > 1
    /* Boot CPU:
     * Wait for other processors to finish their inits since create_thread
     * isn't safe to call until the kernel inits are done. The first
     * threads created in the system must of course be created by CPU.
     * Another possible approach is to initialize all cores and slots
     * for each core by CPU, let the remainder proceed in parallel and
     * signal CPU when all are finished.
     *
     * Other:
     * After last processor completes, it should signal all others to
     * proceed or may signal the next and call thread_exit(). The last one
     * to finish will signal CPU.
     */
    core_thread_init(core);

    if (core != CPU)
    {
        /* No main thread on coprocessors - go idle and wait */
        switch_thread();
        THREAD_PANICF("init_threads() - coprocessor returned", NULL);
    }
#endif /* NUM_CORES */
}