summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--firmware/SOURCES3
-rw-r--r--firmware/include/bitarray.h231
-rw-r--r--firmware/kernel/include/kernel.h1
-rw-r--r--firmware/kernel/include/mutex.h20
-rw-r--r--firmware/kernel/include/queue.h2
-rw-r--r--firmware/kernel/include/thread.h81
-rw-r--r--firmware/kernel/mutex.c65
-rw-r--r--firmware/kernel/queue.c21
-rw-r--r--firmware/kernel/semaphore.c8
-rw-r--r--firmware/kernel/thread.c961
-rw-r--r--firmware/target/hosted/sdl/thread-sdl.c34
11 files changed, 853 insertions, 574 deletions
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 5e37892efe..584254a666 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1825,6 +1825,9 @@ drivers/touchpad.c
1825#ifdef HAVE_CORELOCK_OBJECT 1825#ifdef HAVE_CORELOCK_OBJECT
1826kernel/corelock.c 1826kernel/corelock.c
1827#endif 1827#endif
1828#if 0 /* pending dependent code */
1829kernel/mrsw_lock.c
1830#endif
1828kernel/mutex.c 1831kernel/mutex.c
1829kernel/queue.c 1832kernel/queue.c
1830#ifdef HAVE_SEMAPHORE_OBJECTS 1833#ifdef HAVE_SEMAPHORE_OBJECTS
diff --git a/firmware/include/bitarray.h b/firmware/include/bitarray.h
new file mode 100644
index 0000000000..4777ccb6a4
--- /dev/null
+++ b/firmware/include/bitarray.h
@@ -0,0 +1,231 @@
1/***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
9 *
10 * Copyright (C) 2014 by Michael Sevakis
11 *
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
16 *
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
19 *
20 ****************************************************************************/
21#ifndef BITARRAY_H
22#define BITARRAY_H
23
24/* Type-checked bit array definitions */
25
26/* All this stuff gets optimized into very simple object code */
27
28#define BITARRAY_WORD_BITS \
29 (sizeof (unsigned int) * 8)
30#define BITARRAY_NWORDS(bits) \
31 (((bits) + BITARRAY_WORD_BITS - 1) / BITARRAY_WORD_BITS)
32#define BITARRAY_BITWORD(bitnum) \
33 ((bitnum) / BITARRAY_WORD_BITS)
34#define BITARRAY_WORDBIT(bitnum) \
35 ((bitnum) % BITARRAY_WORD_BITS)
36#define BITARRAY_NBIT(word, bit) \
37 ((word)*BITARRAY_WORD_BITS + (bit))
38#define BITARRAY_BITS(bits) \
39 (BITARRAY_NWORDS(bits)*BITARRAY_WORD_BITS)
40#define BITARRAY_BITN(bitnum) \
41 (1u << BITARRAY_WORDBIT(bitnum))
42
43
44/** Iterators **/
45#include "config.h"
46#include <stdint.h>
47
48#if (defined(CPU_ARM) && ARM_ARCH >= 5) || UINT32_MAX < UINT_MAX
49#define __BITARRAY_CTZ(wval) __builtin_ctz(wval)
50#else
51#include "system.h"
52#define __BITARRAY_CTZ(wval) find_first_set_bit(wval)
53#endif
54#define __BITARRAY_POPCNT(wval) __builtin_popcount(wval)
55
56#ifndef BIT_N
57#define BIT_N(n) (1u << (n))
58#endif
59
60/* Enumerate each word index */
61#define FOR_EACH_BITARRAY_WORD_INDEX(nwords, index) \
62 for (unsigned int index = 0, _nwords = (nwords); \
63 index < _nwords; index++)
64
65/* Enumerate each word value */
66#define FOR_EACH_BITARRAY_WORD(a, wval) \
67 FOR_EACH_BITARRAY_WORD_INDEX(ARRAYLEN((a)->words), _w) \
68 for (unsigned int wval = (a)->words[_w], _ = 1; _; _--)
69
70/* Enumerate the bit number of each set bit of a word in sequence */
71#define FOR_EACH_BITARRAY_SET_WORD_BIT(wval, bit) \
72 for (unsigned int _wval = (wval), bit; \
73 _wval ? (((bit) = __BITARRAY_CTZ(_wval)), 1) : 0; \
74 _wval &= ~BIT_N(bit))
75
76/* Enumerate the bit number of each set bit in the bit array in sequence */
77#define FOR_EACH_BITARRAY_SET_BIT_ARR(nwords, words, nbit) \
78 FOR_EACH_BITARRAY_WORD_INDEX(nwords, _w) \
79 FOR_EACH_BITARRAY_SET_WORD_BIT(words[_w], _bit) \
80 for (unsigned int nbit = BITARRAY_NBIT(_w, _bit), _ = 1; _; _--)
81
82/* As above but takes an array type for an argument */
83#define FOR_EACH_BITARRAY_SET_BIT(a, nbit) \
84 FOR_EACH_BITARRAY_SET_BIT_ARR(ARRAYLEN((a)->words), (a)->words, nbit)
85
86
87/** Base functions (called by typed functions) **/
88
89/* Return the word associated with the bit */
90static inline unsigned int
91__bitarray_get_word(unsigned int words[], unsigned int bitnum)
92{
93 return words[BITARRAY_BITWORD(bitnum)];
94}
95
96/* Set the word associated with the bit */
97static inline void
98__bitarray_set_word(unsigned int words[], unsigned int bitnum,
99 unsigned int wordval)
100{
101 words[BITARRAY_BITWORD(bitnum)] = wordval;
102}
103
104/* Set the bit at index 'bitnum' to '1' */
105static inline void
106__bitarray_set_bit(unsigned int words[], unsigned int bitnum)
107{
108 unsigned int word = BITARRAY_BITWORD(bitnum);
109 unsigned int bit = BITARRAY_BITN(bitnum);
110 words[word] |= bit;
111}
112
113/* Set the bit at index 'bitnum' to '0' */
114static inline void
115__bitarray_clear_bit(unsigned int words[], unsigned int bitnum)
116{
117 unsigned int word = BITARRAY_BITWORD(bitnum);
118 unsigned int bit = BITARRAY_BITN(bitnum);
119 words[word] &= ~bit;
120}
121
122/* Return the value of the specified bit ('0' or '1') */
123static inline unsigned int
124__bitarray_test_bit(const unsigned int words[], unsigned int bitnum)
125{
126 unsigned int word = BITARRAY_BITWORD(bitnum);
127 unsigned int nbit = BITARRAY_WORDBIT(bitnum);
128 return (words[word] >> nbit) & 1u;
129}
130
131/* Check if all bits in the bit array are '0' */
132static inline bool
133__bitarray_is_clear(const unsigned int words[], unsigned int nbits)
134{
135 FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
136 {
137 if (words[word] != 0)
138 return false;
139 }
140
141 return true;
142}
143
144/* Set every bit in the array to '0' */
145static inline void
146__bitarray_clear(unsigned int words[], unsigned int nbits)
147{
148 FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
149 words[word] = 0;
150}
151
152/* Set every bit in the array to '1' */
153static inline void
154__bitarray_set(unsigned int words[], unsigned int nbits)
155{
156 FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
157 words[word] = ~0u;
158}
159
160/* Find the lowest-indexed '1' bit in the bit array, returning the size of
161 the array if none are set */
162static inline unsigned int
163__bitarray_ffs(const unsigned int words[], unsigned int nbits)
164{
165 FOR_EACH_BITARRAY_SET_BIT_ARR(BITARRAY_NWORDS(nbits), words, nbit)
166 return nbit;
167
168 return BITARRAY_BITS(nbits);
169}
170
171/* Return the number of bits currently set to '1' in the bit array */
172static inline unsigned int
173__bitarray_popcount(const unsigned int words[], unsigned int nbits)
174{
175 unsigned int count = 0;
176
177 FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
178 count += __BITARRAY_POPCNT(words[word]);
179
180 return count;
181}
182
183/**
184 * Giant macro to define all the typed functions
185 * typename: The name of the type (e.g. myarr_t myarr;)
186 * fnprefix: The prefix all functions get (e.g. myarr_set_bit)
187 * nbits : The minimum number of bits the array is meant to hold
188 * (the implementation rounds this up to the word size
189 * and all words may be fully utilized)
190 *
191 * uses 'typedef' to freely change from, e.g., struct to union without
192 * changing source code
193 */
194#define BITARRAY_TYPE_DECLARE(typename, fnprefix, nbits) \
195typedef struct \
196{ \
197 unsigned int words[BITARRAY_NWORDS(nbits)]; \
198} typename; \
199static inline unsigned int \
200fnprefix##_get_word(typename *array, unsigned int bitnum) \
201 { return __bitarray_get_word(array->words, bitnum); } \
202static inline void \
203fnprefix##_set_word(typename *array, unsigned int bitnum, \
204 unsigned int wordval) \
205 { __bitarray_set_word(array->words, bitnum, wordval); } \
206static inline void \
207fnprefix##_set_bit(typename *array, unsigned int bitnum) \
208 { __bitarray_set_bit(array->words, bitnum); } \
209static inline void \
210fnprefix##_clear_bit(typename *array, unsigned int bitnum) \
211 { __bitarray_clear_bit(array->words, bitnum); } \
212static inline unsigned int \
213fnprefix##_test_bit(const typename *array, unsigned int bitnum) \
214 { return __bitarray_test_bit(array->words, bitnum); } \
215static inline bool \
216fnprefix##_is_clear(const typename *array) \
217 { return __bitarray_is_clear(array->words, nbits); } \
218static inline void \
219fnprefix##_clear(typename *array) \
220 { __bitarray_clear(array->words, nbits); } \
221static inline void \
222fnprefix##_set(typename *array) \
223 { __bitarray_set(array->words, nbits); } \
224static inline unsigned int \
225fnprefix##_ffs(const typename *array) \
226 { return __bitarray_ffs(array->words, nbits); } \
227static inline unsigned int \
228fnprefix##_popcount(const typename *array) \
229 { return __bitarray_popcount(array->words, nbits); }
230
231#endif /* BITARRAY_H */
diff --git a/firmware/kernel/include/kernel.h b/firmware/kernel/include/kernel.h
index fafff25ce4..d2ffffcda9 100644
--- a/firmware/kernel/include/kernel.h
+++ b/firmware/kernel/include/kernel.h
@@ -26,6 +26,7 @@
26#include "system.h" 26#include "system.h"
27#include "queue.h" 27#include "queue.h"
28#include "mutex.h" 28#include "mutex.h"
29#include "mrsw_lock.h"
29#include "tick.h" 30#include "tick.h"
30 31
31#ifdef INCLUDE_TIMEOUT_API 32#ifdef INCLUDE_TIMEOUT_API
diff --git a/firmware/kernel/include/mutex.h b/firmware/kernel/include/mutex.h
index bcf5701bd9..02b85f331f 100644
--- a/firmware/kernel/include/mutex.h
+++ b/firmware/kernel/include/mutex.h
@@ -28,20 +28,14 @@
28 28
29struct mutex 29struct mutex
30{ 30{
31 struct thread_entry *queue; /* waiter list */ 31 struct thread_entry *queue; /* waiter list */
32 int recursion; /* lock owner recursion count */ 32 int recursion; /* lock owner recursion count */
33 struct blocker blocker; /* priority inheritance info
34 for waiters and owner*/
35 IF_COP( struct corelock cl; ) /* multiprocessor sync */
33#ifdef HAVE_PRIORITY_SCHEDULING 36#ifdef HAVE_PRIORITY_SCHEDULING
34 struct blocker blocker; /* priority inheritance info 37 bool no_preempt;
35 for waiters */
36 bool no_preempt; /* don't allow higher-priority thread
37 to be scheduled even if woken */
38#else
39 struct thread_entry *thread; /* Indicates owner thread - an owner
40 implies a locked state - same goes
41 for priority scheduling
42 (in blocker struct for that) */
43#endif 38#endif
44 IF_COP( struct corelock cl; ) /* multiprocessor sync */
45}; 39};
46 40
47extern void mutex_init(struct mutex *m); 41extern void mutex_init(struct mutex *m);
@@ -56,7 +50,7 @@ static inline void mutex_set_preempt(struct mutex *m, bool preempt)
56#else 50#else
57/* Deprecated but needed for now - firmware/drivers/ata_mmc.c */ 51/* Deprecated but needed for now - firmware/drivers/ata_mmc.c */
58static inline bool mutex_test(const struct mutex *m) 52static inline bool mutex_test(const struct mutex *m)
59 { return m->thread != NULL; } 53 { return m->blocker.thread != NULL; }
60#endif /* HAVE_PRIORITY_SCHEDULING */ 54#endif /* HAVE_PRIORITY_SCHEDULING */
61 55
62#endif /* MUTEX_H */ 56#endif /* MUTEX_H */
diff --git a/firmware/kernel/include/queue.h b/firmware/kernel/include/queue.h
index 1b404f8297..3f24598d5b 100644
--- a/firmware/kernel/include/queue.h
+++ b/firmware/kernel/include/queue.h
@@ -143,6 +143,8 @@ extern bool queue_peek(struct event_queue *q, struct queue_event *ev);
143#define QPEEK_FILTER_COUNT_MASK (0xffu) /* 0x00=1 filter, 0xff=256 filters */ 143#define QPEEK_FILTER_COUNT_MASK (0xffu) /* 0x00=1 filter, 0xff=256 filters */
144#define QPEEK_FILTER_HEAD_ONLY (1u << 8) /* Ignored if no filters */ 144#define QPEEK_FILTER_HEAD_ONLY (1u << 8) /* Ignored if no filters */
145#define QPEEK_REMOVE_EVENTS (1u << 9) /* Remove or discard events */ 145#define QPEEK_REMOVE_EVENTS (1u << 9) /* Remove or discard events */
146#define QPEEK_FILTER1(a) QPEEK_FILTER2((a), (a))
147#define QPEEK_FILTER2(a, b) (&(const long [2]){ (a), (b) })
146extern bool queue_peek_ex(struct event_queue *q, 148extern bool queue_peek_ex(struct event_queue *q,
147 struct queue_event *ev, 149 struct queue_event *ev,
148 unsigned int flags, 150 unsigned int flags,
diff --git a/firmware/kernel/include/thread.h b/firmware/kernel/include/thread.h
index 8c13b462e6..f181f867cb 100644
--- a/firmware/kernel/include/thread.h
+++ b/firmware/kernel/include/thread.h
@@ -28,6 +28,7 @@
28#include <stdbool.h> 28#include <stdbool.h>
29#include "gcc_extensions.h" 29#include "gcc_extensions.h"
30#include "corelock.h" 30#include "corelock.h"
31#include "bitarray.h"
31 32
32/* Priority scheduling (when enabled with HAVE_PRIORITY_SCHEDULING) works 33/* Priority scheduling (when enabled with HAVE_PRIORITY_SCHEDULING) works
33 * by giving high priority threads more CPU time than lower priority threads 34 * by giving high priority threads more CPU time than lower priority threads
@@ -80,6 +81,10 @@
80#endif 81#endif
81 82
82#define MAXTHREADS (BASETHREADS+TARGET_EXTRA_THREADS) 83#define MAXTHREADS (BASETHREADS+TARGET_EXTRA_THREADS)
84
85BITARRAY_TYPE_DECLARE(threadbit_t, threadbit, MAXTHREADS)
86BITARRAY_TYPE_DECLARE(priobit_t, priobit, NUM_PRIORITIES)
87
83/* 88/*
84 * We need more stack when we run under a host 89 * We need more stack when we run under a host
85 * maybe more expensive C lib functions? 90 * maybe more expensive C lib functions?
@@ -134,32 +139,39 @@ struct thread_list
134 struct thread_entry *next; /* Next thread in a list */ 139 struct thread_entry *next; /* Next thread in a list */
135}; 140};
136 141
137#ifdef HAVE_PRIORITY_SCHEDULING 142/* Basic structure describing the owner of an object */
138struct blocker 143struct blocker
139{ 144{
140 struct thread_entry * volatile thread; /* thread blocking other threads 145 struct thread_entry * volatile thread; /* thread blocking other threads
141 (aka. object owner) */ 146 (aka. object owner) */
142 int priority; /* highest priority waiter */ 147#ifdef HAVE_PRIORITY_SCHEDULING
143 struct thread_entry * (*wakeup_protocol)(struct thread_entry *thread); 148 int priority; /* highest priority waiter */
149#endif
144}; 150};
145 151
146/* Choices of wakeup protocol */ 152/* If a thread has a blocker but the blocker's registered thread is NULL,
147 153 then it references this and the struct blocker pointer may be
148/* For transfer of object ownership by one thread to another thread by 154 reinterpreted as such. */
149 * the owning thread itself (mutexes) */ 155struct blocker_splay
150struct thread_entry * 156{
151 wakeup_priority_protocol_transfer(struct thread_entry *thread); 157 struct blocker blocker; /* blocker info (first!) */
158#ifdef HAVE_PRIORITY_SCHEDULING
159 threadbit_t mask; /* mask of nonzero tcounts */
160#if NUM_CORES > 1
161 struct corelock cl; /* mutual exclusion */
162#endif
163#endif /* HAVE_PRIORITY_SCHEDULING */
164};
152 165
153/* For release by owner where ownership doesn't change - other threads, 166#ifdef HAVE_PRIORITY_SCHEDULING
154 * interrupts, timeouts, etc. (mutex timeout, queues) */
155struct thread_entry *
156 wakeup_priority_protocol_release(struct thread_entry *thread);
157 167
168/* Quick-disinherit of priority elevation. Must be a running thread. */
169void priority_disinherit(struct thread_entry *thread, struct blocker *bl);
158 170
159struct priority_distribution 171struct priority_distribution
160{ 172{
161 uint8_t hist[NUM_PRIORITIES]; /* Histogram: Frequency for each priority */ 173 uint8_t hist[NUM_PRIORITIES]; /* Histogram: Frequency for each priority */
162 uint32_t mask; /* Bitmask of hist entries that are not zero */ 174 priobit_t mask; /* Bitmask of hist entries that are not zero */
163}; 175};
164 176
165#endif /* HAVE_PRIORITY_SCHEDULING */ 177#endif /* HAVE_PRIORITY_SCHEDULING */
@@ -210,6 +222,7 @@ struct thread_entry
210 volatile intptr_t retval; /* Return value from a blocked operation/ 222 volatile intptr_t retval; /* Return value from a blocked operation/
211 misc. use */ 223 misc. use */
212#endif 224#endif
225 uint32_t id; /* Current slot id */
213 int __errno; /* Thread error number (errno tls) */ 226 int __errno; /* Thread error number (errno tls) */
214#ifdef HAVE_PRIORITY_SCHEDULING 227#ifdef HAVE_PRIORITY_SCHEDULING
215 /* Priority summary of owned objects that support inheritance */ 228 /* Priority summary of owned objects that support inheritance */
@@ -226,7 +239,6 @@ struct thread_entry
226 unsigned char priority; /* Scheduled priority (higher of base or 239 unsigned char priority; /* Scheduled priority (higher of base or
227 all threads blocked by this one) */ 240 all threads blocked by this one) */
228#endif 241#endif
229 uint16_t id; /* Current slot id */
230 unsigned short stack_size; /* Size of stack in bytes */ 242 unsigned short stack_size; /* Size of stack in bytes */
231 unsigned char state; /* Thread slot state (STATE_*) */ 243 unsigned char state; /* Thread slot state (STATE_*) */
232#ifdef HAVE_SCHEDULER_BOOSTCTRL 244#ifdef HAVE_SCHEDULER_BOOSTCTRL
@@ -238,11 +250,12 @@ struct thread_entry
238}; 250};
239 251
240/*** Macros for internal use ***/ 252/*** Macros for internal use ***/
241/* Thread ID, 16 bits = |VVVVVVVV|SSSSSSSS| */ 253/* Thread ID, 32 bits = |VVVVVVVV|VVVVVVVV|VVVVVVVV|SSSSSSSS| */
242#define THREAD_ID_VERSION_SHIFT 8 254#define THREAD_ID_VERSION_SHIFT 8
243#define THREAD_ID_VERSION_MASK 0xff00 255#define THREAD_ID_VERSION_MASK 0xffffff00
244#define THREAD_ID_SLOT_MASK 0x00ff 256#define THREAD_ID_SLOT_MASK 0x000000ff
245#define THREAD_ID_INIT(n) ((1u << THREAD_ID_VERSION_SHIFT) | (n)) 257#define THREAD_ID_INIT(n) ((1u << THREAD_ID_VERSION_SHIFT) | (n))
258#define THREAD_ID_SLOT(id) ((id) & THREAD_ID_SLOT_MASK)
246 259
247#ifdef HAVE_CORELOCK_OBJECT 260#ifdef HAVE_CORELOCK_OBJECT
248/* Operations to be performed just before stopping a thread and starting 261/* Operations to be performed just before stopping a thread and starting
@@ -337,11 +350,8 @@ void switch_thread(void);
337/* Blocks a thread for at least the specified number of ticks (0 = wait until 350/* Blocks a thread for at least the specified number of ticks (0 = wait until
338 * next tick) */ 351 * next tick) */
339void sleep_thread(int ticks); 352void sleep_thread(int ticks);
340/* Indefinitely blocks the current thread on a thread queue */ 353/* Blocks the current thread on a thread queue (< 0 == infinite) */
341void block_thread(struct thread_entry *current); 354void block_thread(struct thread_entry *current, int timeout);
342/* Blocks the current thread on a thread queue until explicitely woken or
343 * the timeout is reached */
344void block_thread_w_tmo(struct thread_entry *current, int timeout);
345 355
346/* Return bit flags for thread wakeup */ 356/* Return bit flags for thread wakeup */
347#define THREAD_NONE 0x0 /* No thread woken up (exclusive) */ 357#define THREAD_NONE 0x0 /* No thread woken up (exclusive) */
@@ -350,15 +360,32 @@ void block_thread_w_tmo(struct thread_entry *current, int timeout);
350 higher priority than current were woken) */ 360 higher priority than current were woken) */
351 361
352/* A convenience function for waking an entire queue of threads. */ 362/* A convenience function for waking an entire queue of threads. */
353unsigned int thread_queue_wake(struct thread_entry **list); 363unsigned int thread_queue_wake(struct thread_entry **list,
364 volatile int *count);
354 365
355/* Wakeup a thread at the head of a list */ 366/* Wakeup a thread at the head of a list */
356unsigned int wakeup_thread(struct thread_entry **list); 367enum wakeup_thread_protocol
368{
369 WAKEUP_DEFAULT,
370 WAKEUP_TRANSFER,
371 WAKEUP_RELEASE,
372 WAKEUP_TRANSFER_MULTI,
373};
374
375unsigned int wakeup_thread_(struct thread_entry **list
376 IF_PRIO(, enum wakeup_thread_protocol proto));
357 377
358#ifdef HAVE_PRIORITY_SCHEDULING 378#ifdef HAVE_PRIORITY_SCHEDULING
379#define wakeup_thread(list, proto) \
380 wakeup_thread_((list), (proto))
381
359int thread_set_priority(unsigned int thread_id, int priority); 382int thread_set_priority(unsigned int thread_id, int priority);
360int thread_get_priority(unsigned int thread_id); 383int thread_get_priority(unsigned int thread_id);
384#else /* !HAVE_PRIORITY_SCHEDULING */
385#define wakeup_thread(list, proto...) \
386 wakeup_thread_((list));
361#endif /* HAVE_PRIORITY_SCHEDULING */ 387#endif /* HAVE_PRIORITY_SCHEDULING */
388
362#ifdef HAVE_IO_PRIORITY 389#ifdef HAVE_IO_PRIORITY
363void thread_set_io_priority(unsigned int thread_id, int io_priority); 390void thread_set_io_priority(unsigned int thread_id, int io_priority);
364int thread_get_io_priority(unsigned int thread_id); 391int thread_get_io_priority(unsigned int thread_id);
diff --git a/firmware/kernel/mutex.c b/firmware/kernel/mutex.c
index f1e4b3c722..2e90b0f4b1 100644
--- a/firmware/kernel/mutex.c
+++ b/firmware/kernel/mutex.c
@@ -27,31 +27,10 @@
27#include <stdbool.h> 27#include <stdbool.h>
28#include "config.h" 28#include "config.h"
29#include "system.h" 29#include "system.h"
30#include "mutex.h" 30#include "kernel.h"
31#include "corelock.h"
32#include "thread-internal.h" 31#include "thread-internal.h"
33#include "kernel-internal.h" 32#include "kernel-internal.h"
34 33
35static inline void __attribute__((always_inline))
36mutex_set_thread(struct mutex *mtx, struct thread_entry *td)
37{
38#ifdef HAVE_PRIORITY_SCHEDULING
39 mtx->blocker.thread = td;
40#else
41 mtx->thread = td;
42#endif
43}
44
45static inline struct thread_entry * __attribute__((always_inline))
46mutex_get_thread(volatile struct mutex *mtx)
47{
48#ifdef HAVE_PRIORITY_SCHEDULING
49 return mtx->blocker.thread;
50#else
51 return mtx->thread;
52#endif
53}
54
55/* Initialize a mutex object - call before any use and do not call again once 34/* Initialize a mutex object - call before any use and do not call again once
56 * the object is available to other threads */ 35 * the object is available to other threads */
57void mutex_init(struct mutex *m) 36void mutex_init(struct mutex *m)
@@ -59,10 +38,9 @@ void mutex_init(struct mutex *m)
59 corelock_init(&m->cl); 38 corelock_init(&m->cl);
60 m->queue = NULL; 39 m->queue = NULL;
61 m->recursion = 0; 40 m->recursion = 0;
62 mutex_set_thread(m, NULL); 41 m->blocker.thread = NULL;
63#ifdef HAVE_PRIORITY_SCHEDULING 42#ifdef HAVE_PRIORITY_SCHEDULING
64 m->blocker.priority = PRIORITY_IDLE; 43 m->blocker.priority = PRIORITY_IDLE;
65 m->blocker.wakeup_protocol = wakeup_priority_protocol_transfer;
66 m->no_preempt = false; 44 m->no_preempt = false;
67#endif 45#endif
68} 46}
@@ -72,7 +50,7 @@ void mutex_lock(struct mutex *m)
72{ 50{
73 struct thread_entry *current = thread_self_entry(); 51 struct thread_entry *current = thread_self_entry();
74 52
75 if(current == mutex_get_thread(m)) 53 if(current == m->blocker.thread)
76 { 54 {
77 /* current thread already owns this mutex */ 55 /* current thread already owns this mutex */
78 m->recursion++; 56 m->recursion++;
@@ -83,10 +61,10 @@ void mutex_lock(struct mutex *m)
83 corelock_lock(&m->cl); 61 corelock_lock(&m->cl);
84 62
85 /* must read thread again inside cs (a multiprocessor concern really) */ 63 /* must read thread again inside cs (a multiprocessor concern really) */
86 if(LIKELY(mutex_get_thread(m) == NULL)) 64 if(LIKELY(m->blocker.thread == NULL))
87 { 65 {
88 /* lock is open */ 66 /* lock is open */
89 mutex_set_thread(m, current); 67 m->blocker.thread = current;
90 corelock_unlock(&m->cl); 68 corelock_unlock(&m->cl);
91 return; 69 return;
92 } 70 }
@@ -97,7 +75,7 @@ void mutex_lock(struct mutex *m)
97 current->bqp = &m->queue; 75 current->bqp = &m->queue;
98 76
99 disable_irq(); 77 disable_irq();
100 block_thread(current); 78 block_thread(current, TIMEOUT_BLOCK);
101 79
102 corelock_unlock(&m->cl); 80 corelock_unlock(&m->cl);
103 81
@@ -109,9 +87,9 @@ void mutex_lock(struct mutex *m)
109void mutex_unlock(struct mutex *m) 87void mutex_unlock(struct mutex *m)
110{ 88{
111 /* unlocker not being the owner is an unlocking violation */ 89 /* unlocker not being the owner is an unlocking violation */
112 KERNEL_ASSERT(mutex_get_thread(m) == thread_self_entry(), 90 KERNEL_ASSERT(m->blocker.thread == thread_self_entry(),
113 "mutex_unlock->wrong thread (%s != %s)\n", 91 "mutex_unlock->wrong thread (%s != %s)\n",
114 mutex_get_thread(m)->name, 92 m->blocker.thread->name,
115 thread_self_entry()->name); 93 thread_self_entry()->name);
116 94
117 if(m->recursion > 0) 95 if(m->recursion > 0)
@@ -128,25 +106,24 @@ void mutex_unlock(struct mutex *m)
128 if(LIKELY(m->queue == NULL)) 106 if(LIKELY(m->queue == NULL))
129 { 107 {
130 /* no threads waiting - open the lock */ 108 /* no threads waiting - open the lock */
131 mutex_set_thread(m, NULL); 109 m->blocker.thread = NULL;
132 corelock_unlock(&m->cl); 110 corelock_unlock(&m->cl);
133 return; 111 return;
134 } 112 }
135 else
136 {
137 const int oldlevel = disable_irq_save();
138 /* Tranfer of owning thread is handled in the wakeup protocol
139 * if priorities are enabled otherwise just set it from the
140 * queue head. */
141 IFN_PRIO( mutex_set_thread(m, m->queue); )
142 IF_PRIO( unsigned int result = ) wakeup_thread(&m->queue);
143 restore_irq(oldlevel);
144 113
145 corelock_unlock(&m->cl); 114 const int oldlevel = disable_irq_save();
115 /* Tranfer of owning thread is handled in the wakeup protocol
116 * if priorities are enabled otherwise just set it from the
117 * queue head. */
118 IFN_PRIO( m->blocker.thread = m->queue; )
119 unsigned int result = wakeup_thread(&m->queue, WAKEUP_TRANSFER);
120 restore_irq(oldlevel);
121
122 corelock_unlock(&m->cl);
146 123
147#ifdef HAVE_PRIORITY_SCHEDULING 124#ifdef HAVE_PRIORITY_SCHEDULING
148 if((result & THREAD_SWITCH) && !m->no_preempt) 125 if((result & THREAD_SWITCH) && !m->no_preempt)
149 switch_thread(); 126 switch_thread();
150#endif 127#endif
151 } 128 (void)result;
152} 129}
diff --git a/firmware/kernel/queue.c b/firmware/kernel/queue.c
index 379e3f62c8..22a8da9bd3 100644
--- a/firmware/kernel/queue.c
+++ b/firmware/kernel/queue.c
@@ -84,7 +84,7 @@ static void queue_release_sender(struct thread_entry * volatile * sender,
84 *thread->bqp = thread; /* Move blocking queue head to thread since 84 *thread->bqp = thread; /* Move blocking queue head to thread since
85 wakeup_thread wakes the first thread in 85 wakeup_thread wakes the first thread in
86 the list. */ 86 the list. */
87 wakeup_thread(thread->bqp); 87 wakeup_thread(thread->bqp, WAKEUP_RELEASE);
88} 88}
89 89
90/* Releases any waiting threads that are queued with queue_send - 90/* Releases any waiting threads that are queued with queue_send -
@@ -108,16 +108,16 @@ static void queue_release_all_senders(struct event_queue *q)
108 } 108 }
109} 109}
110 110
111#ifdef HAVE_WAKEUP_EXT_CB
111/* Callback to do extra forced removal steps from sender list in addition 112/* Callback to do extra forced removal steps from sender list in addition
112 * to the normal blocking queue removal and priority dis-inherit */ 113 * to the normal blocking queue removal and priority dis-inherit */
113static void queue_remove_sender_thread_cb(struct thread_entry *thread) 114static void queue_remove_sender_thread_cb(struct thread_entry *thread)
114{ 115{
115 *((struct thread_entry **)thread->retval) = NULL; 116 *((struct thread_entry **)thread->retval) = NULL;
116#ifdef HAVE_WAKEUP_EXT_CB
117 thread->wakeup_ext_cb = NULL; 117 thread->wakeup_ext_cb = NULL;
118#endif
119 thread->retval = 0; 118 thread->retval = 0;
120} 119}
120#endif /* HAVE_WAKEUP_EXT_CB */
121 121
122/* Enables queue_send on the specified queue - caller allocates the extra 122/* Enables queue_send on the specified queue - caller allocates the extra
123 * data structure. Only queues which are taken to be owned by a thread should 123 * data structure. Only queues which are taken to be owned by a thread should
@@ -139,7 +139,6 @@ void queue_enable_queue_send(struct event_queue *q,
139 { 139 {
140 memset(send, 0, sizeof(*send)); 140 memset(send, 0, sizeof(*send));
141#ifdef HAVE_PRIORITY_SCHEDULING 141#ifdef HAVE_PRIORITY_SCHEDULING
142 send->blocker.wakeup_protocol = wakeup_priority_protocol_release;
143 send->blocker.priority = PRIORITY_IDLE; 142 send->blocker.priority = PRIORITY_IDLE;
144 if(owner_id != 0) 143 if(owner_id != 0)
145 { 144 {
@@ -268,7 +267,7 @@ void queue_delete(struct event_queue *q)
268 corelock_unlock(&all_queues.cl); 267 corelock_unlock(&all_queues.cl);
269 268
270 /* Release thread(s) waiting on queue head */ 269 /* Release thread(s) waiting on queue head */
271 thread_queue_wake(&q->queue); 270 thread_queue_wake(&q->queue, NULL);
272 271
273#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME 272#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
274 if(q->send) 273 if(q->send)
@@ -325,7 +324,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
325 IF_COP( current->obj_cl = &q->cl; ) 324 IF_COP( current->obj_cl = &q->cl; )
326 current->bqp = &q->queue; 325 current->bqp = &q->queue;
327 326
328 block_thread(current); 327 block_thread(current, TIMEOUT_BLOCK);
329 328
330 corelock_unlock(&q->cl); 329 corelock_unlock(&q->cl);
331 switch_thread(); 330 switch_thread();
@@ -386,7 +385,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
386 IF_COP( current->obj_cl = &q->cl; ) 385 IF_COP( current->obj_cl = &q->cl; )
387 current->bqp = &q->queue; 386 current->bqp = &q->queue;
388 387
389 block_thread_w_tmo(current, ticks); 388 block_thread(current, ticks);
390 corelock_unlock(&q->cl); 389 corelock_unlock(&q->cl);
391 390
392 switch_thread(); 391 switch_thread();
@@ -443,7 +442,7 @@ void queue_post(struct event_queue *q, long id, intptr_t data)
443 queue_do_unblock_sender(q->send, wr); 442 queue_do_unblock_sender(q->send, wr);
444 443
445 /* Wakeup a waiting thread if any */ 444 /* Wakeup a waiting thread if any */
446 wakeup_thread(&q->queue); 445 wakeup_thread(&q->queue, WAKEUP_DEFAULT);
447 446
448 corelock_unlock(&q->cl); 447 corelock_unlock(&q->cl);
449 restore_irq(oldlevel); 448 restore_irq(oldlevel);
@@ -481,7 +480,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
481 } 480 }
482 481
483 /* Wakeup a waiting thread if any */ 482 /* Wakeup a waiting thread if any */
484 wakeup_thread(&q->queue); 483 wakeup_thread(&q->queue, WAKEUP_DEFAULT);
485 484
486 /* Save thread in slot, add to list and wait for reply */ 485 /* Save thread in slot, add to list and wait for reply */
487 *spp = current; 486 *spp = current;
@@ -493,7 +492,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
493 current->retval = (intptr_t)spp; 492 current->retval = (intptr_t)spp;
494 current->bqp = &send->list; 493 current->bqp = &send->list;
495 494
496 block_thread(current); 495 block_thread(current, TIMEOUT_BLOCK);
497 496
498 corelock_unlock(&q->cl); 497 corelock_unlock(&q->cl);
499 switch_thread(); 498 switch_thread();
@@ -502,7 +501,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
502 } 501 }
503 502
504 /* Function as queue_post if sending is not enabled */ 503 /* Function as queue_post if sending is not enabled */
505 wakeup_thread(&q->queue); 504 wakeup_thread(&q->queue, WAKEUP_DEFAULT);
506 505
507 corelock_unlock(&q->cl); 506 corelock_unlock(&q->cl);
508 restore_irq(oldlevel); 507 restore_irq(oldlevel);
diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c
index f9ff0ad987..b6ce7fd742 100644
--- a/firmware/kernel/semaphore.c
+++ b/firmware/kernel/semaphore.c
@@ -82,11 +82,7 @@ int semaphore_wait(struct semaphore *s, int timeout)
82 * explicit in semaphore_release */ 82 * explicit in semaphore_release */
83 current->retval = OBJ_WAIT_TIMEDOUT; 83 current->retval = OBJ_WAIT_TIMEDOUT;
84 84
85 if(timeout > 0) 85 block_thread(current, timeout);
86 block_thread_w_tmo(current, timeout); /* ...or timed out... */
87 else
88 block_thread(current); /* -timeout = infinite */
89
90 corelock_unlock(&s->cl); 86 corelock_unlock(&s->cl);
91 87
92 /* ...and turn control over to next thread */ 88 /* ...and turn control over to next thread */
@@ -118,7 +114,7 @@ void semaphore_release(struct semaphore *s)
118 KERNEL_ASSERT(s->count == 0, 114 KERNEL_ASSERT(s->count == 0,
119 "semaphore_release->threads queued but count=%d!\n", s->count); 115 "semaphore_release->threads queued but count=%d!\n", s->count);
120 s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */ 116 s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */
121 result = wakeup_thread(&s->queue); 117 result = wakeup_thread(&s->queue, WAKEUP_DEFAULT);
122 } 118 }
123 else 119 else
124 { 120 {
diff --git a/firmware/kernel/thread.c b/firmware/kernel/thread.c
index 43ff584a68..0a47f97e93 100644
--- a/firmware/kernel/thread.c
+++ b/firmware/kernel/thread.c
@@ -246,13 +246,13 @@ static void thread_stkov(struct thread_entry *thread)
246 cores[_core].blk_ops.cl_p = &(thread)->slot_cl; }) 246 cores[_core].blk_ops.cl_p = &(thread)->slot_cl; })
247#else 247#else
248#define LOCK_THREAD(thread) \ 248#define LOCK_THREAD(thread) \
249 ({ }) 249 ({ (void)(thread); })
250#define TRY_LOCK_THREAD(thread) \ 250#define TRY_LOCK_THREAD(thread) \
251 ({ }) 251 ({ (void)(thread); })
252#define UNLOCK_THREAD(thread) \ 252#define UNLOCK_THREAD(thread) \
253 ({ }) 253 ({ (void)(thread); })
254#define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \ 254#define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
255 ({ }) 255 ({ (void)(thread); })
256#endif 256#endif
257 257
258/* RTR list */ 258/* RTR list */
@@ -279,6 +279,100 @@ static void thread_stkov(struct thread_entry *thread)
279#define rtr_move_entry_inl(core, from, to) 279#define rtr_move_entry_inl(core, from, to)
280#endif 280#endif
281 281
282static inline void thread_store_context(struct thread_entry *thread)
283{
284#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
285 thread->__errno = errno;
286#endif
287 store_context(&thread->context);
288}
289
290static inline void thread_load_context(struct thread_entry *thread)
291{
292 load_context(&thread->context);
293#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
294 errno = thread->__errno;
295#endif
296}
297
298static inline unsigned int should_switch_tasks(void)
299{
300 unsigned int result = THREAD_OK;
301
302#ifdef HAVE_PRIORITY_SCHEDULING
303 struct thread_entry *current = cores[CURRENT_CORE].running;
304 if (current &&
305 priobit_ffs(&cores[IF_COP_CORE(current->core)].rtr.mask)
306 < current->priority)
307 {
308 /* There is a thread ready to run of higher priority on the same
309 * core as the current one; recommend a task switch. */
310 result |= THREAD_SWITCH;
311 }
312#endif /* HAVE_PRIORITY_SCHEDULING */
313
314 return result;
315}
316
317#ifdef HAVE_PRIORITY_SCHEDULING
318/*---------------------------------------------------------------------------
319 * Locks the thread registered as the owner of the block and makes sure it
320 * didn't change in the meantime
321 *---------------------------------------------------------------------------
322 */
323#if NUM_CORES == 1
324static inline struct thread_entry * lock_blocker_thread(struct blocker *bl)
325{
326 return bl->thread;
327}
328#else /* NUM_CORES > 1 */
329static struct thread_entry * lock_blocker_thread(struct blocker *bl)
330{
331 /* The blocker thread may change during the process of trying to
332 capture it */
333 while (1)
334 {
335 struct thread_entry *t = bl->thread;
336
337 /* TRY, or else deadlocks are possible */
338 if (!t)
339 {
340 struct blocker_splay *blsplay = (struct blocker_splay *)bl;
341 if (corelock_try_lock(&blsplay->cl))
342 {
343 if (!bl->thread)
344 return NULL; /* Still multi */
345
346 corelock_unlock(&blsplay->cl);
347 }
348 }
349 else
350 {
351 if (TRY_LOCK_THREAD(t))
352 {
353 if (bl->thread == t)
354 return t;
355
356 UNLOCK_THREAD(t);
357 }
358 }
359 }
360}
361#endif /* NUM_CORES */
362
363static inline void unlock_blocker_thread(struct blocker *bl)
364{
365#if NUM_CORES > 1
366 struct thread_entry *blt = bl->thread;
367 if (blt)
368 UNLOCK_THREAD(blt);
369 else
370 corelock_unlock(&((struct blocker_splay *)bl)->cl);
371#endif /* NUM_CORES > 1*/
372 (void)bl;
373}
374#endif /* HAVE_PRIORITY_SCHEDULING */
375
282/*--------------------------------------------------------------------------- 376/*---------------------------------------------------------------------------
283 * Thread list structure - circular: 377 * Thread list structure - circular:
284 * +------------------------------+ 378 * +------------------------------+
@@ -420,7 +514,6 @@ static void remove_from_list_tmo(struct thread_entry *thread)
420 } 514 }
421} 515}
422 516
423
424#ifdef HAVE_PRIORITY_SCHEDULING 517#ifdef HAVE_PRIORITY_SCHEDULING
425/*--------------------------------------------------------------------------- 518/*---------------------------------------------------------------------------
426 * Priority distribution structure (one category for each possible priority): 519 * Priority distribution structure (one category for each possible priority):
@@ -476,19 +569,9 @@ static void remove_from_list_tmo(struct thread_entry *thread)
476static inline unsigned int prio_add_entry( 569static inline unsigned int prio_add_entry(
477 struct priority_distribution *pd, int priority) 570 struct priority_distribution *pd, int priority)
478{ 571{
479 unsigned int count; 572 unsigned int count = ++pd->hist[priority];
480 /* Enough size/instruction count difference for ARM makes it worth it to 573 if (count == 1)
481 * use different code (192 bytes for ARM). Only thing better is ASM. */ 574 priobit_set_bit(&pd->mask, priority);
482#ifdef CPU_ARM
483 count = pd->hist[priority];
484 if (++count == 1)
485 pd->mask |= 1 << priority;
486 pd->hist[priority] = count;
487#else /* This one's better for Coldfire */
488 if ((count = ++pd->hist[priority]) == 1)
489 pd->mask |= 1 << priority;
490#endif
491
492 return count; 575 return count;
493} 576}
494 577
@@ -499,18 +582,9 @@ static inline unsigned int prio_add_entry(
499static inline unsigned int prio_subtract_entry( 582static inline unsigned int prio_subtract_entry(
500 struct priority_distribution *pd, int priority) 583 struct priority_distribution *pd, int priority)
501{ 584{
502 unsigned int count; 585 unsigned int count = --pd->hist[priority];
503 586 if (count == 0)
504#ifdef CPU_ARM 587 priobit_clear_bit(&pd->mask, priority);
505 count = pd->hist[priority];
506 if (--count == 0)
507 pd->mask &= ~(1 << priority);
508 pd->hist[priority] = count;
509#else
510 if ((count = --pd->hist[priority]) == 0)
511 pd->mask &= ~(1 << priority);
512#endif
513
514 return count; 588 return count;
515} 589}
516 590
@@ -521,31 +595,38 @@ static inline unsigned int prio_subtract_entry(
521static inline void prio_move_entry( 595static inline void prio_move_entry(
522 struct priority_distribution *pd, int from, int to) 596 struct priority_distribution *pd, int from, int to)
523{ 597{
524 uint32_t mask = pd->mask; 598 if (--pd->hist[from] == 0)
599 priobit_clear_bit(&pd->mask, from);
600
601 if (++pd->hist[to] == 1)
602 priobit_set_bit(&pd->mask, to);
603}
604#endif /* HAVE_PRIORITY_SCHEDULING */
605
606/*---------------------------------------------------------------------------
607 * Move a thread back to a running state on its core.
608 *---------------------------------------------------------------------------
609 */
610static void core_schedule_wakeup(struct thread_entry *thread)
611{
612 const unsigned int core = IF_COP_CORE(thread->core);
525 613
526#ifdef CPU_ARM 614 RTR_LOCK(core);
527 unsigned int count;
528 615
529 count = pd->hist[from]; 616 thread->state = STATE_RUNNING;
530 if (--count == 0)
531 mask &= ~(1 << from);
532 pd->hist[from] = count;
533 617
534 count = pd->hist[to]; 618 add_to_list_l(&cores[core].running, thread);
535 if (++count == 1) 619 rtr_add_entry(core, thread->priority);
536 mask |= 1 << to;
537 pd->hist[to] = count;
538#else
539 if (--pd->hist[from] == 0)
540 mask &= ~(1 << from);
541 620
542 if (++pd->hist[to] == 1) 621 RTR_UNLOCK(core);
543 mask |= 1 << to;
544#endif
545 622
546 pd->mask = mask; 623#if NUM_CORES > 1
624 if (core != CURRENT_CORE)
625 core_wake(core);
626#endif
547} 627}
548 628
629#ifdef HAVE_PRIORITY_SCHEDULING
549/*--------------------------------------------------------------------------- 630/*---------------------------------------------------------------------------
550 * Change the priority and rtr entry for a running thread 631 * Change the priority and rtr entry for a running thread
551 *--------------------------------------------------------------------------- 632 *---------------------------------------------------------------------------
@@ -605,191 +686,211 @@ static int find_highest_priority_in_list_l(
605 * those are prevented, right? :-) 686 * those are prevented, right? :-)
606 *--------------------------------------------------------------------------- 687 *---------------------------------------------------------------------------
607 */ 688 */
608static struct thread_entry * 689static void inherit_priority(
609 blocker_inherit_priority(struct thread_entry *current) 690 struct blocker * const blocker0, struct blocker *bl,
691 struct thread_entry *blt, int newblpr)
610{ 692{
611 const int priority = current->priority; 693 int oldblpr = bl->priority;
612 struct blocker *bl = current->blocker;
613 struct thread_entry * const tstart = current;
614 struct thread_entry *bl_t = bl->thread;
615
616 /* Blocker cannot change since the object protection is held */
617 LOCK_THREAD(bl_t);
618 694
619 for (;;) 695 while (1)
620 { 696 {
621 struct thread_entry *next; 697 if (blt == NULL)
622 int bl_pr = bl->priority; 698 {
699 /* Multiple owners */
700 struct blocker_splay *blsplay = (struct blocker_splay *)bl;
701
702 /* Recurse down the all the branches of this; it's the only way.
703 We might meet the same queue several times if more than one of
704 these threads is waiting the same queue. That isn't a problem
705 for us since we early-terminate, just notable. */
706 FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum)
707 {
708 bl->priority = oldblpr; /* To see the change each time */
709 blt = &threads[slotnum];
710 LOCK_THREAD(blt);
711 inherit_priority(blocker0, bl, blt, newblpr);
712 }
623 713
624 if (priority >= bl_pr) 714 corelock_unlock(&blsplay->cl);
625 break; /* Object priority already high enough */ 715 return;
716 }
626 717
627 bl->priority = priority; 718 bl->priority = newblpr;
628 719
629 /* Add this one */ 720 /* Update blocker thread inheritance record */
630 prio_add_entry(&bl_t->pdist, priority); 721 if (newblpr < PRIORITY_IDLE)
722 prio_add_entry(&blt->pdist, newblpr);
631 723
632 if (bl_pr < PRIORITY_IDLE) 724 if (oldblpr < PRIORITY_IDLE)
633 { 725 prio_subtract_entry(&blt->pdist, oldblpr);
634 /* Not first waiter - subtract old one */
635 prio_subtract_entry(&bl_t->pdist, bl_pr);
636 }
637 726
638 if (priority >= bl_t->priority) 727 int oldpr = blt->priority;
639 break; /* Thread priority high enough */ 728 int newpr = priobit_ffs(&blt->pdist.mask);
729 if (newpr == oldpr)
730 break; /* No blocker thread priority change */
640 731
641 if (bl_t->state == STATE_RUNNING) 732 if (blt->state == STATE_RUNNING)
642 { 733 {
643 /* Blocking thread is a running thread therefore there are no 734 set_running_thread_priority(blt, newpr);
644 * further blockers. Change the "run queue" on which it 735 break; /* Running: last in chain */
645 * resides. */
646 set_running_thread_priority(bl_t, priority);
647 break;
648 } 736 }
649 737
650 bl_t->priority = priority; 738 /* Blocker is blocked */
739 blt->priority = newpr;
651 740
652 /* If blocking thread has a blocker, apply transitive inheritance */ 741 bl = blt->blocker;
653 bl = bl_t->blocker; 742 if (LIKELY(bl == NULL))
743 break; /* Block doesn't support PIP */
654 744
655 if (bl == NULL) 745 if (UNLIKELY(bl == blocker0))
656 break; /* End of chain or object doesn't support inheritance */ 746 break; /* Full circle - deadlock! */
657 747
658 next = bl->thread; 748 /* Blocker becomes current thread and the process repeats */
749 struct thread_entry **bqp = blt->bqp;
750 struct thread_entry *t = blt;
751 blt = lock_blocker_thread(bl);
659 752
660 if (UNLIKELY(next == tstart)) 753 UNLOCK_THREAD(t);
661 break; /* Full-circle - deadlock! */
662 754
663 UNLOCK_THREAD(current); 755 /* Adjust this wait queue */
756 oldblpr = bl->priority;
757 if (newpr <= oldblpr)
758 newblpr = newpr;
759 else if (oldpr <= oldblpr)
760 newblpr = find_highest_priority_in_list_l(*bqp);
664 761
665#if NUM_CORES > 1 762 if (newblpr == oldblpr)
666 for (;;) 763 break; /* Queue priority not changing */
667 { 764 }
668 LOCK_THREAD(next);
669 765
670 /* Blocker could change - retest condition */ 766 UNLOCK_THREAD(blt);
671 if (LIKELY(bl->thread == next)) 767}
672 break;
673 768
674 UNLOCK_THREAD(next); 769/*---------------------------------------------------------------------------
675 next = bl->thread; 770 * Quick-disinherit of priority elevation. 'thread' must be a running thread.
676 } 771 *---------------------------------------------------------------------------
677#endif 772 */
678 current = bl_t; 773static void priority_disinherit_internal(struct thread_entry *thread,
679 bl_t = next; 774 int blpr)
775{
776 if (blpr < PRIORITY_IDLE &&
777 prio_subtract_entry(&thread->pdist, blpr) == 0 &&
778 blpr <= thread->priority)
779 {
780 int priority = priobit_ffs(&thread->pdist.mask);
781 if (priority != thread->priority)
782 set_running_thread_priority(thread, priority);
680 } 783 }
784}
681 785
682 UNLOCK_THREAD(bl_t); 786void priority_disinherit(struct thread_entry *thread, struct blocker *bl)
683 787{
684 return current; 788 LOCK_THREAD(thread);
789 priority_disinherit_internal(thread, bl->priority);
790 UNLOCK_THREAD(thread);
685} 791}
686 792
687/*--------------------------------------------------------------------------- 793/*---------------------------------------------------------------------------
688 * Readjust priorities when waking a thread blocked waiting for another 794 * Transfer ownership from a single owner to a multi-owner splay from a wait
689 * in essence "releasing" the thread's effect on the object owner. Can be 795 * queue
690 * performed from any context.
691 *--------------------------------------------------------------------------- 796 *---------------------------------------------------------------------------
692 */ 797 */
693struct thread_entry * 798static void wakeup_thread_queue_multi_transfer(struct thread_entry *thread)
694 wakeup_priority_protocol_release(struct thread_entry *thread)
695{ 799{
696 const int priority = thread->priority; 800 /* All threads will have the same blocker and queue; only we are changing
697 struct blocker *bl = thread->blocker; 801 it now */
698 struct thread_entry * const tstart = thread; 802 struct thread_entry **bqp = thread->bqp;
699 struct thread_entry *bl_t = bl->thread; 803 struct blocker_splay *blsplay = (struct blocker_splay *)thread->blocker;
804 struct thread_entry *blt = blsplay->blocker.thread;
805
806 /* The first thread is already locked and is assumed tagged "multi" */
807 int count = 1;
808 struct thread_entry *temp_queue = NULL;
809
810 /* 'thread' is locked on entry */
811 while (1)
812 {
813 LOCK_THREAD(blt);
700 814
701 /* Blocker cannot change since object will be locked */ 815 remove_from_list_l(bqp, thread);
702 LOCK_THREAD(bl_t); 816 thread->blocker = NULL;
703 817
704 thread->blocker = NULL; /* Thread not blocked */ 818 struct thread_entry *tnext = *bqp;
819 if (tnext == NULL || tnext->retval == 0)
820 break;
705 821
706 for (;;) 822 add_to_list_l(&temp_queue, thread);
707 {
708 struct thread_entry *next;
709 int bl_pr = bl->priority;
710 823
711 if (priority > bl_pr) 824 UNLOCK_THREAD(thread);
712 break; /* Object priority higher */ 825 UNLOCK_THREAD(blt);
713 826
714 next = *thread->bqp; 827 count++;
828 thread = tnext;
715 829
716 if (next == NULL) 830 LOCK_THREAD(thread);
717 { 831 }
718 /* No more threads in queue */
719 prio_subtract_entry(&bl_t->pdist, bl_pr);
720 bl->priority = PRIORITY_IDLE;
721 }
722 else
723 {
724 /* Check list for highest remaining priority */
725 int queue_pr = find_highest_priority_in_list_l(next);
726 832
727 if (queue_pr == bl_pr) 833 int blpr = blsplay->blocker.priority;
728 break; /* Object priority not changing */ 834 priority_disinherit_internal(blt, blpr);
729 835
730 /* Change queue priority */ 836 /* Locking order reverses here since the threads are no longer on the
731 prio_move_entry(&bl_t->pdist, bl_pr, queue_pr); 837 queue side */
732 bl->priority = queue_pr; 838 if (count > 1)
733 } 839 {
840 add_to_list_l(&temp_queue, thread);
841 UNLOCK_THREAD(thread);
842 corelock_lock(&blsplay->cl);
843
844 blpr = find_highest_priority_in_list_l(*bqp);
845 blsplay->blocker.thread = NULL;
734 846
735 if (bl_pr > bl_t->priority) 847 thread = temp_queue;
736 break; /* thread priority is higher */ 848 LOCK_THREAD(thread);
849 }
850 else
851 {
852 /* Becomes a simple, direct transfer */
853 if (thread->priority <= blpr)
854 blpr = find_highest_priority_in_list_l(*bqp);
855 blsplay->blocker.thread = thread;
856 }
737 857
738 bl_pr = find_first_set_bit(bl_t->pdist.mask); 858 blsplay->blocker.priority = blpr;
739 859
740 if (bl_pr == bl_t->priority) 860 while (1)
741 break; /* Thread priority not changing */ 861 {
862 unsigned int slotnum = THREAD_ID_SLOT(thread->id);
863 threadbit_set_bit(&blsplay->mask, slotnum);
742 864
743 if (bl_t->state == STATE_RUNNING) 865 if (blpr < PRIORITY_IDLE)
744 { 866 {
745 /* No further blockers */ 867 prio_add_entry(&thread->pdist, blpr);
746 set_running_thread_priority(bl_t, bl_pr); 868 if (blpr < thread->priority)
747 break; 869 thread->priority = blpr;
748 } 870 }
749 871
750 bl_t->priority = bl_pr; 872 if (count > 1)
751 873 remove_from_list_l(&temp_queue, thread);
752 /* If blocking thread has a blocker, apply transitive inheritance */
753 bl = bl_t->blocker;
754 874
755 if (bl == NULL) 875 core_schedule_wakeup(thread);
756 break; /* End of chain or object doesn't support inheritance */
757
758 next = bl->thread;
759
760 if (UNLIKELY(next == tstart))
761 break; /* Full-circle - deadlock! */
762 876
763 UNLOCK_THREAD(thread); 877 UNLOCK_THREAD(thread);
764 878
765#if NUM_CORES > 1 879 thread = temp_queue;
766 for (;;) 880 if (thread == NULL)
767 { 881 break;
768 LOCK_THREAD(next);
769
770 /* Blocker could change - retest condition */
771 if (LIKELY(bl->thread == next))
772 break;
773 882
774 UNLOCK_THREAD(next); 883 LOCK_THREAD(thread);
775 next = bl->thread;
776 }
777#endif
778 thread = bl_t;
779 bl_t = next;
780 } 884 }
781 885
782 UNLOCK_THREAD(bl_t); 886 UNLOCK_THREAD(blt);
783 887
784#if NUM_CORES > 1 888 if (count > 1)
785 if (UNLIKELY(thread != tstart))
786 { 889 {
787 /* Relock original if it changed */ 890 corelock_unlock(&blsplay->cl);
788 LOCK_THREAD(tstart);
789 } 891 }
790#endif
791 892
792 return cores[CURRENT_CORE].running; 893 blt->retval = count;
793} 894}
794 895
795/*--------------------------------------------------------------------------- 896/*---------------------------------------------------------------------------
@@ -801,67 +902,95 @@ struct thread_entry *
801 * it is the running thread is made. 902 * it is the running thread is made.
802 *--------------------------------------------------------------------------- 903 *---------------------------------------------------------------------------
803 */ 904 */
804struct thread_entry * 905static void wakeup_thread_transfer(struct thread_entry *thread)
805 wakeup_priority_protocol_transfer(struct thread_entry *thread)
806{ 906{
807 /* Waking thread inherits priority boost from object owner */ 907 /* Waking thread inherits priority boost from object owner (blt) */
808 struct blocker *bl = thread->blocker; 908 struct blocker *bl = thread->blocker;
809 struct thread_entry *bl_t = bl->thread; 909 struct thread_entry *blt = bl->thread;
810 struct thread_entry *next;
811 int bl_pr;
812 910
813 THREAD_ASSERT(cores[CURRENT_CORE].running == bl_t, 911 THREAD_ASSERT(cores[CURRENT_CORE].running == blt,
814 "UPPT->wrong thread", cores[CURRENT_CORE].running); 912 "UPPT->wrong thread", cores[CURRENT_CORE].running);
815 913
816 LOCK_THREAD(bl_t); 914 LOCK_THREAD(blt);
915
916 struct thread_entry **bqp = thread->bqp;
917 remove_from_list_l(bqp, thread);
918 thread->blocker = NULL;
817 919
818 bl_pr = bl->priority; 920 int blpr = bl->priority;
819 921
820 /* Remove the object's boost from the owning thread */ 922 /* Remove the object's boost from the owning thread */
821 if (prio_subtract_entry(&bl_t->pdist, bl_pr) == 0 && 923 if (prio_subtract_entry(&blt->pdist, blpr) == 0 && blpr <= blt->priority)
822 bl_pr <= bl_t->priority)
823 { 924 {
824 /* No more threads at this priority are waiting and the old level is 925 /* No more threads at this priority are waiting and the old level is
825 * at least the thread level */ 926 * at least the thread level */
826 int priority = find_first_set_bit(bl_t->pdist.mask); 927 int priority = priobit_ffs(&blt->pdist.mask);
827 928 if (priority != blt->priority)
828 if (priority != bl_t->priority) 929 set_running_thread_priority(blt, priority);
829 {
830 /* Adjust this thread's priority */
831 set_running_thread_priority(bl_t, priority);
832 }
833 } 930 }
834 931
835 next = *thread->bqp; 932 struct thread_entry *tnext = *bqp;
836 933
837 if (LIKELY(next == NULL)) 934 if (LIKELY(tnext == NULL))
838 { 935 {
839 /* Expected shortcut - no more waiters */ 936 /* Expected shortcut - no more waiters */
840 bl_pr = PRIORITY_IDLE; 937 blpr = PRIORITY_IDLE;
841 } 938 }
842 else 939 else
843 { 940 {
844 if (thread->priority <= bl_pr) 941 /* If lowering, we need to scan threads remaining in queue */
845 { 942 int priority = thread->priority;
846 /* Need to scan threads remaining in queue */ 943 if (priority <= blpr)
847 bl_pr = find_highest_priority_in_list_l(next); 944 blpr = find_highest_priority_in_list_l(tnext);
848 }
849 945
850 if (prio_add_entry(&thread->pdist, bl_pr) == 1 && 946 if (prio_add_entry(&thread->pdist, blpr) == 1 && blpr < priority)
851 bl_pr < thread->priority) 947 thread->priority = blpr; /* Raise new owner */
852 {
853 /* Thread priority must be raised */
854 thread->priority = bl_pr;
855 }
856 } 948 }
857 949
858 bl->thread = thread; /* This thread pwns */ 950 core_schedule_wakeup(thread);
859 bl->priority = bl_pr; /* Save highest blocked priority */ 951 UNLOCK_THREAD(thread);
860 thread->blocker = NULL; /* Thread not blocked */ 952
953 bl->thread = thread; /* This thread pwns */
954 bl->priority = blpr; /* Save highest blocked priority */
955 UNLOCK_THREAD(blt);
956}
957
958/*---------------------------------------------------------------------------
959 * Readjust priorities when waking a thread blocked waiting for another
960 * in essence "releasing" the thread's effect on the object owner. Can be
961 * performed from any context.
962 *---------------------------------------------------------------------------
963 */
964static void wakeup_thread_release(struct thread_entry *thread)
965{
966 struct blocker *bl = thread->blocker;
967 struct thread_entry *blt = lock_blocker_thread(bl);
968 struct thread_entry **bqp = thread->bqp;
969 remove_from_list_l(bqp, thread);
970 thread->blocker = NULL;
971
972 /* Off to see the wizard... */
973 core_schedule_wakeup(thread);
974
975 if (thread->priority > bl->priority)
976 {
977 /* Queue priority won't change */
978 UNLOCK_THREAD(thread);
979 unlock_blocker_thread(bl);
980 return;
981 }
982
983 UNLOCK_THREAD(thread);
861 984
862 UNLOCK_THREAD(bl_t); 985 int newblpr = find_highest_priority_in_list_l(*bqp);
986 if (newblpr == bl->priority)
987 {
988 /* Blocker priority won't change */
989 unlock_blocker_thread(bl);
990 return;
991 }
863 992
864 return bl_t; 993 inherit_priority(bl, bl, blt, newblpr);
865} 994}
866 995
867/*--------------------------------------------------------------------------- 996/*---------------------------------------------------------------------------
@@ -877,9 +1006,8 @@ static void __attribute__((noinline)) check_for_obj_waiters(
877{ 1006{
878 /* Only one bit in the mask should be set with a frequency on 1 which 1007 /* Only one bit in the mask should be set with a frequency on 1 which
879 * represents the thread's own base priority */ 1008 * represents the thread's own base priority */
880 uint32_t mask = thread->pdist.mask; 1009 if (priobit_popcount(&thread->pdist.mask) != 1 ||
881 if ((mask & (mask - 1)) != 0 || 1010 thread->pdist.hist[priobit_ffs(&thread->pdist.mask)] > 1)
882 thread->pdist.hist[find_first_set_bit(mask)] > 1)
883 { 1011 {
884 unsigned char name[32]; 1012 unsigned char name[32];
885 thread_get_name(name, 32, thread); 1013 thread_get_name(name, 32, thread);
@@ -889,26 +1017,72 @@ static void __attribute__((noinline)) check_for_obj_waiters(
889#endif /* HAVE_PRIORITY_SCHEDULING */ 1017#endif /* HAVE_PRIORITY_SCHEDULING */
890 1018
891/*--------------------------------------------------------------------------- 1019/*---------------------------------------------------------------------------
892 * Move a thread back to a running state on its core. 1020 * Explicitly wakeup a thread on a blocking queue. Only effects threads of
1021 * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
1022 *
1023 * This code should be considered a critical section by the caller meaning
1024 * that the object's corelock should be held.
1025 *
1026 * INTERNAL: Intended for use by kernel objects and not for programs.
893 *--------------------------------------------------------------------------- 1027 *---------------------------------------------------------------------------
894 */ 1028 */
895static void core_schedule_wakeup(struct thread_entry *thread) 1029unsigned int wakeup_thread_(struct thread_entry **list
1030 IF_PRIO(, enum wakeup_thread_protocol proto))
896{ 1031{
897 const unsigned int core = IF_COP_CORE(thread->core); 1032 struct thread_entry *thread = *list;
898 1033
899 RTR_LOCK(core); 1034 /* Check if there is a blocked thread at all. */
1035 if (*list == NULL)
1036 return THREAD_NONE;
900 1037
901 thread->state = STATE_RUNNING; 1038 LOCK_THREAD(thread);
902 1039
903 add_to_list_l(&cores[core].running, thread); 1040 /* Determine thread's current state. */
904 rtr_add_entry(core, thread->priority); 1041 switch (thread->state)
1042 {
1043 case STATE_BLOCKED:
1044 case STATE_BLOCKED_W_TMO:
1045#ifdef HAVE_PRIORITY_SCHEDULING
1046 /* Threads with PIP blockers cannot specify "WAKEUP_DEFAULT" */
1047 if (thread->blocker != NULL)
1048 {
1049 static void (* const funcs[])(struct thread_entry *thread)
1050 ICONST_ATTR =
1051 {
1052 [WAKEUP_DEFAULT] = NULL,
1053 [WAKEUP_TRANSFER] = wakeup_thread_transfer,
1054 [WAKEUP_RELEASE] = wakeup_thread_release,
1055 [WAKEUP_TRANSFER_MULTI] = wakeup_thread_queue_multi_transfer,
1056 };
1057
1058 /* Call the specified unblocking PIP (does the rest) */
1059 funcs[proto](thread);
1060 }
1061 else
1062#endif /* HAVE_PRIORITY_SCHEDULING */
1063 {
1064 /* No PIP - just boost the thread by aging */
1065#ifdef HAVE_PRIORITY_SCHEDULING
1066 IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
1067 thread->skip_count = thread->priority;
1068#endif /* HAVE_PRIORITY_SCHEDULING */
1069 remove_from_list_l(list, thread);
1070 core_schedule_wakeup(thread);
1071 UNLOCK_THREAD(thread);
1072 }
905 1073
906 RTR_UNLOCK(core); 1074 return should_switch_tasks();
907 1075
908#if NUM_CORES > 1 1076 /* Nothing to do. State is not blocked. */
909 if (core != CURRENT_CORE) 1077 default:
910 core_wake(core); 1078#if THREAD_EXTRA_CHECKS
1079 THREAD_PANICF("wakeup_thread->block invalid", thread);
1080 case STATE_RUNNING:
1081 case STATE_KILLED:
911#endif 1082#endif
1083 UNLOCK_THREAD(thread);
1084 return THREAD_NONE;
1085 }
912} 1086}
913 1087
914/*--------------------------------------------------------------------------- 1088/*---------------------------------------------------------------------------
@@ -990,8 +1164,6 @@ void check_tmo_threads(void)
990 } 1164 }
991#endif /* NUM_CORES */ 1165#endif /* NUM_CORES */
992 1166
993 remove_from_list_l(curr->bqp, curr);
994
995#ifdef HAVE_WAKEUP_EXT_CB 1167#ifdef HAVE_WAKEUP_EXT_CB
996 if (curr->wakeup_ext_cb != NULL) 1168 if (curr->wakeup_ext_cb != NULL)
997 curr->wakeup_ext_cb(curr); 1169 curr->wakeup_ext_cb(curr);
@@ -999,8 +1171,11 @@ void check_tmo_threads(void)
999 1171
1000#ifdef HAVE_PRIORITY_SCHEDULING 1172#ifdef HAVE_PRIORITY_SCHEDULING
1001 if (curr->blocker != NULL) 1173 if (curr->blocker != NULL)
1002 wakeup_priority_protocol_release(curr); 1174 wakeup_thread_release(curr);
1175 else
1003#endif 1176#endif
1177 remove_from_list_l(curr->bqp, curr);
1178
1004 corelock_unlock(ocl); 1179 corelock_unlock(ocl);
1005 } 1180 }
1006 /* else state == STATE_SLEEPING */ 1181 /* else state == STATE_SLEEPING */
@@ -1161,8 +1336,7 @@ void switch_thread(void)
1161 /* Begin task switching by saving our current context so that we can 1336 /* Begin task switching by saving our current context so that we can
1162 * restore the state of the current thread later to the point prior 1337 * restore the state of the current thread later to the point prior
1163 * to this call. */ 1338 * to this call. */
1164 store_context(&thread->context); 1339 thread_store_context(thread);
1165
1166#ifdef DEBUG 1340#ifdef DEBUG
1167 /* Check core_ctx buflib integrity */ 1341 /* Check core_ctx buflib integrity */
1168 core_check_valid(); 1342 core_check_valid();
@@ -1212,8 +1386,7 @@ void switch_thread(void)
1212 /* Select the new task based on priorities and the last time a 1386 /* Select the new task based on priorities and the last time a
1213 * process got CPU time relative to the highest priority runnable 1387 * process got CPU time relative to the highest priority runnable
1214 * task. */ 1388 * task. */
1215 struct priority_distribution *pd = &cores[core].rtr; 1389 int max = priobit_ffs(&cores[core].rtr.mask);
1216 int max = find_first_set_bit(pd->mask);
1217 1390
1218 if (block == NULL) 1391 if (block == NULL)
1219 { 1392 {
@@ -1269,7 +1442,7 @@ void switch_thread(void)
1269 } 1442 }
1270 1443
1271 /* And finally give control to the next thread. */ 1444 /* And finally give control to the next thread. */
1272 load_context(&thread->context); 1445 thread_load_context(thread);
1273 1446
1274#ifdef RB_PROFILE 1447#ifdef RB_PROFILE
1275 profile_thread_started(thread->id & THREAD_ID_SLOT_MASK); 1448 profile_thread_started(thread->id & THREAD_ID_SLOT_MASK);
@@ -1291,140 +1464,59 @@ void sleep_thread(int ticks)
1291 LOCK_THREAD(current); 1464 LOCK_THREAD(current);
1292 1465
1293 /* Set our timeout, remove from run list and join timeout list. */ 1466 /* Set our timeout, remove from run list and join timeout list. */
1294 current->tmo_tick = current_tick + ticks + 1; 1467 current->tmo_tick = current_tick + MAX(ticks, 0) + 1;
1295 block_thread_on_l(current, STATE_SLEEPING); 1468 block_thread_on_l(current, STATE_SLEEPING);
1296 1469
1297 UNLOCK_THREAD(current); 1470 UNLOCK_THREAD(current);
1298} 1471}
1299 1472
1300/*--------------------------------------------------------------------------- 1473/*---------------------------------------------------------------------------
1301 * Indefinitely block a thread on a blocking queue for explicit wakeup. 1474 * Block a thread on a blocking queue for explicit wakeup. If timeout is
1475 * negative, the block is infinite.
1302 * 1476 *
1303 * INTERNAL: Intended for use by kernel objects and not for programs. 1477 * INTERNAL: Intended for use by kernel objects and not for programs.
1304 *--------------------------------------------------------------------------- 1478 *---------------------------------------------------------------------------
1305 */ 1479 */
1306void block_thread(struct thread_entry *current) 1480void block_thread(struct thread_entry *current, int timeout)
1307{ 1481{
1308 /* Set the state to blocked and take us off of the run queue until we
1309 * are explicitly woken */
1310 LOCK_THREAD(current); 1482 LOCK_THREAD(current);
1311 1483
1312 /* Set the list for explicit wakeup */ 1484 struct blocker *bl = NULL;
1313 block_thread_on_l(current, STATE_BLOCKED);
1314
1315#ifdef HAVE_PRIORITY_SCHEDULING 1485#ifdef HAVE_PRIORITY_SCHEDULING
1316 if (current->blocker != NULL) 1486 bl = current->blocker;
1487 struct thread_entry *blt = bl ? lock_blocker_thread(bl) : NULL;
1488#endif /* HAVE_PRIORITY_SCHEDULING */
1489
1490 if (LIKELY(timeout < 0))
1317 { 1491 {
1318 /* Object supports PIP */ 1492 /* Block until explicitly woken */
1319 current = blocker_inherit_priority(current); 1493 block_thread_on_l(current, STATE_BLOCKED);
1320 } 1494 }
1321#endif 1495 else
1322
1323 UNLOCK_THREAD(current);
1324}
1325
1326/*---------------------------------------------------------------------------
1327 * Block a thread on a blocking queue for a specified time interval or until
1328 * explicitly woken - whichever happens first.
1329 *
1330 * INTERNAL: Intended for use by kernel objects and not for programs.
1331 *---------------------------------------------------------------------------
1332 */
1333void block_thread_w_tmo(struct thread_entry *current, int timeout)
1334{
1335 /* Get the entry for the current running thread. */
1336 LOCK_THREAD(current);
1337
1338 /* Set the state to blocked with the specified timeout */
1339 current->tmo_tick = current_tick + timeout;
1340
1341 /* Set the list for explicit wakeup */
1342 block_thread_on_l(current, STATE_BLOCKED_W_TMO);
1343
1344#ifdef HAVE_PRIORITY_SCHEDULING
1345 if (current->blocker != NULL)
1346 { 1496 {
1347 /* Object supports PIP */ 1497 /* Set the state to blocked with the specified timeout */
1348 current = blocker_inherit_priority(current); 1498 current->tmo_tick = current_tick + timeout;
1499 block_thread_on_l(current, STATE_BLOCKED_W_TMO);
1349 } 1500 }
1350#endif
1351 1501
1352 UNLOCK_THREAD(current); 1502 if (bl == NULL)
1353}
1354
1355/*---------------------------------------------------------------------------
1356 * Explicitly wakeup a thread on a blocking queue. Only effects threads of
1357 * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
1358 *
1359 * This code should be considered a critical section by the caller meaning
1360 * that the object's corelock should be held.
1361 *
1362 * INTERNAL: Intended for use by kernel objects and not for programs.
1363 *---------------------------------------------------------------------------
1364 */
1365unsigned int wakeup_thread(struct thread_entry **list)
1366{
1367 struct thread_entry *thread = *list;
1368 unsigned int result = THREAD_NONE;
1369
1370 /* Check if there is a blocked thread at all. */
1371 if (thread == NULL)
1372 return result;
1373
1374 LOCK_THREAD(thread);
1375
1376 /* Determine thread's current state. */
1377 switch (thread->state)
1378 { 1503 {
1379 case STATE_BLOCKED: 1504 UNLOCK_THREAD(current);
1380 case STATE_BLOCKED_W_TMO: 1505 return;
1381 remove_from_list_l(list, thread); 1506 }
1382
1383 result = THREAD_OK;
1384 1507
1385#ifdef HAVE_PRIORITY_SCHEDULING 1508#ifdef HAVE_PRIORITY_SCHEDULING
1386 struct thread_entry *current; 1509 int newblpr = current->priority;
1387 struct blocker *bl = thread->blocker; 1510 UNLOCK_THREAD(current);
1388
1389 if (bl == NULL)
1390 {
1391 /* No inheritance - just boost the thread by aging */
1392 IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
1393 thread->skip_count = thread->priority;
1394 current = cores[CURRENT_CORE].running;
1395 }
1396 else
1397 {
1398 /* Call the specified unblocking PIP */
1399 current = bl->wakeup_protocol(thread);
1400 }
1401
1402 if (current != NULL &&
1403 find_first_set_bit(cores[IF_COP_CORE(current->core)].rtr.mask)
1404 < current->priority)
1405 {
1406 /* There is a thread ready to run of higher or same priority on
1407 * the same core as the current one; recommend a task switch.
1408 * Knowing if this is an interrupt call would be helpful here. */
1409 result |= THREAD_SWITCH;
1410 }
1411#endif /* HAVE_PRIORITY_SCHEDULING */
1412
1413 core_schedule_wakeup(thread);
1414 break;
1415 1511
1416 /* Nothing to do. State is not blocked. */ 1512 if (newblpr >= bl->priority)
1417#if THREAD_EXTRA_CHECKS 1513 {
1418 default: 1514 unlock_blocker_thread(bl);
1419 THREAD_PANICF("wakeup_thread->block invalid", thread); 1515 return; /* Queue priority won't change */
1420 case STATE_RUNNING:
1421 case STATE_KILLED:
1422 break;
1423#endif
1424 } 1516 }
1425 1517
1426 UNLOCK_THREAD(thread); 1518 inherit_priority(bl, bl, blt, newblpr);
1427 return result; 1519#endif /* HAVE_PRIORITY_SCHEDULING */
1428} 1520}
1429 1521
1430/*--------------------------------------------------------------------------- 1522/*---------------------------------------------------------------------------
@@ -1435,25 +1527,31 @@ unsigned int wakeup_thread(struct thread_entry **list)
1435 * INTERNAL: Intended for use by kernel objects and not for programs. 1527 * INTERNAL: Intended for use by kernel objects and not for programs.
1436 *--------------------------------------------------------------------------- 1528 *---------------------------------------------------------------------------
1437 */ 1529 */
1438unsigned int thread_queue_wake(struct thread_entry **list) 1530unsigned int thread_queue_wake(struct thread_entry **list,
1531 volatile int *count)
1439{ 1532{
1533 int num = 0;
1440 unsigned result = THREAD_NONE; 1534 unsigned result = THREAD_NONE;
1441 1535
1442 for (;;) 1536 for (;;)
1443 { 1537 {
1444 unsigned int rc = wakeup_thread(list); 1538 unsigned int rc = wakeup_thread(list, WAKEUP_DEFAULT);
1445 1539
1446 if (rc == THREAD_NONE) 1540 if (rc == THREAD_NONE)
1447 break; /* No more threads */ 1541 break; /* No more threads */
1448 1542
1449 result |= rc; 1543 result |= rc;
1544 num++;
1450 } 1545 }
1451 1546
1547 if (count)
1548 *count = num;
1549
1452 return result; 1550 return result;
1453} 1551}
1454 1552
1455/*--------------------------------------------------------------------------- 1553/*---------------------------------------------------------------------------
1456 * Assign the thread slot a new ID. Version is 1-255. 1554 * Assign the thread slot a new ID. Version is 0x00000100..0xffffff00.
1457 *--------------------------------------------------------------------------- 1555 *---------------------------------------------------------------------------
1458 */ 1556 */
1459static void new_thread_id(unsigned int slot_num, 1557static void new_thread_id(unsigned int slot_num,
@@ -1693,7 +1791,7 @@ void thread_wait(unsigned int thread_id)
1693 current->bqp = &thread->queue; 1791 current->bqp = &thread->queue;
1694 1792
1695 disable_irq(); 1793 disable_irq();
1696 block_thread(current); 1794 block_thread(current, TIMEOUT_BLOCK);
1697 1795
1698 corelock_unlock(&thread->waiter_cl); 1796 corelock_unlock(&thread->waiter_cl);
1699 1797
@@ -1723,7 +1821,7 @@ static inline void thread_final_exit(struct thread_entry *current)
1723 * execution except the slot itself. */ 1821 * execution except the slot itself. */
1724 1822
1725 /* Signal this thread */ 1823 /* Signal this thread */
1726 thread_queue_wake(&current->queue); 1824 thread_queue_wake(&current->queue, NULL);
1727 corelock_unlock(&current->waiter_cl); 1825 corelock_unlock(&current->waiter_cl);
1728 switch_thread(); 1826 switch_thread();
1729 /* This should never and must never be reached - if it is, the 1827 /* This should never and must never be reached - if it is, the
@@ -1912,20 +2010,18 @@ IF_COP( retry_state: )
1912 } 2010 }
1913 } 2011 }
1914#endif 2012#endif
1915 remove_from_list_l(thread->bqp, thread);
1916
1917#ifdef HAVE_WAKEUP_EXT_CB 2013#ifdef HAVE_WAKEUP_EXT_CB
1918 if (thread->wakeup_ext_cb != NULL) 2014 if (thread->wakeup_ext_cb != NULL)
1919 thread->wakeup_ext_cb(thread); 2015 thread->wakeup_ext_cb(thread);
1920#endif 2016#endif
1921 2017
1922#ifdef HAVE_PRIORITY_SCHEDULING 2018#ifdef HAVE_PRIORITY_SCHEDULING
2019 /* Remove thread's priority influence from its chain if needed */
1923 if (thread->blocker != NULL) 2020 if (thread->blocker != NULL)
1924 {
1925 /* Remove thread's priority influence from its chain */
1926 wakeup_priority_protocol_release(thread); 2021 wakeup_priority_protocol_release(thread);
1927 } 2022 else
1928#endif 2023#endif
2024 remove_from_list_l(thread->bqp, thread);
1929 2025
1930#if NUM_CORES > 1 2026#if NUM_CORES > 1
1931 if (ocl != NULL) 2027 if (ocl != NULL)
@@ -1970,130 +2066,77 @@ thread_killed: /* Thread was already killed */
1970 */ 2066 */
1971int thread_set_priority(unsigned int thread_id, int priority) 2067int thread_set_priority(unsigned int thread_id, int priority)
1972{ 2068{
2069 if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
2070 return -1; /* Invalid priority argument */
2071
1973 int old_base_priority = -1; 2072 int old_base_priority = -1;
1974 struct thread_entry *thread = thread_id_entry(thread_id); 2073 struct thread_entry *thread = thread_id_entry(thread_id);
1975 2074
1976 /* A little safety measure */
1977 if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
1978 return -1;
1979
1980 /* Thread could be on any list and therefore on an interrupt accessible 2075 /* Thread could be on any list and therefore on an interrupt accessible
1981 one - disable interrupts */ 2076 one - disable interrupts */
1982 int oldlevel = disable_irq_save(); 2077 const int oldlevel = disable_irq_save();
1983
1984 LOCK_THREAD(thread); 2078 LOCK_THREAD(thread);
1985 2079
1986 /* Make sure it's not killed */ 2080 if (thread->id != thread_id || thread->state == STATE_KILLED)
1987 if (thread->id == thread_id && thread->state != STATE_KILLED) 2081 goto done; /* Invalid thread */
1988 {
1989 int old_priority = thread->priority;
1990
1991 old_base_priority = thread->base_priority;
1992 thread->base_priority = priority;
1993
1994 prio_move_entry(&thread->pdist, old_base_priority, priority);
1995 priority = find_first_set_bit(thread->pdist.mask);
1996
1997 if (old_priority == priority)
1998 {
1999 /* No priority change - do nothing */
2000 }
2001 else if (thread->state == STATE_RUNNING)
2002 {
2003 /* This thread is running - change location on the run
2004 * queue. No transitive inheritance needed. */
2005 set_running_thread_priority(thread, priority);
2006 }
2007 else
2008 {
2009 thread->priority = priority;
2010
2011 if (thread->blocker != NULL)
2012 {
2013 /* Bubble new priority down the chain */
2014 struct blocker *bl = thread->blocker; /* Blocker struct */
2015 struct thread_entry *bl_t = bl->thread; /* Blocking thread */
2016 struct thread_entry * const tstart = thread; /* Initial thread */
2017 const int highest = MIN(priority, old_priority); /* Higher of new or old */
2018
2019 for (;;)
2020 {
2021 struct thread_entry *next; /* Next thread to check */
2022 int bl_pr; /* Highest blocked thread */
2023 int queue_pr; /* New highest blocked thread */
2024#if NUM_CORES > 1
2025 /* Owner can change but thread cannot be dislodged - thread
2026 * may not be the first in the queue which allows other
2027 * threads ahead in the list to be given ownership during the
2028 * operation. If thread is next then the waker will have to
2029 * wait for us and the owner of the object will remain fixed.
2030 * If we successfully grab the owner -- which at some point
2031 * is guaranteed -- then the queue remains fixed until we
2032 * pass by. */
2033 for (;;)
2034 {
2035 LOCK_THREAD(bl_t);
2036
2037 /* Double-check the owner - retry if it changed */
2038 if (LIKELY(bl->thread == bl_t))
2039 break;
2040
2041 UNLOCK_THREAD(bl_t);
2042 bl_t = bl->thread;
2043 }
2044#endif
2045 bl_pr = bl->priority;
2046
2047 if (highest > bl_pr)
2048 break; /* Object priority won't change */
2049 2082
2050 /* This will include the thread being set */ 2083 old_base_priority = thread->base_priority;
2051 queue_pr = find_highest_priority_in_list_l(*thread->bqp); 2084 if (priority == old_base_priority)
2085 goto done; /* No base priority change */
2052 2086
2053 if (queue_pr == bl_pr) 2087 thread->base_priority = priority;
2054 break; /* Object priority not changing */
2055 2088
2056 /* Update thread boost for this object */ 2089 /* Adjust the thread's priority influence on itself */
2057 bl->priority = queue_pr; 2090 prio_move_entry(&thread->pdist, old_base_priority, priority);
2058 prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
2059 bl_pr = find_first_set_bit(bl_t->pdist.mask);
2060 2091
2061 if (bl_t->priority == bl_pr) 2092 int old_priority = thread->priority;
2062 break; /* Blocking thread priority not changing */ 2093 int new_priority = priobit_ffs(&thread->pdist.mask);
2063 2094
2064 if (bl_t->state == STATE_RUNNING) 2095 if (old_priority == new_priority)
2065 { 2096 goto done; /* No running priority change */
2066 /* Thread not blocked - we're done */
2067 set_running_thread_priority(bl_t, bl_pr);
2068 break;
2069 }
2070 2097
2071 bl_t->priority = bl_pr; 2098 if (thread->state == STATE_RUNNING)
2072 bl = bl_t->blocker; /* Blocking thread has a blocker? */ 2099 {
2100 /* This thread is running - just change location on the run queue.
2101 Also sets thread->priority. */
2102 set_running_thread_priority(thread, new_priority);
2103 goto done;
2104 }
2073 2105
2074 if (bl == NULL) 2106 /* Thread is blocked */
2075 break; /* End of chain */ 2107 struct blocker *bl = thread->blocker;
2108 if (bl == NULL)
2109 {
2110 thread->priority = new_priority;
2111 goto done; /* End of transitive blocks */
2112 }
2076 2113
2077 next = bl->thread; 2114 struct thread_entry *blt = lock_blocker_thread(bl);
2115 struct thread_entry **bqp = thread->bqp;
2078 2116
2079 if (UNLIKELY(next == tstart)) 2117 thread->priority = new_priority;
2080 break; /* Full-circle */
2081 2118
2082 UNLOCK_THREAD(thread); 2119 UNLOCK_THREAD(thread);
2120 thread = NULL;
2083 2121
2084 thread = bl_t; 2122 int oldblpr = bl->priority;
2085 bl_t = next; 2123 int newblpr = oldblpr;
2086 } /* for (;;) */ 2124 if (new_priority < oldblpr)
2125 newblpr = new_priority;
2126 else if (old_priority <= oldblpr)
2127 newblpr = find_highest_priority_in_list_l(*bqp);
2087 2128
2088 UNLOCK_THREAD(bl_t); 2129 if (newblpr == oldblpr)
2089 } 2130 {
2090 } 2131 unlock_blocker_thread(bl);
2132 goto done;
2091 } 2133 }
2092 2134
2093 UNLOCK_THREAD(thread); 2135 inherit_priority(bl, bl, blt, newblpr);
2094 2136done:
2137 if (thread)
2138 UNLOCK_THREAD(thread);
2095 restore_irq(oldlevel); 2139 restore_irq(oldlevel);
2096
2097 return old_base_priority; 2140 return old_base_priority;
2098} 2141}
2099 2142
diff --git a/firmware/target/hosted/sdl/thread-sdl.c b/firmware/target/hosted/sdl/thread-sdl.c
index c17e793833..eaf59e245d 100644
--- a/firmware/target/hosted/sdl/thread-sdl.c
+++ b/firmware/target/hosted/sdl/thread-sdl.c
@@ -406,20 +406,20 @@ void sleep_thread(int ticks)
406 current->tmo_tick = (1000/HZ) * ticks + ((1000/HZ)-1) - rem; 406 current->tmo_tick = (1000/HZ) * ticks + ((1000/HZ)-1) - rem;
407} 407}
408 408
409void block_thread(struct thread_entry *current) 409void block_thread(struct thread_entry *current, int ticks)
410{ 410{
411 current->state = STATE_BLOCKED; 411 if (ticks < 0)
412 add_to_list_l(current->bqp, current); 412 current->state = STATE_BLOCKED;
413} 413 else
414 {
415 current->state = STATE_BLOCKED_W_TMO;
416 current->tmo_tick = (1000/HZ)*ticks;
417 }
414 418
415void block_thread_w_tmo(struct thread_entry *current, int ticks)
416{
417 current->state = STATE_BLOCKED_W_TMO;
418 current->tmo_tick = (1000/HZ)*ticks;
419 add_to_list_l(current->bqp, current); 419 add_to_list_l(current->bqp, current);
420} 420}
421 421
422unsigned int wakeup_thread(struct thread_entry **list) 422unsigned int wakeup_thread_(struct thread_entry **list)
423{ 423{
424 struct thread_entry *thread = *list; 424 struct thread_entry *thread = *list;
425 425
@@ -439,20 +439,26 @@ unsigned int wakeup_thread(struct thread_entry **list)
439 return THREAD_NONE; 439 return THREAD_NONE;
440} 440}
441 441
442unsigned int thread_queue_wake(struct thread_entry **list) 442unsigned int thread_queue_wake(struct thread_entry **list,
443 volatile int *count)
443{ 444{
444 unsigned int result = THREAD_NONE; 445 unsigned int result = THREAD_NONE;
446 int num = 0;
445 447
446 for (;;) 448 for (;;)
447 { 449 {
448 unsigned int rc = wakeup_thread(list); 450 unsigned int rc = wakeup_thread_(list);
449 451
450 if (rc == THREAD_NONE) 452 if (rc == THREAD_NONE)
451 break; 453 break;
452 454
453 result |= rc; 455 result |= rc;
456 num++;
454 } 457 }
455 458
459 if (count)
460 *count = num;
461
456 return result; 462 return result;
457} 463}
458 464
@@ -615,7 +621,7 @@ void remove_thread(unsigned int thread_id)
615 621
616 new_thread_id(thread->id, thread); 622 new_thread_id(thread->id, thread);
617 thread->state = STATE_KILLED; 623 thread->state = STATE_KILLED;
618 thread_queue_wake(&thread->queue); 624 thread_queue_wake(&thread->queue, NULL);
619 625
620 SDL_DestroySemaphore(s); 626 SDL_DestroySemaphore(s);
621 627
@@ -652,7 +658,7 @@ void thread_wait(unsigned int thread_id)
652 if (thread->id == thread_id && thread->state != STATE_KILLED) 658 if (thread->id == thread_id && thread->state != STATE_KILLED)
653 { 659 {
654 current->bqp = &thread->queue; 660 current->bqp = &thread->queue;
655 block_thread(current); 661 block_thread(current, TIMEOUT_BLOCK);
656 switch_thread(); 662 switch_thread();
657 } 663 }
658} 664}
generated by cgit v1.2.3 (git 2.39.1) at 2024-10-02 18:17:24 +0000