diff options
-rw-r--r-- | firmware/SOURCES | 3 | ||||
-rw-r--r-- | firmware/include/bitarray.h | 231 | ||||
-rw-r--r-- | firmware/kernel/include/kernel.h | 1 | ||||
-rw-r--r-- | firmware/kernel/include/mutex.h | 20 | ||||
-rw-r--r-- | firmware/kernel/include/queue.h | 2 | ||||
-rw-r--r-- | firmware/kernel/include/thread.h | 81 | ||||
-rw-r--r-- | firmware/kernel/mutex.c | 65 | ||||
-rw-r--r-- | firmware/kernel/queue.c | 21 | ||||
-rw-r--r-- | firmware/kernel/semaphore.c | 8 | ||||
-rw-r--r-- | firmware/kernel/thread.c | 961 | ||||
-rw-r--r-- | firmware/target/hosted/sdl/thread-sdl.c | 34 |
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 |
1826 | kernel/corelock.c | 1826 | kernel/corelock.c |
1827 | #endif | 1827 | #endif |
1828 | #if 0 /* pending dependent code */ | ||
1829 | kernel/mrsw_lock.c | ||
1830 | #endif | ||
1828 | kernel/mutex.c | 1831 | kernel/mutex.c |
1829 | kernel/queue.c | 1832 | kernel/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 */ | ||
90 | static 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 */ | ||
97 | static 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' */ | ||
105 | static 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' */ | ||
114 | static 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') */ | ||
123 | static 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' */ | ||
132 | static 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' */ | ||
145 | static 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' */ | ||
153 | static 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 */ | ||
162 | static 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 */ | ||
172 | static 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) \ | ||
195 | typedef struct \ | ||
196 | { \ | ||
197 | unsigned int words[BITARRAY_NWORDS(nbits)]; \ | ||
198 | } typename; \ | ||
199 | static inline unsigned int \ | ||
200 | fnprefix##_get_word(typename *array, unsigned int bitnum) \ | ||
201 | { return __bitarray_get_word(array->words, bitnum); } \ | ||
202 | static inline void \ | ||
203 | fnprefix##_set_word(typename *array, unsigned int bitnum, \ | ||
204 | unsigned int wordval) \ | ||
205 | { __bitarray_set_word(array->words, bitnum, wordval); } \ | ||
206 | static inline void \ | ||
207 | fnprefix##_set_bit(typename *array, unsigned int bitnum) \ | ||
208 | { __bitarray_set_bit(array->words, bitnum); } \ | ||
209 | static inline void \ | ||
210 | fnprefix##_clear_bit(typename *array, unsigned int bitnum) \ | ||
211 | { __bitarray_clear_bit(array->words, bitnum); } \ | ||
212 | static inline unsigned int \ | ||
213 | fnprefix##_test_bit(const typename *array, unsigned int bitnum) \ | ||
214 | { return __bitarray_test_bit(array->words, bitnum); } \ | ||
215 | static inline bool \ | ||
216 | fnprefix##_is_clear(const typename *array) \ | ||
217 | { return __bitarray_is_clear(array->words, nbits); } \ | ||
218 | static inline void \ | ||
219 | fnprefix##_clear(typename *array) \ | ||
220 | { __bitarray_clear(array->words, nbits); } \ | ||
221 | static inline void \ | ||
222 | fnprefix##_set(typename *array) \ | ||
223 | { __bitarray_set(array->words, nbits); } \ | ||
224 | static inline unsigned int \ | ||
225 | fnprefix##_ffs(const typename *array) \ | ||
226 | { return __bitarray_ffs(array->words, nbits); } \ | ||
227 | static inline unsigned int \ | ||
228 | fnprefix##_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 | ||
29 | struct mutex | 29 | struct 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 | ||
47 | extern void mutex_init(struct mutex *m); | 41 | extern 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 */ |
58 | static inline bool mutex_test(const struct mutex *m) | 52 | static 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) }) | ||
146 | extern bool queue_peek_ex(struct event_queue *q, | 148 | extern 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 | |||
85 | BITARRAY_TYPE_DECLARE(threadbit_t, threadbit, MAXTHREADS) | ||
86 | BITARRAY_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 */ |
138 | struct blocker | 143 | struct 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) */ | 155 | struct blocker_splay |
150 | struct 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) */ | ||
155 | struct thread_entry * | ||
156 | wakeup_priority_protocol_release(struct thread_entry *thread); | ||
157 | 167 | ||
168 | /* Quick-disinherit of priority elevation. Must be a running thread. */ | ||
169 | void priority_disinherit(struct thread_entry *thread, struct blocker *bl); | ||
158 | 170 | ||
159 | struct priority_distribution | 171 | struct 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) */ |
339 | void sleep_thread(int ticks); | 352 | void 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) */ |
341 | void block_thread(struct thread_entry *current); | 354 | void 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 */ | ||
344 | void 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. */ |
353 | unsigned int thread_queue_wake(struct thread_entry **list); | 363 | unsigned 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 */ |
356 | unsigned int wakeup_thread(struct thread_entry **list); | 367 | enum wakeup_thread_protocol |
368 | { | ||
369 | WAKEUP_DEFAULT, | ||
370 | WAKEUP_TRANSFER, | ||
371 | WAKEUP_RELEASE, | ||
372 | WAKEUP_TRANSFER_MULTI, | ||
373 | }; | ||
374 | |||
375 | unsigned 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 | |||
359 | int thread_set_priority(unsigned int thread_id, int priority); | 382 | int thread_set_priority(unsigned int thread_id, int priority); |
360 | int thread_get_priority(unsigned int thread_id); | 383 | int 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 |
363 | void thread_set_io_priority(unsigned int thread_id, int io_priority); | 390 | void thread_set_io_priority(unsigned int thread_id, int io_priority); |
364 | int thread_get_io_priority(unsigned int thread_id); | 391 | int 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 | ||
35 | static inline void __attribute__((always_inline)) | ||
36 | mutex_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 | |||
45 | static inline struct thread_entry * __attribute__((always_inline)) | ||
46 | mutex_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 */ |
57 | void mutex_init(struct mutex *m) | 36 | void 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) | |||
109 | void mutex_unlock(struct mutex *m) | 87 | void 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 */ |
113 | static void queue_remove_sender_thread_cb(struct thread_entry *thread) | 114 | static 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 | ||
282 | static 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 | |||
290 | static 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 | |||
298 | static 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 | ||
324 | static inline struct thread_entry * lock_blocker_thread(struct blocker *bl) | ||
325 | { | ||
326 | return bl->thread; | ||
327 | } | ||
328 | #else /* NUM_CORES > 1 */ | ||
329 | static 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 | |||
363 | static 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) | |||
476 | static inline unsigned int prio_add_entry( | 569 | static 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( | |||
499 | static inline unsigned int prio_subtract_entry( | 582 | static 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( | |||
521 | static inline void prio_move_entry( | 595 | static 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 | */ | ||
610 | static 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 | */ |
608 | static struct thread_entry * | 689 | static 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; | 773 | static 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); | 786 | void 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 | */ |
693 | struct thread_entry * | 798 | static 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 | */ |
804 | struct thread_entry * | 905 | static 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 | */ | ||
964 | static 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 | */ |
895 | static void core_schedule_wakeup(struct thread_entry *thread) | 1029 | unsigned 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 | */ |
1306 | void block_thread(struct thread_entry *current) | 1480 | void 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 | */ | ||
1333 | void 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 | */ | ||
1365 | unsigned 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 | */ |
1438 | unsigned int thread_queue_wake(struct thread_entry **list) | 1530 | unsigned 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 | */ |
1459 | static void new_thread_id(unsigned int slot_num, | 1557 | static 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(¤t->queue); | 1824 | thread_queue_wake(¤t->queue, NULL); |
1727 | corelock_unlock(¤t->waiter_cl); | 1825 | corelock_unlock(¤t->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 | */ |
1971 | int thread_set_priority(unsigned int thread_id, int priority) | 2067 | int 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 | 2136 | done: | |
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 | ||
409 | void block_thread(struct thread_entry *current) | 409 | void 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 | ||
415 | void 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 | ||
422 | unsigned int wakeup_thread(struct thread_entry **list) | 422 | unsigned 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 | ||
442 | unsigned int thread_queue_wake(struct thread_entry **list) | 442 | unsigned 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 | } |