diff options
-rw-r--r-- | apps/debug_menu.c | 31 | ||||
-rw-r--r-- | firmware/asm/m68k/thread.c | 5 | ||||
-rw-r--r-- | firmware/export/system.h | 38 | ||||
-rw-r--r-- | firmware/kernel/include/mrsw_lock.h | 7 | ||||
-rw-r--r-- | firmware/kernel/include/mutex.h | 12 | ||||
-rw-r--r-- | firmware/kernel/include/queue.h | 4 | ||||
-rw-r--r-- | firmware/kernel/include/semaphore.h | 8 | ||||
-rw-r--r-- | firmware/kernel/include/thread.h | 18 | ||||
-rw-r--r-- | firmware/kernel/mrsw_lock.c | 103 | ||||
-rw-r--r-- | firmware/kernel/mutex.c | 28 | ||||
-rw-r--r-- | firmware/kernel/pthread/thread.c | 81 | ||||
-rw-r--r-- | firmware/kernel/queue.c | 145 | ||||
-rw-r--r-- | firmware/kernel/semaphore.c | 59 | ||||
-rw-r--r-- | firmware/kernel/thread-common.c | 247 | ||||
-rw-r--r-- | firmware/kernel/thread-internal.h | 407 | ||||
-rw-r--r-- | firmware/kernel/thread.c | 1837 | ||||
-rw-r--r-- | firmware/libc/errno.c | 2 | ||||
-rw-r--r-- | firmware/target/arm/pp/app-pp.lds | 1 | ||||
-rw-r--r-- | firmware/target/arm/pp/thread-pp.c | 95 | ||||
-rw-r--r-- | firmware/target/hosted/sdl/thread-sdl.c | 429 |
20 files changed, 1525 insertions, 2032 deletions
diff --git a/apps/debug_menu.c b/apps/debug_menu.c index a11cff9350..61698f5025 100644 --- a/apps/debug_menu.c +++ b/apps/debug_menu.c | |||
@@ -151,25 +151,21 @@ static const char* threads_getname(int selected_item, void *data, | |||
151 | selected_item -= NUM_CORES; | 151 | selected_item -= NUM_CORES; |
152 | #endif | 152 | #endif |
153 | 153 | ||
154 | const char *fmtstr = "%2d: ---"; | ||
155 | |||
154 | struct thread_debug_info threadinfo; | 156 | struct thread_debug_info threadinfo; |
155 | if (thread_get_debug_info(selected_item, &threadinfo) <= 0) | 157 | if (thread_get_debug_info(selected_item, &threadinfo) > 0) |
156 | { | 158 | { |
157 | snprintf(buffer, buffer_len, "%2d: ---", selected_item); | 159 | fmtstr = "%2d:" IF_COP(" (%d)") " %s" IF_PRIO(" %d %d") |
158 | return buffer; | 160 | IFN_SDL(" %2d%%") " %s"; |
159 | } | 161 | } |
160 | 162 | ||
161 | snprintf(buffer, buffer_len, | 163 | snprintf(buffer, buffer_len, fmtstr, |
162 | "%2d: " IF_COP("(%d) ") "%s " IF_PRIO("%d %d ") "%2d%% %s", | ||
163 | selected_item, | 164 | selected_item, |
164 | #if NUM_CORES > 1 | 165 | IF_COP(threadinfo.core,) |
165 | threadinfo.core, | ||
166 | #endif | ||
167 | threadinfo.statusstr, | 166 | threadinfo.statusstr, |
168 | #ifdef HAVE_PRIORITY_SCHEDULING | 167 | IF_PRIO(threadinfo.base_priority, threadinfo.current_priority,) |
169 | threadinfo.base_priority, | 168 | IFN_SDL(threadinfo.stack_usage,) |
170 | threadinfo.current_priority, | ||
171 | #endif | ||
172 | threadinfo.stack_usage, | ||
173 | threadinfo.name); | 169 | threadinfo.name); |
174 | 170 | ||
175 | return buffer; | 171 | return buffer; |
@@ -187,16 +183,9 @@ static bool dbg_os(void) | |||
187 | { | 183 | { |
188 | struct simplelist_info info; | 184 | struct simplelist_info info; |
189 | simplelist_info_init(&info, IF_COP("Core and ") "Stack usage:", | 185 | simplelist_info_init(&info, IF_COP("Core and ") "Stack usage:", |
190 | #if NUM_CORES == 1 | 186 | MAXTHREADS IF_COP( + NUM_CORES ), NULL); |
191 | MAXTHREADS, | ||
192 | #else | ||
193 | MAXTHREADS+NUM_CORES, | ||
194 | #endif | ||
195 | NULL); | ||
196 | #ifndef ROCKBOX_HAS_LOGF | ||
197 | info.hide_selection = true; | 187 | info.hide_selection = true; |
198 | info.scroll_all = true; | 188 | info.scroll_all = true; |
199 | #endif | ||
200 | info.action_callback = dbg_threads_action_callback; | 189 | info.action_callback = dbg_threads_action_callback; |
201 | info.get_name = threads_getname; | 190 | info.get_name = threads_getname; |
202 | return simplelist_show_list(&info); | 191 | return simplelist_show_list(&info); |
diff --git a/firmware/asm/m68k/thread.c b/firmware/asm/m68k/thread.c index 7df89001d7..de07b29729 100644 --- a/firmware/asm/m68k/thread.c +++ b/firmware/asm/m68k/thread.c | |||
@@ -86,6 +86,8 @@ static inline void load_context(const void* addr) | |||
86 | ); | 86 | ); |
87 | } | 87 | } |
88 | 88 | ||
89 | |||
90 | #ifdef RB_PROFILE | ||
89 | /*--------------------------------------------------------------------------- | 91 | /*--------------------------------------------------------------------------- |
90 | * Call this from asm to make sure the sp is pointing to the | 92 | * Call this from asm to make sure the sp is pointing to the |
91 | * correct place before the context is saved. | 93 | * correct place before the context is saved. |
@@ -99,3 +101,6 @@ static inline void _profile_thread_stopped(int current_thread) | |||
99 | :: [id] "r" (current_thread) | 101 | :: [id] "r" (current_thread) |
100 | : "cc", "memory"); | 102 | : "cc", "memory"); |
101 | } | 103 | } |
104 | |||
105 | #define profile_thread_stopped _profile_thread_stopped | ||
106 | #endif /* RB_PROFILE */ | ||
diff --git a/firmware/export/system.h b/firmware/export/system.h index 0a13ec2208..5064fcd91d 100644 --- a/firmware/export/system.h +++ b/firmware/export/system.h | |||
@@ -118,15 +118,17 @@ int get_cpu_boost_counter(void); | |||
118 | #define ALIGN_UP(n, a) ALIGN_DOWN((n)+((a)-1),a) | 118 | #define ALIGN_UP(n, a) ALIGN_DOWN((n)+((a)-1),a) |
119 | 119 | ||
120 | /* align start and end of buffer to nearest integer multiple of a */ | 120 | /* align start and end of buffer to nearest integer multiple of a */ |
121 | #define ALIGN_BUFFER(ptr,len,align) \ | 121 | #define ALIGN_BUFFER(ptr, size, align) \ |
122 | {\ | 122 | ({ \ |
123 | uintptr_t tmp_ptr1 = (uintptr_t)ptr; \ | 123 | size_t __sz = (size); \ |
124 | uintptr_t tmp_ptr2 = tmp_ptr1 + len;\ | 124 | size_t __ali = (align); \ |
125 | tmp_ptr1 = ALIGN_UP(tmp_ptr1,align); \ | 125 | uintptr_t __a1 = (uintptr_t)(ptr); \ |
126 | tmp_ptr2 = ALIGN_DOWN(tmp_ptr2,align); \ | 126 | uintptr_t __a2 = __a1 + __sz; \ |
127 | len = tmp_ptr2 - tmp_ptr1; \ | 127 | __a1 = ALIGN_UP(__a1, __ali); \ |
128 | ptr = (typeof(ptr))tmp_ptr1; \ | 128 | __a2 = ALIGN_DOWN(__a2, __ali); \ |
129 | } | 129 | (ptr) = (typeof (ptr))__a1; \ |
130 | (size) = __a2 > __a1 ? __a2 - __a1 : 0; \ | ||
131 | }) | ||
130 | 132 | ||
131 | #define PTR_ADD(ptr, x) ((typeof(ptr))((char*)(ptr) + (x))) | 133 | #define PTR_ADD(ptr, x) ((typeof(ptr))((char*)(ptr) + (x))) |
132 | #define PTR_SUB(ptr, x) ((typeof(ptr))((char*)(ptr) - (x))) | 134 | #define PTR_SUB(ptr, x) ((typeof(ptr))((char*)(ptr) - (x))) |
@@ -150,11 +152,16 @@ int get_cpu_boost_counter(void); | |||
150 | #endif | 152 | #endif |
151 | 153 | ||
152 | /* Get the byte offset of a type's member */ | 154 | /* Get the byte offset of a type's member */ |
153 | #define OFFSETOF(type, membername) ((off_t)&((type *)0)->membername) | 155 | #ifndef offsetof |
156 | #define offsetof(type, member) __builtin_offsetof(type, member) | ||
157 | #endif | ||
154 | 158 | ||
155 | /* Get the type pointer from one of its members */ | 159 | /* Get the containing item of *ptr in type */ |
156 | #define TYPE_FROM_MEMBER(type, memberptr, membername) \ | 160 | #ifndef container_of |
157 | ((type *)((intptr_t)(memberptr) - OFFSETOF(type, membername))) | 161 | #define container_of(ptr, type, member) ({ \ |
162 | const typeof (((type *)0)->member) *__mptr = (ptr); \ | ||
163 | (type *)((void *)(__mptr) - offsetof(type, member)); }) | ||
164 | #endif | ||
158 | 165 | ||
159 | /* returns index of first set bit or 32 if no bits are set */ | 166 | /* returns index of first set bit or 32 if no bits are set */ |
160 | #if defined(CPU_ARM) && ARM_ARCH >= 5 && !defined(__thumb__) | 167 | #if defined(CPU_ARM) && ARM_ARCH >= 5 && !defined(__thumb__) |
@@ -324,6 +331,11 @@ static inline uint32_t swaw32_hw(uint32_t value) | |||
324 | * for all ARM CPUs. */ | 331 | * for all ARM CPUs. */ |
325 | #ifdef CPU_ARM | 332 | #ifdef CPU_ARM |
326 | #define HAVE_CPU_CACHE_ALIGN | 333 | #define HAVE_CPU_CACHE_ALIGN |
334 | #define MIN_STACK_ALIGN 8 | ||
335 | #endif | ||
336 | |||
337 | #ifndef MIN_STACK_ALIGN | ||
338 | #define MIN_STACK_ALIGN (sizeof (uintptr_t)) | ||
327 | #endif | 339 | #endif |
328 | 340 | ||
329 | /* Calculate CACHEALIGN_SIZE from CACHEALIGN_BITS */ | 341 | /* Calculate CACHEALIGN_SIZE from CACHEALIGN_BITS */ |
diff --git a/firmware/kernel/include/mrsw_lock.h b/firmware/kernel/include/mrsw_lock.h index d919f7be26..7511f87e93 100644 --- a/firmware/kernel/include/mrsw_lock.h +++ b/firmware/kernel/include/mrsw_lock.h | |||
@@ -39,10 +39,9 @@ | |||
39 | */ | 39 | */ |
40 | struct mrsw_lock | 40 | struct mrsw_lock |
41 | { | 41 | { |
42 | int volatile count; /* rd/wr counter; >0 = reader(s), <0 = writer */ | 42 | int volatile count; /* counter; >0 = reader(s), <0 = writer */ |
43 | struct thread_entry *queue; | 43 | struct __wait_queue queue; /* waiter list */ |
44 | struct blocker_splay splay; /* priority inheritance info | 44 | struct blocker_splay splay; /* priority inheritance/owner info */ |
45 | for waiters */ | ||
46 | uint8_t rdrecursion[MAXTHREADS]; /* per-thread reader recursion counts */ | 45 | uint8_t rdrecursion[MAXTHREADS]; /* per-thread reader recursion counts */ |
47 | IF_COP( struct corelock cl; ) | 46 | IF_COP( struct corelock cl; ) |
48 | }; | 47 | }; |
diff --git a/firmware/kernel/include/mutex.h b/firmware/kernel/include/mutex.h index 72736ec8fd..b74bfe23f5 100644 --- a/firmware/kernel/include/mutex.h +++ b/firmware/kernel/include/mutex.h | |||
@@ -26,13 +26,13 @@ | |||
26 | 26 | ||
27 | struct mutex | 27 | struct mutex |
28 | { | 28 | { |
29 | struct thread_entry *queue; /* waiter list */ | 29 | struct __wait_queue queue; /* waiter list */ |
30 | int recursion; /* lock owner recursion count */ | 30 | int recursion; /* lock owner recursion count */ |
31 | struct blocker blocker; /* priority inheritance info | 31 | struct blocker blocker; /* priority inheritance info |
32 | for waiters and owner*/ | 32 | for waiters and owner*/ |
33 | IF_COP( struct corelock cl; ) /* multiprocessor sync */ | 33 | IF_COP( struct corelock cl; ) /* multiprocessor sync */ |
34 | #ifdef HAVE_PRIORITY_SCHEDULING | 34 | #ifdef HAVE_PRIORITY_SCHEDULING |
35 | bool no_preempt; | 35 | bool no_preempt; |
36 | #endif | 36 | #endif |
37 | }; | 37 | }; |
38 | 38 | ||
diff --git a/firmware/kernel/include/queue.h b/firmware/kernel/include/queue.h index 3f24598d5b..afee4c90ff 100644 --- a/firmware/kernel/include/queue.h +++ b/firmware/kernel/include/queue.h | |||
@@ -88,7 +88,7 @@ struct queue_sender_list | |||
88 | /* If non-NULL, there is a thread waiting for the corresponding event */ | 88 | /* If non-NULL, there is a thread waiting for the corresponding event */ |
89 | /* Must be statically allocated to put in non-cached ram. */ | 89 | /* Must be statically allocated to put in non-cached ram. */ |
90 | struct thread_entry *senders[QUEUE_LENGTH]; /* message->thread map */ | 90 | struct thread_entry *senders[QUEUE_LENGTH]; /* message->thread map */ |
91 | struct thread_entry *list; /* list of senders in map */ | 91 | struct __wait_queue list; /* list of senders in map */ |
92 | /* Send info for last message dequeued or NULL if replied or not sent */ | 92 | /* Send info for last message dequeued or NULL if replied or not sent */ |
93 | struct thread_entry * volatile curr_sender; | 93 | struct thread_entry * volatile curr_sender; |
94 | #ifdef HAVE_PRIORITY_SCHEDULING | 94 | #ifdef HAVE_PRIORITY_SCHEDULING |
@@ -108,7 +108,7 @@ struct queue_sender_list | |||
108 | 108 | ||
109 | struct event_queue | 109 | struct event_queue |
110 | { | 110 | { |
111 | struct thread_entry *queue; /* waiter list */ | 111 | struct __wait_queue queue; /* waiter list */ |
112 | struct queue_event events[QUEUE_LENGTH]; /* list of events */ | 112 | struct queue_event events[QUEUE_LENGTH]; /* list of events */ |
113 | unsigned int volatile read; /* head of queue */ | 113 | unsigned int volatile read; /* head of queue */ |
114 | unsigned int volatile write; /* tail of queue */ | 114 | unsigned int volatile write; /* tail of queue */ |
diff --git a/firmware/kernel/include/semaphore.h b/firmware/kernel/include/semaphore.h index 16095d9c2d..1d604a4e76 100644 --- a/firmware/kernel/include/semaphore.h +++ b/firmware/kernel/include/semaphore.h | |||
@@ -26,10 +26,10 @@ | |||
26 | 26 | ||
27 | struct semaphore | 27 | struct semaphore |
28 | { | 28 | { |
29 | struct thread_entry *queue; /* Waiter list */ | 29 | struct __wait_queue queue; /* Waiter list */ |
30 | int volatile count; /* # of waits remaining before unsignaled */ | 30 | int volatile count; /* # of waits remaining before unsignaled */ |
31 | int max; /* maximum # of waits to remain signaled */ | 31 | int max; /* maximum # of waits to remain signaled */ |
32 | IF_COP( struct corelock cl; ) /* multiprocessor sync */ | 32 | IF_COP( struct corelock cl; ) /* multiprocessor sync */ |
33 | }; | 33 | }; |
34 | 34 | ||
35 | extern void semaphore_init(struct semaphore *s, int max, int start); | 35 | extern void semaphore_init(struct semaphore *s, int max, int start); |
diff --git a/firmware/kernel/include/thread.h b/firmware/kernel/include/thread.h index 5a8bff0107..dfb632785e 100644 --- a/firmware/kernel/include/thread.h +++ b/firmware/kernel/include/thread.h | |||
@@ -26,6 +26,7 @@ | |||
26 | #include <stdbool.h> | 26 | #include <stdbool.h> |
27 | #include "config.h" | 27 | #include "config.h" |
28 | #include "gcc_extensions.h" | 28 | #include "gcc_extensions.h" |
29 | #include "linked_list.h" | ||
29 | #include "bitarray.h" | 30 | #include "bitarray.h" |
30 | #include "corelock.h" | 31 | #include "corelock.h" |
31 | 32 | ||
@@ -52,7 +53,7 @@ | |||
52 | #define PRIORITY_REALTIME_4 4 | 53 | #define PRIORITY_REALTIME_4 4 |
53 | #define PRIORITY_REALTIME 4 /* Lowest realtime range */ | 54 | #define PRIORITY_REALTIME 4 /* Lowest realtime range */ |
54 | #define PRIORITY_BUFFERING 15 /* Codec buffering thread */ | 55 | #define PRIORITY_BUFFERING 15 /* Codec buffering thread */ |
55 | #define PRIORITY_USER_INTERFACE 16 /* The main thread */ | 56 | #define PRIORITY_USER_INTERFACE 16 /* For most UI thrads */ |
56 | #define PRIORITY_RECORDING 16 /* Recording thread */ | 57 | #define PRIORITY_RECORDING 16 /* Recording thread */ |
57 | #define PRIORITY_PLAYBACK 16 /* Variable between this and MAX */ | 58 | #define PRIORITY_PLAYBACK 16 /* Variable between this and MAX */ |
58 | #define PRIORITY_PLAYBACK_MAX 5 /* Maximum allowable playback priority */ | 59 | #define PRIORITY_PLAYBACK_MAX 5 /* Maximum allowable playback priority */ |
@@ -61,6 +62,7 @@ | |||
61 | #define NUM_PRIORITIES 32 | 62 | #define NUM_PRIORITIES 32 |
62 | #define PRIORITY_IDLE 32 /* Priority representative of no tasks */ | 63 | #define PRIORITY_IDLE 32 /* Priority representative of no tasks */ |
63 | 64 | ||
65 | #define PRIORITY_MAIN_THREAD PRIORITY_USER_INTERFACE | ||
64 | #define IO_PRIORITY_IMMEDIATE 0 | 66 | #define IO_PRIORITY_IMMEDIATE 0 |
65 | #define IO_PRIORITY_BACKGROUND 32 | 67 | #define IO_PRIORITY_BACKGROUND 32 |
66 | 68 | ||
@@ -108,6 +110,9 @@ extern unsigned sleep(unsigned ticks); | |||
108 | #define IFN_PRIO(...) __VA_ARGS__ | 110 | #define IFN_PRIO(...) __VA_ARGS__ |
109 | #endif | 111 | #endif |
110 | 112 | ||
113 | #define __wait_queue lld_head | ||
114 | #define __wait_queue_node lld_node | ||
115 | |||
111 | /* Basic structure describing the owner of an object */ | 116 | /* Basic structure describing the owner of an object */ |
112 | struct blocker | 117 | struct blocker |
113 | { | 118 | { |
@@ -168,6 +173,7 @@ int thread_get_priority(unsigned int thread_id); | |||
168 | void thread_set_io_priority(unsigned int thread_id, int io_priority); | 173 | void thread_set_io_priority(unsigned int thread_id, int io_priority); |
169 | int thread_get_io_priority(unsigned int thread_id); | 174 | int thread_get_io_priority(unsigned int thread_id); |
170 | #endif /* HAVE_IO_PRIORITY */ | 175 | #endif /* HAVE_IO_PRIORITY */ |
176 | |||
171 | #if NUM_CORES > 1 | 177 | #if NUM_CORES > 1 |
172 | unsigned int switch_core(unsigned int new_core); | 178 | unsigned int switch_core(unsigned int new_core); |
173 | #endif | 179 | #endif |
@@ -186,11 +192,21 @@ int core_get_debug_info(unsigned int core, struct core_debug_info *infop); | |||
186 | 192 | ||
187 | #endif /* NUM_CORES */ | 193 | #endif /* NUM_CORES */ |
188 | 194 | ||
195 | #ifdef HAVE_SDL_THREADS | ||
196 | #define IF_SDL(x...) x | ||
197 | #define IFN_SDL(x...) | ||
198 | #else | ||
199 | #define IF_SDL(x...) | ||
200 | #define IFN_SDL(x...) x | ||
201 | #endif | ||
202 | |||
189 | struct thread_debug_info | 203 | struct thread_debug_info |
190 | { | 204 | { |
191 | char statusstr[4]; | 205 | char statusstr[4]; |
192 | char name[32]; | 206 | char name[32]; |
207 | #ifndef HAVE_SDL_THREADS | ||
193 | unsigned int stack_usage; | 208 | unsigned int stack_usage; |
209 | #endif | ||
194 | #if NUM_CORES > 1 | 210 | #if NUM_CORES > 1 |
195 | unsigned int core; | 211 | unsigned int core; |
196 | #endif | 212 | #endif |
diff --git a/firmware/kernel/mrsw_lock.c b/firmware/kernel/mrsw_lock.c index 45c8801b74..b683f63d5f 100644 --- a/firmware/kernel/mrsw_lock.c +++ b/firmware/kernel/mrsw_lock.c | |||
@@ -19,7 +19,8 @@ | |||
19 | * | 19 | * |
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | #include "kernel-internal.h" | 21 | #include "kernel-internal.h" |
22 | #include "mrsw-lock.h" | 22 | #include <string.h> |
23 | #include "mrsw_lock.h" | ||
23 | 24 | ||
24 | #ifdef HAVE_PRIORITY_SCHEDULING | 25 | #ifdef HAVE_PRIORITY_SCHEDULING |
25 | 26 | ||
@@ -34,13 +35,14 @@ mrsw_reader_claim(struct mrsw_lock *mrsw, struct thread_entry *current, | |||
34 | 35 | ||
35 | static FORCE_INLINE void | 36 | static FORCE_INLINE void |
36 | mrsw_reader_relinquish(struct mrsw_lock *mrsw, struct thread_entry *current, | 37 | mrsw_reader_relinquish(struct mrsw_lock *mrsw, struct thread_entry *current, |
37 | int count, unsigned int slotnum) | 38 | struct thread_entry *first, int count, |
39 | unsigned int slotnum) | ||
38 | { | 40 | { |
39 | /* If no writer is queued or has ownership then noone is queued; | 41 | /* If no writer is queued or has ownership then noone is queued; |
40 | if a writer owns it, then the reader would be blocked instead. | 42 | if a writer owns it, then the reader would be blocked instead. |
41 | Therefore, if the queue has threads, then the next after the | 43 | Therefore, if the queue has threads, then the next after the |
42 | owning readers is a writer and this is not the last reader. */ | 44 | owning readers is a writer and this is not the last reader. */ |
43 | if (mrsw->queue) | 45 | if (first) |
44 | corelock_lock(&mrsw->splay.cl); | 46 | corelock_lock(&mrsw->splay.cl); |
45 | 47 | ||
46 | threadbit_clear_bit(&mrsw->splay.mask, slotnum); | 48 | threadbit_clear_bit(&mrsw->splay.mask, slotnum); |
@@ -61,10 +63,10 @@ mrsw_reader_relinquish(struct mrsw_lock *mrsw, struct thread_entry *current, | |||
61 | threadbit_popcount(&mrsw->splay.mask)); | 63 | threadbit_popcount(&mrsw->splay.mask)); |
62 | /* switch owner to sole remaining reader */ | 64 | /* switch owner to sole remaining reader */ |
63 | slotnum = threadbit_ffs(&mrsw->splay.mask); | 65 | slotnum = threadbit_ffs(&mrsw->splay.mask); |
64 | mrsw->splay.blocker.thread = thread_id_entry(slotnum); | 66 | mrsw->splay.blocker.thread = __thread_slot_entry(slotnum); |
65 | } | 67 | } |
66 | 68 | ||
67 | if (mrsw->queue) | 69 | if (first) |
68 | { | 70 | { |
69 | priority_disinherit(current, &mrsw->splay.blocker); | 71 | priority_disinherit(current, &mrsw->splay.blocker); |
70 | corelock_unlock(&mrsw->splay.cl); | 72 | corelock_unlock(&mrsw->splay.cl); |
@@ -72,23 +74,25 @@ mrsw_reader_relinquish(struct mrsw_lock *mrsw, struct thread_entry *current, | |||
72 | } | 74 | } |
73 | 75 | ||
74 | static FORCE_INLINE unsigned int | 76 | static FORCE_INLINE unsigned int |
75 | mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw, unsigned int slotnum) | 77 | mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw, struct thread_entry *thread, |
78 | unsigned int slotnum) | ||
76 | { | 79 | { |
77 | threadbit_clear_bit(&mrsw->splay.mask, slotnum); | 80 | threadbit_clear_bit(&mrsw->splay.mask, slotnum); |
78 | return wakeup_thread(&mrsw->queue, WAKEUP_TRANSFER); | 81 | return wakeup_thread(thread, WAKEUP_TRANSFER); |
79 | } | 82 | } |
80 | 83 | ||
81 | static FORCE_INLINE unsigned int | 84 | static FORCE_INLINE unsigned int |
82 | mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw) | 85 | mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw, struct thread_entry *thread) |
83 | { | 86 | { |
84 | return wakeup_thread(&mrsw->queue, WAKEUP_TRANSFER); | 87 | return wakeup_thread(thread, WAKEUP_TRANSFER); |
88 | (void)mrsw; | ||
85 | } | 89 | } |
86 | 90 | ||
87 | static FORCE_INLINE unsigned int | 91 | static FORCE_INLINE unsigned int |
88 | mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) | 92 | mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw, struct thread_entry *first) |
89 | { | 93 | { |
90 | unsigned int result = wakeup_thread(&mrsw->queue, WAKEUP_TRANSFER_MULTI); | 94 | unsigned int result = wakeup_thread(first, WAKEUP_TRANSFER_MULTI); |
91 | mrsw->count = thread_self_entry()->retval; | 95 | mrsw->count = __running_self_entry()->retval; |
92 | return result; | 96 | return result; |
93 | } | 97 | } |
94 | 98 | ||
@@ -97,32 +101,36 @@ mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) | |||
97 | #define mrsw_reader_claim(mrsw, current, count, slotnum) \ | 101 | #define mrsw_reader_claim(mrsw, current, count, slotnum) \ |
98 | do {} while (0) | 102 | do {} while (0) |
99 | 103 | ||
100 | #define mrsw_reader_relinquish(mrsw, current, count, slotnum) \ | 104 | #define mrsw_reader_relinquish(mrsw, current, first, count, slotnum) \ |
101 | do {} while (0) | 105 | do {} while (0) |
102 | 106 | ||
103 | static FORCE_INLINE unsigned int | 107 | static FORCE_INLINE unsigned int |
104 | mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw) | 108 | mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw, struct thread_entry *thread) |
105 | { | 109 | { |
106 | mrsw->splay.blocker.thread = mrsw->queue; | 110 | mrsw->splay.blocker.thread = thread; |
107 | return wakeup_thread(&mrsw->queue); | 111 | return wakeup_thread(thread); |
108 | } | 112 | } |
109 | 113 | ||
110 | static FORCE_INLINE unsigned int | 114 | static FORCE_INLINE unsigned int |
111 | mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw) | 115 | mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw, struct thread_entry *thread) |
112 | { | 116 | { |
113 | mrsw->splay.blocker.thread = mrsw->queue; | 117 | mrsw->splay.blocker.thread = thread; |
114 | return wakeup_thread(&mrsw->queue); | 118 | return wakeup_thread(thread); |
115 | } | 119 | } |
116 | 120 | ||
117 | static FORCE_INLINE unsigned int | 121 | static FORCE_INLINE unsigned int |
118 | mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) | 122 | mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw, struct thread_entry *first) |
119 | { | 123 | { |
120 | mrsw->splay.blocker.thread = NULL; | 124 | mrsw->splay.blocker.thread = NULL; |
121 | int count = 0; | 125 | int count = 1; |
122 | 126 | ||
123 | while (mrsw->queue && mrsw->queue->retval != 0) | 127 | while (1) |
124 | { | 128 | { |
125 | wakeup_thread(&mrsw->queue); | 129 | wakeup_thread(first); |
130 | |||
131 | if (!(first = WQ_THREAD_FIRST(&mrsw->queue)) || first->retval == 0) | ||
132 | break; | ||
133 | |||
126 | count++; | 134 | count++; |
127 | } | 135 | } |
128 | 136 | ||
@@ -138,14 +146,11 @@ mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) | |||
138 | void mrsw_init(struct mrsw_lock *mrsw) | 146 | void mrsw_init(struct mrsw_lock *mrsw) |
139 | { | 147 | { |
140 | mrsw->count = 0; | 148 | mrsw->count = 0; |
141 | mrsw->queue = NULL; | 149 | wait_queue_init(&mrsw->queue); |
142 | mrsw->splay.blocker.thread = NULL; | 150 | blocker_splay_init(&mrsw->splay); |
143 | #ifdef HAVE_PRIORITY_SCHEDULING | 151 | #ifdef HAVE_PRIORITY_SCHEDULING |
144 | mrsw->splay.blocker.priority = PRIORITY_IDLE; | ||
145 | threadbit_clear(&mrsw->splay.mask); | ||
146 | corelock_init(&mrsw->splay.cl); | ||
147 | memset(mrsw->rdrecursion, 0, sizeof (mrsw->rdrecursion)); | 152 | memset(mrsw->rdrecursion, 0, sizeof (mrsw->rdrecursion)); |
148 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 153 | #endif |
149 | corelock_init(&mrsw->cl); | 154 | corelock_init(&mrsw->cl); |
150 | } | 155 | } |
151 | 156 | ||
@@ -154,7 +159,7 @@ void mrsw_init(struct mrsw_lock *mrsw) | |||
154 | * access recursively. The current writer is ignored and gets access. */ | 159 | * access recursively. The current writer is ignored and gets access. */ |
155 | void mrsw_read_acquire(struct mrsw_lock *mrsw) | 160 | void mrsw_read_acquire(struct mrsw_lock *mrsw) |
156 | { | 161 | { |
157 | struct thread_entry *current = thread_self_entry(); | 162 | struct thread_entry *current = __running_self_entry(); |
158 | 163 | ||
159 | if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 )) | 164 | if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 )) |
160 | return; /* Read request while holding write access; pass */ | 165 | return; /* Read request while holding write access; pass */ |
@@ -178,7 +183,7 @@ void mrsw_read_acquire(struct mrsw_lock *mrsw) | |||
178 | 183 | ||
179 | int count = mrsw->count; | 184 | int count = mrsw->count; |
180 | 185 | ||
181 | if (LIKELY(count >= 0 && !mrsw->queue)) | 186 | if (LIKELY(count >= 0 && mrsw->queue.head == NULL)) |
182 | { | 187 | { |
183 | /* Lock open to readers: | 188 | /* Lock open to readers: |
184 | IFN_PRIO, mrsw->count tracks reader recursion */ | 189 | IFN_PRIO, mrsw->count tracks reader recursion */ |
@@ -189,13 +194,10 @@ void mrsw_read_acquire(struct mrsw_lock *mrsw) | |||
189 | } | 194 | } |
190 | 195 | ||
191 | /* A writer owns it or is waiting; block... */ | 196 | /* A writer owns it or is waiting; block... */ |
192 | IF_COP( current->obj_cl = &mrsw->cl; ) | ||
193 | IF_PRIO( current->blocker = &mrsw->splay.blocker; ) | ||
194 | current->bqp = &mrsw->queue; | ||
195 | current->retval = 1; /* indicate multi-wake candidate */ | 197 | current->retval = 1; /* indicate multi-wake candidate */ |
196 | 198 | ||
197 | disable_irq(); | 199 | disable_irq(); |
198 | block_thread(current, TIMEOUT_BLOCK); | 200 | block_thread(current, TIMEOUT_BLOCK, &mrsw->queue, &mrsw->splay.blocker); |
199 | 201 | ||
200 | corelock_unlock(&mrsw->cl); | 202 | corelock_unlock(&mrsw->cl); |
201 | 203 | ||
@@ -207,7 +209,7 @@ void mrsw_read_acquire(struct mrsw_lock *mrsw) | |||
207 | * leave opens up access to writer threads. The current writer is ignored. */ | 209 | * leave opens up access to writer threads. The current writer is ignored. */ |
208 | void mrsw_read_release(struct mrsw_lock *mrsw) | 210 | void mrsw_read_release(struct mrsw_lock *mrsw) |
209 | { | 211 | { |
210 | struct thread_entry *current = thread_self_entry(); | 212 | struct thread_entry *current = __running_self_entry(); |
211 | 213 | ||
212 | if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 )) | 214 | if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 )) |
213 | return; /* Read release while holding write access; ignore */ | 215 | return; /* Read release while holding write access; ignore */ |
@@ -237,17 +239,18 @@ void mrsw_read_release(struct mrsw_lock *mrsw) | |||
237 | unsigned int result = THREAD_NONE; | 239 | unsigned int result = THREAD_NONE; |
238 | const int oldlevel = disable_irq_save(); | 240 | const int oldlevel = disable_irq_save(); |
239 | 241 | ||
240 | if (--count == 0 && mrsw->queue) | 242 | struct thread_entry *thread = WQ_THREAD_FIRST(&mrsw->queue); |
243 | if (--count == 0 && thread != NULL) | ||
241 | { | 244 | { |
242 | /* No readers remain and a writer is waiting */ | 245 | /* No readers remain and a writer is waiting */ |
243 | mrsw->count = -1; | 246 | mrsw->count = -1; |
244 | result = mrsw_reader_wakeup_writer(mrsw IF_PRIO(, slotnum)); | 247 | result = mrsw_reader_wakeup_writer(mrsw, thread IF_PRIO(, slotnum)); |
245 | } | 248 | } |
246 | else | 249 | else |
247 | { | 250 | { |
248 | /* Giving up readership; we may be the last, or not */ | 251 | /* Giving up readership; we may be the last, or not */ |
249 | mrsw->count = count; | 252 | mrsw->count = count; |
250 | mrsw_reader_relinquish(mrsw, current, count, slotnum); | 253 | mrsw_reader_relinquish(mrsw, current, thread, count, slotnum); |
251 | } | 254 | } |
252 | 255 | ||
253 | restore_irq(oldlevel); | 256 | restore_irq(oldlevel); |
@@ -265,7 +268,7 @@ void mrsw_read_release(struct mrsw_lock *mrsw) | |||
265 | * safely call recursively. */ | 268 | * safely call recursively. */ |
266 | void mrsw_write_acquire(struct mrsw_lock *mrsw) | 269 | void mrsw_write_acquire(struct mrsw_lock *mrsw) |
267 | { | 270 | { |
268 | struct thread_entry *current = thread_self_entry(); | 271 | struct thread_entry *current = __running_self_entry(); |
269 | 272 | ||
270 | if (current == mrsw->splay.blocker.thread) | 273 | if (current == mrsw->splay.blocker.thread) |
271 | { | 274 | { |
@@ -288,13 +291,10 @@ void mrsw_write_acquire(struct mrsw_lock *mrsw) | |||
288 | } | 291 | } |
289 | 292 | ||
290 | /* Readers present or a writer owns it - block... */ | 293 | /* Readers present or a writer owns it - block... */ |
291 | IF_COP( current->obj_cl = &mrsw->cl; ) | ||
292 | IF_PRIO( current->blocker = &mrsw->splay.blocker; ) | ||
293 | current->bqp = &mrsw->queue; | ||
294 | current->retval = 0; /* indicate single-wake candidate */ | 294 | current->retval = 0; /* indicate single-wake candidate */ |
295 | 295 | ||
296 | disable_irq(); | 296 | disable_irq(); |
297 | block_thread(current, TIMEOUT_BLOCK); | 297 | block_thread(current, TIMEOUT_BLOCK, &mrsw->queue, &mrsw->splay.blocker); |
298 | 298 | ||
299 | corelock_unlock(&mrsw->cl); | 299 | corelock_unlock(&mrsw->cl); |
300 | 300 | ||
@@ -305,9 +305,9 @@ void mrsw_write_acquire(struct mrsw_lock *mrsw) | |||
305 | /* Release writer thread lock and open the lock to readers and writers */ | 305 | /* Release writer thread lock and open the lock to readers and writers */ |
306 | void mrsw_write_release(struct mrsw_lock *mrsw) | 306 | void mrsw_write_release(struct mrsw_lock *mrsw) |
307 | { | 307 | { |
308 | KERNEL_ASSERT(thread_self_entry() == mrsw->splay.blocker.thread, | 308 | KERNEL_ASSERT(__running_self_entry() == mrsw->splay.blocker.thread, |
309 | "mrsw_write_release->wrong thread (%s != %s)\n", | 309 | "mrsw_write_release->wrong thread (%s != %s)\n", |
310 | thread_self_entry()->name, | 310 | __running_self_entry()->name, |
311 | mrsw->splay.blocker.thread->name); | 311 | mrsw->splay.blocker.thread->name); |
312 | 312 | ||
313 | int count = mrsw->count; | 313 | int count = mrsw->count; |
@@ -323,15 +323,16 @@ void mrsw_write_release(struct mrsw_lock *mrsw) | |||
323 | corelock_lock(&mrsw->cl); | 323 | corelock_lock(&mrsw->cl); |
324 | const int oldlevel = disable_irq_save(); | 324 | const int oldlevel = disable_irq_save(); |
325 | 325 | ||
326 | if (mrsw->queue == NULL) /* 'count' becomes zero */ | 326 | struct thread_entry *thread = WQ_THREAD_FIRST(&mrsw->queue); |
327 | if (thread == NULL) /* 'count' becomes zero */ | ||
327 | { | 328 | { |
328 | mrsw->splay.blocker.thread = NULL; | 329 | mrsw->splay.blocker.thread = NULL; |
329 | mrsw->count = 0; | 330 | mrsw->count = 0; |
330 | } | 331 | } |
331 | else if (mrsw->queue->retval == 0) /* 'count' stays -1 */ | 332 | else if (thread->retval == 0) /* 'count' stays -1 */ |
332 | result = mrsw_writer_wakeup_writer(mrsw); | 333 | result = mrsw_writer_wakeup_writer(mrsw, thread); |
333 | else /* 'count' becomes # of readers */ | 334 | else /* 'count' becomes # of readers */ |
334 | result = mrsw_writer_wakeup_readers(mrsw); | 335 | result = mrsw_writer_wakeup_readers(mrsw, thread); |
335 | 336 | ||
336 | restore_irq(oldlevel); | 337 | restore_irq(oldlevel); |
337 | corelock_unlock(&mrsw->cl); | 338 | corelock_unlock(&mrsw->cl); |
diff --git a/firmware/kernel/mutex.c b/firmware/kernel/mutex.c index e5729dc893..fc49cc6d09 100644 --- a/firmware/kernel/mutex.c +++ b/firmware/kernel/mutex.c | |||
@@ -30,20 +30,19 @@ | |||
30 | * the object is available to other threads */ | 30 | * the object is available to other threads */ |
31 | void mutex_init(struct mutex *m) | 31 | void mutex_init(struct mutex *m) |
32 | { | 32 | { |
33 | corelock_init(&m->cl); | 33 | wait_queue_init(&m->queue); |
34 | m->queue = NULL; | ||
35 | m->recursion = 0; | 34 | m->recursion = 0; |
36 | m->blocker.thread = NULL; | 35 | blocker_init(&m->blocker); |
37 | #ifdef HAVE_PRIORITY_SCHEDULING | 36 | #ifdef HAVE_PRIORITY_SCHEDULING |
38 | m->blocker.priority = PRIORITY_IDLE; | ||
39 | m->no_preempt = false; | 37 | m->no_preempt = false; |
40 | #endif | 38 | #endif |
39 | corelock_init(&m->cl); | ||
41 | } | 40 | } |
42 | 41 | ||
43 | /* Gain ownership of a mutex object or block until it becomes free */ | 42 | /* Gain ownership of a mutex object or block until it becomes free */ |
44 | void mutex_lock(struct mutex *m) | 43 | void mutex_lock(struct mutex *m) |
45 | { | 44 | { |
46 | struct thread_entry *current = thread_self_entry(); | 45 | struct thread_entry *current = __running_self_entry(); |
47 | 46 | ||
48 | if(current == m->blocker.thread) | 47 | if(current == m->blocker.thread) |
49 | { | 48 | { |
@@ -65,12 +64,8 @@ void mutex_lock(struct mutex *m) | |||
65 | } | 64 | } |
66 | 65 | ||
67 | /* block until the lock is open... */ | 66 | /* block until the lock is open... */ |
68 | IF_COP( current->obj_cl = &m->cl; ) | ||
69 | IF_PRIO( current->blocker = &m->blocker; ) | ||
70 | current->bqp = &m->queue; | ||
71 | |||
72 | disable_irq(); | 67 | disable_irq(); |
73 | block_thread(current, TIMEOUT_BLOCK); | 68 | block_thread(current, TIMEOUT_BLOCK, &m->queue, &m->blocker); |
74 | 69 | ||
75 | corelock_unlock(&m->cl); | 70 | corelock_unlock(&m->cl); |
76 | 71 | ||
@@ -82,10 +77,10 @@ void mutex_lock(struct mutex *m) | |||
82 | void mutex_unlock(struct mutex *m) | 77 | void mutex_unlock(struct mutex *m) |
83 | { | 78 | { |
84 | /* unlocker not being the owner is an unlocking violation */ | 79 | /* unlocker not being the owner is an unlocking violation */ |
85 | KERNEL_ASSERT(m->blocker.thread == thread_self_entry(), | 80 | KERNEL_ASSERT(m->blocker.thread == __running_self_entry(), |
86 | "mutex_unlock->wrong thread (%s != %s)\n", | 81 | "mutex_unlock->wrong thread (%s != %s)\n", |
87 | m->blocker.thread->name, | 82 | m->blocker.thread->name, |
88 | thread_self_entry()->name); | 83 | __running_self_entry()->name); |
89 | 84 | ||
90 | if(m->recursion > 0) | 85 | if(m->recursion > 0) |
91 | { | 86 | { |
@@ -98,7 +93,8 @@ void mutex_unlock(struct mutex *m) | |||
98 | corelock_lock(&m->cl); | 93 | corelock_lock(&m->cl); |
99 | 94 | ||
100 | /* transfer to next queued thread if any */ | 95 | /* transfer to next queued thread if any */ |
101 | if(LIKELY(m->queue == NULL)) | 96 | struct thread_entry *thread = WQ_THREAD_FIRST(&m->queue); |
97 | if(LIKELY(thread == NULL)) | ||
102 | { | 98 | { |
103 | /* no threads waiting - open the lock */ | 99 | /* no threads waiting - open the lock */ |
104 | m->blocker.thread = NULL; | 100 | m->blocker.thread = NULL; |
@@ -107,11 +103,7 @@ void mutex_unlock(struct mutex *m) | |||
107 | } | 103 | } |
108 | 104 | ||
109 | const int oldlevel = disable_irq_save(); | 105 | const int oldlevel = disable_irq_save(); |
110 | /* Tranfer of owning thread is handled in the wakeup protocol | 106 | unsigned int result = wakeup_thread(thread, WAKEUP_TRANSFER); |
111 | * if priorities are enabled otherwise just set it from the | ||
112 | * queue head. */ | ||
113 | IFN_PRIO( m->blocker.thread = m->queue; ) | ||
114 | unsigned int result = wakeup_thread(&m->queue, WAKEUP_TRANSFER); | ||
115 | restore_irq(oldlevel); | 107 | restore_irq(oldlevel); |
116 | 108 | ||
117 | corelock_unlock(&m->cl); | 109 | corelock_unlock(&m->cl); |
diff --git a/firmware/kernel/pthread/thread.c b/firmware/kernel/pthread/thread.c index 354a946698..71cbd1d136 100644 --- a/firmware/kernel/pthread/thread.c +++ b/firmware/kernel/pthread/thread.c | |||
@@ -3,8 +3,8 @@ | |||
3 | #include <errno.h> | 3 | #include <errno.h> |
4 | #include <pthread.h> | 4 | #include <pthread.h> |
5 | #include "/usr/include/semaphore.h" | 5 | #include "/usr/include/semaphore.h" |
6 | #include "thread-internal.h" | ||
6 | #include "kernel.h" | 7 | #include "kernel.h" |
7 | #include "thread.h" | ||
8 | 8 | ||
9 | #define NSEC_PER_SEC 1000000000L | 9 | #define NSEC_PER_SEC 1000000000L |
10 | static inline void timespec_add_ns(struct timespec *a, uint64_t ns) | 10 | static inline void timespec_add_ns(struct timespec *a, uint64_t ns) |
@@ -25,11 +25,6 @@ struct thread_init_data { | |||
25 | 25 | ||
26 | __thread struct thread_entry *_current; | 26 | __thread struct thread_entry *_current; |
27 | 27 | ||
28 | struct thread_entry* thread_self_entry(void) | ||
29 | { | ||
30 | return _current; | ||
31 | } | ||
32 | |||
33 | unsigned int thread_self(void) | 28 | unsigned int thread_self(void) |
34 | { | 29 | { |
35 | return (unsigned) pthread_self(); | 30 | return (unsigned) pthread_self(); |
@@ -70,12 +65,10 @@ static void *trampoline(void *arg) | |||
70 | if (data->start_frozen) | 65 | if (data->start_frozen) |
71 | { | 66 | { |
72 | struct corelock thaw_lock; | 67 | struct corelock thaw_lock; |
73 | struct thread_entry *queue = NULL; | ||
74 | corelock_init(&thaw_lock); | 68 | corelock_init(&thaw_lock); |
75 | corelock_lock(&thaw_lock); | 69 | corelock_lock(&thaw_lock); |
76 | 70 | ||
77 | _current->lock = &thaw_lock; | 71 | _current->lock = &thaw_lock; |
78 | _current->bqp = &queue; | ||
79 | sem_post(&data->init_sem); | 72 | sem_post(&data->init_sem); |
80 | block_thread_switch(_current, _current->lock); | 73 | block_thread_switch(_current, _current->lock); |
81 | _current->lock = NULL; | 74 | _current->lock = NULL; |
@@ -97,7 +90,7 @@ void thread_thaw(unsigned int thread_id) | |||
97 | if (e->lock) | 90 | if (e->lock) |
98 | { | 91 | { |
99 | corelock_lock(e->lock); | 92 | corelock_lock(e->lock); |
100 | wakeup_thread(e->bqp); | 93 | wakeup_thread(e); |
101 | corelock_unlock(e->lock); | 94 | corelock_unlock(e->lock); |
102 | } | 95 | } |
103 | /* else: no lock. must be running already */ | 96 | /* else: no lock. must be running already */ |
@@ -135,7 +128,7 @@ unsigned int create_thread(void (*function)(void), | |||
135 | data->entry = entry; | 128 | data->entry = entry; |
136 | pthread_cond_init(&entry->cond, NULL); | 129 | pthread_cond_init(&entry->cond, NULL); |
137 | entry->runnable = true; | 130 | entry->runnable = true; |
138 | entry->l = (struct thread_list) { NULL, NULL }; | 131 | |
139 | sem_init(&data->init_sem, 0, 0); | 132 | sem_init(&data->init_sem, 0, 0); |
140 | 133 | ||
141 | if (pthread_create(&retval, NULL, trampoline, data) < 0) | 134 | if (pthread_create(&retval, NULL, trampoline, data) < 0) |
@@ -153,58 +146,19 @@ unsigned int create_thread(void (*function)(void), | |||
153 | return retval; | 146 | return retval; |
154 | } | 147 | } |
155 | 148 | ||
156 | static void add_to_list_l(struct thread_entry **list, | ||
157 | struct thread_entry *thread) | ||
158 | { | ||
159 | if (*list == NULL) | ||
160 | { | ||
161 | /* Insert into unoccupied list */ | ||
162 | thread->l.next = thread; | ||
163 | thread->l.prev = thread; | ||
164 | *list = thread; | ||
165 | } | ||
166 | else | ||
167 | { | ||
168 | /* Insert last */ | ||
169 | thread->l.next = *list; | ||
170 | thread->l.prev = (*list)->l.prev; | ||
171 | thread->l.prev->l.next = thread; | ||
172 | (*list)->l.prev = thread; | ||
173 | } | ||
174 | } | ||
175 | |||
176 | static void remove_from_list_l(struct thread_entry **list, | ||
177 | struct thread_entry *thread) | ||
178 | { | ||
179 | if (thread == thread->l.next) | ||
180 | { | ||
181 | /* The only item */ | ||
182 | *list = NULL; | ||
183 | return; | ||
184 | } | ||
185 | |||
186 | if (thread == *list) | ||
187 | { | ||
188 | /* List becomes next item */ | ||
189 | *list = thread->l.next; | ||
190 | } | ||
191 | |||
192 | /* Fix links to jump over the removed entry. */ | ||
193 | thread->l.prev->l.next = thread->l.next; | ||
194 | thread->l.next->l.prev = thread->l.prev; | ||
195 | } | ||
196 | |||
197 | /* for block_thread(), _w_tmp() and wakeup_thread() t->lock must point | 149 | /* for block_thread(), _w_tmp() and wakeup_thread() t->lock must point |
198 | * to a corelock instance, and this corelock must be held by the caller */ | 150 | * to a corelock instance, and this corelock must be held by the caller */ |
199 | void block_thread_switch(struct thread_entry *t, struct corelock *cl) | 151 | void block_thread_switch(struct thread_entry *t, struct corelock *cl) |
200 | { | 152 | { |
201 | t->runnable = false; | 153 | t->runnable = false; |
202 | add_to_list_l(t->bqp, t); | 154 | if (wait_queue_ptr(t)) |
155 | wait_queue_register(t); | ||
203 | while(!t->runnable) | 156 | while(!t->runnable) |
204 | pthread_cond_wait(&t->cond, &cl->mutex); | 157 | pthread_cond_wait(&t->cond, &cl->mutex); |
205 | } | 158 | } |
206 | 159 | ||
207 | void block_thread_switch_w_tmo(struct thread_entry *t, int timeout, struct corelock *cl) | 160 | void block_thread_switch_w_tmo(struct thread_entry *t, int timeout, |
161 | struct corelock *cl) | ||
208 | { | 162 | { |
209 | int err = 0; | 163 | int err = 0; |
210 | struct timespec ts; | 164 | struct timespec ts; |
@@ -213,30 +167,25 @@ void block_thread_switch_w_tmo(struct thread_entry *t, int timeout, struct corel | |||
213 | timespec_add_ns(&ts, timeout * (NSEC_PER_SEC/HZ)); | 167 | timespec_add_ns(&ts, timeout * (NSEC_PER_SEC/HZ)); |
214 | 168 | ||
215 | t->runnable = false; | 169 | t->runnable = false; |
216 | add_to_list_l(t->bqp, t); | 170 | wait_queue_register(t->wqp, t); |
217 | while(!t->runnable && !err) | 171 | while(!t->runnable && !err) |
218 | err = pthread_cond_timedwait(&t->cond, &cl->mutex, &ts); | 172 | err = pthread_cond_timedwait(&t->cond, &cl->mutex, &ts); |
219 | 173 | ||
220 | if (err == ETIMEDOUT) | 174 | if (err == ETIMEDOUT) |
221 | { /* the thread timed out and was not explicitely woken up. | 175 | { /* the thread timed out and was not explicitely woken up. |
222 | * we need to do this now to mark it runnable again */ | 176 | * we need to do this now to mark it runnable again */ |
223 | remove_from_list_l(t->bqp, t); | ||
224 | t->runnable = true; | 177 | t->runnable = true; |
225 | if (t->wakeup_ext_cb) | 178 | /* NOTE: objects do their own removal upon timer expiration */ |
226 | t->wakeup_ext_cb(t); | ||
227 | } | 179 | } |
228 | } | 180 | } |
229 | 181 | ||
230 | unsigned int wakeup_thread(struct thread_entry **list) | 182 | unsigned int wakeup_thread(struct thread_entry *t) |
231 | { | 183 | { |
232 | struct thread_entry *t = *list; | 184 | if (t->wqp) |
233 | if (t) | 185 | wait_queue_remove(t); |
234 | { | 186 | t->runnable = true; |
235 | remove_from_list_l(list, t); | 187 | pthread_cond_signal(&t->cond); |
236 | t->runnable = true; | 188 | return THREAD_OK; |
237 | pthread_cond_signal(&t->cond); | ||
238 | } | ||
239 | return THREAD_NONE; | ||
240 | } | 189 | } |
241 | 190 | ||
242 | 191 | ||
diff --git a/firmware/kernel/queue.c b/firmware/kernel/queue.c index 0ba7d7e00b..927e55274c 100644 --- a/firmware/kernel/queue.c +++ b/firmware/kernel/queue.c | |||
@@ -51,7 +51,7 @@ static struct | |||
51 | * q->events[]: | XX | E1 | E2 | E3 | E4 | XX | | 51 | * q->events[]: | XX | E1 | E2 | E3 | E4 | XX | |
52 | * q->send->senders[]: | NULL | T1 | T2 | NULL | T3 | NULL | | 52 | * q->send->senders[]: | NULL | T1 | T2 | NULL | T3 | NULL | |
53 | * \/ \/ \/ | 53 | * \/ \/ \/ |
54 | * q->send->list: >->|T0|<->|T1|<->|T2|<-------->|T3|<-< | 54 | * q->send->list: 0<-|T0|<->|T1|<->|T2|<-------->|T3|->0 |
55 | * q->send->curr_sender: /\ | 55 | * q->send->curr_sender: /\ |
56 | * | 56 | * |
57 | * Thread has E0 in its own struct queue_event. | 57 | * Thread has E0 in its own struct queue_event. |
@@ -65,20 +65,20 @@ static struct | |||
65 | * more efficent to reject the majority of cases that don't need this | 65 | * more efficent to reject the majority of cases that don't need this |
66 | * called. | 66 | * called. |
67 | */ | 67 | */ |
68 | static void queue_release_sender(struct thread_entry * volatile * sender, | 68 | static void queue_release_sender_inner( |
69 | intptr_t retval) | 69 | struct thread_entry * volatile * sender, intptr_t retval) |
70 | { | 70 | { |
71 | struct thread_entry *thread = *sender; | 71 | struct thread_entry *thread = *sender; |
72 | |||
73 | *sender = NULL; /* Clear slot. */ | 72 | *sender = NULL; /* Clear slot. */ |
74 | #ifdef HAVE_WAKEUP_EXT_CB | ||
75 | thread->wakeup_ext_cb = NULL; /* Clear callback. */ | ||
76 | #endif | ||
77 | thread->retval = retval; /* Assign thread-local return value. */ | 73 | thread->retval = retval; /* Assign thread-local return value. */ |
78 | *thread->bqp = thread; /* Move blocking queue head to thread since | 74 | wakeup_thread(thread, WAKEUP_RELEASE); |
79 | wakeup_thread wakes the first thread in | 75 | } |
80 | the list. */ | 76 | |
81 | wakeup_thread(thread->bqp, WAKEUP_RELEASE); | 77 | static inline void queue_release_sender( |
78 | struct thread_entry * volatile * sender, intptr_t retval) | ||
79 | { | ||
80 | if(UNLIKELY(*sender)) | ||
81 | queue_release_sender_inner(sender, retval); | ||
82 | } | 82 | } |
83 | 83 | ||
84 | /* Releases any waiting threads that are queued with queue_send - | 84 | /* Releases any waiting threads that are queued with queue_send - |
@@ -93,26 +93,11 @@ static void queue_release_all_senders(struct event_queue *q) | |||
93 | { | 93 | { |
94 | struct thread_entry **spp = | 94 | struct thread_entry **spp = |
95 | &q->send->senders[i & QUEUE_LENGTH_MASK]; | 95 | &q->send->senders[i & QUEUE_LENGTH_MASK]; |
96 | 96 | queue_release_sender(spp, 0); | |
97 | if(*spp) | ||
98 | { | ||
99 | queue_release_sender(spp, 0); | ||
100 | } | ||
101 | } | 97 | } |
102 | } | 98 | } |
103 | } | 99 | } |
104 | 100 | ||
105 | #ifdef HAVE_WAKEUP_EXT_CB | ||
106 | /* Callback to do extra forced removal steps from sender list in addition | ||
107 | * to the normal blocking queue removal and priority dis-inherit */ | ||
108 | static void queue_remove_sender_thread_cb(struct thread_entry *thread) | ||
109 | { | ||
110 | *((struct thread_entry **)thread->retval) = NULL; | ||
111 | thread->wakeup_ext_cb = NULL; | ||
112 | thread->retval = 0; | ||
113 | } | ||
114 | #endif /* HAVE_WAKEUP_EXT_CB */ | ||
115 | |||
116 | /* Enables queue_send on the specified queue - caller allocates the extra | 101 | /* Enables queue_send on the specified queue - caller allocates the extra |
117 | * data structure. Only queues which are taken to be owned by a thread should | 102 | * data structure. Only queues which are taken to be owned by a thread should |
118 | * enable this however an official owner is not compulsory but must be | 103 | * enable this however an official owner is not compulsory but must be |
@@ -132,11 +117,12 @@ void queue_enable_queue_send(struct event_queue *q, | |||
132 | if(send != NULL && q->send == NULL) | 117 | if(send != NULL && q->send == NULL) |
133 | { | 118 | { |
134 | memset(send, 0, sizeof(*send)); | 119 | memset(send, 0, sizeof(*send)); |
120 | wait_queue_init(&send->list); | ||
135 | #ifdef HAVE_PRIORITY_SCHEDULING | 121 | #ifdef HAVE_PRIORITY_SCHEDULING |
136 | send->blocker.priority = PRIORITY_IDLE; | 122 | blocker_init(&send->blocker); |
137 | if(owner_id != 0) | 123 | if(owner_id != 0) |
138 | { | 124 | { |
139 | send->blocker.thread = thread_id_entry(owner_id); | 125 | send->blocker.thread = __thread_id_entry(owner_id); |
140 | q->blocker_p = &send->blocker; | 126 | q->blocker_p = &send->blocker; |
141 | } | 127 | } |
142 | #endif | 128 | #endif |
@@ -154,24 +140,14 @@ static inline void queue_do_unblock_sender(struct queue_sender_list *send, | |||
154 | unsigned int i) | 140 | unsigned int i) |
155 | { | 141 | { |
156 | if(send) | 142 | if(send) |
157 | { | 143 | queue_release_sender(&send->senders[i], 0); |
158 | struct thread_entry **spp = &send->senders[i]; | ||
159 | |||
160 | if(UNLIKELY(*spp)) | ||
161 | { | ||
162 | queue_release_sender(spp, 0); | ||
163 | } | ||
164 | } | ||
165 | } | 144 | } |
166 | 145 | ||
167 | /* Perform the auto-reply sequence */ | 146 | /* Perform the auto-reply sequence */ |
168 | static inline void queue_do_auto_reply(struct queue_sender_list *send) | 147 | static inline void queue_do_auto_reply(struct queue_sender_list *send) |
169 | { | 148 | { |
170 | if(send && send->curr_sender) | 149 | if(send) |
171 | { | ||
172 | /* auto-reply */ | ||
173 | queue_release_sender(&send->curr_sender, 0); | 150 | queue_release_sender(&send->curr_sender, 0); |
174 | } | ||
175 | } | 151 | } |
176 | 152 | ||
177 | /* Moves waiting thread's refrence from the senders array to the | 153 | /* Moves waiting thread's refrence from the senders array to the |
@@ -191,7 +167,6 @@ static inline void queue_do_fetch_sender(struct queue_sender_list *send, | |||
191 | /* Move thread reference from array to the next thread | 167 | /* Move thread reference from array to the next thread |
192 | that queue_reply will release */ | 168 | that queue_reply will release */ |
193 | send->curr_sender = *spp; | 169 | send->curr_sender = *spp; |
194 | (*spp)->retval = (intptr_t)spp; | ||
195 | *spp = NULL; | 170 | *spp = NULL; |
196 | } | 171 | } |
197 | /* else message was posted asynchronously with queue_post */ | 172 | /* else message was posted asynchronously with queue_post */ |
@@ -205,18 +180,28 @@ static inline void queue_do_fetch_sender(struct queue_sender_list *send, | |||
205 | #define queue_do_fetch_sender(send, rd) | 180 | #define queue_do_fetch_sender(send, rd) |
206 | #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */ | 181 | #endif /* HAVE_EXTENDED_MESSAGING_AND_NAME */ |
207 | 182 | ||
183 | static void queue_wake_waiter_inner(struct thread_entry *thread) | ||
184 | { | ||
185 | wakeup_thread(thread, WAKEUP_DEFAULT); | ||
186 | } | ||
187 | |||
188 | static inline void queue_wake_waiter(struct event_queue *q) | ||
189 | { | ||
190 | struct thread_entry *thread = WQ_THREAD_FIRST(&q->queue); | ||
191 | if(thread != NULL) | ||
192 | queue_wake_waiter_inner(thread); | ||
193 | } | ||
194 | |||
208 | /* Queue must not be available for use during this call */ | 195 | /* Queue must not be available for use during this call */ |
209 | void queue_init(struct event_queue *q, bool register_queue) | 196 | void queue_init(struct event_queue *q, bool register_queue) |
210 | { | 197 | { |
211 | int oldlevel = disable_irq_save(); | 198 | int oldlevel = disable_irq_save(); |
212 | 199 | ||
213 | if(register_queue) | 200 | if(register_queue) |
214 | { | ||
215 | corelock_lock(&all_queues.cl); | 201 | corelock_lock(&all_queues.cl); |
216 | } | ||
217 | 202 | ||
218 | corelock_init(&q->cl); | 203 | corelock_init(&q->cl); |
219 | q->queue = NULL; | 204 | wait_queue_init(&q->queue); |
220 | /* What garbage is in write is irrelevant because of the masking design- | 205 | /* What garbage is in write is irrelevant because of the masking design- |
221 | * any other functions the empty the queue do this as well so that | 206 | * any other functions the empty the queue do this as well so that |
222 | * queue_count and queue_empty return sane values in the case of a | 207 | * queue_count and queue_empty return sane values in the case of a |
@@ -261,7 +246,7 @@ void queue_delete(struct event_queue *q) | |||
261 | corelock_unlock(&all_queues.cl); | 246 | corelock_unlock(&all_queues.cl); |
262 | 247 | ||
263 | /* Release thread(s) waiting on queue head */ | 248 | /* Release thread(s) waiting on queue head */ |
264 | thread_queue_wake(&q->queue); | 249 | wait_queue_wake(&q->queue); |
265 | 250 | ||
266 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME | 251 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME |
267 | if(q->send) | 252 | if(q->send) |
@@ -293,7 +278,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev) | |||
293 | 278 | ||
294 | #ifdef HAVE_PRIORITY_SCHEDULING | 279 | #ifdef HAVE_PRIORITY_SCHEDULING |
295 | KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL || | 280 | KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL || |
296 | QUEUE_GET_THREAD(q) == thread_self_entry(), | 281 | QUEUE_GET_THREAD(q) == __running_self_entry(), |
297 | "queue_wait->wrong thread\n"); | 282 | "queue_wait->wrong thread\n"); |
298 | #endif | 283 | #endif |
299 | 284 | ||
@@ -307,18 +292,12 @@ void queue_wait(struct event_queue *q, struct queue_event *ev) | |||
307 | 292 | ||
308 | while(1) | 293 | while(1) |
309 | { | 294 | { |
310 | struct thread_entry *current; | ||
311 | |||
312 | rd = q->read; | 295 | rd = q->read; |
313 | if (rd != q->write) /* A waking message could disappear */ | 296 | if (rd != q->write) /* A waking message could disappear */ |
314 | break; | 297 | break; |
315 | 298 | ||
316 | current = thread_self_entry(); | 299 | struct thread_entry *current = __running_self_entry(); |
317 | 300 | block_thread(current, TIMEOUT_BLOCK, &q->queue, NULL); | |
318 | IF_COP( current->obj_cl = &q->cl; ) | ||
319 | current->bqp = &q->queue; | ||
320 | |||
321 | block_thread(current, TIMEOUT_BLOCK); | ||
322 | 301 | ||
323 | corelock_unlock(&q->cl); | 302 | corelock_unlock(&q->cl); |
324 | switch_thread(); | 303 | switch_thread(); |
@@ -349,16 +328,9 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks) | |||
349 | int oldlevel; | 328 | int oldlevel; |
350 | unsigned int rd, wr; | 329 | unsigned int rd, wr; |
351 | 330 | ||
352 | /* this function works only with a positive number (or zero) of ticks */ | ||
353 | if (ticks == TIMEOUT_BLOCK) | ||
354 | { | ||
355 | queue_wait(q, ev); | ||
356 | return; | ||
357 | } | ||
358 | |||
359 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME | 331 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME |
360 | KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL || | 332 | KERNEL_ASSERT(QUEUE_GET_THREAD(q) == NULL || |
361 | QUEUE_GET_THREAD(q) == thread_self_entry(), | 333 | QUEUE_GET_THREAD(q) == __running_self_entry(), |
362 | "queue_wait_w_tmo->wrong thread\n"); | 334 | "queue_wait_w_tmo->wrong thread\n"); |
363 | #endif | 335 | #endif |
364 | 336 | ||
@@ -372,14 +344,10 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks) | |||
372 | 344 | ||
373 | rd = q->read; | 345 | rd = q->read; |
374 | wr = q->write; | 346 | wr = q->write; |
375 | if (rd == wr && ticks > 0) | 347 | if (rd == wr && ticks != 0) |
376 | { | 348 | { |
377 | struct thread_entry *current = thread_self_entry(); | 349 | struct thread_entry *current = __running_self_entry(); |
378 | 350 | block_thread(current, ticks, &q->queue, NULL); | |
379 | IF_COP( current->obj_cl = &q->cl; ) | ||
380 | current->bqp = &q->queue; | ||
381 | |||
382 | block_thread(current, ticks); | ||
383 | corelock_unlock(&q->cl); | 351 | corelock_unlock(&q->cl); |
384 | 352 | ||
385 | switch_thread(); | 353 | switch_thread(); |
@@ -389,6 +357,8 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks) | |||
389 | 357 | ||
390 | rd = q->read; | 358 | rd = q->read; |
391 | wr = q->write; | 359 | wr = q->write; |
360 | |||
361 | wait_queue_try_remove(current); | ||
392 | } | 362 | } |
393 | 363 | ||
394 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME | 364 | #ifdef HAVE_EXTENDED_MESSAGING_AND_NAME |
@@ -436,7 +406,7 @@ void queue_post(struct event_queue *q, long id, intptr_t data) | |||
436 | queue_do_unblock_sender(q->send, wr); | 406 | queue_do_unblock_sender(q->send, wr); |
437 | 407 | ||
438 | /* Wakeup a waiting thread if any */ | 408 | /* Wakeup a waiting thread if any */ |
439 | wakeup_thread(&q->queue, WAKEUP_DEFAULT); | 409 | queue_wake_waiter(q); |
440 | 410 | ||
441 | corelock_unlock(&q->cl); | 411 | corelock_unlock(&q->cl); |
442 | restore_irq(oldlevel); | 412 | restore_irq(oldlevel); |
@@ -465,28 +435,17 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data) | |||
465 | { | 435 | { |
466 | struct queue_sender_list *send = q->send; | 436 | struct queue_sender_list *send = q->send; |
467 | struct thread_entry **spp = &send->senders[wr]; | 437 | struct thread_entry **spp = &send->senders[wr]; |
468 | struct thread_entry *current = thread_self_entry(); | 438 | struct thread_entry *current = __running_self_entry(); |
469 | 439 | ||
470 | if(UNLIKELY(*spp)) | 440 | /* overflow protect - unblock any thread waiting at this index */ |
471 | { | 441 | queue_release_sender(spp, 0); |
472 | /* overflow protect - unblock any thread waiting at this index */ | ||
473 | queue_release_sender(spp, 0); | ||
474 | } | ||
475 | 442 | ||
476 | /* Wakeup a waiting thread if any */ | 443 | /* Wakeup a waiting thread if any */ |
477 | wakeup_thread(&q->queue, WAKEUP_DEFAULT); | 444 | queue_wake_waiter(q); |
478 | 445 | ||
479 | /* Save thread in slot, add to list and wait for reply */ | 446 | /* Save thread in slot, add to list and wait for reply */ |
480 | *spp = current; | 447 | *spp = current; |
481 | IF_COP( current->obj_cl = &q->cl; ) | 448 | block_thread(current, TIMEOUT_BLOCK, &send->list, q->blocker_p); |
482 | IF_PRIO( current->blocker = q->blocker_p; ) | ||
483 | #ifdef HAVE_WAKEUP_EXT_CB | ||
484 | current->wakeup_ext_cb = queue_remove_sender_thread_cb; | ||
485 | #endif | ||
486 | current->retval = (intptr_t)spp; | ||
487 | current->bqp = &send->list; | ||
488 | |||
489 | block_thread(current, TIMEOUT_BLOCK); | ||
490 | 449 | ||
491 | corelock_unlock(&q->cl); | 450 | corelock_unlock(&q->cl); |
492 | switch_thread(); | 451 | switch_thread(); |
@@ -495,7 +454,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data) | |||
495 | } | 454 | } |
496 | 455 | ||
497 | /* Function as queue_post if sending is not enabled */ | 456 | /* Function as queue_post if sending is not enabled */ |
498 | wakeup_thread(&q->queue, WAKEUP_DEFAULT); | 457 | queue_wake_waiter(q); |
499 | 458 | ||
500 | corelock_unlock(&q->cl); | 459 | corelock_unlock(&q->cl); |
501 | restore_irq(oldlevel); | 460 | restore_irq(oldlevel); |
@@ -530,16 +489,12 @@ void queue_reply(struct event_queue *q, intptr_t retval) | |||
530 | { | 489 | { |
531 | if(q->send && q->send->curr_sender) | 490 | if(q->send && q->send->curr_sender) |
532 | { | 491 | { |
533 | struct queue_sender_list *sender; | ||
534 | |||
535 | int oldlevel = disable_irq_save(); | 492 | int oldlevel = disable_irq_save(); |
536 | corelock_lock(&q->cl); | 493 | corelock_lock(&q->cl); |
537 | 494 | ||
538 | sender = q->send; | 495 | struct queue_sender_list *send = q->send; |
539 | 496 | if(send) | |
540 | /* Double-check locking */ | 497 | queue_release_sender(&send->curr_sender, retval); |
541 | if(LIKELY(sender && sender->curr_sender)) | ||
542 | queue_release_sender(&sender->curr_sender, retval); | ||
543 | 498 | ||
544 | corelock_unlock(&q->cl); | 499 | corelock_unlock(&q->cl); |
545 | restore_irq(oldlevel); | 500 | restore_irq(oldlevel); |
diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c index 1505038fbc..5e9e46798f 100644 --- a/firmware/kernel/semaphore.c +++ b/firmware/kernel/semaphore.c | |||
@@ -24,6 +24,7 @@ | |||
24 | /**************************************************************************** | 24 | /**************************************************************************** |
25 | * Simple semaphore functions ;) | 25 | * Simple semaphore functions ;) |
26 | ****************************************************************************/ | 26 | ****************************************************************************/ |
27 | |||
27 | /* Initialize the semaphore object. | 28 | /* Initialize the semaphore object. |
28 | * max = maximum up count the semaphore may assume (max >= 1) | 29 | * max = maximum up count the semaphore may assume (max >= 1) |
29 | * start = initial count of semaphore (0 <= count <= max) */ | 30 | * start = initial count of semaphore (0 <= count <= max) */ |
@@ -31,7 +32,7 @@ void semaphore_init(struct semaphore *s, int max, int start) | |||
31 | { | 32 | { |
32 | KERNEL_ASSERT(max > 0 && start >= 0 && start <= max, | 33 | KERNEL_ASSERT(max > 0 && start >= 0 && start <= max, |
33 | "semaphore_init->inv arg\n"); | 34 | "semaphore_init->inv arg\n"); |
34 | s->queue = NULL; | 35 | wait_queue_init(&s->queue); |
35 | s->max = max; | 36 | s->max = max; |
36 | s->count = start; | 37 | s->count = start; |
37 | corelock_init(&s->cl); | 38 | corelock_init(&s->cl); |
@@ -42,44 +43,49 @@ void semaphore_init(struct semaphore *s, int max, int start) | |||
42 | * safely be used in an ISR. */ | 43 | * safely be used in an ISR. */ |
43 | int semaphore_wait(struct semaphore *s, int timeout) | 44 | int semaphore_wait(struct semaphore *s, int timeout) |
44 | { | 45 | { |
45 | int ret; | 46 | int ret = OBJ_WAIT_TIMEDOUT; |
46 | int oldlevel; | ||
47 | int count; | ||
48 | 47 | ||
49 | oldlevel = disable_irq_save(); | 48 | int oldlevel = disable_irq_save(); |
50 | corelock_lock(&s->cl); | 49 | corelock_lock(&s->cl); |
51 | 50 | ||
52 | count = s->count; | 51 | int count = s->count; |
53 | |||
54 | if(LIKELY(count > 0)) | 52 | if(LIKELY(count > 0)) |
55 | { | 53 | { |
56 | /* count is not zero; down it */ | 54 | /* count is not zero; down it */ |
57 | s->count = count - 1; | 55 | s->count = count - 1; |
58 | ret = OBJ_WAIT_SUCCEEDED; | 56 | ret = OBJ_WAIT_SUCCEEDED; |
59 | } | 57 | } |
60 | else if(timeout == 0) | 58 | else if(timeout != 0) |
61 | { | ||
62 | /* just polling it */ | ||
63 | ret = OBJ_WAIT_TIMEDOUT; | ||
64 | } | ||
65 | else | ||
66 | { | 59 | { |
67 | /* too many waits - block until count is upped... */ | 60 | /* too many waits - block until count is upped... */ |
68 | struct thread_entry * current = thread_self_entry(); | 61 | struct thread_entry *current = __running_self_entry(); |
69 | IF_COP( current->obj_cl = &s->cl; ) | 62 | |
70 | current->bqp = &s->queue; | 63 | block_thread(current, timeout, &s->queue, NULL); |
71 | /* return value will be OBJ_WAIT_SUCCEEDED after wait if wake was | ||
72 | * explicit in semaphore_release */ | ||
73 | current->retval = OBJ_WAIT_TIMEDOUT; | ||
74 | |||
75 | block_thread(current, timeout); | ||
76 | corelock_unlock(&s->cl); | 64 | corelock_unlock(&s->cl); |
77 | 65 | ||
78 | /* ...and turn control over to next thread */ | 66 | /* ...and turn control over to next thread */ |
79 | switch_thread(); | 67 | switch_thread(); |
80 | 68 | ||
81 | return current->retval; | 69 | /* if explicit wake indicated; do no more */ |
70 | if(LIKELY(!wait_queue_ptr(current))) | ||
71 | return OBJ_WAIT_SUCCEEDED; | ||
72 | |||
73 | disable_irq(); | ||
74 | corelock_lock(&s->cl); | ||
75 | |||
76 | /* see if anyone got us after the expired wait */ | ||
77 | if(wait_queue_try_remove(current)) | ||
78 | { | ||
79 | count = s->count; | ||
80 | if(count > 0) | ||
81 | { | ||
82 | /* down it lately */ | ||
83 | s->count = count - 1; | ||
84 | ret = OBJ_WAIT_SUCCEEDED; | ||
85 | } | ||
86 | } | ||
82 | } | 87 | } |
88 | /* else just polling it */ | ||
83 | 89 | ||
84 | corelock_unlock(&s->cl); | 90 | corelock_unlock(&s->cl); |
85 | restore_irq(oldlevel); | 91 | restore_irq(oldlevel); |
@@ -93,18 +99,17 @@ int semaphore_wait(struct semaphore *s, int timeout) | |||
93 | void semaphore_release(struct semaphore *s) | 99 | void semaphore_release(struct semaphore *s) |
94 | { | 100 | { |
95 | unsigned int result = THREAD_NONE; | 101 | unsigned int result = THREAD_NONE; |
96 | int oldlevel; | ||
97 | 102 | ||
98 | oldlevel = disable_irq_save(); | 103 | int oldlevel = disable_irq_save(); |
99 | corelock_lock(&s->cl); | 104 | corelock_lock(&s->cl); |
100 | 105 | ||
101 | if(LIKELY(s->queue != NULL)) | 106 | struct thread_entry *thread = WQ_THREAD_FIRST(&s->queue); |
107 | if(LIKELY(thread != NULL)) | ||
102 | { | 108 | { |
103 | /* a thread was queued - wake it up and keep count at 0 */ | 109 | /* a thread was queued - wake it up and keep count at 0 */ |
104 | KERNEL_ASSERT(s->count == 0, | 110 | KERNEL_ASSERT(s->count == 0, |
105 | "semaphore_release->threads queued but count=%d!\n", s->count); | 111 | "semaphore_release->threads queued but count=%d!\n", s->count); |
106 | s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */ | 112 | result = wakeup_thread(thread, WAKEUP_DEFAULT); |
107 | result = wakeup_thread(&s->queue, WAKEUP_DEFAULT); | ||
108 | } | 113 | } |
109 | else | 114 | else |
110 | { | 115 | { |
diff --git a/firmware/kernel/thread-common.c b/firmware/kernel/thread-common.c index b8b8ffbd4c..aad6610feb 100644 --- a/firmware/kernel/thread-common.c +++ b/firmware/kernel/thread-common.c | |||
@@ -18,39 +18,222 @@ | |||
18 | * KIND, either express or implied. | 18 | * KIND, either express or implied. |
19 | * | 19 | * |
20 | ****************************************************************************/ | 20 | ****************************************************************************/ |
21 | #include "thread-internal.h" | 21 | #include "kernel-internal.h" |
22 | #include "system.h" | 22 | #include "system.h" |
23 | 23 | ||
24 | /* Unless otherwise defined, do nothing */ | ||
25 | #ifndef YIELD_KERNEL_HOOK | ||
26 | #define YIELD_KERNEL_HOOK() false | ||
27 | #endif | ||
28 | #ifndef SLEEP_KERNEL_HOOK | ||
29 | #define SLEEP_KERNEL_HOOK(ticks) false | ||
30 | #endif | ||
31 | |||
32 | const char __main_thread_name_str[] = "main"; | ||
33 | |||
34 | /* Array indexing is more efficient in inlines if the elements are a native | ||
35 | word size (100s of bytes fewer instructions) */ | ||
36 | |||
37 | #if NUM_CORES > 1 | ||
38 | static struct core_entry __core_entries[NUM_CORES] IBSS_ATTR; | ||
39 | struct core_entry *__cores[NUM_CORES] IBSS_ATTR; | ||
40 | #else | ||
41 | struct core_entry __cores[NUM_CORES] IBSS_ATTR; | ||
42 | #endif | ||
43 | |||
44 | static struct thread_entry __thread_entries[MAXTHREADS] IBSS_ATTR; | ||
45 | struct thread_entry *__threads[MAXTHREADS] IBSS_ATTR; | ||
46 | |||
47 | |||
48 | /** Internal functions **/ | ||
49 | |||
24 | /*--------------------------------------------------------------------------- | 50 | /*--------------------------------------------------------------------------- |
25 | * Wakeup an entire queue of threads - returns bitwise-or of return bitmask | 51 | * Find an empty thread slot or NULL if none found. The slot returned will |
26 | * from each operation or THREAD_NONE of nothing was awakened. Object owning | 52 | * be locked on multicore. |
27 | * the queue must be locked first. | ||
28 | * | ||
29 | * INTERNAL: Intended for use by kernel objects and not for programs. | ||
30 | *--------------------------------------------------------------------------- | 53 | *--------------------------------------------------------------------------- |
31 | */ | 54 | */ |
32 | unsigned int thread_queue_wake(struct thread_entry **list) | 55 | static struct threadalloc |
33 | { | 56 | { |
34 | unsigned result = THREAD_NONE; | 57 | threadbit_t avail; |
58 | #if NUM_CORES > 1 | ||
59 | struct corelock cl; | ||
60 | #endif | ||
61 | } threadalloc SHAREDBSS_ATTR; | ||
62 | |||
63 | /*--------------------------------------------------------------------------- | ||
64 | * Initialize the thread allocator | ||
65 | *--------------------------------------------------------------------------- | ||
66 | */ | ||
67 | void thread_alloc_init(void) | ||
68 | { | ||
69 | corelock_init(&threadalloc.cl); | ||
35 | 70 | ||
36 | for (;;) | 71 | for (unsigned int core = 0; core < NUM_CORES; core++) |
37 | { | 72 | { |
38 | unsigned int rc = wakeup_thread(list, WAKEUP_DEFAULT); | 73 | #if NUM_CORES > 1 |
74 | struct core_entry *c = &__core_entries[core]; | ||
75 | __cores[core] = c; | ||
76 | #else | ||
77 | struct core_entry *c = &__cores[core]; | ||
78 | #endif | ||
79 | rtr_queue_init(&c->rtr); | ||
80 | corelock_init(&c->rtr_cl); | ||
81 | tmo_queue_init(&c->tmo); | ||
82 | c->next_tmo_check = current_tick; /* Something not in the past */ | ||
83 | } | ||
39 | 84 | ||
40 | if (rc == THREAD_NONE) | 85 | for (unsigned int slotnum = 0; slotnum < MAXTHREADS; slotnum++) |
41 | break; /* No more threads */ | 86 | { |
87 | struct thread_entry *t = &__thread_entries[slotnum]; | ||
88 | __threads[slotnum] = t; | ||
89 | corelock_init(&t->waiter_cl); | ||
90 | corelock_init(&t->slot_cl); | ||
91 | t->id = THREAD_ID_INIT(slotnum); | ||
92 | threadbit_set_bit(&threadalloc.avail, slotnum); | ||
93 | } | ||
94 | } | ||
95 | |||
96 | /*--------------------------------------------------------------------------- | ||
97 | * Allocate a thread alot | ||
98 | *--------------------------------------------------------------------------- | ||
99 | */ | ||
100 | struct thread_entry * thread_alloc(void) | ||
101 | { | ||
102 | struct thread_entry *thread = NULL; | ||
42 | 103 | ||
43 | result |= rc; | 104 | corelock_lock(&threadalloc.cl); |
105 | |||
106 | unsigned int slotnum = threadbit_ffs(&threadalloc.avail); | ||
107 | if (slotnum < MAXTHREADS) | ||
108 | { | ||
109 | threadbit_clear_bit(&threadalloc.avail, slotnum); | ||
110 | thread = __threads[slotnum]; | ||
44 | } | 111 | } |
45 | 112 | ||
113 | corelock_unlock(&threadalloc.cl); | ||
114 | |||
115 | return thread; | ||
116 | } | ||
117 | |||
118 | /*--------------------------------------------------------------------------- | ||
119 | * Free the thread slot of 'thread' | ||
120 | *--------------------------------------------------------------------------- | ||
121 | */ | ||
122 | void thread_free(struct thread_entry *thread) | ||
123 | { | ||
124 | corelock_lock(&threadalloc.cl); | ||
125 | threadbit_set_bit(&threadalloc.avail, THREAD_ID_SLOT(thread->id)); | ||
126 | corelock_unlock(&threadalloc.cl); | ||
127 | } | ||
128 | |||
129 | /*--------------------------------------------------------------------------- | ||
130 | * Assign the thread slot a new ID. Version is 0x00000100..0xffffff00. | ||
131 | *--------------------------------------------------------------------------- | ||
132 | */ | ||
133 | void new_thread_id(struct thread_entry *thread) | ||
134 | { | ||
135 | uint32_t id = thread->id + (1u << THREAD_ID_VERSION_SHIFT); | ||
136 | |||
137 | /* If wrapped to 0, make it 1 */ | ||
138 | if ((id & THREAD_ID_VERSION_MASK) == 0) | ||
139 | id |= (1u << THREAD_ID_VERSION_SHIFT); | ||
140 | |||
141 | thread->id = id; | ||
142 | } | ||
143 | |||
144 | /*--------------------------------------------------------------------------- | ||
145 | * Wakeup an entire queue of threads - returns bitwise-or of return bitmask | ||
146 | * from each operation or THREAD_NONE of nothing was awakened. | ||
147 | *--------------------------------------------------------------------------- | ||
148 | */ | ||
149 | unsigned int wait_queue_wake(struct __wait_queue *wqp) | ||
150 | { | ||
151 | unsigned result = THREAD_NONE; | ||
152 | struct thread_entry *thread; | ||
153 | |||
154 | while ((thread = WQ_THREAD_FIRST(wqp))) | ||
155 | result |= wakeup_thread(thread, WAKEUP_DEFAULT); | ||
156 | |||
46 | return result; | 157 | return result; |
47 | } | 158 | } |
48 | 159 | ||
49 | 160 | ||
161 | /** Public functions **/ | ||
162 | |||
163 | #ifdef RB_PROFILE | ||
164 | void profile_thread(void) | ||
165 | { | ||
166 | profstart(THREAD_ID_SLOT(__running_self_entry()->id)); | ||
167 | } | ||
168 | #endif | ||
169 | |||
170 | /*--------------------------------------------------------------------------- | ||
171 | * Return the thread id of the calling thread | ||
172 | * -------------------------------------------------------------------------- | ||
173 | */ | ||
174 | unsigned int thread_self(void) | ||
175 | { | ||
176 | return __running_self_entry()->id; | ||
177 | } | ||
178 | |||
179 | /*--------------------------------------------------------------------------- | ||
180 | * Suspends a thread's execution for at least the specified number of ticks. | ||
181 | * | ||
182 | * May result in CPU core entering wait-for-interrupt mode if no other thread | ||
183 | * may be scheduled. | ||
184 | * | ||
185 | * NOTE: sleep(0) sleeps until the end of the current tick | ||
186 | * sleep(n) that doesn't result in rescheduling: | ||
187 | * n <= ticks suspended < n + 1 | ||
188 | * n to n+1 is a lower bound. Other factors may affect the actual time | ||
189 | * a thread is suspended before it runs again. | ||
190 | *--------------------------------------------------------------------------- | ||
191 | */ | ||
192 | unsigned sleep(unsigned ticks) | ||
193 | { | ||
194 | /* In certain situations, certain bootloaders in particular, a normal | ||
195 | * threading call is inappropriate. */ | ||
196 | if (SLEEP_KERNEL_HOOK(ticks)) | ||
197 | return 0; /* Handled */ | ||
198 | |||
199 | disable_irq(); | ||
200 | sleep_thread(ticks); | ||
201 | switch_thread(); | ||
202 | return 0; | ||
203 | } | ||
204 | |||
205 | /*--------------------------------------------------------------------------- | ||
206 | * Elects another thread to run or, if no other thread may be made ready to | ||
207 | * run, immediately returns control back to the calling thread. | ||
208 | *--------------------------------------------------------------------------- | ||
209 | */ | ||
210 | void yield(void) | ||
211 | { | ||
212 | /* In certain situations, certain bootloaders in particular, a normal | ||
213 | * threading call is inappropriate. */ | ||
214 | if (YIELD_KERNEL_HOOK()) | ||
215 | return; /* Handled */ | ||
216 | |||
217 | switch_thread(); | ||
218 | } | ||
219 | |||
220 | |||
50 | /** Debug screen stuff **/ | 221 | /** Debug screen stuff **/ |
51 | 222 | ||
223 | void format_thread_name(char *buf, size_t bufsize, | ||
224 | const struct thread_entry *thread) | ||
225 | { | ||
226 | const char *name = thread->name; | ||
227 | if (!name) | ||
228 | name = ""; | ||
229 | |||
230 | const char *fmt = *name ? "%s" : "%s%08lX"; | ||
231 | snprintf(buf, bufsize, fmt, name, thread->id); | ||
232 | } | ||
233 | |||
234 | #ifndef HAVE_SDL_THREADS | ||
52 | /*--------------------------------------------------------------------------- | 235 | /*--------------------------------------------------------------------------- |
53 | * returns the stack space used in bytes | 236 | * Returns the maximum percentage of the stack ever used during runtime. |
54 | *--------------------------------------------------------------------------- | 237 | *--------------------------------------------------------------------------- |
55 | */ | 238 | */ |
56 | static unsigned int stack_usage(uintptr_t *stackptr, size_t stack_size) | 239 | static unsigned int stack_usage(uintptr_t *stackptr, size_t stack_size) |
@@ -69,13 +252,9 @@ static unsigned int stack_usage(uintptr_t *stackptr, size_t stack_size) | |||
69 | 252 | ||
70 | return usage; | 253 | return usage; |
71 | } | 254 | } |
255 | #endif /* HAVE_SDL_THREADS */ | ||
72 | 256 | ||
73 | #if NUM_CORES > 1 | 257 | #if NUM_CORES > 1 |
74 | /*--------------------------------------------------------------------------- | ||
75 | * Returns the maximum percentage of the core's idle stack ever used during | ||
76 | * runtime. | ||
77 | *--------------------------------------------------------------------------- | ||
78 | */ | ||
79 | int core_get_debug_info(unsigned int core, struct core_debug_info *infop) | 258 | int core_get_debug_info(unsigned int core, struct core_debug_info *infop) |
80 | { | 259 | { |
81 | extern uintptr_t * const idle_stacks[NUM_CORES]; | 260 | extern uintptr_t * const idle_stacks[NUM_CORES]; |
@@ -105,29 +284,29 @@ int thread_get_debug_info(unsigned int thread_id, | |||
105 | if (!infop) | 284 | if (!infop) |
106 | return -1; | 285 | return -1; |
107 | 286 | ||
108 | unsigned int slot = THREAD_ID_SLOT(thread_id); | 287 | unsigned int slotnum = THREAD_ID_SLOT(thread_id); |
109 | if (slot >= MAXTHREADS) | 288 | if (slotnum >= MAXTHREADS) |
110 | return -1; | 289 | return -1; |
111 | 290 | ||
112 | extern struct thread_entry threads[MAXTHREADS]; | 291 | struct thread_entry *thread = __thread_slot_entry(slotnum); |
113 | struct thread_entry *thread = &threads[slot]; | ||
114 | 292 | ||
115 | int oldlevel = disable_irq_save(); | 293 | int oldlevel = disable_irq_save(); |
116 | LOCK_THREAD(thread); | 294 | corelock_lock(&threadalloc.cl); |
295 | corelock_lock(&thread->slot_cl); | ||
117 | 296 | ||
118 | unsigned int state = thread->state; | 297 | unsigned int state = thread->state; |
119 | 298 | ||
120 | if (state != STATE_KILLED) | 299 | int ret = 0; |
121 | { | ||
122 | const char *name = thread->name; | ||
123 | if (!name) | ||
124 | name = ""; | ||
125 | 300 | ||
301 | if (threadbit_test_bit(&threadalloc.avail, slotnum) == 0) | ||
302 | { | ||
126 | bool cpu_boost = false; | 303 | bool cpu_boost = false; |
127 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | 304 | #ifdef HAVE_SCHEDULER_BOOSTCTRL |
128 | cpu_boost = thread->cpu_boost; | 305 | cpu_boost = thread->cpu_boost; |
129 | #endif | 306 | #endif |
307 | #ifndef HAVE_SDL_THREADS | ||
130 | infop->stack_usage = stack_usage(thread->stack, thread->stack_size); | 308 | infop->stack_usage = stack_usage(thread->stack, thread->stack_size); |
309 | #endif | ||
131 | #if NUM_CORES > 1 | 310 | #if NUM_CORES > 1 |
132 | infop->core = thread->core; | 311 | infop->core = thread->core; |
133 | #endif | 312 | #endif |
@@ -140,13 +319,13 @@ int thread_get_debug_info(unsigned int thread_id, | |||
140 | cpu_boost ? '+' : (state == STATE_RUNNING ? '*' : ' '), | 319 | cpu_boost ? '+' : (state == STATE_RUNNING ? '*' : ' '), |
141 | status_chars[state]); | 320 | status_chars[state]); |
142 | 321 | ||
143 | const char *fmt = *name ? "%s" : "%s%08lX"; | 322 | format_thread_name(infop->name, sizeof (infop->name), thread); |
144 | snprintf(infop->name, sizeof (infop->name), fmt, name, | 323 | ret = 1; |
145 | thread->id); | ||
146 | } | 324 | } |
147 | 325 | ||
148 | UNLOCK_THREAD(thread); | 326 | corelock_unlock(&thread->slot_cl); |
327 | corelock_unlock(&threadalloc.cl); | ||
149 | restore_irq(oldlevel); | 328 | restore_irq(oldlevel); |
150 | 329 | ||
151 | return state == STATE_KILLED ? 0 : 1; | 330 | return ret; |
152 | } | 331 | } |
diff --git a/firmware/kernel/thread-internal.h b/firmware/kernel/thread-internal.h index 894bd1fe7c..10606a54a6 100644 --- a/firmware/kernel/thread-internal.h +++ b/firmware/kernel/thread-internal.h | |||
@@ -78,30 +78,11 @@ struct priority_distribution | |||
78 | 78 | ||
79 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 79 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
80 | 80 | ||
81 | #ifdef HAVE_CORELOCK_OBJECT | 81 | #define __rtr_queue lldc_head |
82 | /* Operations to be performed just before stopping a thread and starting | 82 | #define __rtr_queue_node lldc_node |
83 | a new one if specified before calling switch_thread */ | ||
84 | enum | ||
85 | { | ||
86 | TBOP_CLEAR = 0, /* No operation to do */ | ||
87 | TBOP_UNLOCK_CORELOCK, /* Unlock a corelock variable */ | ||
88 | TBOP_SWITCH_CORE, /* Call the core switch preparation routine */ | ||
89 | }; | ||
90 | 83 | ||
91 | struct thread_blk_ops | 84 | #define __tmo_queue ll_head |
92 | { | 85 | #define __tmo_queue_node ll_node |
93 | struct corelock *cl_p; /* pointer to corelock */ | ||
94 | unsigned char flags; /* TBOP_* flags */ | ||
95 | }; | ||
96 | #endif /* NUM_CORES > 1 */ | ||
97 | |||
98 | /* Link information for lists thread is in */ | ||
99 | struct thread_entry; /* forward */ | ||
100 | struct thread_list | ||
101 | { | ||
102 | struct thread_entry *prev; /* Previous thread in a list */ | ||
103 | struct thread_entry *next; /* Next thread in a list */ | ||
104 | }; | ||
105 | 86 | ||
106 | /* Information kept in each thread slot | 87 | /* Information kept in each thread slot |
107 | * members are arranged according to size - largest first - in order | 88 | * members are arranged according to size - largest first - in order |
@@ -109,73 +90,64 @@ struct thread_list | |||
109 | */ | 90 | */ |
110 | struct thread_entry | 91 | struct thread_entry |
111 | { | 92 | { |
112 | struct regs context; /* Register context at switch - | 93 | struct regs context; /* Register context at switch - |
113 | _must_ be first member */ | 94 | _must_ be first member */ |
114 | uintptr_t *stack; /* Pointer to top of stack */ | 95 | #ifndef HAVE_SDL_THREADS |
115 | const char *name; /* Thread name */ | 96 | uintptr_t *stack; /* Pointer to top of stack */ |
116 | long tmo_tick; /* Tick when thread should be woken from | ||
117 | timeout - | ||
118 | states: STATE_SLEEPING/STATE_BLOCKED_W_TMO */ | ||
119 | struct thread_list l; /* Links for blocked/waking/running - | ||
120 | circular linkage in both directions */ | ||
121 | struct thread_list tmo; /* Links for timeout list - | ||
122 | Circular in reverse direction, NULL-terminated in | ||
123 | forward direction - | ||
124 | states: STATE_SLEEPING/STATE_BLOCKED_W_TMO */ | ||
125 | struct thread_entry **bqp; /* Pointer to list variable in kernel | ||
126 | object where thread is blocked - used | ||
127 | for implicit unblock and explicit wake | ||
128 | states: STATE_BLOCKED/STATE_BLOCKED_W_TMO */ | ||
129 | #ifdef HAVE_CORELOCK_OBJECT | ||
130 | struct corelock *obj_cl; /* Object corelock where thead is blocked - | ||
131 | states: STATE_BLOCKED/STATE_BLOCKED_W_TMO */ | ||
132 | struct corelock waiter_cl; /* Corelock for thread_wait */ | ||
133 | struct corelock slot_cl; /* Corelock to lock thread slot */ | ||
134 | unsigned char core; /* The core to which thread belongs */ | ||
135 | #endif | ||
136 | struct thread_entry *queue; /* List of threads waiting for thread to be | ||
137 | removed */ | ||
138 | #ifdef HAVE_WAKEUP_EXT_CB | ||
139 | void (*wakeup_ext_cb)(struct thread_entry *thread); /* Callback that | ||
140 | performs special steps needed when being | ||
141 | forced off of an object's wait queue that | ||
142 | go beyond the standard wait queue removal | ||
143 | and priority disinheritance */ | ||
144 | /* Only enabled when using queue_send for now */ | ||
145 | #endif | 97 | #endif |
146 | #if defined(HAVE_SEMAPHORE_OBJECTS) || \ | 98 | const char *name; /* Thread name */ |
147 | defined(HAVE_EXTENDED_MESSAGING_AND_NAME) || \ | 99 | long tmo_tick; /* Tick when thread should be woken */ |
148 | NUM_CORES > 1 | 100 | struct __rtr_queue_node rtr; /* Node for run queue */ |
149 | volatile intptr_t retval; /* Return value from a blocked operation/ | 101 | struct __tmo_queue_node tmo; /* Links for timeout list */ |
150 | misc. use */ | 102 | struct __wait_queue_node wq; /* Node for wait queue */ |
103 | struct __wait_queue *volatile wqp; /* Pointer to registered wait queue */ | ||
104 | #if NUM_CORES > 1 | ||
105 | struct corelock waiter_cl; /* Corelock for thread_wait */ | ||
106 | struct corelock slot_cl; /* Corelock to lock thread slot */ | ||
107 | unsigned char core; /* The core to which thread belongs */ | ||
151 | #endif | 108 | #endif |
152 | uint32_t id; /* Current slot id */ | 109 | struct __wait_queue queue; /* List of threads waiting for thread to be |
153 | int __errno; /* Thread error number (errno tls) */ | 110 | removed */ |
111 | volatile intptr_t retval; /* Return value from a blocked operation/ | ||
112 | misc. use */ | ||
113 | uint32_t id; /* Current slot id */ | ||
114 | int __errno; /* Thread error number (errno tls) */ | ||
154 | #ifdef HAVE_PRIORITY_SCHEDULING | 115 | #ifdef HAVE_PRIORITY_SCHEDULING |
155 | /* Priority summary of owned objects that support inheritance */ | 116 | /* Priority summary of owned objects that support inheritance */ |
156 | struct blocker *blocker; /* Pointer to blocker when this thread is blocked | 117 | struct blocker *blocker; /* Pointer to blocker when this thread is blocked |
157 | on an object that supports PIP - | 118 | on an object that supports PIP - |
158 | states: STATE_BLOCKED/STATE_BLOCKED_W_TMO */ | 119 | states: STATE_BLOCKED/STATE_BLOCKED_W_TMO */ |
159 | struct priority_distribution pdist; /* Priority summary of owned objects | 120 | struct priority_distribution pdist; /* Priority summary of owned objects |
160 | that have blocked threads and thread's own | 121 | that have blocked threads and thread's own |
161 | base priority */ | 122 | base priority */ |
162 | int skip_count; /* Number of times skipped if higher priority | 123 | int skip_count; /* Number of times skipped if higher priority |
163 | thread was running */ | 124 | thread was running */ |
164 | unsigned char base_priority; /* Base priority (set explicitly during | 125 | unsigned char base_priority; /* Base priority (set explicitly during |
165 | creation or thread_set_priority) */ | 126 | creation or thread_set_priority) */ |
166 | unsigned char priority; /* Scheduled priority (higher of base or | 127 | unsigned char priority; /* Scheduled priority (higher of base or |
167 | all threads blocked by this one) */ | 128 | all threads blocked by this one) */ |
168 | #endif | 129 | #endif |
169 | unsigned short stack_size; /* Size of stack in bytes */ | 130 | #ifndef HAVE_SDL_THREADS |
170 | unsigned char state; /* Thread slot state (STATE_*) */ | 131 | unsigned short stack_size; /* Size of stack in bytes */ |
132 | #endif | ||
133 | unsigned char state; /* Thread slot state (STATE_*) */ | ||
171 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | 134 | #ifdef HAVE_SCHEDULER_BOOSTCTRL |
172 | unsigned char cpu_boost; /* CPU frequency boost flag */ | 135 | unsigned char cpu_boost; /* CPU frequency boost flag */ |
173 | #endif | 136 | #endif |
174 | #ifdef HAVE_IO_PRIORITY | 137 | #ifdef HAVE_IO_PRIORITY |
175 | unsigned char io_priority; | 138 | unsigned char io_priority; |
176 | #endif | 139 | #endif |
177 | }; | 140 | }; |
178 | 141 | ||
142 | /* Thread ID, 32 bits = |VVVVVVVV|VVVVVVVV|VVVVVVVV|SSSSSSSS| */ | ||
143 | #define THREAD_ID_VERSION_SHIFT 8 | ||
144 | #define THREAD_ID_VERSION_MASK 0xffffff00 | ||
145 | #define THREAD_ID_SLOT_MASK 0x000000ff | ||
146 | #define THREAD_ID_INIT(n) ((1u << THREAD_ID_VERSION_SHIFT) | (n)) | ||
147 | #define THREAD_ID_SLOT(id) ((id) & THREAD_ID_SLOT_MASK) | ||
148 | |||
149 | #define DEADBEEF ((uintptr_t)0xdeadbeefdeadbeefull) | ||
150 | |||
179 | /* Information kept for each core | 151 | /* Information kept for each core |
180 | * Members are arranged for the same reason as in thread_entry | 152 | * Members are arranged for the same reason as in thread_entry |
181 | */ | 153 | */ |
@@ -183,53 +155,97 @@ struct core_entry | |||
183 | { | 155 | { |
184 | /* "Active" lists - core is constantly active on these and are never | 156 | /* "Active" lists - core is constantly active on these and are never |
185 | locked and interrupts do not access them */ | 157 | locked and interrupts do not access them */ |
186 | struct thread_entry *running; /* threads that are running (RTR) */ | 158 | struct __rtr_queue rtr; /* Threads that are runnable */ |
187 | struct thread_entry *timeout; /* threads that are on a timeout before | 159 | struct __tmo_queue tmo; /* Threads on a bounded wait */ |
188 | running again */ | 160 | struct thread_entry *running; /* Currently running thread */ |
189 | struct thread_entry *block_task; /* Task going off running list */ | ||
190 | #ifdef HAVE_PRIORITY_SCHEDULING | 161 | #ifdef HAVE_PRIORITY_SCHEDULING |
191 | struct priority_distribution rtr; /* Summary of running and ready-to-run | 162 | struct priority_distribution rtr_dist; /* Summary of runnables */ |
192 | threads */ | ||
193 | #endif | 163 | #endif |
194 | long next_tmo_check; /* soonest time to check tmo threads */ | 164 | long next_tmo_check; /* Next due timeout check */ |
195 | #ifdef HAVE_CORELOCK_OBJECT | 165 | #if NUM_CORES > 1 |
196 | struct thread_blk_ops blk_ops; /* operations to perform when | 166 | struct corelock rtr_cl; /* Lock for rtr list */ |
197 | blocking a thread */ | ||
198 | struct corelock rtr_cl; /* Lock for rtr list */ | ||
199 | #endif /* NUM_CORES */ | 167 | #endif /* NUM_CORES */ |
200 | }; | 168 | }; |
201 | 169 | ||
202 | /* Thread ID, 32 bits = |VVVVVVVV|VVVVVVVV|VVVVVVVV|SSSSSSSS| */ | 170 | /* Hide a few scheduler details from itself to make allocation more flexible */ |
203 | #define THREAD_ID_VERSION_SHIFT 8 | 171 | #define __main_thread_name \ |
204 | #define THREAD_ID_VERSION_MASK 0xffffff00 | 172 | ({ extern const char __main_thread_name_str[]; \ |
205 | #define THREAD_ID_SLOT_MASK 0x000000ff | 173 | __main_thread_name_str; }) |
206 | #define THREAD_ID_INIT(n) ((1u << THREAD_ID_VERSION_SHIFT) | (n)) | 174 | |
207 | #define THREAD_ID_SLOT(id) ((id) & THREAD_ID_SLOT_MASK) | 175 | static FORCE_INLINE |
176 | void * __get_main_stack(size_t *stacksize) | ||
177 | { | ||
178 | #if (CONFIG_PLATFORM & PLATFORM_NATIVE) | ||
179 | extern uintptr_t stackbegin[]; | ||
180 | extern uintptr_t stackend[]; | ||
181 | #else | ||
182 | extern uintptr_t *stackbegin; | ||
183 | extern uintptr_t *stackend; | ||
184 | #endif | ||
185 | *stacksize = (uintptr_t)stackend - (uintptr_t)stackbegin; | ||
186 | return stackbegin; | ||
187 | } | ||
208 | 188 | ||
209 | /* Thread locking */ | 189 | void format_thread_name(char *buf, size_t bufsize, |
190 | const struct thread_entry *thread); | ||
191 | |||
192 | static FORCE_INLINE | ||
193 | struct core_entry * __core_id_entry(unsigned int core) | ||
194 | { | ||
210 | #if NUM_CORES > 1 | 195 | #if NUM_CORES > 1 |
211 | #define LOCK_THREAD(thread) \ | 196 | extern struct core_entry * __cores[NUM_CORES]; |
212 | ({ corelock_lock(&(thread)->slot_cl); }) | 197 | return __cores[core]; |
213 | #define TRY_LOCK_THREAD(thread) \ | 198 | #else |
214 | ({ corelock_try_lock(&(thread)->slot_cl); }) | 199 | extern struct core_entry __cores[NUM_CORES]; |
215 | #define UNLOCK_THREAD(thread) \ | 200 | return &__cores[core]; |
216 | ({ corelock_unlock(&(thread)->slot_cl); }) | 201 | #endif |
217 | #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \ | 202 | } |
218 | ({ unsigned int _core = (thread)->core; \ | ||
219 | cores[_core].blk_ops.flags |= TBOP_UNLOCK_CORELOCK; \ | ||
220 | cores[_core].blk_ops.cl_p = &(thread)->slot_cl; }) | ||
221 | #else /* NUM_CORES == 1*/ | ||
222 | #define LOCK_THREAD(thread) \ | ||
223 | ({ (void)(thread); }) | ||
224 | #define TRY_LOCK_THREAD(thread) \ | ||
225 | ({ (void)(thread); }) | ||
226 | #define UNLOCK_THREAD(thread) \ | ||
227 | ({ (void)(thread); }) | ||
228 | #define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \ | ||
229 | ({ (void)(thread); }) | ||
230 | #endif /* NUM_CORES */ | ||
231 | 203 | ||
232 | #define DEADBEEF ((uintptr_t)0xdeadbeefdeadbeefull) | 204 | #define __running_self_entry() \ |
205 | __core_id_entry(CURRENT_CORE)->running | ||
206 | |||
207 | static FORCE_INLINE | ||
208 | struct thread_entry * __thread_slot_entry(unsigned int slotnum) | ||
209 | { | ||
210 | extern struct thread_entry * __threads[MAXTHREADS]; | ||
211 | return __threads[slotnum]; | ||
212 | } | ||
213 | |||
214 | #define __thread_id_entry(id) \ | ||
215 | __thread_slot_entry(THREAD_ID_SLOT(id)) | ||
216 | |||
217 | #define THREAD_FROM(p, member) \ | ||
218 | container_of(p, struct thread_entry, member) | ||
219 | |||
220 | #define RTR_EMPTY(rtrp) \ | ||
221 | ({ (rtrp)->head == NULL; }) | ||
222 | |||
223 | #define RTR_THREAD_FIRST(rtrp) \ | ||
224 | ({ THREAD_FROM((rtrp)->head, rtr); }) | ||
225 | |||
226 | #define RTR_THREAD_NEXT(thread) \ | ||
227 | ({ THREAD_FROM((thread)->rtr.next, rtr); }) | ||
228 | |||
229 | #define TMO_THREAD_FIRST(tmop) \ | ||
230 | ({ struct __tmo_queue *__tmop = (tmop); \ | ||
231 | __tmop->head ? THREAD_FROM(__tmop->head, tmo) : NULL; }) | ||
232 | |||
233 | #define TMO_THREAD_NEXT(thread) \ | ||
234 | ({ struct __tmo_queue_node *__next = (thread)->tmo.next; \ | ||
235 | __next ? THREAD_FROM(__next, tmo) : NULL; }) | ||
236 | |||
237 | #define WQ_THREAD_FIRST(wqp) \ | ||
238 | ({ struct __wait_queue *__wqp = (wqp); \ | ||
239 | __wqp->head ? THREAD_FROM(__wqp->head, wq) : NULL; }) | ||
240 | |||
241 | #define WQ_THREAD_NEXT(thread) \ | ||
242 | ({ struct __wait_queue_node *__next = (thread)->wq.next; \ | ||
243 | __next ? THREAD_FROM(__next, wq) : NULL; }) | ||
244 | |||
245 | void thread_alloc_init(void) INIT_ATTR; | ||
246 | struct thread_entry * thread_alloc(void); | ||
247 | void thread_free(struct thread_entry *thread); | ||
248 | void new_thread_id(struct thread_entry *thread); | ||
233 | 249 | ||
234 | /* Switch to next runnable thread */ | 250 | /* Switch to next runnable thread */ |
235 | void switch_thread(void); | 251 | void switch_thread(void); |
@@ -237,7 +253,21 @@ void switch_thread(void); | |||
237 | * next tick) */ | 253 | * next tick) */ |
238 | void sleep_thread(int ticks); | 254 | void sleep_thread(int ticks); |
239 | /* Blocks the current thread on a thread queue (< 0 == infinite) */ | 255 | /* Blocks the current thread on a thread queue (< 0 == infinite) */ |
240 | void block_thread(struct thread_entry *current, int timeout); | 256 | void block_thread_(struct thread_entry *current, int timeout); |
257 | |||
258 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
259 | #define block_thread(thread, timeout, __wqp, bl) \ | ||
260 | ({ struct thread_entry *__t = (thread); \ | ||
261 | __t->wqp = (__wqp); \ | ||
262 | if (!__builtin_constant_p(bl) || (bl)) \ | ||
263 | __t->blocker = (bl); \ | ||
264 | block_thread_(__t, (timeout)); }) | ||
265 | #else | ||
266 | #define block_thread(thread, timeout, __wqp, bl...) \ | ||
267 | ({ struct thread_entry *__t = (thread); \ | ||
268 | __t->wqp = (__wqp); \ | ||
269 | block_thread_(__t, (timeout)); }) | ||
270 | #endif | ||
241 | 271 | ||
242 | /* Return bit flags for thread wakeup */ | 272 | /* Return bit flags for thread wakeup */ |
243 | #define THREAD_NONE 0x0 /* No thread woken up (exclusive) */ | 273 | #define THREAD_NONE 0x0 /* No thread woken up (exclusive) */ |
@@ -246,7 +276,7 @@ void block_thread(struct thread_entry *current, int timeout); | |||
246 | higher priority than current were woken) */ | 276 | higher priority than current were woken) */ |
247 | 277 | ||
248 | /* A convenience function for waking an entire queue of threads. */ | 278 | /* A convenience function for waking an entire queue of threads. */ |
249 | unsigned int thread_queue_wake(struct thread_entry **list); | 279 | unsigned int wait_queue_wake(struct __wait_queue *wqp); |
250 | 280 | ||
251 | /* Wakeup a thread at the head of a list */ | 281 | /* Wakeup a thread at the head of a list */ |
252 | enum wakeup_thread_protocol | 282 | enum wakeup_thread_protocol |
@@ -257,36 +287,139 @@ enum wakeup_thread_protocol | |||
257 | WAKEUP_TRANSFER_MULTI, | 287 | WAKEUP_TRANSFER_MULTI, |
258 | }; | 288 | }; |
259 | 289 | ||
260 | unsigned int wakeup_thread_(struct thread_entry **list | 290 | unsigned int wakeup_thread_(struct thread_entry *thread |
261 | IF_PRIO(, enum wakeup_thread_protocol proto)); | 291 | IF_PRIO(, enum wakeup_thread_protocol proto)); |
262 | 292 | ||
263 | #ifdef HAVE_PRIORITY_SCHEDULING | 293 | #ifdef HAVE_PRIORITY_SCHEDULING |
264 | #define wakeup_thread(list, proto) \ | 294 | #define wakeup_thread(thread, proto) \ |
265 | wakeup_thread_((list), (proto)) | 295 | wakeup_thread_((thread), (proto)) |
266 | #else /* !HAVE_PRIORITY_SCHEDULING */ | 296 | #else |
267 | #define wakeup_thread(list, proto...) \ | 297 | #define wakeup_thread(thread, proto...) \ |
268 | wakeup_thread_((list)); | 298 | wakeup_thread_((thread)); |
269 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 299 | #endif |
270 | 300 | ||
271 | #ifdef HAVE_IO_PRIORITY | 301 | #ifdef RB_PROFILE |
272 | void thread_set_io_priority(unsigned int thread_id, int io_priority); | 302 | void profile_thread(void); |
273 | int thread_get_io_priority(unsigned int thread_id); | ||
274 | #endif /* HAVE_IO_PRIORITY */ | ||
275 | #if NUM_CORES > 1 | ||
276 | unsigned int switch_core(unsigned int new_core); | ||
277 | #endif | 303 | #endif |
278 | 304 | ||
279 | /* Return the id of the calling thread. */ | 305 | static inline void rtr_queue_init(struct __rtr_queue *rtrp) |
280 | unsigned int thread_self(void); | 306 | { |
307 | lldc_init(rtrp); | ||
308 | } | ||
309 | |||
310 | static inline void rtr_queue_make_first(struct __rtr_queue *rtrp, | ||
311 | struct thread_entry *thread) | ||
312 | { | ||
313 | rtrp->head = &thread->rtr; | ||
314 | } | ||
281 | 315 | ||
282 | /* Return the thread_entry for the calling thread */ | 316 | static inline void rtr_queue_add(struct __rtr_queue *rtrp, |
283 | struct thread_entry* thread_self_entry(void); | 317 | struct thread_entry *thread) |
318 | { | ||
319 | lldc_insert_last(rtrp, &thread->rtr); | ||
320 | } | ||
284 | 321 | ||
285 | /* Return thread entry from id */ | 322 | static inline void rtr_queue_remove(struct __rtr_queue *rtrp, |
286 | struct thread_entry *thread_id_entry(unsigned int thread_id); | 323 | struct thread_entry *thread) |
324 | { | ||
325 | lldc_remove(rtrp, &thread->rtr); | ||
326 | } | ||
287 | 327 | ||
288 | #ifdef RB_PROFILE | 328 | #define TMO_NOT_QUEUED (NULL + 1) |
289 | void profile_thread(void); | 329 | |
330 | static inline bool tmo_is_queued(struct thread_entry *thread) | ||
331 | { | ||
332 | return thread->tmo.next != TMO_NOT_QUEUED; | ||
333 | } | ||
334 | |||
335 | static inline void tmo_set_dequeued(struct thread_entry *thread) | ||
336 | { | ||
337 | thread->tmo.next = TMO_NOT_QUEUED; | ||
338 | } | ||
339 | |||
340 | static inline void tmo_queue_init(struct __tmo_queue *tmop) | ||
341 | { | ||
342 | ll_init(tmop); | ||
343 | } | ||
344 | |||
345 | static inline void tmo_queue_expire(struct __tmo_queue *tmop, | ||
346 | struct thread_entry *prev, | ||
347 | struct thread_entry *thread) | ||
348 | { | ||
349 | ll_remove_next(tmop, prev ? &prev->tmo : NULL); | ||
350 | tmo_set_dequeued(thread); | ||
351 | } | ||
352 | |||
353 | static inline void tmo_queue_remove(struct __tmo_queue *tmop, | ||
354 | struct thread_entry *thread) | ||
355 | { | ||
356 | if (tmo_is_queued(thread)) | ||
357 | { | ||
358 | ll_remove(tmop, &thread->tmo); | ||
359 | tmo_set_dequeued(thread); | ||
360 | } | ||
361 | } | ||
362 | |||
363 | static inline void tmo_queue_register(struct __tmo_queue *tmop, | ||
364 | struct thread_entry *thread) | ||
365 | { | ||
366 | if (!tmo_is_queued(thread)) | ||
367 | ll_insert_last(tmop, &thread->tmo); | ||
368 | } | ||
369 | |||
370 | static inline void wait_queue_init(struct __wait_queue *wqp) | ||
371 | { | ||
372 | lld_init(wqp); | ||
373 | } | ||
374 | |||
375 | static inline void wait_queue_register(struct thread_entry *thread) | ||
376 | { | ||
377 | lld_insert_last(thread->wqp, &thread->wq); | ||
378 | } | ||
379 | |||
380 | static inline struct __wait_queue * | ||
381 | wait_queue_ptr(struct thread_entry *thread) | ||
382 | { | ||
383 | return thread->wqp; | ||
384 | } | ||
385 | |||
386 | static inline struct __wait_queue * | ||
387 | wait_queue_remove(struct thread_entry *thread) | ||
388 | { | ||
389 | struct __wait_queue *wqp = thread->wqp; | ||
390 | thread->wqp = NULL; | ||
391 | lld_remove(wqp, &thread->wq); | ||
392 | return wqp; | ||
393 | } | ||
394 | |||
395 | static inline struct __wait_queue * | ||
396 | wait_queue_try_remove(struct thread_entry *thread) | ||
397 | { | ||
398 | struct __wait_queue *wqp = thread->wqp; | ||
399 | if (wqp) | ||
400 | { | ||
401 | thread->wqp = NULL; | ||
402 | lld_remove(wqp, &thread->wq); | ||
403 | } | ||
404 | |||
405 | return wqp; | ||
406 | } | ||
407 | |||
408 | static inline void blocker_init(struct blocker *bl) | ||
409 | { | ||
410 | bl->thread = NULL; | ||
411 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
412 | bl->priority = PRIORITY_IDLE; | ||
413 | #endif | ||
414 | } | ||
415 | |||
416 | static inline void blocker_splay_init(struct blocker_splay *blsplay) | ||
417 | { | ||
418 | blocker_init(&blsplay->blocker); | ||
419 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
420 | threadbit_clear(&blsplay->mask); | ||
290 | #endif | 421 | #endif |
422 | corelock_init(&blsplay->cl); | ||
423 | } | ||
291 | 424 | ||
292 | #endif /* THREAD_INTERNAL_H */ | 425 | #endif /* THREAD_INTERNAL_H */ |
diff --git a/firmware/kernel/thread.c b/firmware/kernel/thread.c index c148f6b76e..b916c3b521 100644 --- a/firmware/kernel/thread.c +++ b/firmware/kernel/thread.c | |||
@@ -37,11 +37,6 @@ | |||
37 | #endif | 37 | #endif |
38 | #include "core_alloc.h" | 38 | #include "core_alloc.h" |
39 | 39 | ||
40 | /**************************************************************************** | ||
41 | * ATTENTION!! * | ||
42 | * See notes below on implementing processor-specific portions! * | ||
43 | ***************************************************************************/ | ||
44 | |||
45 | /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */ | 40 | /* Define THREAD_EXTRA_CHECKS as 1 to enable additional state checks */ |
46 | #ifdef DEBUG | 41 | #ifdef DEBUG |
47 | #define THREAD_EXTRA_CHECKS 1 /* Always 1 for DEBUG */ | 42 | #define THREAD_EXTRA_CHECKS 1 /* Always 1 for DEBUG */ |
@@ -49,7 +44,11 @@ | |||
49 | #define THREAD_EXTRA_CHECKS 0 | 44 | #define THREAD_EXTRA_CHECKS 0 |
50 | #endif | 45 | #endif |
51 | 46 | ||
52 | /** | 47 | /**************************************************************************** |
48 | * ATTENTION!! * | ||
49 | * See notes below on implementing processor-specific portions! * | ||
50 | **************************************************************************** | ||
51 | * | ||
53 | * General locking order to guarantee progress. Order must be observed but | 52 | * General locking order to guarantee progress. Order must be observed but |
54 | * all stages are not nescessarily obligatory. Going from 1) to 3) is | 53 | * all stages are not nescessarily obligatory. Going from 1) to 3) is |
55 | * perfectly legal. | 54 | * perfectly legal. |
@@ -66,14 +65,14 @@ | |||
66 | * unlock and the other processor's handler may proceed at that time. Not | 65 | * unlock and the other processor's handler may proceed at that time. Not |
67 | * nescessary when the resource in question is definitely not available to | 66 | * nescessary when the resource in question is definitely not available to |
68 | * interrupt handlers. | 67 | * interrupt handlers. |
69 | * | 68 | * |
70 | * 2) Kernel Object | 69 | * 2) Kernel Object |
71 | * 1) May be needed beforehand if the kernel object allows dual-use such as | 70 | * 1) May be needed beforehand if the kernel object allows dual-use such as |
72 | * event queues. The kernel object must have a scheme to protect itself from | 71 | * event queues. The kernel object must have a scheme to protect itself from |
73 | * access by another processor and is responsible for serializing the calls | 72 | * access by another processor and is responsible for serializing the calls |
74 | * to block_thread(_w_tmo) and wakeup_thread both to themselves and to each | 73 | * to block_thread and wakeup_thread both to themselves and to each other. |
75 | * other. Objects' queues are also protected here. | 74 | * Objects' queues are also protected here. |
76 | * | 75 | * |
77 | * 3) Thread Slot | 76 | * 3) Thread Slot |
78 | * This locks access to the thread's slot such that its state cannot be | 77 | * This locks access to the thread's slot such that its state cannot be |
79 | * altered by another processor when a state change is in progress such as | 78 | * altered by another processor when a state change is in progress such as |
@@ -121,68 +120,62 @@ | |||
121 | * available then some careful non-blocking synchonization is needed (as on | 120 | * available then some careful non-blocking synchonization is needed (as on |
122 | * PP targets at the moment). | 121 | * PP targets at the moment). |
123 | *--------------------------------------------------------------------------- | 122 | *--------------------------------------------------------------------------- |
123 | * | ||
124 | * | ||
125 | *--------------------------------------------------------------------------- | ||
126 | * Priority distribution structure (one category for each possible priority): | ||
127 | * | ||
128 | * +----+----+----+ ... +------+ | ||
129 | * hist: | F0 | F1 | F2 | | Fn-1 | | ||
130 | * +----+----+----+ ... +------+ | ||
131 | * mask: | b0 | b1 | b2 | | bn-1 | | ||
132 | * +----+----+----+ ... +------+ | ||
133 | * | ||
134 | * F = count of threads at priority category n (frequency) | ||
135 | * b = bitmask of non-zero priority categories (occupancy) | ||
136 | * | ||
137 | * / if H[n] != 0 : 1 | ||
138 | * b[n] = | | ||
139 | * \ else : 0 | ||
140 | * | ||
141 | *--------------------------------------------------------------------------- | ||
142 | * Basic priority inheritance priotocol (PIP): | ||
143 | * | ||
144 | * Mn = mutex n, Tn = thread n | ||
145 | * | ||
146 | * A lower priority thread inherits the priority of the highest priority | ||
147 | * thread blocked waiting for it to complete an action (such as release a | ||
148 | * mutex or respond to a message via queue_send): | ||
149 | * | ||
150 | * 1) T2->M1->T1 | ||
151 | * | ||
152 | * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher | ||
153 | * priority than T1 then T1 inherits the priority of T2. | ||
154 | * | ||
155 | * 2) T3 | ||
156 | * \/ | ||
157 | * T2->M1->T1 | ||
158 | * | ||
159 | * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so | ||
160 | * T1 inherits the higher of T2 and T3. | ||
161 | * | ||
162 | * 3) T3->M2->T2->M1->T1 | ||
163 | * | ||
164 | * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2, | ||
165 | * then T1 inherits the priority of T3 through T2. | ||
166 | * | ||
167 | * Blocking chains can grow arbitrarily complex (though it's best that they | ||
168 | * not form at all very often :) and build-up from these units. | ||
169 | *--------------------------------------------------------------------------- | ||
124 | */ | 170 | */ |
125 | 171 | static FORCE_INLINE void core_sleep(IF_COP_VOID(unsigned int core)); | |
126 | /* Cast to the the machine pointer size, whose size could be < 4 or > 32 | 172 | static FORCE_INLINE void store_context(void* addr); |
127 | * (someday :). */ | 173 | static FORCE_INLINE void load_context(const void* addr); |
128 | static struct core_entry cores[NUM_CORES] IBSS_ATTR; | ||
129 | struct thread_entry threads[MAXTHREADS] IBSS_ATTR; | ||
130 | |||
131 | static const char main_thread_name[] = "main"; | ||
132 | #if (CONFIG_PLATFORM & PLATFORM_NATIVE) | ||
133 | extern uintptr_t stackbegin[]; | ||
134 | extern uintptr_t stackend[]; | ||
135 | #else | ||
136 | extern uintptr_t *stackbegin; | ||
137 | extern uintptr_t *stackend; | ||
138 | #endif | ||
139 | |||
140 | static inline void core_sleep(IF_COP_VOID(unsigned int core)) | ||
141 | __attribute__((always_inline)); | ||
142 | |||
143 | void check_tmo_threads(void) | ||
144 | __attribute__((noinline)); | ||
145 | |||
146 | static inline void block_thread_on_l(struct thread_entry *thread, unsigned state) | ||
147 | __attribute__((always_inline)); | ||
148 | |||
149 | static void add_to_list_tmo(struct thread_entry *thread) | ||
150 | __attribute__((noinline)); | ||
151 | |||
152 | static void core_schedule_wakeup(struct thread_entry *thread) | ||
153 | __attribute__((noinline)); | ||
154 | |||
155 | #if NUM_CORES > 1 | ||
156 | static inline void run_blocking_ops( | ||
157 | unsigned int core, struct thread_entry *thread) | ||
158 | __attribute__((always_inline)); | ||
159 | #endif | ||
160 | |||
161 | static void thread_stkov(struct thread_entry *thread) | ||
162 | __attribute__((noinline)); | ||
163 | |||
164 | static inline void store_context(void* addr) | ||
165 | __attribute__((always_inline)); | ||
166 | |||
167 | static inline void load_context(const void* addr) | ||
168 | __attribute__((always_inline)); | ||
169 | |||
170 | #if NUM_CORES > 1 | ||
171 | static void thread_final_exit_do(struct thread_entry *current) | ||
172 | __attribute__((noinline)) NORETURN_ATTR USED_ATTR; | ||
173 | #else | ||
174 | static inline void thread_final_exit(struct thread_entry *current) | ||
175 | __attribute__((always_inline)) NORETURN_ATTR; | ||
176 | #endif | ||
177 | |||
178 | void switch_thread(void) | ||
179 | __attribute__((noinline)); | ||
180 | 174 | ||
181 | /**************************************************************************** | 175 | /**************************************************************************** |
182 | * Processor/OS-specific section - include necessary core support | 176 | * Processor/OS-specific section - include necessary core support |
183 | */ | 177 | */ |
184 | 178 | ||
185 | |||
186 | #include "asm/thread.c" | 179 | #include "asm/thread.c" |
187 | 180 | ||
188 | #if defined (CPU_PP) | 181 | #if defined (CPU_PP) |
@@ -193,20 +186,17 @@ void switch_thread(void) | |||
193 | * End Processor-specific section | 186 | * End Processor-specific section |
194 | ***************************************************************************/ | 187 | ***************************************************************************/ |
195 | 188 | ||
196 | static NO_INLINE | 189 | static NO_INLINE NORETURN_ATTR |
197 | void thread_panicf(const char *msg, struct thread_entry *thread) | 190 | void thread_panicf(const char *msg, struct thread_entry *thread) |
198 | { | 191 | { |
199 | IF_COP( const unsigned int core = thread->core; ) | 192 | IF_COP( const unsigned int core = thread->core; ) |
200 | static char namebuf[sizeof (((struct thread_debug_info *)0)->name)]; | 193 | static char name[sizeof (((struct thread_debug_info *)0)->name)]; |
201 | const char *name = thread->name; | 194 | format_thread_name(name, sizeof (name), thread); |
202 | if (!name) | ||
203 | name = ""; | ||
204 | snprintf(namebuf, sizeof (namebuf), *name ? "%s" : "%s%08lX", | ||
205 | name, (unsigned long)thread->id); | ||
206 | panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core)); | 195 | panicf ("%s %s" IF_COP(" (%d)"), msg, name IF_COP(, core)); |
196 | while (1); | ||
207 | } | 197 | } |
208 | 198 | ||
209 | static void thread_stkov(struct thread_entry *thread) | 199 | static NO_INLINE void thread_stkov(struct thread_entry *thread) |
210 | { | 200 | { |
211 | thread_panicf("Stkov", thread); | 201 | thread_panicf("Stkov", thread); |
212 | } | 202 | } |
@@ -218,36 +208,51 @@ static void thread_stkov(struct thread_entry *thread) | |||
218 | ({ if (!({ exp; })) thread_panicf((msg), (thread)); }) | 208 | ({ if (!({ exp; })) thread_panicf((msg), (thread)); }) |
219 | #else | 209 | #else |
220 | #define THREAD_PANICF(msg, thread) \ | 210 | #define THREAD_PANICF(msg, thread) \ |
221 | do {} while (0) | 211 | do {} while (1) |
222 | #define THREAD_ASSERT(exp, msg, thread) \ | 212 | #define THREAD_ASSERT(exp, msg, thread) \ |
223 | do {} while (0) | 213 | do {} while (0) |
224 | #endif /* THREAD_EXTRA_CHECKS */ | 214 | #endif /* THREAD_EXTRA_CHECKS */ |
225 | 215 | ||
216 | /* Thread locking */ | ||
217 | #if NUM_CORES > 1 | ||
218 | #define LOCK_THREAD(thread) \ | ||
219 | ({ corelock_lock(&(thread)->slot_cl); }) | ||
220 | #define TRY_LOCK_THREAD(thread) \ | ||
221 | ({ corelock_try_lock(&(thread)->slot_cl); }) | ||
222 | #define UNLOCK_THREAD(thread) \ | ||
223 | ({ corelock_unlock(&(thread)->slot_cl); }) | ||
224 | #else /* NUM_CORES == 1*/ | ||
225 | #define LOCK_THREAD(thread) \ | ||
226 | ({ (void)(thread); }) | ||
227 | #define TRY_LOCK_THREAD(thread) \ | ||
228 | ({ (void)(thread); }) | ||
229 | #define UNLOCK_THREAD(thread) \ | ||
230 | ({ (void)(thread); }) | ||
231 | #endif /* NUM_CORES */ | ||
232 | |||
226 | /* RTR list */ | 233 | /* RTR list */ |
227 | #define RTR_LOCK(core) \ | 234 | #define RTR_LOCK(corep) \ |
228 | ({ corelock_lock(&cores[core].rtr_cl); }) | 235 | corelock_lock(&(corep)->rtr_cl) |
229 | #define RTR_UNLOCK(core) \ | 236 | #define RTR_UNLOCK(corep) \ |
230 | ({ corelock_unlock(&cores[core].rtr_cl); }) | 237 | corelock_unlock(&(corep)->rtr_cl) |
231 | 238 | ||
232 | #ifdef HAVE_PRIORITY_SCHEDULING | 239 | #ifdef HAVE_PRIORITY_SCHEDULING |
233 | #define rtr_add_entry(core, priority) \ | 240 | #define rtr_add_entry(corep, priority) \ |
234 | prio_add_entry(&cores[core].rtr, (priority)) | 241 | prio_add_entry(&(corep)->rtr_dist, (priority)) |
235 | 242 | #define rtr_subtract_entry(corep, priority) \ | |
236 | #define rtr_subtract_entry(core, priority) \ | 243 | prio_subtract_entry(&(corep)->rtr_dist, (priority)) |
237 | prio_subtract_entry(&cores[core].rtr, (priority)) | 244 | #define rtr_move_entry(corep, from, to) \ |
238 | 245 | prio_move_entry(&(corep)->rtr_dist, (from), (to)) | |
239 | #define rtr_move_entry(core, from, to) \ | 246 | #else /* !HAVE_PRIORITY_SCHEDULING */ |
240 | prio_move_entry(&cores[core].rtr, (from), (to)) | 247 | #define rtr_add_entry(corep, priority) \ |
241 | #else | 248 | do {} while (0) |
242 | #define rtr_add_entry(core, priority) | 249 | #define rtr_subtract_entry(corep, priority) \ |
243 | #define rtr_add_entry_inl(core, priority) | 250 | do {} while (0) |
244 | #define rtr_subtract_entry(core, priority) | 251 | #define rtr_move_entry(corep, from, to) \ |
245 | #define rtr_subtract_entry_inl(core, priotity) | 252 | do {} while (0) |
246 | #define rtr_move_entry(core, from, to) | 253 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
247 | #define rtr_move_entry_inl(core, from, to) | ||
248 | #endif | ||
249 | 254 | ||
250 | static inline void thread_store_context(struct thread_entry *thread) | 255 | static FORCE_INLINE void thread_store_context(struct thread_entry *thread) |
251 | { | 256 | { |
252 | #if (CONFIG_PLATFORM & PLATFORM_HOSTED) | 257 | #if (CONFIG_PLATFORM & PLATFORM_HOSTED) |
253 | thread->__errno = errno; | 258 | thread->__errno = errno; |
@@ -255,7 +260,7 @@ static inline void thread_store_context(struct thread_entry *thread) | |||
255 | store_context(&thread->context); | 260 | store_context(&thread->context); |
256 | } | 261 | } |
257 | 262 | ||
258 | static inline void thread_load_context(struct thread_entry *thread) | 263 | static FORCE_INLINE void thread_load_context(struct thread_entry *thread) |
259 | { | 264 | { |
260 | load_context(&thread->context); | 265 | load_context(&thread->context); |
261 | #if (CONFIG_PLATFORM & PLATFORM_HOSTED) | 266 | #if (CONFIG_PLATFORM & PLATFORM_HOSTED) |
@@ -263,272 +268,31 @@ static inline void thread_load_context(struct thread_entry *thread) | |||
263 | #endif | 268 | #endif |
264 | } | 269 | } |
265 | 270 | ||
266 | static inline unsigned int should_switch_tasks(void) | 271 | static FORCE_INLINE unsigned int |
272 | should_switch_tasks(struct thread_entry *thread) | ||
267 | { | 273 | { |
268 | unsigned int result = THREAD_OK; | ||
269 | |||
270 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
271 | struct thread_entry *current = cores[CURRENT_CORE].running; | ||
272 | if (current && | ||
273 | priobit_ffs(&cores[IF_COP_CORE(current->core)].rtr.mask) | ||
274 | < current->priority) | ||
275 | { | ||
276 | /* There is a thread ready to run of higher priority on the same | ||
277 | * core as the current one; recommend a task switch. */ | ||
278 | result |= THREAD_SWITCH; | ||
279 | } | ||
280 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
281 | |||
282 | return result; | ||
283 | } | ||
284 | |||
285 | #ifdef HAVE_PRIORITY_SCHEDULING | 274 | #ifdef HAVE_PRIORITY_SCHEDULING |
286 | /*--------------------------------------------------------------------------- | 275 | const unsigned int core = CURRENT_CORE; |
287 | * Locks the thread registered as the owner of the block and makes sure it | ||
288 | * didn't change in the meantime | ||
289 | *--------------------------------------------------------------------------- | ||
290 | */ | ||
291 | #if NUM_CORES == 1 | ||
292 | static inline struct thread_entry * lock_blocker_thread(struct blocker *bl) | ||
293 | { | ||
294 | return bl->thread; | ||
295 | } | ||
296 | #else /* NUM_CORES > 1 */ | ||
297 | static struct thread_entry * lock_blocker_thread(struct blocker *bl) | ||
298 | { | ||
299 | /* The blocker thread may change during the process of trying to | ||
300 | capture it */ | ||
301 | while (1) | ||
302 | { | ||
303 | struct thread_entry *t = bl->thread; | ||
304 | |||
305 | /* TRY, or else deadlocks are possible */ | ||
306 | if (!t) | ||
307 | { | ||
308 | struct blocker_splay *blsplay = (struct blocker_splay *)bl; | ||
309 | if (corelock_try_lock(&blsplay->cl)) | ||
310 | { | ||
311 | if (!bl->thread) | ||
312 | return NULL; /* Still multi */ | ||
313 | |||
314 | corelock_unlock(&blsplay->cl); | ||
315 | } | ||
316 | } | ||
317 | else | ||
318 | { | ||
319 | if (TRY_LOCK_THREAD(t)) | ||
320 | { | ||
321 | if (bl->thread == t) | ||
322 | return t; | ||
323 | |||
324 | UNLOCK_THREAD(t); | ||
325 | } | ||
326 | } | ||
327 | } | ||
328 | } | ||
329 | #endif /* NUM_CORES */ | ||
330 | |||
331 | static inline void unlock_blocker_thread(struct blocker *bl) | ||
332 | { | ||
333 | #if NUM_CORES > 1 | 276 | #if NUM_CORES > 1 |
334 | struct thread_entry *blt = bl->thread; | 277 | /* Forget about it if different CPU */ |
335 | if (blt) | 278 | if (thread->core != core) |
336 | UNLOCK_THREAD(blt); | 279 | return THREAD_OK; |
337 | else | 280 | #endif |
338 | corelock_unlock(&((struct blocker_splay *)bl)->cl); | 281 | /* Just woke something therefore a thread is on the run queue */ |
339 | #endif /* NUM_CORES > 1*/ | 282 | struct thread_entry *current = |
340 | (void)bl; | 283 | RTR_THREAD_FIRST(&__core_id_entry(core)->rtr); |
341 | } | 284 | if (LIKELY(thread->priority >= current->priority)) |
285 | return THREAD_OK; | ||
286 | |||
287 | /* There is a thread ready to run of higher priority on the same | ||
288 | * core as the current one; recommend a task switch. */ | ||
289 | return THREAD_OK | THREAD_SWITCH; | ||
290 | #else | ||
291 | return THREAD_OK; | ||
342 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 292 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
343 | |||
344 | /*--------------------------------------------------------------------------- | ||
345 | * Thread list structure - circular: | ||
346 | * +------------------------------+ | ||
347 | * | | | ||
348 | * +--+---+<-+---+<-+---+<-+---+<-+ | ||
349 | * Head->| T | | T | | T | | T | | ||
350 | * +->+---+->+---+->+---+->+---+--+ | ||
351 | * | | | ||
352 | * +------------------------------+ | ||
353 | *--------------------------------------------------------------------------- | ||
354 | */ | ||
355 | |||
356 | /*--------------------------------------------------------------------------- | ||
357 | * Adds a thread to a list of threads using "insert last". Uses the "l" | ||
358 | * links. | ||
359 | *--------------------------------------------------------------------------- | ||
360 | */ | ||
361 | static void add_to_list_l(struct thread_entry **list, | ||
362 | struct thread_entry *thread) | ||
363 | { | ||
364 | struct thread_entry *l = *list; | ||
365 | |||
366 | if (l == NULL) | ||
367 | { | ||
368 | /* Insert into unoccupied list */ | ||
369 | thread->l.prev = thread; | ||
370 | thread->l.next = thread; | ||
371 | *list = thread; | ||
372 | return; | ||
373 | } | ||
374 | |||
375 | /* Insert last */ | ||
376 | thread->l.prev = l->l.prev; | ||
377 | thread->l.next = l; | ||
378 | l->l.prev->l.next = thread; | ||
379 | l->l.prev = thread; | ||
380 | } | ||
381 | |||
382 | /*--------------------------------------------------------------------------- | ||
383 | * Removes a thread from a list of threads. Uses the "l" links. | ||
384 | *--------------------------------------------------------------------------- | ||
385 | */ | ||
386 | static void remove_from_list_l(struct thread_entry **list, | ||
387 | struct thread_entry *thread) | ||
388 | { | ||
389 | struct thread_entry *prev, *next; | ||
390 | |||
391 | next = thread->l.next; | ||
392 | |||
393 | if (thread == next) | ||
394 | { | ||
395 | /* The only item */ | ||
396 | *list = NULL; | ||
397 | return; | ||
398 | } | ||
399 | |||
400 | if (thread == *list) | ||
401 | { | ||
402 | /* List becomes next item */ | ||
403 | *list = next; | ||
404 | } | ||
405 | |||
406 | prev = thread->l.prev; | ||
407 | |||
408 | /* Fix links to jump over the removed entry. */ | ||
409 | next->l.prev = prev; | ||
410 | prev->l.next = next; | ||
411 | } | ||
412 | |||
413 | /*--------------------------------------------------------------------------- | ||
414 | * Timeout list structure - circular reverse (to make "remove item" O(1)), | ||
415 | * NULL-terminated forward (to ease the far more common forward traversal): | ||
416 | * +------------------------------+ | ||
417 | * | | | ||
418 | * +--+---+<-+---+<-+---+<-+---+<-+ | ||
419 | * Head->| T | | T | | T | | T | | ||
420 | * +---+->+---+->+---+->+---+-X | ||
421 | *--------------------------------------------------------------------------- | ||
422 | */ | ||
423 | |||
424 | /*--------------------------------------------------------------------------- | ||
425 | * Add a thread from the core's timout list by linking the pointers in its | ||
426 | * tmo structure. | ||
427 | *--------------------------------------------------------------------------- | ||
428 | */ | ||
429 | static void add_to_list_tmo(struct thread_entry *thread) | ||
430 | { | ||
431 | struct thread_entry *tmo = cores[IF_COP_CORE(thread->core)].timeout; | ||
432 | THREAD_ASSERT(thread->tmo.prev == NULL, | ||
433 | "add_to_list_tmo->already listed", thread); | ||
434 | |||
435 | thread->tmo.next = NULL; | ||
436 | |||
437 | if (tmo == NULL) | ||
438 | { | ||
439 | /* Insert into unoccupied list */ | ||
440 | thread->tmo.prev = thread; | ||
441 | cores[IF_COP_CORE(thread->core)].timeout = thread; | ||
442 | return; | ||
443 | } | ||
444 | |||
445 | /* Insert Last */ | ||
446 | thread->tmo.prev = tmo->tmo.prev; | ||
447 | tmo->tmo.prev->tmo.next = thread; | ||
448 | tmo->tmo.prev = thread; | ||
449 | } | ||
450 | |||
451 | /*--------------------------------------------------------------------------- | ||
452 | * Remove a thread from the core's timout list by unlinking the pointers in | ||
453 | * its tmo structure. Sets thread->tmo.prev to NULL to indicate the timeout | ||
454 | * is cancelled. | ||
455 | *--------------------------------------------------------------------------- | ||
456 | */ | ||
457 | static void remove_from_list_tmo(struct thread_entry *thread) | ||
458 | { | ||
459 | struct thread_entry **list = &cores[IF_COP_CORE(thread->core)].timeout; | ||
460 | struct thread_entry *prev = thread->tmo.prev; | ||
461 | struct thread_entry *next = thread->tmo.next; | ||
462 | |||
463 | THREAD_ASSERT(prev != NULL, "remove_from_list_tmo->not listed", thread); | ||
464 | |||
465 | if (next != NULL) | ||
466 | next->tmo.prev = prev; | ||
467 | |||
468 | if (thread == *list) | ||
469 | { | ||
470 | /* List becomes next item and empty if next == NULL */ | ||
471 | *list = next; | ||
472 | /* Mark as unlisted */ | ||
473 | thread->tmo.prev = NULL; | ||
474 | } | ||
475 | else | ||
476 | { | ||
477 | if (next == NULL) | ||
478 | (*list)->tmo.prev = prev; | ||
479 | prev->tmo.next = next; | ||
480 | /* Mark as unlisted */ | ||
481 | thread->tmo.prev = NULL; | ||
482 | } | ||
483 | } | 293 | } |
484 | 294 | ||
485 | #ifdef HAVE_PRIORITY_SCHEDULING | 295 | #ifdef HAVE_PRIORITY_SCHEDULING |
486 | /*--------------------------------------------------------------------------- | ||
487 | * Priority distribution structure (one category for each possible priority): | ||
488 | * | ||
489 | * +----+----+----+ ... +-----+ | ||
490 | * hist: | F0 | F1 | F2 | | F31 | | ||
491 | * +----+----+----+ ... +-----+ | ||
492 | * mask: | b0 | b1 | b2 | | b31 | | ||
493 | * +----+----+----+ ... +-----+ | ||
494 | * | ||
495 | * F = count of threads at priority category n (frequency) | ||
496 | * b = bitmask of non-zero priority categories (occupancy) | ||
497 | * | ||
498 | * / if H[n] != 0 : 1 | ||
499 | * b[n] = | | ||
500 | * \ else : 0 | ||
501 | * | ||
502 | *--------------------------------------------------------------------------- | ||
503 | * Basic priority inheritance priotocol (PIP): | ||
504 | * | ||
505 | * Mn = mutex n, Tn = thread n | ||
506 | * | ||
507 | * A lower priority thread inherits the priority of the highest priority | ||
508 | * thread blocked waiting for it to complete an action (such as release a | ||
509 | * mutex or respond to a message via queue_send): | ||
510 | * | ||
511 | * 1) T2->M1->T1 | ||
512 | * | ||
513 | * T1 owns M1, T2 is waiting for M1 to realease M1. If T2 has a higher | ||
514 | * priority than T1 then T1 inherits the priority of T2. | ||
515 | * | ||
516 | * 2) T3 | ||
517 | * \/ | ||
518 | * T2->M1->T1 | ||
519 | * | ||
520 | * Situation is like 1) but T2 and T3 are both queued waiting for M1 and so | ||
521 | * T1 inherits the higher of T2 and T3. | ||
522 | * | ||
523 | * 3) T3->M2->T2->M1->T1 | ||
524 | * | ||
525 | * T1 owns M1, T2 owns M2. If T3 has a higher priority than both T1 and T2, | ||
526 | * then T1 inherits the priority of T3 through T2. | ||
527 | * | ||
528 | * Blocking chains can grow arbitrarily complex (though it's best that they | ||
529 | * not form at all very often :) and build-up from these units. | ||
530 | *--------------------------------------------------------------------------- | ||
531 | */ | ||
532 | 296 | ||
533 | /*--------------------------------------------------------------------------- | 297 | /*--------------------------------------------------------------------------- |
534 | * Increment frequency at category "priority" | 298 | * Increment frequency at category "priority" |
@@ -569,25 +333,86 @@ static inline void prio_move_entry( | |||
569 | if (++pd->hist[to] == 1) | 333 | if (++pd->hist[to] == 1) |
570 | priobit_set_bit(&pd->mask, to); | 334 | priobit_set_bit(&pd->mask, to); |
571 | } | 335 | } |
336 | |||
572 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 337 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
573 | 338 | ||
574 | /*--------------------------------------------------------------------------- | 339 | /*--------------------------------------------------------------------------- |
575 | * Move a thread back to a running state on its core. | 340 | * Common init for new thread basic info |
576 | *--------------------------------------------------------------------------- | 341 | *--------------------------------------------------------------------------- |
577 | */ | 342 | */ |
578 | static void core_schedule_wakeup(struct thread_entry *thread) | 343 | static void new_thread_base_init(struct thread_entry *thread, |
344 | void **stackp, size_t *stack_sizep, | ||
345 | const char *name IF_PRIO(, int priority) | ||
346 | IF_COP(, unsigned int core)) | ||
579 | { | 347 | { |
580 | const unsigned int core = IF_COP_CORE(thread->core); | 348 | ALIGN_BUFFER(*stackp, *stack_sizep, MIN_STACK_ALIGN); |
349 | thread->stack = *stackp; | ||
350 | thread->stack_size = *stack_sizep; | ||
581 | 351 | ||
582 | RTR_LOCK(core); | 352 | thread->name = name; |
353 | wait_queue_init(&thread->queue); | ||
354 | thread->wqp = NULL; | ||
355 | tmo_set_dequeued(thread); | ||
356 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
357 | thread->skip_count = 0; | ||
358 | thread->blocker = NULL; | ||
359 | thread->base_priority = priority; | ||
360 | thread->priority = priority; | ||
361 | memset(&thread->pdist, 0, sizeof(thread->pdist)); | ||
362 | prio_add_entry(&thread->pdist, priority); | ||
363 | #endif | ||
364 | #if NUM_CORES > 1 | ||
365 | thread->core = core; | ||
366 | #endif | ||
367 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | ||
368 | thread->cpu_boost = 0; | ||
369 | #endif | ||
370 | #ifdef HAVE_IO_PRIORITY | ||
371 | /* Default to high (foreground) priority */ | ||
372 | thread->io_priority = IO_PRIORITY_IMMEDIATE; | ||
373 | #endif | ||
374 | } | ||
583 | 375 | ||
376 | /*--------------------------------------------------------------------------- | ||
377 | * Move a thread onto the core's run queue and promote it | ||
378 | *--------------------------------------------------------------------------- | ||
379 | */ | ||
380 | static inline void core_rtr_add(struct core_entry *corep, | ||
381 | struct thread_entry *thread) | ||
382 | { | ||
383 | RTR_LOCK(corep); | ||
384 | rtr_queue_add(&corep->rtr, thread); | ||
385 | rtr_add_entry(corep, thread->priority); | ||
386 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
387 | thread->skip_count = thread->base_priority; | ||
388 | #endif | ||
584 | thread->state = STATE_RUNNING; | 389 | thread->state = STATE_RUNNING; |
390 | RTR_UNLOCK(corep); | ||
391 | } | ||
585 | 392 | ||
586 | add_to_list_l(&cores[core].running, thread); | 393 | /*--------------------------------------------------------------------------- |
587 | rtr_add_entry(core, thread->priority); | 394 | * Remove a thread from the core's run queue |
588 | 395 | *--------------------------------------------------------------------------- | |
589 | RTR_UNLOCK(core); | 396 | */ |
397 | static inline void core_rtr_remove(struct core_entry *corep, | ||
398 | struct thread_entry *thread) | ||
399 | { | ||
400 | RTR_LOCK(corep); | ||
401 | rtr_queue_remove(&corep->rtr, thread); | ||
402 | rtr_subtract_entry(corep, thread->priority); | ||
403 | /* Does not demote state */ | ||
404 | RTR_UNLOCK(corep); | ||
405 | } | ||
590 | 406 | ||
407 | /*--------------------------------------------------------------------------- | ||
408 | * Move a thread back to a running state on its core | ||
409 | *--------------------------------------------------------------------------- | ||
410 | */ | ||
411 | static NO_INLINE void core_schedule_wakeup(struct thread_entry *thread) | ||
412 | { | ||
413 | const unsigned int core = IF_COP_CORE(thread->core); | ||
414 | struct core_entry *corep = __core_id_entry(core); | ||
415 | core_rtr_add(corep, thread); | ||
591 | #if NUM_CORES > 1 | 416 | #if NUM_CORES > 1 |
592 | if (core != CURRENT_CORE) | 417 | if (core != CURRENT_CORE) |
593 | core_wake(core); | 418 | core_wake(core); |
@@ -596,17 +421,75 @@ static void core_schedule_wakeup(struct thread_entry *thread) | |||
596 | 421 | ||
597 | #ifdef HAVE_PRIORITY_SCHEDULING | 422 | #ifdef HAVE_PRIORITY_SCHEDULING |
598 | /*--------------------------------------------------------------------------- | 423 | /*--------------------------------------------------------------------------- |
424 | * Locks the thread registered as the owner of the block and makes sure it | ||
425 | * didn't change in the meantime | ||
426 | *--------------------------------------------------------------------------- | ||
427 | */ | ||
428 | #if NUM_CORES == 1 | ||
429 | static inline struct thread_entry * lock_blocker_thread(struct blocker *bl) | ||
430 | { | ||
431 | return bl->thread; | ||
432 | } | ||
433 | #else /* NUM_CORES > 1 */ | ||
434 | static struct thread_entry * lock_blocker_thread(struct blocker *bl) | ||
435 | { | ||
436 | /* The blocker thread may change during the process of trying to | ||
437 | capture it */ | ||
438 | while (1) | ||
439 | { | ||
440 | struct thread_entry *t = bl->thread; | ||
441 | |||
442 | /* TRY, or else deadlocks are possible */ | ||
443 | if (!t) | ||
444 | { | ||
445 | struct blocker_splay *blsplay = (struct blocker_splay *)bl; | ||
446 | if (corelock_try_lock(&blsplay->cl)) | ||
447 | { | ||
448 | if (!bl->thread) | ||
449 | return NULL; /* Still multi */ | ||
450 | |||
451 | corelock_unlock(&blsplay->cl); | ||
452 | } | ||
453 | } | ||
454 | else | ||
455 | { | ||
456 | if (TRY_LOCK_THREAD(t)) | ||
457 | { | ||
458 | if (bl->thread == t) | ||
459 | return t; | ||
460 | |||
461 | UNLOCK_THREAD(t); | ||
462 | } | ||
463 | } | ||
464 | } | ||
465 | } | ||
466 | #endif /* NUM_CORES */ | ||
467 | |||
468 | static inline void unlock_blocker_thread(struct blocker *bl) | ||
469 | { | ||
470 | #if NUM_CORES > 1 | ||
471 | struct thread_entry *blt = bl->thread; | ||
472 | if (blt) | ||
473 | UNLOCK_THREAD(blt); | ||
474 | else | ||
475 | corelock_unlock(&((struct blocker_splay *)bl)->cl); | ||
476 | #endif /* NUM_CORES > 1*/ | ||
477 | (void)bl; | ||
478 | } | ||
479 | |||
480 | /*--------------------------------------------------------------------------- | ||
599 | * Change the priority and rtr entry for a running thread | 481 | * Change the priority and rtr entry for a running thread |
600 | *--------------------------------------------------------------------------- | 482 | *--------------------------------------------------------------------------- |
601 | */ | 483 | */ |
602 | static inline void set_running_thread_priority( | 484 | static inline void set_rtr_thread_priority( |
603 | struct thread_entry *thread, int priority) | 485 | struct thread_entry *thread, int priority) |
604 | { | 486 | { |
605 | const unsigned int core = IF_COP_CORE(thread->core); | 487 | const unsigned int core = IF_COP_CORE(thread->core); |
606 | RTR_LOCK(core); | 488 | struct core_entry *corep = __core_id_entry(core); |
607 | rtr_move_entry(core, thread->priority, priority); | 489 | RTR_LOCK(corep); |
490 | rtr_move_entry(corep, thread->priority, priority); | ||
608 | thread->priority = priority; | 491 | thread->priority = priority; |
609 | RTR_UNLOCK(core); | 492 | RTR_UNLOCK(corep); |
610 | } | 493 | } |
611 | 494 | ||
612 | /*--------------------------------------------------------------------------- | 495 | /*--------------------------------------------------------------------------- |
@@ -619,30 +502,21 @@ static inline void set_running_thread_priority( | |||
619 | * penalty under high contention. | 502 | * penalty under high contention. |
620 | *--------------------------------------------------------------------------- | 503 | *--------------------------------------------------------------------------- |
621 | */ | 504 | */ |
622 | static int find_highest_priority_in_list_l( | 505 | static int wait_queue_find_priority(struct __wait_queue *wqp) |
623 | struct thread_entry * const thread) | ||
624 | { | 506 | { |
625 | if (LIKELY(thread != NULL)) | 507 | int highest_priority = PRIORITY_IDLE; |
626 | { | 508 | struct thread_entry *thread = WQ_THREAD_FIRST(wqp); |
627 | /* Go though list until the ending up at the initial thread */ | ||
628 | int highest_priority = thread->priority; | ||
629 | struct thread_entry *curr = thread; | ||
630 | 509 | ||
631 | do | 510 | while (thread != NULL) |
632 | { | 511 | { |
633 | int priority = curr->priority; | 512 | int priority = thread->priority; |
634 | 513 | if (priority < highest_priority) | |
635 | if (priority < highest_priority) | 514 | highest_priority = priority; |
636 | highest_priority = priority; | ||
637 | |||
638 | curr = curr->l.next; | ||
639 | } | ||
640 | while (curr != thread); | ||
641 | 515 | ||
642 | return highest_priority; | 516 | thread = WQ_THREAD_NEXT(thread); |
643 | } | 517 | } |
644 | 518 | ||
645 | return PRIORITY_IDLE; | 519 | return highest_priority; |
646 | } | 520 | } |
647 | 521 | ||
648 | /*--------------------------------------------------------------------------- | 522 | /*--------------------------------------------------------------------------- |
@@ -666,7 +540,7 @@ static void inherit_priority( | |||
666 | { | 540 | { |
667 | /* Multiple owners */ | 541 | /* Multiple owners */ |
668 | struct blocker_splay *blsplay = (struct blocker_splay *)bl; | 542 | struct blocker_splay *blsplay = (struct blocker_splay *)bl; |
669 | 543 | ||
670 | /* Recurse down the all the branches of this; it's the only way. | 544 | /* Recurse down the all the branches of this; it's the only way. |
671 | We might meet the same queue several times if more than one of | 545 | We might meet the same queue several times if more than one of |
672 | these threads is waiting the same queue. That isn't a problem | 546 | these threads is waiting the same queue. That isn't a problem |
@@ -674,7 +548,7 @@ static void inherit_priority( | |||
674 | FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum) | 548 | FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum) |
675 | { | 549 | { |
676 | bl->priority = oldblpr; /* To see the change each time */ | 550 | bl->priority = oldblpr; /* To see the change each time */ |
677 | blt = &threads[slotnum]; | 551 | blt = __thread_slot_entry(slotnum); |
678 | LOCK_THREAD(blt); | 552 | LOCK_THREAD(blt); |
679 | inherit_priority(blocker0, bl, blt, newblpr); | 553 | inherit_priority(blocker0, bl, blt, newblpr); |
680 | } | 554 | } |
@@ -699,7 +573,7 @@ static void inherit_priority( | |||
699 | 573 | ||
700 | if (blt->state == STATE_RUNNING) | 574 | if (blt->state == STATE_RUNNING) |
701 | { | 575 | { |
702 | set_running_thread_priority(blt, newpr); | 576 | set_rtr_thread_priority(blt, newpr); |
703 | break; /* Running: last in chain */ | 577 | break; /* Running: last in chain */ |
704 | } | 578 | } |
705 | 579 | ||
@@ -714,7 +588,7 @@ static void inherit_priority( | |||
714 | break; /* Full circle - deadlock! */ | 588 | break; /* Full circle - deadlock! */ |
715 | 589 | ||
716 | /* Blocker becomes current thread and the process repeats */ | 590 | /* Blocker becomes current thread and the process repeats */ |
717 | struct thread_entry **bqp = blt->bqp; | 591 | struct __wait_queue *wqp = wait_queue_ptr(blt); |
718 | struct thread_entry *t = blt; | 592 | struct thread_entry *t = blt; |
719 | blt = lock_blocker_thread(bl); | 593 | blt = lock_blocker_thread(bl); |
720 | 594 | ||
@@ -725,7 +599,7 @@ static void inherit_priority( | |||
725 | if (newpr <= oldblpr) | 599 | if (newpr <= oldblpr) |
726 | newblpr = newpr; | 600 | newblpr = newpr; |
727 | else if (oldpr <= oldblpr) | 601 | else if (oldpr <= oldblpr) |
728 | newblpr = find_highest_priority_in_list_l(*bqp); | 602 | newblpr = wait_queue_find_priority(wqp); |
729 | 603 | ||
730 | if (newblpr == oldblpr) | 604 | if (newblpr == oldblpr) |
731 | break; /* Queue priority not changing */ | 605 | break; /* Queue priority not changing */ |
@@ -735,22 +609,46 @@ static void inherit_priority( | |||
735 | } | 609 | } |
736 | 610 | ||
737 | /*--------------------------------------------------------------------------- | 611 | /*--------------------------------------------------------------------------- |
738 | * Quick-disinherit of priority elevation. 'thread' must be a running thread. | 612 | * Quick-inherit of priority elevation. 'thread' must be not runnable |
739 | *--------------------------------------------------------------------------- | 613 | *--------------------------------------------------------------------------- |
740 | */ | 614 | */ |
741 | static void priority_disinherit_internal(struct thread_entry *thread, | 615 | static void priority_inherit_internal_inner(struct thread_entry *thread, |
742 | int blpr) | 616 | int blpr) |
617 | { | ||
618 | if (prio_add_entry(&thread->pdist, blpr) == 1 && blpr < thread->priority) | ||
619 | thread->priority = blpr; | ||
620 | } | ||
621 | |||
622 | static inline void priority_inherit_internal(struct thread_entry *thread, | ||
623 | int blpr) | ||
743 | { | 624 | { |
744 | if (blpr < PRIORITY_IDLE && | 625 | if (blpr < PRIORITY_IDLE) |
745 | prio_subtract_entry(&thread->pdist, blpr) == 0 && | 626 | priority_inherit_internal_inner(thread, blpr); |
627 | } | ||
628 | |||
629 | /*--------------------------------------------------------------------------- | ||
630 | * Quick-disinherit of priority elevation. 'thread' must current | ||
631 | *--------------------------------------------------------------------------- | ||
632 | */ | ||
633 | static void priority_disinherit_internal_inner(struct thread_entry *thread, | ||
634 | int blpr) | ||
635 | { | ||
636 | if (prio_subtract_entry(&thread->pdist, blpr) == 0 && | ||
746 | blpr <= thread->priority) | 637 | blpr <= thread->priority) |
747 | { | 638 | { |
748 | int priority = priobit_ffs(&thread->pdist.mask); | 639 | int priority = priobit_ffs(&thread->pdist.mask); |
749 | if (priority != thread->priority) | 640 | if (priority != thread->priority) |
750 | set_running_thread_priority(thread, priority); | 641 | set_rtr_thread_priority(thread, priority); |
751 | } | 642 | } |
752 | } | 643 | } |
753 | 644 | ||
645 | static inline void priority_disinherit_internal(struct thread_entry *thread, | ||
646 | int blpr) | ||
647 | { | ||
648 | if (blpr < PRIORITY_IDLE) | ||
649 | priority_disinherit_internal_inner(thread, blpr); | ||
650 | } | ||
651 | |||
754 | void priority_disinherit(struct thread_entry *thread, struct blocker *bl) | 652 | void priority_disinherit(struct thread_entry *thread, struct blocker *bl) |
755 | { | 653 | { |
756 | LOCK_THREAD(thread); | 654 | LOCK_THREAD(thread); |
@@ -767,30 +665,32 @@ static void wakeup_thread_queue_multi_transfer(struct thread_entry *thread) | |||
767 | { | 665 | { |
768 | /* All threads will have the same blocker and queue; only we are changing | 666 | /* All threads will have the same blocker and queue; only we are changing |
769 | it now */ | 667 | it now */ |
770 | struct thread_entry **bqp = thread->bqp; | 668 | struct __wait_queue *wqp = wait_queue_ptr(thread); |
771 | struct blocker_splay *blsplay = (struct blocker_splay *)thread->blocker; | 669 | struct blocker *bl = thread->blocker; |
772 | struct thread_entry *blt = blsplay->blocker.thread; | 670 | struct blocker_splay *blsplay = (struct blocker_splay *)bl; |
671 | struct thread_entry *blt = bl->thread; | ||
773 | 672 | ||
774 | /* The first thread is already locked and is assumed tagged "multi" */ | 673 | /* The first thread is already locked and is assumed tagged "multi" */ |
775 | int count = 1; | 674 | int count = 1; |
776 | struct thread_entry *temp_queue = NULL; | ||
777 | 675 | ||
778 | /* 'thread' is locked on entry */ | 676 | /* Multiple versions of the wait queue may be seen if doing more than |
677 | one thread; queue removal isn't destructive to the pointers of the node | ||
678 | being removed; this may lead to the blocker priority being wrong for a | ||
679 | time but it gets fixed up below after getting exclusive access to the | ||
680 | queue */ | ||
779 | while (1) | 681 | while (1) |
780 | { | 682 | { |
781 | LOCK_THREAD(blt); | ||
782 | |||
783 | remove_from_list_l(bqp, thread); | ||
784 | thread->blocker = NULL; | 683 | thread->blocker = NULL; |
684 | wait_queue_remove(thread); | ||
785 | 685 | ||
786 | struct thread_entry *tnext = *bqp; | 686 | unsigned int slotnum = THREAD_ID_SLOT(thread->id); |
687 | threadbit_set_bit(&blsplay->mask, slotnum); | ||
688 | |||
689 | struct thread_entry *tnext = WQ_THREAD_NEXT(thread); | ||
787 | if (tnext == NULL || tnext->retval == 0) | 690 | if (tnext == NULL || tnext->retval == 0) |
788 | break; | 691 | break; |
789 | 692 | ||
790 | add_to_list_l(&temp_queue, thread); | ||
791 | |||
792 | UNLOCK_THREAD(thread); | 693 | UNLOCK_THREAD(thread); |
793 | UNLOCK_THREAD(blt); | ||
794 | 694 | ||
795 | count++; | 695 | count++; |
796 | thread = tnext; | 696 | thread = tnext; |
@@ -798,65 +698,51 @@ static void wakeup_thread_queue_multi_transfer(struct thread_entry *thread) | |||
798 | LOCK_THREAD(thread); | 698 | LOCK_THREAD(thread); |
799 | } | 699 | } |
800 | 700 | ||
801 | int blpr = blsplay->blocker.priority; | ||
802 | priority_disinherit_internal(blt, blpr); | ||
803 | |||
804 | /* Locking order reverses here since the threads are no longer on the | 701 | /* Locking order reverses here since the threads are no longer on the |
805 | queue side */ | 702 | queued side */ |
806 | if (count > 1) | 703 | if (count > 1) |
807 | { | ||
808 | add_to_list_l(&temp_queue, thread); | ||
809 | UNLOCK_THREAD(thread); | ||
810 | corelock_lock(&blsplay->cl); | 704 | corelock_lock(&blsplay->cl); |
811 | 705 | ||
812 | blpr = find_highest_priority_in_list_l(*bqp); | 706 | LOCK_THREAD(blt); |
707 | |||
708 | int blpr = bl->priority; | ||
709 | priority_disinherit_internal(blt, blpr); | ||
710 | |||
711 | if (count > 1) | ||
712 | { | ||
813 | blsplay->blocker.thread = NULL; | 713 | blsplay->blocker.thread = NULL; |
814 | 714 | ||
815 | thread = temp_queue; | 715 | blpr = wait_queue_find_priority(wqp); |
816 | LOCK_THREAD(thread); | 716 | |
717 | FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum) | ||
718 | { | ||
719 | UNLOCK_THREAD(thread); | ||
720 | thread = __thread_slot_entry(slotnum); | ||
721 | LOCK_THREAD(thread); | ||
722 | priority_inherit_internal(thread, blpr); | ||
723 | core_schedule_wakeup(thread); | ||
724 | } | ||
817 | } | 725 | } |
818 | else | 726 | else |
819 | { | 727 | { |
820 | /* Becomes a simple, direct transfer */ | 728 | /* Becomes a simple, direct transfer */ |
821 | if (thread->priority <= blpr) | ||
822 | blpr = find_highest_priority_in_list_l(*bqp); | ||
823 | blsplay->blocker.thread = thread; | 729 | blsplay->blocker.thread = thread; |
824 | } | ||
825 | |||
826 | blsplay->blocker.priority = blpr; | ||
827 | 730 | ||
828 | while (1) | 731 | if (thread->priority <= blpr) |
829 | { | 732 | blpr = wait_queue_find_priority(wqp); |
830 | unsigned int slotnum = THREAD_ID_SLOT(thread->id); | ||
831 | threadbit_set_bit(&blsplay->mask, slotnum); | ||
832 | |||
833 | if (blpr < PRIORITY_IDLE) | ||
834 | { | ||
835 | prio_add_entry(&thread->pdist, blpr); | ||
836 | if (blpr < thread->priority) | ||
837 | thread->priority = blpr; | ||
838 | } | ||
839 | |||
840 | if (count > 1) | ||
841 | remove_from_list_l(&temp_queue, thread); | ||
842 | 733 | ||
734 | priority_inherit_internal(thread, blpr); | ||
843 | core_schedule_wakeup(thread); | 735 | core_schedule_wakeup(thread); |
736 | } | ||
844 | 737 | ||
845 | UNLOCK_THREAD(thread); | 738 | UNLOCK_THREAD(thread); |
846 | |||
847 | thread = temp_queue; | ||
848 | if (thread == NULL) | ||
849 | break; | ||
850 | 739 | ||
851 | LOCK_THREAD(thread); | 740 | bl->priority = blpr; |
852 | } | ||
853 | 741 | ||
854 | UNLOCK_THREAD(blt); | 742 | UNLOCK_THREAD(blt); |
855 | 743 | ||
856 | if (count > 1) | 744 | if (count > 1) |
857 | { | ||
858 | corelock_unlock(&blsplay->cl); | 745 | corelock_unlock(&blsplay->cl); |
859 | } | ||
860 | 746 | ||
861 | blt->retval = count; | 747 | blt->retval = count; |
862 | } | 748 | } |
@@ -876,29 +762,20 @@ static void wakeup_thread_transfer(struct thread_entry *thread) | |||
876 | struct blocker *bl = thread->blocker; | 762 | struct blocker *bl = thread->blocker; |
877 | struct thread_entry *blt = bl->thread; | 763 | struct thread_entry *blt = bl->thread; |
878 | 764 | ||
879 | THREAD_ASSERT(cores[CURRENT_CORE].running == blt, | 765 | THREAD_ASSERT(__running_self_entry() == blt, |
880 | "UPPT->wrong thread", cores[CURRENT_CORE].running); | 766 | "UPPT->wrong thread", __running_self_entry()); |
881 | 767 | ||
882 | LOCK_THREAD(blt); | 768 | LOCK_THREAD(blt); |
883 | 769 | ||
884 | struct thread_entry **bqp = thread->bqp; | ||
885 | remove_from_list_l(bqp, thread); | ||
886 | thread->blocker = NULL; | 770 | thread->blocker = NULL; |
771 | struct __wait_queue *wqp = wait_queue_remove(thread); | ||
887 | 772 | ||
888 | int blpr = bl->priority; | 773 | int blpr = bl->priority; |
889 | 774 | ||
890 | /* Remove the object's boost from the owning thread */ | 775 | /* Remove the object's boost from the owning thread */ |
891 | if (prio_subtract_entry(&blt->pdist, blpr) == 0 && blpr <= blt->priority) | 776 | priority_disinherit_internal_inner(blt, blpr); |
892 | { | ||
893 | /* No more threads at this priority are waiting and the old level is | ||
894 | * at least the thread level */ | ||
895 | int priority = priobit_ffs(&blt->pdist.mask); | ||
896 | if (priority != blt->priority) | ||
897 | set_running_thread_priority(blt, priority); | ||
898 | } | ||
899 | |||
900 | struct thread_entry *tnext = *bqp; | ||
901 | 777 | ||
778 | struct thread_entry *tnext = WQ_THREAD_FIRST(wqp); | ||
902 | if (LIKELY(tnext == NULL)) | 779 | if (LIKELY(tnext == NULL)) |
903 | { | 780 | { |
904 | /* Expected shortcut - no more waiters */ | 781 | /* Expected shortcut - no more waiters */ |
@@ -906,20 +783,20 @@ static void wakeup_thread_transfer(struct thread_entry *thread) | |||
906 | } | 783 | } |
907 | else | 784 | else |
908 | { | 785 | { |
909 | /* If lowering, we need to scan threads remaining in queue */ | 786 | /* If thread is at the blocker priority, its removal may drop it */ |
910 | int priority = thread->priority; | 787 | if (thread->priority <= blpr) |
911 | if (priority <= blpr) | 788 | blpr = wait_queue_find_priority(wqp); |
912 | blpr = find_highest_priority_in_list_l(tnext); | ||
913 | 789 | ||
914 | if (prio_add_entry(&thread->pdist, blpr) == 1 && blpr < priority) | 790 | priority_inherit_internal_inner(thread, blpr); |
915 | thread->priority = blpr; /* Raise new owner */ | ||
916 | } | 791 | } |
917 | 792 | ||
793 | bl->thread = thread; /* This thread pwns */ | ||
794 | |||
918 | core_schedule_wakeup(thread); | 795 | core_schedule_wakeup(thread); |
919 | UNLOCK_THREAD(thread); | 796 | UNLOCK_THREAD(thread); |
920 | 797 | ||
921 | bl->thread = thread; /* This thread pwns */ | 798 | bl->priority = blpr; /* Save highest blocked priority */ |
922 | bl->priority = blpr; /* Save highest blocked priority */ | 799 | |
923 | UNLOCK_THREAD(blt); | 800 | UNLOCK_THREAD(blt); |
924 | } | 801 | } |
925 | 802 | ||
@@ -933,9 +810,9 @@ static void wakeup_thread_release(struct thread_entry *thread) | |||
933 | { | 810 | { |
934 | struct blocker *bl = thread->blocker; | 811 | struct blocker *bl = thread->blocker; |
935 | struct thread_entry *blt = lock_blocker_thread(bl); | 812 | struct thread_entry *blt = lock_blocker_thread(bl); |
936 | struct thread_entry **bqp = thread->bqp; | 813 | |
937 | remove_from_list_l(bqp, thread); | ||
938 | thread->blocker = NULL; | 814 | thread->blocker = NULL; |
815 | struct __wait_queue *wqp = wait_queue_remove(thread); | ||
939 | 816 | ||
940 | /* Off to see the wizard... */ | 817 | /* Off to see the wizard... */ |
941 | core_schedule_wakeup(thread); | 818 | core_schedule_wakeup(thread); |
@@ -950,7 +827,7 @@ static void wakeup_thread_release(struct thread_entry *thread) | |||
950 | 827 | ||
951 | UNLOCK_THREAD(thread); | 828 | UNLOCK_THREAD(thread); |
952 | 829 | ||
953 | int newblpr = find_highest_priority_in_list_l(*bqp); | 830 | int newblpr = wait_queue_find_priority(wqp); |
954 | if (newblpr == bl->priority) | 831 | if (newblpr == bl->priority) |
955 | { | 832 | { |
956 | /* Blocker priority won't change */ | 833 | /* Blocker priority won't change */ |
@@ -963,25 +840,17 @@ static void wakeup_thread_release(struct thread_entry *thread) | |||
963 | 840 | ||
964 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 841 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
965 | 842 | ||
843 | |||
966 | /*--------------------------------------------------------------------------- | 844 | /*--------------------------------------------------------------------------- |
967 | * Explicitly wakeup a thread on a blocking queue. Only effects threads of | 845 | * Explicitly wakeup a thread on a blocking queue. Only effects threads of |
968 | * STATE_BLOCKED and STATE_BLOCKED_W_TMO. | 846 | * STATE_BLOCKED and STATE_BLOCKED_W_TMO. |
969 | * | 847 | * |
970 | * This code should be considered a critical section by the caller meaning | 848 | * INTERNAL: Intended for use by kernel and not programs. |
971 | * that the object's corelock should be held. | ||
972 | * | ||
973 | * INTERNAL: Intended for use by kernel objects and not for programs. | ||
974 | *--------------------------------------------------------------------------- | 849 | *--------------------------------------------------------------------------- |
975 | */ | 850 | */ |
976 | unsigned int wakeup_thread_(struct thread_entry **list | 851 | unsigned int wakeup_thread_(struct thread_entry *thread |
977 | IF_PRIO(, enum wakeup_thread_protocol proto)) | 852 | IF_PRIO(, enum wakeup_thread_protocol proto)) |
978 | { | 853 | { |
979 | struct thread_entry *thread = *list; | ||
980 | |||
981 | /* Check if there is a blocked thread at all. */ | ||
982 | if (*list == NULL) | ||
983 | return THREAD_NONE; | ||
984 | |||
985 | LOCK_THREAD(thread); | 854 | LOCK_THREAD(thread); |
986 | 855 | ||
987 | /* Determine thread's current state. */ | 856 | /* Determine thread's current state. */ |
@@ -1008,24 +877,21 @@ unsigned int wakeup_thread_(struct thread_entry **list | |||
1008 | else | 877 | else |
1009 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 878 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
1010 | { | 879 | { |
1011 | /* No PIP - just boost the thread by aging */ | 880 | wait_queue_remove(thread); |
1012 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
1013 | thread->skip_count = thread->priority; | ||
1014 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
1015 | remove_from_list_l(list, thread); | ||
1016 | core_schedule_wakeup(thread); | 881 | core_schedule_wakeup(thread); |
1017 | UNLOCK_THREAD(thread); | 882 | UNLOCK_THREAD(thread); |
1018 | } | 883 | } |
1019 | 884 | ||
1020 | return should_switch_tasks(); | 885 | return should_switch_tasks(thread); |
1021 | 886 | ||
1022 | /* Nothing to do. State is not blocked. */ | ||
1023 | default: | ||
1024 | #if THREAD_EXTRA_CHECKS | ||
1025 | THREAD_PANICF("wakeup_thread->block invalid", thread); | ||
1026 | case STATE_RUNNING: | 887 | case STATE_RUNNING: |
1027 | case STATE_KILLED: | 888 | if (wait_queue_try_remove(thread)) |
1028 | #endif | 889 | { |
890 | UNLOCK_THREAD(thread); | ||
891 | return THREAD_OK; /* timed out */ | ||
892 | } | ||
893 | |||
894 | default: | ||
1029 | UNLOCK_THREAD(thread); | 895 | UNLOCK_THREAD(thread); |
1030 | return THREAD_NONE; | 896 | return THREAD_NONE; |
1031 | } | 897 | } |
@@ -1037,201 +903,102 @@ unsigned int wakeup_thread_(struct thread_entry **list | |||
1037 | * tick when the next check will occur. | 903 | * tick when the next check will occur. |
1038 | *--------------------------------------------------------------------------- | 904 | *--------------------------------------------------------------------------- |
1039 | */ | 905 | */ |
1040 | void check_tmo_threads(void) | 906 | static NO_INLINE void check_tmo_expired_inner(struct core_entry *corep) |
1041 | { | 907 | { |
1042 | const unsigned int core = CURRENT_CORE; | ||
1043 | const long tick = current_tick; /* snapshot the current tick */ | 908 | const long tick = current_tick; /* snapshot the current tick */ |
1044 | long next_tmo_check = tick + 60*HZ; /* minimum duration: once/minute */ | 909 | long next_tmo_check = tick + 60*HZ; /* minimum duration: once/minute */ |
1045 | struct thread_entry *next = cores[core].timeout; | 910 | struct thread_entry *prev = NULL; |
911 | struct thread_entry *thread = TMO_THREAD_FIRST(&corep->tmo); | ||
1046 | 912 | ||
1047 | /* If there are no processes waiting for a timeout, just keep the check | 913 | /* If there are no processes waiting for a timeout, just keep the check |
1048 | tick from falling into the past. */ | 914 | tick from falling into the past. */ |
1049 | 915 | ||
1050 | /* Break the loop once we have walked through the list of all | 916 | /* Break the loop once we have walked through the list of all |
1051 | * sleeping processes or have removed them all. */ | 917 | * sleeping processes or have removed them all. */ |
1052 | while (next != NULL) | 918 | while (thread != NULL) |
1053 | { | 919 | { |
1054 | /* Check sleeping threads. Allow interrupts between checks. */ | 920 | /* Check sleeping threads. Allow interrupts between checks. */ |
1055 | enable_irq(); | 921 | enable_irq(); |
1056 | 922 | ||
1057 | struct thread_entry *curr = next; | 923 | struct thread_entry *next = TMO_THREAD_NEXT(thread); |
1058 | |||
1059 | next = curr->tmo.next; | ||
1060 | 924 | ||
1061 | /* Lock thread slot against explicit wakeup */ | 925 | /* Lock thread slot against explicit wakeup */ |
1062 | disable_irq(); | 926 | disable_irq(); |
1063 | LOCK_THREAD(curr); | 927 | LOCK_THREAD(thread); |
1064 | 928 | ||
1065 | unsigned state = curr->state; | 929 | unsigned int state = thread->state; |
1066 | 930 | ||
1067 | if (state < TIMEOUT_STATE_FIRST) | 931 | if (LIKELY(state >= TIMEOUT_STATE_FIRST && |
1068 | { | 932 | TIME_BEFORE(tick, thread->tmo_tick))) |
1069 | /* Cleanup threads no longer on a timeout but still on the | ||
1070 | * list. */ | ||
1071 | remove_from_list_tmo(curr); | ||
1072 | } | ||
1073 | else if (LIKELY(TIME_BEFORE(tick, curr->tmo_tick))) | ||
1074 | { | 933 | { |
1075 | /* Timeout still pending - this will be the usual case */ | 934 | /* Timeout still pending - this will be the usual case */ |
1076 | if (TIME_BEFORE(curr->tmo_tick, next_tmo_check)) | 935 | if (TIME_BEFORE(thread->tmo_tick, next_tmo_check)) |
1077 | { | 936 | { |
1078 | /* Earliest timeout found so far - move the next check up | 937 | /* Move the next check up to its time */ |
1079 | to its time */ | 938 | next_tmo_check = thread->tmo_tick; |
1080 | next_tmo_check = curr->tmo_tick; | ||
1081 | } | 939 | } |
940 | |||
941 | prev = thread; | ||
1082 | } | 942 | } |
1083 | else | 943 | else |
1084 | { | 944 | { |
1085 | /* Sleep timeout has been reached so bring the thread back to | 945 | /* TODO: there are no priority-inheriting timeout blocks |
1086 | * life again. */ | 946 | right now but the procedure should be established */ |
1087 | if (state == STATE_BLOCKED_W_TMO) | ||
1088 | { | ||
1089 | #ifdef HAVE_CORELOCK_OBJECT | ||
1090 | /* Lock the waiting thread's kernel object */ | ||
1091 | struct corelock *ocl = curr->obj_cl; | ||
1092 | |||
1093 | if (UNLIKELY(corelock_try_lock(ocl) == 0)) | ||
1094 | { | ||
1095 | /* Need to retry in the correct order though the need is | ||
1096 | * unlikely */ | ||
1097 | UNLOCK_THREAD(curr); | ||
1098 | corelock_lock(ocl); | ||
1099 | LOCK_THREAD(curr); | ||
1100 | |||
1101 | if (UNLIKELY(curr->state != STATE_BLOCKED_W_TMO)) | ||
1102 | { | ||
1103 | /* Thread was woken or removed explicitely while slot | ||
1104 | * was unlocked */ | ||
1105 | corelock_unlock(ocl); | ||
1106 | remove_from_list_tmo(curr); | ||
1107 | UNLOCK_THREAD(curr); | ||
1108 | continue; | ||
1109 | } | ||
1110 | } | ||
1111 | #endif /* NUM_CORES */ | ||
1112 | |||
1113 | #ifdef HAVE_WAKEUP_EXT_CB | ||
1114 | if (curr->wakeup_ext_cb != NULL) | ||
1115 | curr->wakeup_ext_cb(curr); | ||
1116 | #endif | ||
1117 | |||
1118 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
1119 | if (curr->blocker != NULL) | ||
1120 | wakeup_thread_release(curr); | ||
1121 | else | ||
1122 | #endif | ||
1123 | remove_from_list_l(curr->bqp, curr); | ||
1124 | |||
1125 | corelock_unlock(ocl); | ||
1126 | } | ||
1127 | /* else state == STATE_SLEEPING */ | ||
1128 | 947 | ||
1129 | remove_from_list_tmo(curr); | 948 | /* Sleep timeout has been reached / garbage collect stale list |
949 | items */ | ||
950 | tmo_queue_expire(&corep->tmo, prev, thread); | ||
1130 | 951 | ||
1131 | RTR_LOCK(core); | 952 | if (state >= TIMEOUT_STATE_FIRST) |
953 | core_rtr_add(corep, thread); | ||
1132 | 954 | ||
1133 | curr->state = STATE_RUNNING; | 955 | /* removed this one - prev doesn't change */ |
1134 | |||
1135 | add_to_list_l(&cores[core].running, curr); | ||
1136 | rtr_add_entry(core, curr->priority); | ||
1137 | |||
1138 | RTR_UNLOCK(core); | ||
1139 | } | 956 | } |
1140 | 957 | ||
1141 | UNLOCK_THREAD(curr); | 958 | UNLOCK_THREAD(thread); |
1142 | } | ||
1143 | |||
1144 | cores[core].next_tmo_check = next_tmo_check; | ||
1145 | } | ||
1146 | |||
1147 | /*--------------------------------------------------------------------------- | ||
1148 | * Performs operations that must be done before blocking a thread but after | ||
1149 | * the state is saved. | ||
1150 | *--------------------------------------------------------------------------- | ||
1151 | */ | ||
1152 | #if NUM_CORES > 1 | ||
1153 | static inline void run_blocking_ops( | ||
1154 | unsigned int core, struct thread_entry *thread) | ||
1155 | { | ||
1156 | struct thread_blk_ops *ops = &cores[core].blk_ops; | ||
1157 | const unsigned flags = ops->flags; | ||
1158 | |||
1159 | if (LIKELY(flags == TBOP_CLEAR)) | ||
1160 | return; | ||
1161 | 959 | ||
1162 | switch (flags) | 960 | thread = next; |
1163 | { | ||
1164 | case TBOP_SWITCH_CORE: | ||
1165 | core_switch_blk_op(core, thread); | ||
1166 | /* Fall-through */ | ||
1167 | case TBOP_UNLOCK_CORELOCK: | ||
1168 | corelock_unlock(ops->cl_p); | ||
1169 | break; | ||
1170 | } | 961 | } |
1171 | 962 | ||
1172 | ops->flags = TBOP_CLEAR; | 963 | corep->next_tmo_check = next_tmo_check; |
1173 | } | 964 | } |
1174 | #endif /* NUM_CORES > 1 */ | ||
1175 | 965 | ||
1176 | #ifdef RB_PROFILE | 966 | static FORCE_INLINE void check_tmo_expired(struct core_entry *corep) |
1177 | void profile_thread(void) | ||
1178 | { | 967 | { |
1179 | profstart(cores[CURRENT_CORE].running - threads); | 968 | if (!TIME_BEFORE(current_tick, corep->next_tmo_check)) |
969 | check_tmo_expired_inner(corep); | ||
1180 | } | 970 | } |
1181 | #endif | ||
1182 | 971 | ||
1183 | /*--------------------------------------------------------------------------- | 972 | /*--------------------------------------------------------------------------- |
1184 | * Prepares a thread to block on an object's list and/or for a specified | 973 | * Prepares a the current thread to sleep forever or for the given duration. |
1185 | * duration - expects object and slot to be appropriately locked if needed | ||
1186 | * and interrupts to be masked. | ||
1187 | *--------------------------------------------------------------------------- | 974 | *--------------------------------------------------------------------------- |
1188 | */ | 975 | */ |
1189 | static inline void block_thread_on_l(struct thread_entry *thread, | 976 | static FORCE_INLINE void prepare_block(struct thread_entry *current, |
1190 | unsigned state) | 977 | unsigned int state, int timeout) |
1191 | { | 978 | { |
1192 | /* If inlined, unreachable branches will be pruned with no size penalty | 979 | const unsigned int core = IF_COP_CORE(current->core); |
1193 | because state is passed as a constant parameter. */ | ||
1194 | const unsigned int core = IF_COP_CORE(thread->core); | ||
1195 | 980 | ||
1196 | /* Remove the thread from the list of running threads. */ | 981 | /* Remove the thread from the list of running threads. */ |
1197 | RTR_LOCK(core); | 982 | struct core_entry *corep = __core_id_entry(core); |
1198 | remove_from_list_l(&cores[core].running, thread); | 983 | core_rtr_remove(corep, current); |
1199 | rtr_subtract_entry(core, thread->priority); | ||
1200 | RTR_UNLOCK(core); | ||
1201 | 984 | ||
1202 | /* Add a timeout to the block if not infinite */ | 985 | if (timeout >= 0) |
1203 | switch (state) | ||
1204 | { | 986 | { |
1205 | case STATE_BLOCKED: | 987 | /* Sleep may expire. */ |
1206 | case STATE_BLOCKED_W_TMO: | 988 | long tmo_tick = current_tick + timeout; |
1207 | /* Put the thread into a new list of inactive threads. */ | 989 | current->tmo_tick = tmo_tick; |
1208 | add_to_list_l(thread->bqp, thread); | ||
1209 | 990 | ||
1210 | if (state == STATE_BLOCKED) | 991 | if (TIME_BEFORE(tmo_tick, corep->next_tmo_check)) |
1211 | break; | 992 | corep->next_tmo_check = tmo_tick; |
1212 | 993 | ||
1213 | /* Fall-through */ | 994 | tmo_queue_register(&corep->tmo, current); |
1214 | case STATE_SLEEPING: | ||
1215 | /* If this thread times out sooner than any other thread, update | ||
1216 | next_tmo_check to its timeout */ | ||
1217 | if (TIME_BEFORE(thread->tmo_tick, cores[core].next_tmo_check)) | ||
1218 | { | ||
1219 | cores[core].next_tmo_check = thread->tmo_tick; | ||
1220 | } | ||
1221 | 995 | ||
1222 | if (thread->tmo.prev == NULL) | 996 | if (state == STATE_BLOCKED) |
1223 | { | 997 | state = STATE_BLOCKED_W_TMO; |
1224 | add_to_list_tmo(thread); | ||
1225 | } | ||
1226 | /* else thread was never removed from list - just keep it there */ | ||
1227 | break; | ||
1228 | } | 998 | } |
1229 | 999 | ||
1230 | /* Remember the the next thread about to block. */ | ||
1231 | cores[core].block_task = thread; | ||
1232 | |||
1233 | /* Report new state. */ | 1000 | /* Report new state. */ |
1234 | thread->state = state; | 1001 | current->state = state; |
1235 | } | 1002 | } |
1236 | 1003 | ||
1237 | /*--------------------------------------------------------------------------- | 1004 | /*--------------------------------------------------------------------------- |
@@ -1239,178 +1006,120 @@ static inline void block_thread_on_l(struct thread_entry *thread, | |||
1239 | * that removed itself from the running list first must specify itself in | 1006 | * that removed itself from the running list first must specify itself in |
1240 | * the paramter. | 1007 | * the paramter. |
1241 | * | 1008 | * |
1242 | * INTERNAL: Intended for use by kernel and not for programs. | 1009 | * INTERNAL: Intended for use by kernel and not programs. |
1243 | *--------------------------------------------------------------------------- | 1010 | *--------------------------------------------------------------------------- |
1244 | */ | 1011 | */ |
1245 | void switch_thread(void) | 1012 | void switch_thread(void) |
1246 | { | 1013 | { |
1247 | |||
1248 | const unsigned int core = CURRENT_CORE; | 1014 | const unsigned int core = CURRENT_CORE; |
1249 | struct thread_entry *block = cores[core].block_task; | 1015 | struct core_entry *corep = __core_id_entry(core); |
1250 | struct thread_entry *thread = cores[core].running; | 1016 | struct thread_entry *thread = corep->running; |
1251 | 1017 | ||
1252 | /* Get context to save - next thread to run is unknown until all wakeups | 1018 | if (thread) |
1253 | * are evaluated */ | ||
1254 | if (block != NULL) | ||
1255 | { | 1019 | { |
1256 | cores[core].block_task = NULL; | ||
1257 | |||
1258 | #if NUM_CORES > 1 | ||
1259 | if (UNLIKELY(thread == block)) | ||
1260 | { | ||
1261 | /* This was the last thread running and another core woke us before | ||
1262 | * reaching here. Force next thread selection to give tmo threads or | ||
1263 | * other threads woken before this block a first chance. */ | ||
1264 | block = NULL; | ||
1265 | } | ||
1266 | else | ||
1267 | #endif | ||
1268 | { | ||
1269 | /* Blocking task is the old one */ | ||
1270 | thread = block; | ||
1271 | } | ||
1272 | } | ||
1273 | |||
1274 | #ifdef RB_PROFILE | 1020 | #ifdef RB_PROFILE |
1275 | #ifdef CPU_COLDFIRE | 1021 | profile_thread_stopped(THREAD_ID_SLOT(thread->id)); |
1276 | _profile_thread_stopped(thread->id & THREAD_ID_SLOT_MASK); | ||
1277 | #else | ||
1278 | profile_thread_stopped(thread->id & THREAD_ID_SLOT_MASK); | ||
1279 | #endif | 1022 | #endif |
1280 | #endif | ||
1281 | |||
1282 | /* Begin task switching by saving our current context so that we can | ||
1283 | * restore the state of the current thread later to the point prior | ||
1284 | * to this call. */ | ||
1285 | thread_store_context(thread); | ||
1286 | #ifdef DEBUG | 1023 | #ifdef DEBUG |
1287 | /* Check core_ctx buflib integrity */ | 1024 | /* Check core_ctx buflib integrity */ |
1288 | core_check_valid(); | 1025 | core_check_valid(); |
1289 | #endif | ||
1290 | |||
1291 | /* Check if the current thread stack is overflown */ | ||
1292 | if (UNLIKELY(thread->stack[0] != DEADBEEF) && thread->stack_size > 0) | ||
1293 | thread_stkov(thread); | ||
1294 | |||
1295 | #if NUM_CORES > 1 | ||
1296 | /* Run any blocking operations requested before switching/sleeping */ | ||
1297 | run_blocking_ops(core, thread); | ||
1298 | #endif | 1026 | #endif |
1027 | thread_store_context(thread); | ||
1299 | 1028 | ||
1300 | #ifdef HAVE_PRIORITY_SCHEDULING | 1029 | /* Check if the current thread stack is overflown */ |
1301 | /* Reset the value of thread's skip count */ | 1030 | if (UNLIKELY(thread->stack[0] != DEADBEEF) && thread->stack_size > 0) |
1302 | thread->skip_count = 0; | 1031 | thread_stkov(thread); |
1303 | #endif | 1032 | } |
1304 | 1033 | ||
1034 | /* TODO: make a real idle task */ | ||
1305 | for (;;) | 1035 | for (;;) |
1306 | { | 1036 | { |
1307 | /* If there are threads on a timeout and the earliest wakeup is due, | ||
1308 | * check the list and wake any threads that need to start running | ||
1309 | * again. */ | ||
1310 | if (!TIME_BEFORE(current_tick, cores[core].next_tmo_check)) | ||
1311 | { | ||
1312 | check_tmo_threads(); | ||
1313 | } | ||
1314 | |||
1315 | disable_irq(); | 1037 | disable_irq(); |
1316 | RTR_LOCK(core); | ||
1317 | 1038 | ||
1318 | thread = cores[core].running; | 1039 | /* Check for expired timeouts */ |
1040 | check_tmo_expired(corep); | ||
1319 | 1041 | ||
1320 | if (UNLIKELY(thread == NULL)) | 1042 | RTR_LOCK(corep); |
1321 | { | ||
1322 | /* Enter sleep mode to reduce power usage - woken up on interrupt | ||
1323 | * or wakeup request from another core - expected to enable | ||
1324 | * interrupts. */ | ||
1325 | RTR_UNLOCK(core); | ||
1326 | core_sleep(IF_COP(core)); | ||
1327 | } | ||
1328 | else | ||
1329 | { | ||
1330 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
1331 | /* Select the new task based on priorities and the last time a | ||
1332 | * process got CPU time relative to the highest priority runnable | ||
1333 | * task. */ | ||
1334 | int max = priobit_ffs(&cores[core].rtr.mask); | ||
1335 | 1043 | ||
1336 | if (block == NULL) | 1044 | if (!RTR_EMPTY(&corep->rtr)) |
1337 | { | 1045 | break; |
1338 | /* Not switching on a block, tentatively select next thread */ | ||
1339 | thread = thread->l.next; | ||
1340 | } | ||
1341 | 1046 | ||
1342 | for (;;) | 1047 | thread = NULL; |
1343 | { | 1048 | |
1344 | int priority = thread->priority; | 1049 | /* Enter sleep mode to reduce power usage */ |
1345 | int diff; | 1050 | RTR_UNLOCK(corep); |
1346 | 1051 | core_sleep(IF_COP(core)); | |
1347 | /* This ridiculously simple method of aging seems to work | 1052 | |
1348 | * suspiciously well. It does tend to reward CPU hogs (under | 1053 | /* Awakened by interrupt or other CPU */ |
1349 | * yielding) but that's generally not desirable at all. On | 1054 | } |
1350 | * the plus side, it, relatively to other threads, penalizes | 1055 | |
1351 | * excess yielding which is good if some high priority thread | 1056 | thread = (thread && thread->state == STATE_RUNNING) ? |
1352 | * is performing no useful work such as polling for a device | 1057 | RTR_THREAD_NEXT(thread) : RTR_THREAD_FIRST(&corep->rtr); |
1353 | * to be ready. Of course, aging is only employed when higher | 1058 | |
1354 | * and lower priority threads are runnable. The highest | 1059 | #ifdef HAVE_PRIORITY_SCHEDULING |
1355 | * priority runnable thread(s) are never skipped unless a | 1060 | /* Select the new task based on priorities and the last time a |
1356 | * lower-priority process has aged sufficiently. Priorities | 1061 | * process got CPU time relative to the highest priority runnable |
1357 | * of REALTIME class are run strictly according to priority | 1062 | * task. If priority is not a feature, then FCFS is used (above). */ |
1358 | * thus are not subject to switchout due to lower-priority | 1063 | int max = priobit_ffs(&corep->rtr_dist.mask); |
1359 | * processes aging; they must give up the processor by going | ||
1360 | * off the run list. */ | ||
1361 | if (LIKELY(priority <= max) || | ||
1362 | (priority > PRIORITY_REALTIME && | ||
1363 | (diff = priority - max, | ||
1364 | ++thread->skip_count > diff*diff))) | ||
1365 | { | ||
1366 | cores[core].running = thread; | ||
1367 | break; | ||
1368 | } | ||
1369 | |||
1370 | thread = thread->l.next; | ||
1371 | } | ||
1372 | #else | ||
1373 | /* Without priority use a simple FCFS algorithm */ | ||
1374 | if (block == NULL) | ||
1375 | { | ||
1376 | /* Not switching on a block, select next thread */ | ||
1377 | thread = thread->l.next; | ||
1378 | cores[core].running = thread; | ||
1379 | } | ||
1380 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
1381 | 1064 | ||
1382 | RTR_UNLOCK(core); | 1065 | for (;;) |
1383 | enable_irq(); | 1066 | { |
1067 | int priority = thread->priority; | ||
1068 | int diff; | ||
1069 | |||
1070 | /* This ridiculously simple method of aging seems to work | ||
1071 | * suspiciously well. It does tend to reward CPU hogs (under | ||
1072 | * yielding) but that's generally not desirable at all. On | ||
1073 | * the plus side, it, relatively to other threads, penalizes | ||
1074 | * excess yielding which is good if some high priority thread | ||
1075 | * is performing no useful work such as polling for a device | ||
1076 | * to be ready. Of course, aging is only employed when higher | ||
1077 | * and lower priority threads are runnable. The highest | ||
1078 | * priority runnable thread(s) are never skipped unless a | ||
1079 | * lower-priority process has aged sufficiently. Priorities | ||
1080 | * of REALTIME class are run strictly according to priority | ||
1081 | * thus are not subject to switchout due to lower-priority | ||
1082 | * processes aging; they must give up the processor by going | ||
1083 | * off the run list. */ | ||
1084 | if (LIKELY(priority <= max) || | ||
1085 | (priority > PRIORITY_REALTIME && | ||
1086 | (diff = priority - max, ++thread->skip_count > diff*diff))) | ||
1087 | { | ||
1384 | break; | 1088 | break; |
1385 | } | 1089 | } |
1090 | |||
1091 | thread = RTR_THREAD_NEXT(thread); | ||
1386 | } | 1092 | } |
1387 | 1093 | ||
1388 | /* And finally give control to the next thread. */ | 1094 | thread->skip_count = 0; /* Reset aging counter */ |
1095 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
1096 | |||
1097 | rtr_queue_make_first(&corep->rtr, thread); | ||
1098 | corep->running = thread; | ||
1099 | |||
1100 | RTR_UNLOCK(corep); | ||
1101 | enable_irq(); | ||
1102 | |||
1103 | /* And finally, give control to the next thread. */ | ||
1389 | thread_load_context(thread); | 1104 | thread_load_context(thread); |
1390 | 1105 | ||
1391 | #ifdef RB_PROFILE | 1106 | #ifdef RB_PROFILE |
1392 | profile_thread_started(thread->id & THREAD_ID_SLOT_MASK); | 1107 | profile_thread_started(THREAD_ID_SLOT(thread->id)); |
1393 | #endif | 1108 | #endif |
1394 | |||
1395 | } | 1109 | } |
1396 | 1110 | ||
1397 | /*--------------------------------------------------------------------------- | 1111 | /*--------------------------------------------------------------------------- |
1398 | * Sleeps a thread for at least a specified number of ticks with zero being | 1112 | * Sleeps a thread for at least a specified number of ticks with zero being |
1399 | * a wait until the next tick. | 1113 | * a wait until the next tick. |
1400 | * | 1114 | * |
1401 | * INTERNAL: Intended for use by kernel and not for programs. | 1115 | * INTERNAL: Intended for use by kernel and not programs. |
1402 | *--------------------------------------------------------------------------- | 1116 | *--------------------------------------------------------------------------- |
1403 | */ | 1117 | */ |
1404 | void sleep_thread(int ticks) | 1118 | void sleep_thread(int ticks) |
1405 | { | 1119 | { |
1406 | struct thread_entry *current = cores[CURRENT_CORE].running; | 1120 | struct thread_entry *current = __running_self_entry(); |
1407 | |||
1408 | LOCK_THREAD(current); | 1121 | LOCK_THREAD(current); |
1409 | 1122 | prepare_block(current, STATE_SLEEPING, MAX(ticks, 0) + 1); | |
1410 | /* Set our timeout, remove from run list and join timeout list. */ | ||
1411 | current->tmo_tick = current_tick + MAX(ticks, 0) + 1; | ||
1412 | block_thread_on_l(current, STATE_SLEEPING); | ||
1413 | |||
1414 | UNLOCK_THREAD(current); | 1123 | UNLOCK_THREAD(current); |
1415 | } | 1124 | } |
1416 | 1125 | ||
@@ -1418,131 +1127,42 @@ void sleep_thread(int ticks) | |||
1418 | * Block a thread on a blocking queue for explicit wakeup. If timeout is | 1127 | * Block a thread on a blocking queue for explicit wakeup. If timeout is |
1419 | * negative, the block is infinite. | 1128 | * negative, the block is infinite. |
1420 | * | 1129 | * |
1421 | * INTERNAL: Intended for use by kernel objects and not for programs. | 1130 | * INTERNAL: Intended for use by kernel and not programs. |
1422 | *--------------------------------------------------------------------------- | 1131 | *--------------------------------------------------------------------------- |
1423 | */ | 1132 | */ |
1424 | void block_thread(struct thread_entry *current, int timeout) | 1133 | void block_thread_(struct thread_entry *current, int timeout) |
1425 | { | 1134 | { |
1426 | LOCK_THREAD(current); | 1135 | LOCK_THREAD(current); |
1427 | 1136 | ||
1428 | struct blocker *bl = NULL; | ||
1429 | #ifdef HAVE_PRIORITY_SCHEDULING | 1137 | #ifdef HAVE_PRIORITY_SCHEDULING |
1430 | bl = current->blocker; | 1138 | struct blocker *bl = current->blocker; |
1431 | struct thread_entry *blt = bl ? lock_blocker_thread(bl) : NULL; | 1139 | struct thread_entry *blt = NULL; |
1432 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 1140 | if (bl != NULL) |
1433 | |||
1434 | if (LIKELY(timeout < 0)) | ||
1435 | { | ||
1436 | /* Block until explicitly woken */ | ||
1437 | block_thread_on_l(current, STATE_BLOCKED); | ||
1438 | } | ||
1439 | else | ||
1440 | { | 1141 | { |
1441 | /* Set the state to blocked with the specified timeout */ | 1142 | current->blocker = bl; |
1442 | current->tmo_tick = current_tick + timeout; | 1143 | blt = lock_blocker_thread(bl); |
1443 | block_thread_on_l(current, STATE_BLOCKED_W_TMO); | ||
1444 | } | 1144 | } |
1145 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
1445 | 1146 | ||
1446 | if (bl == NULL) | 1147 | wait_queue_register(current); |
1447 | { | 1148 | prepare_block(current, STATE_BLOCKED, timeout); |
1448 | UNLOCK_THREAD(current); | ||
1449 | return; | ||
1450 | } | ||
1451 | 1149 | ||
1452 | #ifdef HAVE_PRIORITY_SCHEDULING | 1150 | #ifdef HAVE_PRIORITY_SCHEDULING |
1453 | int newblpr = current->priority; | 1151 | if (bl != NULL) |
1454 | UNLOCK_THREAD(current); | ||
1455 | |||
1456 | if (newblpr >= bl->priority) | ||
1457 | { | 1152 | { |
1458 | unlock_blocker_thread(bl); | 1153 | int newblpr = current->priority; |
1459 | return; /* Queue priority won't change */ | 1154 | UNLOCK_THREAD(current); |
1460 | } | ||
1461 | 1155 | ||
1462 | inherit_priority(bl, bl, blt, newblpr); | 1156 | if (newblpr < bl->priority) |
1157 | inherit_priority(bl, bl, blt, newblpr); | ||
1158 | else | ||
1159 | unlock_blocker_thread(bl); /* Queue priority won't change */ | ||
1160 | } | ||
1161 | else | ||
1463 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 1162 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
1464 | } | ||
1465 | |||
1466 | /*--------------------------------------------------------------------------- | ||
1467 | * Assign the thread slot a new ID. Version is 0x00000100..0xffffff00. | ||
1468 | *--------------------------------------------------------------------------- | ||
1469 | */ | ||
1470 | static void new_thread_id(unsigned int slot_num, | ||
1471 | struct thread_entry *thread) | ||
1472 | { | ||
1473 | unsigned int version = | ||
1474 | (thread->id + (1u << THREAD_ID_VERSION_SHIFT)) | ||
1475 | & THREAD_ID_VERSION_MASK; | ||
1476 | |||
1477 | /* If wrapped to 0, make it 1 */ | ||
1478 | if (version == 0) | ||
1479 | version = 1u << THREAD_ID_VERSION_SHIFT; | ||
1480 | |||
1481 | thread->id = version | (slot_num & THREAD_ID_SLOT_MASK); | ||
1482 | } | ||
1483 | |||
1484 | /*--------------------------------------------------------------------------- | ||
1485 | * Find an empty thread slot or MAXTHREADS if none found. The slot returned | ||
1486 | * will be locked on multicore. | ||
1487 | *--------------------------------------------------------------------------- | ||
1488 | */ | ||
1489 | static struct thread_entry * find_empty_thread_slot(void) | ||
1490 | { | ||
1491 | /* Any slot could be on an interrupt-accessible list */ | ||
1492 | IF_COP( int oldlevel = disable_irq_save(); ) | ||
1493 | struct thread_entry *thread = NULL; | ||
1494 | int n; | ||
1495 | |||
1496 | for (n = 0; n < MAXTHREADS; n++) | ||
1497 | { | 1163 | { |
1498 | /* Obtain current slot state - lock it on multicore */ | 1164 | UNLOCK_THREAD(current); |
1499 | struct thread_entry *t = &threads[n]; | ||
1500 | LOCK_THREAD(t); | ||
1501 | |||
1502 | if (t->state == STATE_KILLED) | ||
1503 | { | ||
1504 | /* Slot is empty - leave it locked and caller will unlock */ | ||
1505 | thread = t; | ||
1506 | break; | ||
1507 | } | ||
1508 | |||
1509 | /* Finished examining slot - no longer busy - unlock on multicore */ | ||
1510 | UNLOCK_THREAD(t); | ||
1511 | } | 1165 | } |
1512 | |||
1513 | IF_COP( restore_irq(oldlevel); ) /* Reenable interrups - this slot is | ||
1514 | not accesible to them yet */ | ||
1515 | return thread; | ||
1516 | } | ||
1517 | |||
1518 | /*--------------------------------------------------------------------------- | ||
1519 | * Return the thread_entry pointer for a thread_id. Return the current | ||
1520 | * thread if the ID is (unsigned int)-1 (alias for current). | ||
1521 | *--------------------------------------------------------------------------- | ||
1522 | */ | ||
1523 | struct thread_entry * thread_id_entry(unsigned int thread_id) | ||
1524 | { | ||
1525 | return &threads[thread_id & THREAD_ID_SLOT_MASK]; | ||
1526 | } | ||
1527 | |||
1528 | /*--------------------------------------------------------------------------- | ||
1529 | * Return the thread id of the calling thread | ||
1530 | * -------------------------------------------------------------------------- | ||
1531 | */ | ||
1532 | unsigned int thread_self(void) | ||
1533 | { | ||
1534 | return cores[CURRENT_CORE].running->id; | ||
1535 | } | ||
1536 | |||
1537 | /*--------------------------------------------------------------------------- | ||
1538 | * Return the thread entry of the calling thread. | ||
1539 | * | ||
1540 | * INTERNAL: Intended for use by kernel and not for programs. | ||
1541 | *--------------------------------------------------------------------------- | ||
1542 | */ | ||
1543 | struct thread_entry* thread_self_entry(void) | ||
1544 | { | ||
1545 | return cores[CURRENT_CORE].running; | ||
1546 | } | 1166 | } |
1547 | 1167 | ||
1548 | /*--------------------------------------------------------------------------- | 1168 | /*--------------------------------------------------------------------------- |
@@ -1552,9 +1172,8 @@ struct thread_entry* thread_self_entry(void) | |||
1552 | */ | 1172 | */ |
1553 | void core_idle(void) | 1173 | void core_idle(void) |
1554 | { | 1174 | { |
1555 | IF_COP( const unsigned int core = CURRENT_CORE; ) | ||
1556 | disable_irq(); | 1175 | disable_irq(); |
1557 | core_sleep(IF_COP(core)); | 1176 | core_sleep(IF_COP(CURRENT_CORE)); |
1558 | } | 1177 | } |
1559 | 1178 | ||
1560 | /*--------------------------------------------------------------------------- | 1179 | /*--------------------------------------------------------------------------- |
@@ -1570,141 +1189,64 @@ unsigned int create_thread(void (*function)(void), | |||
1570 | IF_PRIO(, int priority) | 1189 | IF_PRIO(, int priority) |
1571 | IF_COP(, unsigned int core)) | 1190 | IF_COP(, unsigned int core)) |
1572 | { | 1191 | { |
1573 | unsigned int i; | 1192 | struct thread_entry *thread = thread_alloc(); |
1574 | unsigned int stack_words; | ||
1575 | uintptr_t stackptr, stackend; | ||
1576 | struct thread_entry *thread; | ||
1577 | unsigned state; | ||
1578 | int oldlevel; | ||
1579 | |||
1580 | thread = find_empty_thread_slot(); | ||
1581 | if (thread == NULL) | 1193 | if (thread == NULL) |
1582 | { | ||
1583 | return 0; | 1194 | return 0; |
1584 | } | ||
1585 | |||
1586 | oldlevel = disable_irq_save(); | ||
1587 | |||
1588 | /* Munge the stack to make it easy to spot stack overflows */ | ||
1589 | stackptr = ALIGN_UP((uintptr_t)stack, sizeof (uintptr_t)); | ||
1590 | stackend = ALIGN_DOWN((uintptr_t)stack + stack_size, sizeof (uintptr_t)); | ||
1591 | stack_size = stackend - stackptr; | ||
1592 | stack_words = stack_size / sizeof (uintptr_t); | ||
1593 | 1195 | ||
1594 | for (i = 0; i < stack_words; i++) | 1196 | new_thread_base_init(thread, &stack, &stack_size, name |
1595 | { | 1197 | IF_PRIO(, priority) IF_COP(, core)); |
1596 | ((uintptr_t *)stackptr)[i] = DEADBEEF; | ||
1597 | } | ||
1598 | 1198 | ||
1599 | /* Store interesting information */ | 1199 | unsigned int stack_words = stack_size / sizeof (uintptr_t); |
1600 | thread->name = name; | 1200 | if (stack_words == 0) |
1601 | thread->stack = (uintptr_t *)stackptr; | 1201 | return 0; |
1602 | thread->stack_size = stack_size; | ||
1603 | thread->queue = NULL; | ||
1604 | #ifdef HAVE_WAKEUP_EXT_CB | ||
1605 | thread->wakeup_ext_cb = NULL; | ||
1606 | #endif | ||
1607 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | ||
1608 | thread->cpu_boost = 0; | ||
1609 | #endif | ||
1610 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
1611 | memset(&thread->pdist, 0, sizeof(thread->pdist)); | ||
1612 | thread->blocker = NULL; | ||
1613 | thread->base_priority = priority; | ||
1614 | thread->priority = priority; | ||
1615 | thread->skip_count = priority; | ||
1616 | prio_add_entry(&thread->pdist, priority); | ||
1617 | #endif | ||
1618 | 1202 | ||
1619 | #ifdef HAVE_IO_PRIORITY | 1203 | /* Munge the stack to make it easy to spot stack overflows */ |
1620 | /* Default to high (foreground) priority */ | 1204 | for (unsigned int i = 0; i < stack_words; i++) |
1621 | thread->io_priority = IO_PRIORITY_IMMEDIATE; | 1205 | ((uintptr_t *)stack)[i] = DEADBEEF; |
1622 | #endif | ||
1623 | 1206 | ||
1624 | #if NUM_CORES > 1 | 1207 | #if NUM_CORES > 1 |
1625 | thread->core = core; | ||
1626 | |||
1627 | /* Writeback stack munging or anything else before starting */ | 1208 | /* Writeback stack munging or anything else before starting */ |
1628 | if (core != CURRENT_CORE) | 1209 | if (core != CURRENT_CORE) |
1629 | { | ||
1630 | commit_dcache(); | 1210 | commit_dcache(); |
1631 | } | ||
1632 | #endif | 1211 | #endif |
1633 | 1212 | ||
1634 | /* Thread is not on any timeout list but be a bit paranoid */ | 1213 | thread->context.sp = (typeof (thread->context.sp))(stack + stack_size); |
1635 | thread->tmo.prev = NULL; | ||
1636 | |||
1637 | state = (flags & CREATE_THREAD_FROZEN) ? | ||
1638 | STATE_FROZEN : STATE_RUNNING; | ||
1639 | |||
1640 | thread->context.sp = (typeof (thread->context.sp))stackend; | ||
1641 | |||
1642 | /* Load the thread's context structure with needed startup information */ | ||
1643 | THREAD_STARTUP_INIT(core, thread, function); | 1214 | THREAD_STARTUP_INIT(core, thread, function); |
1644 | 1215 | ||
1645 | thread->state = state; | 1216 | int oldlevel = disable_irq_save(); |
1646 | i = thread->id; /* Snapshot while locked */ | 1217 | LOCK_THREAD(thread); |
1218 | |||
1219 | thread->state = STATE_FROZEN; | ||
1647 | 1220 | ||
1648 | if (state == STATE_RUNNING) | 1221 | if (!(flags & CREATE_THREAD_FROZEN)) |
1649 | core_schedule_wakeup(thread); | 1222 | core_schedule_wakeup(thread); |
1650 | 1223 | ||
1224 | unsigned int id = thread->id; /* Snapshot while locked */ | ||
1225 | |||
1651 | UNLOCK_THREAD(thread); | 1226 | UNLOCK_THREAD(thread); |
1652 | restore_irq(oldlevel); | 1227 | restore_irq(oldlevel); |
1653 | 1228 | ||
1654 | return i; | 1229 | return id; |
1655 | } | 1230 | } |
1656 | 1231 | ||
1657 | #ifdef HAVE_SCHEDULER_BOOSTCTRL | ||
1658 | /*--------------------------------------------------------------------------- | ||
1659 | * Change the boost state of a thread boosting or unboosting the CPU | ||
1660 | * as required. | ||
1661 | *--------------------------------------------------------------------------- | ||
1662 | */ | ||
1663 | static inline void boost_thread(struct thread_entry *thread, bool boost) | ||
1664 | { | ||
1665 | if ((thread->cpu_boost != 0) != boost) | ||
1666 | { | ||
1667 | thread->cpu_boost = boost; | ||
1668 | cpu_boost(boost); | ||
1669 | } | ||
1670 | } | ||
1671 | |||
1672 | void trigger_cpu_boost(void) | ||
1673 | { | ||
1674 | struct thread_entry *current = cores[CURRENT_CORE].running; | ||
1675 | boost_thread(current, true); | ||
1676 | } | ||
1677 | |||
1678 | void cancel_cpu_boost(void) | ||
1679 | { | ||
1680 | struct thread_entry *current = cores[CURRENT_CORE].running; | ||
1681 | boost_thread(current, false); | ||
1682 | } | ||
1683 | #endif /* HAVE_SCHEDULER_BOOSTCTRL */ | ||
1684 | |||
1685 | /*--------------------------------------------------------------------------- | 1232 | /*--------------------------------------------------------------------------- |
1686 | * Block the current thread until another thread terminates. A thread may | 1233 | * Block the current thread until another thread terminates. A thread may |
1687 | * wait on itself to terminate which prevents it from running again and it | 1234 | * wait on itself to terminate but that will deadlock |
1688 | * will need to be killed externally. | 1235 | *. |
1689 | * Parameter is the ID as returned from create_thread(). | 1236 | * Parameter is the ID as returned from create_thread(). |
1690 | *--------------------------------------------------------------------------- | 1237 | *--------------------------------------------------------------------------- |
1691 | */ | 1238 | */ |
1692 | void thread_wait(unsigned int thread_id) | 1239 | void thread_wait(unsigned int thread_id) |
1693 | { | 1240 | { |
1694 | struct thread_entry *current = cores[CURRENT_CORE].running; | 1241 | struct thread_entry *current = __running_self_entry(); |
1695 | struct thread_entry *thread = thread_id_entry(thread_id); | 1242 | struct thread_entry *thread = __thread_id_entry(thread_id); |
1696 | 1243 | ||
1697 | /* Lock thread-as-waitable-object lock */ | ||
1698 | corelock_lock(&thread->waiter_cl); | 1244 | corelock_lock(&thread->waiter_cl); |
1699 | 1245 | ||
1700 | /* Be sure it hasn't been killed yet */ | ||
1701 | if (thread->id == thread_id && thread->state != STATE_KILLED) | 1246 | if (thread->id == thread_id && thread->state != STATE_KILLED) |
1702 | { | 1247 | { |
1703 | IF_COP( current->obj_cl = &thread->waiter_cl; ) | ||
1704 | current->bqp = &thread->queue; | ||
1705 | |||
1706 | disable_irq(); | 1248 | disable_irq(); |
1707 | block_thread(current, TIMEOUT_BLOCK); | 1249 | block_thread(current, TIMEOUT_BLOCK, &thread->queue, NULL); |
1708 | 1250 | ||
1709 | corelock_unlock(&thread->waiter_cl); | 1251 | corelock_unlock(&thread->waiter_cl); |
1710 | 1252 | ||
@@ -1716,36 +1258,35 @@ void thread_wait(unsigned int thread_id) | |||
1716 | } | 1258 | } |
1717 | 1259 | ||
1718 | /*--------------------------------------------------------------------------- | 1260 | /*--------------------------------------------------------------------------- |
1719 | * Exit the current thread. The Right Way to Do Things (TM). | 1261 | * Exit the current thread |
1720 | *--------------------------------------------------------------------------- | 1262 | *--------------------------------------------------------------------------- |
1721 | */ | 1263 | */ |
1722 | /* This is done to foil optimizations that may require the current stack, | 1264 | static USED_ATTR NORETURN_ATTR |
1723 | * such as optimizing subexpressions that put variables on the stack that | 1265 | void thread_exit_final(struct thread_entry *current) |
1724 | * get used after switching stacks. */ | ||
1725 | #if NUM_CORES > 1 | ||
1726 | /* Called by ASM stub */ | ||
1727 | static void thread_final_exit_do(struct thread_entry *current) | ||
1728 | #else | ||
1729 | /* No special procedure is required before calling */ | ||
1730 | static inline void thread_final_exit(struct thread_entry *current) | ||
1731 | #endif | ||
1732 | { | 1266 | { |
1733 | /* At this point, this thread isn't using resources allocated for | 1267 | /* Slot is no longer this thread */ |
1734 | * execution except the slot itself. */ | 1268 | new_thread_id(current); |
1269 | current->name = NULL; | ||
1735 | 1270 | ||
1736 | /* Signal this thread */ | 1271 | /* No longer using resources from creator */ |
1737 | thread_queue_wake(¤t->queue); | 1272 | wait_queue_wake(¤t->queue); |
1273 | |||
1274 | UNLOCK_THREAD(current); | ||
1738 | corelock_unlock(¤t->waiter_cl); | 1275 | corelock_unlock(¤t->waiter_cl); |
1276 | |||
1277 | thread_free(current); | ||
1278 | |||
1739 | switch_thread(); | 1279 | switch_thread(); |
1280 | |||
1740 | /* This should never and must never be reached - if it is, the | 1281 | /* This should never and must never be reached - if it is, the |
1741 | * state is corrupted */ | 1282 | * state is corrupted */ |
1742 | THREAD_PANICF("thread_exit->K:*R", current); | 1283 | THREAD_PANICF("thread_exit->K:*R", current); |
1743 | while (1); | ||
1744 | } | 1284 | } |
1745 | 1285 | ||
1746 | void thread_exit(void) | 1286 | void thread_exit(void) |
1747 | { | 1287 | { |
1748 | register struct thread_entry * current = cores[CURRENT_CORE].running; | 1288 | struct core_entry *corep = __core_id_entry(CURRENT_CORE); |
1289 | register struct thread_entry *current = corep->running; | ||
1749 | 1290 | ||
1750 | /* Cancel CPU boost if any */ | 1291 | /* Cancel CPU boost if any */ |
1751 | cancel_cpu_boost(); | 1292 | cancel_cpu_boost(); |
@@ -1764,24 +1305,21 @@ void thread_exit(void) | |||
1764 | thread_panicf("abandon ship!", current); | 1305 | thread_panicf("abandon ship!", current); |
1765 | #endif /* HAVE_PRIORITY_SCHEDULING */ | 1306 | #endif /* HAVE_PRIORITY_SCHEDULING */ |
1766 | 1307 | ||
1767 | if (current->tmo.prev != NULL) | 1308 | /* Remove from scheduler lists */ |
1768 | { | 1309 | tmo_queue_remove(&corep->tmo, current); |
1769 | /* Cancel pending timeout list removal */ | 1310 | prepare_block(current, STATE_KILLED, -1); |
1770 | remove_from_list_tmo(current); | 1311 | corep->running = NULL; /* No switch_thread context save */ |
1771 | } | ||
1772 | |||
1773 | /* Switch tasks and never return */ | ||
1774 | block_thread_on_l(current, STATE_KILLED); | ||
1775 | |||
1776 | /* Slot must be unusable until thread is really gone */ | ||
1777 | UNLOCK_THREAD_AT_TASK_SWITCH(current); | ||
1778 | 1312 | ||
1779 | /* Update ID for this slot */ | 1313 | #ifdef RB_PROFILE |
1780 | new_thread_id(current->id, current); | 1314 | profile_thread_stopped(THREAD_ID_SLOT(current->id)); |
1781 | current->name = NULL; | 1315 | #endif |
1782 | 1316 | ||
1783 | /* Do final cleanup and remove the thread */ | 1317 | /* Do final release of resources and remove the thread */ |
1784 | thread_final_exit(current); | 1318 | #if NUM_CORES > 1 |
1319 | thread_exit_finalize(current->core, current); | ||
1320 | #else | ||
1321 | thread_exit_final(current); | ||
1322 | #endif | ||
1785 | } | 1323 | } |
1786 | 1324 | ||
1787 | #ifdef HAVE_PRIORITY_SCHEDULING | 1325 | #ifdef HAVE_PRIORITY_SCHEDULING |
@@ -1796,10 +1334,8 @@ int thread_set_priority(unsigned int thread_id, int priority) | |||
1796 | return -1; /* Invalid priority argument */ | 1334 | return -1; /* Invalid priority argument */ |
1797 | 1335 | ||
1798 | int old_base_priority = -1; | 1336 | int old_base_priority = -1; |
1799 | struct thread_entry *thread = thread_id_entry(thread_id); | 1337 | struct thread_entry *thread = __thread_id_entry(thread_id); |
1800 | 1338 | ||
1801 | /* Thread could be on any list and therefore on an interrupt accessible | ||
1802 | one - disable interrupts */ | ||
1803 | const int oldlevel = disable_irq_save(); | 1339 | const int oldlevel = disable_irq_save(); |
1804 | LOCK_THREAD(thread); | 1340 | LOCK_THREAD(thread); |
1805 | 1341 | ||
@@ -1825,7 +1361,7 @@ int thread_set_priority(unsigned int thread_id, int priority) | |||
1825 | { | 1361 | { |
1826 | /* This thread is running - just change location on the run queue. | 1362 | /* This thread is running - just change location on the run queue. |
1827 | Also sets thread->priority. */ | 1363 | Also sets thread->priority. */ |
1828 | set_running_thread_priority(thread, new_priority); | 1364 | set_rtr_thread_priority(thread, new_priority); |
1829 | goto done; | 1365 | goto done; |
1830 | } | 1366 | } |
1831 | 1367 | ||
@@ -1838,7 +1374,7 @@ int thread_set_priority(unsigned int thread_id, int priority) | |||
1838 | } | 1374 | } |
1839 | 1375 | ||
1840 | struct thread_entry *blt = lock_blocker_thread(bl); | 1376 | struct thread_entry *blt = lock_blocker_thread(bl); |
1841 | struct thread_entry **bqp = thread->bqp; | 1377 | struct __wait_queue *wqp = wait_queue_ptr(thread); |
1842 | 1378 | ||
1843 | thread->priority = new_priority; | 1379 | thread->priority = new_priority; |
1844 | 1380 | ||
@@ -1850,7 +1386,7 @@ int thread_set_priority(unsigned int thread_id, int priority) | |||
1850 | if (new_priority < oldblpr) | 1386 | if (new_priority < oldblpr) |
1851 | newblpr = new_priority; | 1387 | newblpr = new_priority; |
1852 | else if (old_priority <= oldblpr) | 1388 | else if (old_priority <= oldblpr) |
1853 | newblpr = find_highest_priority_in_list_l(*bqp); | 1389 | newblpr = wait_queue_find_priority(wqp); |
1854 | 1390 | ||
1855 | if (newblpr == oldblpr) | 1391 | if (newblpr == oldblpr) |
1856 | { | 1392 | { |
@@ -1872,7 +1408,7 @@ done: | |||
1872 | */ | 1408 | */ |
1873 | int thread_get_priority(unsigned int thread_id) | 1409 | int thread_get_priority(unsigned int thread_id) |
1874 | { | 1410 | { |
1875 | struct thread_entry *thread = thread_id_entry(thread_id); | 1411 | struct thread_entry *thread = __thread_id_entry(thread_id); |
1876 | int base_priority = thread->base_priority; | 1412 | int base_priority = thread->base_priority; |
1877 | 1413 | ||
1878 | /* Simply check without locking slot. It may or may not be valid by the | 1414 | /* Simply check without locking slot. It may or may not be valid by the |
@@ -1888,13 +1424,13 @@ int thread_get_priority(unsigned int thread_id) | |||
1888 | #ifdef HAVE_IO_PRIORITY | 1424 | #ifdef HAVE_IO_PRIORITY |
1889 | int thread_get_io_priority(unsigned int thread_id) | 1425 | int thread_get_io_priority(unsigned int thread_id) |
1890 | { | 1426 | { |
1891 | struct thread_entry *thread = thread_id_entry(thread_id); | 1427 | struct thread_entry *thread = __thread_id_entry(thread_id); |
1892 | return thread->io_priority; | 1428 | return thread->io_priority; |
1893 | } | 1429 | } |
1894 | 1430 | ||
1895 | void thread_set_io_priority(unsigned int thread_id,int io_priority) | 1431 | void thread_set_io_priority(unsigned int thread_id,int io_priority) |
1896 | { | 1432 | { |
1897 | struct thread_entry *thread = thread_id_entry(thread_id); | 1433 | struct thread_entry *thread = __thread_id_entry(thread_id); |
1898 | thread->io_priority = io_priority; | 1434 | thread->io_priority = io_priority; |
1899 | } | 1435 | } |
1900 | #endif | 1436 | #endif |
@@ -1907,7 +1443,7 @@ void thread_set_io_priority(unsigned int thread_id,int io_priority) | |||
1907 | */ | 1443 | */ |
1908 | void thread_thaw(unsigned int thread_id) | 1444 | void thread_thaw(unsigned int thread_id) |
1909 | { | 1445 | { |
1910 | struct thread_entry *thread = thread_id_entry(thread_id); | 1446 | struct thread_entry *thread = __thread_id_entry(thread_id); |
1911 | int oldlevel = disable_irq_save(); | 1447 | int oldlevel = disable_irq_save(); |
1912 | 1448 | ||
1913 | LOCK_THREAD(thread); | 1449 | LOCK_THREAD(thread); |
@@ -1926,68 +1462,72 @@ void thread_thaw(unsigned int thread_id) | |||
1926 | * Switch the processor that the currently executing thread runs on. | 1462 | * Switch the processor that the currently executing thread runs on. |
1927 | *--------------------------------------------------------------------------- | 1463 | *--------------------------------------------------------------------------- |
1928 | */ | 1464 | */ |
1465 | static USED_ATTR NORETURN_ATTR | ||
1466 | void switch_core_final(unsigned int old_core, struct thread_entry *current) | ||
1467 | { | ||
1468 | /* Old core won't be using slot resources at this point */ | ||
1469 | core_schedule_wakeup(current); | ||
1470 | UNLOCK_THREAD(current); | ||
1471 | #ifdef RB_PROFILE | ||
1472 | profile_thread_stopped(THREAD_ID_SLOT(current->id)); | ||
1473 | #endif | ||
1474 | switch_thread(); | ||
1475 | /* not reached */ | ||
1476 | THREAD_PANICF("switch_core_final->same core!", current); | ||
1477 | (void)old_core; | ||
1478 | } | ||
1479 | |||
1929 | unsigned int switch_core(unsigned int new_core) | 1480 | unsigned int switch_core(unsigned int new_core) |
1930 | { | 1481 | { |
1931 | const unsigned int core = CURRENT_CORE; | 1482 | const unsigned int old_core = CURRENT_CORE; |
1932 | struct thread_entry *current = cores[core].running; | 1483 | if (old_core == new_core) |
1484 | return old_core; /* No change */ | ||
1933 | 1485 | ||
1934 | if (core == new_core) | 1486 | struct core_entry *corep = __core_id_entry(old_core); |
1935 | { | 1487 | struct thread_entry *current = corep->running; |
1936 | /* No change - just return same core */ | ||
1937 | return core; | ||
1938 | } | ||
1939 | 1488 | ||
1940 | disable_irq(); | 1489 | disable_irq(); |
1941 | LOCK_THREAD(current); | 1490 | LOCK_THREAD(current); |
1942 | 1491 | ||
1943 | /* Get us off the running list for the current core */ | 1492 | /* Remove us from old core lists */ |
1944 | RTR_LOCK(core); | 1493 | tmo_queue_remove(&corep->tmo, current); |
1945 | remove_from_list_l(&cores[core].running, current); | 1494 | core_rtr_remove(corep, current); |
1946 | rtr_subtract_entry(core, current->priority); | 1495 | corep->running = NULL; /* No switch_thread context save */ |
1947 | RTR_UNLOCK(core); | ||
1948 | |||
1949 | /* Stash return value (old core) in a safe place */ | ||
1950 | current->retval = core; | ||
1951 | |||
1952 | /* If a timeout hadn't yet been cleaned-up it must be removed now or | ||
1953 | * the other core will likely attempt a removal from the wrong list! */ | ||
1954 | if (current->tmo.prev != NULL) | ||
1955 | { | ||
1956 | remove_from_list_tmo(current); | ||
1957 | } | ||
1958 | 1496 | ||
1959 | /* Change the core number for this thread slot */ | 1497 | /* Do the actual migration */ |
1960 | current->core = new_core; | 1498 | current->core = new_core; |
1499 | switch_thread_core(old_core, current); | ||
1961 | 1500 | ||
1962 | /* Do not use core_schedule_wakeup here since this will result in | 1501 | /* Executing on new core */ |
1963 | * the thread starting to run on the other core before being finished on | 1502 | return old_core; |
1964 | * this one. Delay the list unlock to keep the other core stuck | 1503 | } |
1965 | * until this thread is ready. */ | 1504 | #endif /* NUM_CORES > 1 */ |
1966 | RTR_LOCK(new_core); | ||
1967 | |||
1968 | rtr_add_entry(new_core, current->priority); | ||
1969 | add_to_list_l(&cores[new_core].running, current); | ||
1970 | |||
1971 | /* Make a callback into device-specific code, unlock the wakeup list so | ||
1972 | * that execution may resume on the new core, unlock our slot and finally | ||
1973 | * restore the interrupt level */ | ||
1974 | cores[core].blk_ops.flags = TBOP_SWITCH_CORE; | ||
1975 | cores[core].blk_ops.cl_p = &cores[new_core].rtr_cl; | ||
1976 | cores[core].block_task = current; | ||
1977 | |||
1978 | UNLOCK_THREAD(current); | ||
1979 | 1505 | ||
1980 | /* Alert other core to activity */ | 1506 | #ifdef HAVE_SCHEDULER_BOOSTCTRL |
1981 | core_wake(new_core); | 1507 | /*--------------------------------------------------------------------------- |
1508 | * Change the boost state of a thread boosting or unboosting the CPU | ||
1509 | * as required. | ||
1510 | *--------------------------------------------------------------------------- | ||
1511 | */ | ||
1512 | static inline void boost_thread(struct thread_entry *thread, bool boost) | ||
1513 | { | ||
1514 | if ((thread->cpu_boost != 0) != boost) | ||
1515 | { | ||
1516 | thread->cpu_boost = boost; | ||
1517 | cpu_boost(boost); | ||
1518 | } | ||
1519 | } | ||
1982 | 1520 | ||
1983 | /* Do the stack switching, cache_maintenence and switch_thread call - | 1521 | void trigger_cpu_boost(void) |
1984 | requires native code */ | 1522 | { |
1985 | switch_thread_core(core, current); | 1523 | boost_thread(__running_self_entry(), true); |
1524 | } | ||
1986 | 1525 | ||
1987 | /* Finally return the old core to caller */ | 1526 | void cancel_cpu_boost(void) |
1988 | return current->retval; | 1527 | { |
1528 | boost_thread(__running_self_entry(), false); | ||
1989 | } | 1529 | } |
1990 | #endif /* NUM_CORES > 1 */ | 1530 | #endif /* HAVE_SCHEDULER_BOOSTCTRL */ |
1991 | 1531 | ||
1992 | /*--------------------------------------------------------------------------- | 1532 | /*--------------------------------------------------------------------------- |
1993 | * Initialize threading API. This assumes interrupts are not yet enabled. On | 1533 | * Initialize threading API. This assumes interrupts are not yet enabled. On |
@@ -1998,127 +1538,56 @@ unsigned int switch_core(unsigned int new_core) | |||
1998 | void INIT_ATTR init_threads(void) | 1538 | void INIT_ATTR init_threads(void) |
1999 | { | 1539 | { |
2000 | const unsigned int core = CURRENT_CORE; | 1540 | const unsigned int core = CURRENT_CORE; |
2001 | struct thread_entry *thread; | ||
2002 | 1541 | ||
2003 | if (core == CPU) | 1542 | if (core == CPU) |
2004 | { | 1543 | { |
2005 | /* Initialize core locks and IDs in all slots */ | 1544 | thread_alloc_init(); /* before using cores! */ |
2006 | int n; | 1545 | |
2007 | for (n = 0; n < MAXTHREADS; n++) | 1546 | /* Create main thread */ |
1547 | struct thread_entry *thread = thread_alloc(); | ||
1548 | if (thread == NULL) | ||
2008 | { | 1549 | { |
2009 | thread = &threads[n]; | 1550 | /* WTF? There really must be a slot available at this stage. |
2010 | corelock_init(&thread->waiter_cl); | 1551 | * This can fail if, for example, .bss isn't zero'ed out by the |
2011 | corelock_init(&thread->slot_cl); | 1552 | * loader or threads is in the wrong section. */ |
2012 | thread->id = THREAD_ID_INIT(n); | 1553 | THREAD_PANICF("init_threads->no slot", NULL); |
2013 | } | 1554 | } |
2014 | } | ||
2015 | |||
2016 | /* CPU will initialize first and then sleep */ | ||
2017 | thread = find_empty_thread_slot(); | ||
2018 | 1555 | ||
2019 | if (thread == NULL) | 1556 | size_t stack_size; |
2020 | { | 1557 | void *stack = __get_main_stack(&stack_size); |
2021 | /* WTF? There really must be a slot available at this stage. | 1558 | new_thread_base_init(thread, &stack, &stack_size, __main_thread_name |
2022 | * This can fail if, for example, .bss isn't zero'ed out by the loader | 1559 | IF_PRIO(, PRIORITY_MAIN_THREAD) IF_COP(, core)); |
2023 | * or threads is in the wrong section. */ | ||
2024 | THREAD_PANICF("init_threads->no slot", NULL); | ||
2025 | } | ||
2026 | 1560 | ||
2027 | /* Initialize initially non-zero members of core */ | 1561 | struct core_entry *corep = __core_id_entry(core); |
2028 | cores[core].next_tmo_check = current_tick; /* Something not in the past */ | 1562 | core_rtr_add(corep, thread); |
1563 | corep->running = thread; | ||
2029 | 1564 | ||
2030 | /* Initialize initially non-zero members of slot */ | 1565 | #ifdef INIT_MAIN_THREAD |
2031 | UNLOCK_THREAD(thread); /* No sync worries yet */ | 1566 | init_main_thread(&thread->context); |
2032 | thread->name = main_thread_name; | ||
2033 | thread->state = STATE_RUNNING; | ||
2034 | IF_COP( thread->core = core; ) | ||
2035 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
2036 | corelock_init(&cores[core].rtr_cl); | ||
2037 | thread->base_priority = PRIORITY_USER_INTERFACE; | ||
2038 | prio_add_entry(&thread->pdist, PRIORITY_USER_INTERFACE); | ||
2039 | thread->priority = PRIORITY_USER_INTERFACE; | ||
2040 | rtr_add_entry(core, PRIORITY_USER_INTERFACE); | ||
2041 | #endif | 1567 | #endif |
1568 | } | ||
2042 | 1569 | ||
2043 | add_to_list_l(&cores[core].running, thread); | 1570 | #if NUM_CORES > 1 |
2044 | 1571 | /* Boot CPU: | |
2045 | if (core == CPU) | 1572 | * Wait for other processors to finish their inits since create_thread |
2046 | { | 1573 | * isn't safe to call until the kernel inits are done. The first |
2047 | thread->stack = stackbegin; | 1574 | * threads created in the system must of course be created by CPU. |
2048 | thread->stack_size = (uintptr_t)stackend - (uintptr_t)stackbegin; | 1575 | * Another possible approach is to initialize all cores and slots |
2049 | #if NUM_CORES > 1 /* This code path will not be run on single core targets */ | 1576 | * for each core by CPU, let the remainder proceed in parallel and |
2050 | /* Wait for other processors to finish their inits since create_thread | 1577 | * signal CPU when all are finished. |
2051 | * isn't safe to call until the kernel inits are done. The first | 1578 | * |
2052 | * threads created in the system must of course be created by CPU. | 1579 | * Other: |
2053 | * Another possible approach is to initialize all cores and slots | 1580 | * After last processor completes, it should signal all others to |
2054 | * for each core by CPU, let the remainder proceed in parallel and | 1581 | * proceed or may signal the next and call thread_exit(). The last one |
2055 | * signal CPU when all are finished. */ | 1582 | * to finish will signal CPU. |
2056 | core_thread_init(CPU); | 1583 | */ |
2057 | } | 1584 | core_thread_init(core); |
2058 | else | 1585 | |
1586 | if (core != CPU) | ||
2059 | { | 1587 | { |
2060 | /* Initial stack is the idle stack */ | 1588 | /* No main thread on coprocessors - go idle and wait */ |
2061 | thread->stack = idle_stacks[core]; | 1589 | switch_thread(); |
2062 | thread->stack_size = IDLE_STACK_SIZE; | 1590 | THREAD_PANICF("init_threads() - coprocessor returned", NULL); |
2063 | /* After last processor completes, it should signal all others to | ||
2064 | * proceed or may signal the next and call thread_exit(). The last one | ||
2065 | * to finish will signal CPU. */ | ||
2066 | core_thread_init(core); | ||
2067 | /* Other cores do not have a main thread - go idle inside switch_thread | ||
2068 | * until a thread can run on the core. */ | ||
2069 | thread_exit(); | ||
2070 | #endif /* NUM_CORES */ | ||
2071 | } | 1591 | } |
2072 | #ifdef INIT_MAIN_THREAD | 1592 | #endif /* NUM_CORES */ |
2073 | init_main_thread(&thread->context); | ||
2074 | #endif | ||
2075 | } | ||
2076 | |||
2077 | /* Unless otherwise defined, do nothing */ | ||
2078 | #ifndef YIELD_KERNEL_HOOK | ||
2079 | #define YIELD_KERNEL_HOOK() false | ||
2080 | #endif | ||
2081 | #ifndef SLEEP_KERNEL_HOOK | ||
2082 | #define SLEEP_KERNEL_HOOK(ticks) false | ||
2083 | #endif | ||
2084 | |||
2085 | /*--------------------------------------------------------------------------- | ||
2086 | * Suspends a thread's execution for at least the specified number of ticks. | ||
2087 | * | ||
2088 | * May result in CPU core entering wait-for-interrupt mode if no other thread | ||
2089 | * may be scheduled. | ||
2090 | * | ||
2091 | * NOTE: sleep(0) sleeps until the end of the current tick | ||
2092 | * sleep(n) that doesn't result in rescheduling: | ||
2093 | * n <= ticks suspended < n + 1 | ||
2094 | * n to n+1 is a lower bound. Other factors may affect the actual time | ||
2095 | * a thread is suspended before it runs again. | ||
2096 | *--------------------------------------------------------------------------- | ||
2097 | */ | ||
2098 | unsigned sleep(unsigned ticks) | ||
2099 | { | ||
2100 | /* In certain situations, certain bootloaders in particular, a normal | ||
2101 | * threading call is inappropriate. */ | ||
2102 | if (SLEEP_KERNEL_HOOK(ticks)) | ||
2103 | return 0; /* Handled */ | ||
2104 | |||
2105 | disable_irq(); | ||
2106 | sleep_thread(ticks); | ||
2107 | switch_thread(); | ||
2108 | return 0; | ||
2109 | } | ||
2110 | |||
2111 | /*--------------------------------------------------------------------------- | ||
2112 | * Elects another thread to run or, if no other thread may be made ready to | ||
2113 | * run, immediately returns control back to the calling thread. | ||
2114 | *--------------------------------------------------------------------------- | ||
2115 | */ | ||
2116 | void yield(void) | ||
2117 | { | ||
2118 | /* In certain situations, certain bootloaders in particular, a normal | ||
2119 | * threading call is inappropriate. */ | ||
2120 | if (YIELD_KERNEL_HOOK()) | ||
2121 | return; /* handled */ | ||
2122 | |||
2123 | switch_thread(); | ||
2124 | } | 1593 | } |
diff --git a/firmware/libc/errno.c b/firmware/libc/errno.c index 146d6196ca..0672768484 100644 --- a/firmware/libc/errno.c +++ b/firmware/libc/errno.c | |||
@@ -1,5 +1,5 @@ | |||
1 | #include "../thread-internal.h" | 1 | #include "../thread-internal.h" |
2 | int * __errno(void) | 2 | int * __errno(void) |
3 | { | 3 | { |
4 | return &thread_self_entry()->__errno; | 4 | return &__running_self_entry()->__errno; |
5 | } | 5 | } |
diff --git a/firmware/target/arm/pp/app-pp.lds b/firmware/target/arm/pp/app-pp.lds index e6c2b255dd..0b8cbd8430 100644 --- a/firmware/target/arm/pp/app-pp.lds +++ b/firmware/target/arm/pp/app-pp.lds | |||
@@ -125,6 +125,7 @@ SECTIONS | |||
125 | .idle_stacks (NOLOAD) : | 125 | .idle_stacks (NOLOAD) : |
126 | { | 126 | { |
127 | *(.idle_stacks) | 127 | *(.idle_stacks) |
128 | . = ALIGN(8); | ||
128 | #if NUM_CORES > 1 | 129 | #if NUM_CORES > 1 |
129 | cpu_idlestackbegin = .; | 130 | cpu_idlestackbegin = .; |
130 | . += IDLE_STACK_SIZE; | 131 | . += IDLE_STACK_SIZE; |
diff --git a/firmware/target/arm/pp/thread-pp.c b/firmware/target/arm/pp/thread-pp.c index 184d243e8d..0af8caa43a 100644 --- a/firmware/target/arm/pp/thread-pp.c +++ b/firmware/target/arm/pp/thread-pp.c | |||
@@ -82,46 +82,22 @@ static void INIT_ATTR core_thread_init(unsigned int core) | |||
82 | * to use a stack from an unloaded module until another thread runs on it. | 82 | * to use a stack from an unloaded module until another thread runs on it. |
83 | *--------------------------------------------------------------------------- | 83 | *--------------------------------------------------------------------------- |
84 | */ | 84 | */ |
85 | static inline void NORETURN_ATTR __attribute__((always_inline)) | 85 | static void __attribute__((naked, noinline, noreturn)) |
86 | thread_final_exit(struct thread_entry *current) | 86 | thread_exit_finalize(unsigned int core, struct thread_entry *current) |
87 | { | 87 | { |
88 | asm volatile ( | 88 | asm volatile ( |
89 | "cmp %1, #0 \n" /* CPU? */ | 89 | "ldr r2, =idle_stacks \n" /* switch to idle stack */ |
90 | "ldr sp, [r2, r0, lsl #2] \n" | ||
91 | "add sp, sp, %0*4 \n" | ||
92 | "cmp r0, #0 \n" /* CPU? */ | ||
93 | "mov r4, r1 \n" | ||
90 | "blne commit_dcache \n" | 94 | "blne commit_dcache \n" |
91 | "mov r0, %0 \n" /* copy thread parameter */ | 95 | "mov r0, r4 \n" |
92 | "mov sp, %2 \n" /* switch to idle stack */ | 96 | "b thread_exit_final \n" |
93 | "bl thread_final_exit_do \n" /* finish removal */ | 97 | : : "i"(IDLE_STACK_WORDS)); |
94 | : : "r"(current), | ||
95 | "r"(current->core), | ||
96 | "r"(&idle_stacks[current->core][IDLE_STACK_WORDS]) | ||
97 | : "r0", "r1", "r2", "r3", "ip", "lr"); /* Because of flush call, | ||
98 | force inputs out | ||
99 | of scratch regs */ | ||
100 | while (1); | ||
101 | } | ||
102 | 98 | ||
103 | /*--------------------------------------------------------------------------- | 99 | while (1); |
104 | * Perform core switch steps that need to take place inside switch_thread. | 100 | (void)core; (void)current; |
105 | * | ||
106 | * These steps must take place while before changing the processor and after | ||
107 | * having entered switch_thread since switch_thread may not do a normal return | ||
108 | * because the stack being used for anything the compiler saved will not belong | ||
109 | * to the thread's destination core and it may have been recycled for other | ||
110 | * purposes by the time a normal context load has taken place. switch_thread | ||
111 | * will also clobber anything stashed in the thread's context or stored in the | ||
112 | * nonvolatile registers if it is saved there before the call since the | ||
113 | * compiler's order of operations cannot be known for certain. | ||
114 | */ | ||
115 | static void core_switch_blk_op(unsigned int core, struct thread_entry *thread) | ||
116 | { | ||
117 | /* Flush our data to ram */ | ||
118 | commit_dcache(); | ||
119 | /* Stash thread in r4 slot */ | ||
120 | thread->context.r[0] = (uint32_t)thread; | ||
121 | /* Stash restart address in r5 slot */ | ||
122 | thread->context.r[1] = thread->context.start; | ||
123 | /* Save sp in context.sp while still running on old core */ | ||
124 | thread->context.sp = idle_stacks[core][IDLE_STACK_WORDS-1]; | ||
125 | } | 101 | } |
126 | 102 | ||
127 | /*--------------------------------------------------------------------------- | 103 | /*--------------------------------------------------------------------------- |
@@ -136,31 +112,32 @@ static void core_switch_blk_op(unsigned int core, struct thread_entry *thread) | |||
136 | /*--------------------------------------------------------------------------- | 112 | /*--------------------------------------------------------------------------- |
137 | * This actually performs the core switch. | 113 | * This actually performs the core switch. |
138 | */ | 114 | */ |
139 | static void __attribute__((naked)) | 115 | static void __attribute__((naked, noinline)) |
140 | switch_thread_core(unsigned int core, struct thread_entry *thread) | 116 | switch_thread_core(unsigned int old_core, struct thread_entry *thread) |
141 | { | 117 | { |
142 | /* Pure asm for this because compiler behavior isn't sufficiently predictable. | ||
143 | * Stack access also isn't permitted until restoring the original stack and | ||
144 | * context. */ | ||
145 | asm volatile ( | 118 | asm volatile ( |
146 | "stmfd sp!, { r4-r11, lr } \n" /* Stack all non-volatile context on current core */ | 119 | "stmfd sp!, { r4-r5, lr } \n" /* can't use the first two ctx fields */ |
147 | "ldr r2, =idle_stacks \n" /* r2 = &idle_stacks[core][IDLE_STACK_WORDS] */ | 120 | "add r2, r1, #8 \n" |
148 | "ldr r2, [r2, r0, lsl #2] \n" | 121 | "stmia r2, { r6-r11, sp } \n" /* save remaining context */ |
149 | "add r2, r2, %0*4 \n" | 122 | "adr r2, .new_core_restart \n" /* save context ptr + restart address */ |
150 | "stmfd r2!, { sp } \n" /* save original stack pointer on idle stack */ | 123 | "str r2, [r1, #40] \n" /* make 'start' non-null */ |
151 | "mov sp, r2 \n" /* switch stacks */ | 124 | "stmia r1, { r1-r2 } \n" |
152 | "adr r2, 1f \n" /* r2 = new core restart address */ | 125 | "ldr r2, =idle_stacks \n" /* switch to idle stack on old core */ |
153 | "str r2, [r1, #40] \n" /* thread->context.start = r2 */ | 126 | "ldr sp, [r2, r0, lsl #2] \n" |
154 | "ldr pc, =switch_thread \n" /* r0 = thread after call - see load_context */ | 127 | "add sp, sp, %0*4 \n" |
155 | "1: \n" | 128 | "stmfd sp!, { r0-r1 } \n" |
156 | "ldr sp, [r0, #32] \n" /* Reload original sp from context structure */ | 129 | "bl commit_dcache \n" /* write back everything */ |
157 | "mov r1, #0 \n" /* Clear start address */ | 130 | "ldmfd sp!, { r0-r1 } \n" |
158 | "str r1, [r0, #40] \n" | 131 | "b switch_core_final \n" |
159 | "bl commit_discard_idcache \n" /* Invalidate new core's cache */ | 132 | ".new_core_restart: \n" |
160 | "ldmfd sp!, { r4-r11, pc } \n" /* Restore non-volatile context to new core and return */ | 133 | "mov r1, #0 \n" /* mark as started */ |
161 | : : "i"(IDLE_STACK_WORDS) | 134 | "str r1, [r0, #40] \n" |
162 | ); | 135 | "add r0, r0, #8 \n" |
163 | (void)core; (void)thread; | 136 | "ldmia r0, { r6-r11, sp } \n" /* restore non-volatiles and stack */ |
137 | "bl commit_discard_idcache \n" /* invalidate new core's cache */ | ||
138 | "ldmfd sp!, { r4-r5, pc } \n" /* restore remaining context */ | ||
139 | : : "i"(IDLE_STACK_WORDS)); | ||
140 | (void)old_core; (void)thread; | ||
164 | } | 141 | } |
165 | 142 | ||
166 | /** PP-model-specific dual-core code **/ | 143 | /** PP-model-specific dual-core code **/ |
diff --git a/firmware/target/hosted/sdl/thread-sdl.c b/firmware/target/hosted/sdl/thread-sdl.c index fda877e0f5..a76941f103 100644 --- a/firmware/target/hosted/sdl/thread-sdl.c +++ b/firmware/target/hosted/sdl/thread-sdl.c | |||
@@ -32,13 +32,13 @@ | |||
32 | #include "core_alloc.h" | 32 | #include "core_alloc.h" |
33 | 33 | ||
34 | /* Define this as 1 to show informational messages that are not errors. */ | 34 | /* Define this as 1 to show informational messages that are not errors. */ |
35 | #define THREAD_SDL_DEBUGF_ENABLED 0 | 35 | #define THREAD_SDL_DEBUGF_ENABLED 1 |
36 | 36 | ||
37 | #if THREAD_SDL_DEBUGF_ENABLED | 37 | #if THREAD_SDL_DEBUGF_ENABLED |
38 | #define THREAD_SDL_DEBUGF(...) DEBUGF(__VA_ARGS__) | 38 | #define THREAD_SDL_DEBUGF(...) DEBUGF(__VA_ARGS__) |
39 | static char __name[32]; | 39 | static char __name[sizeof (((struct thread_debug_info *)0)->name)]; |
40 | #define THREAD_SDL_GET_NAME(thread) \ | 40 | #define THREAD_SDL_GET_NAME(thread) \ |
41 | ({ thread_get_name(__name, ARRAYLEN(__name), thread); __name; }) | 41 | ({ format_thread_name(__name, sizeof (__name), thread); __name; }) |
42 | #else | 42 | #else |
43 | #define THREAD_SDL_DEBUGF(...) | 43 | #define THREAD_SDL_DEBUGF(...) |
44 | #define THREAD_SDL_GET_NAME(thread) | 44 | #define THREAD_SDL_GET_NAME(thread) |
@@ -47,9 +47,6 @@ static char __name[32]; | |||
47 | #define THREAD_PANICF(str...) \ | 47 | #define THREAD_PANICF(str...) \ |
48 | ({ fprintf(stderr, str); exit(-1); }) | 48 | ({ fprintf(stderr, str); exit(-1); }) |
49 | 49 | ||
50 | /* Thread/core entries as in rockbox core */ | ||
51 | static struct core_entry cores[NUM_CORES]; | ||
52 | struct thread_entry threads[MAXTHREADS]; | ||
53 | /* Jump buffers for graceful exit - kernel threads don't stay neatly | 50 | /* Jump buffers for graceful exit - kernel threads don't stay neatly |
54 | * in their start routines responding to messages so this is the only | 51 | * in their start routines responding to messages so this is the only |
55 | * way to get them back in there so they may exit */ | 52 | * way to get them back in there so they may exit */ |
@@ -74,7 +71,7 @@ void sim_thread_shutdown(void) | |||
74 | 71 | ||
75 | /* Tell all threads jump back to their start routines, unlock and exit | 72 | /* Tell all threads jump back to their start routines, unlock and exit |
76 | gracefully - we'll check each one in turn for it's status. Threads | 73 | gracefully - we'll check each one in turn for it's status. Threads |
77 | _could_ terminate via remove_thread or multiple threads could exit | 74 | _could_ terminate via thread_exit or multiple threads could exit |
78 | on each unlock but that is safe. */ | 75 | on each unlock but that is safe. */ |
79 | 76 | ||
80 | /* Do this before trying to acquire lock */ | 77 | /* Do this before trying to acquire lock */ |
@@ -86,7 +83,7 @@ void sim_thread_shutdown(void) | |||
86 | /* Signal all threads on delay or block */ | 83 | /* Signal all threads on delay or block */ |
87 | for (i = 0; i < MAXTHREADS; i++) | 84 | for (i = 0; i < MAXTHREADS; i++) |
88 | { | 85 | { |
89 | struct thread_entry *thread = &threads[i]; | 86 | struct thread_entry *thread = __thread_slot_entry(i); |
90 | if (thread->context.s == NULL) | 87 | if (thread->context.s == NULL) |
91 | continue; | 88 | continue; |
92 | SDL_SemPost(thread->context.s); | 89 | SDL_SemPost(thread->context.s); |
@@ -95,7 +92,7 @@ void sim_thread_shutdown(void) | |||
95 | /* Wait for all threads to finish and cleanup old ones. */ | 92 | /* Wait for all threads to finish and cleanup old ones. */ |
96 | for (i = 0; i < MAXTHREADS; i++) | 93 | for (i = 0; i < MAXTHREADS; i++) |
97 | { | 94 | { |
98 | struct thread_entry *thread = &threads[i]; | 95 | struct thread_entry *thread = __thread_slot_entry(i); |
99 | SDL_Thread *t = thread->context.t; | 96 | SDL_Thread *t = thread->context.t; |
100 | 97 | ||
101 | if (t != NULL) | 98 | if (t != NULL) |
@@ -111,11 +108,11 @@ void sim_thread_shutdown(void) | |||
111 | } | 108 | } |
112 | else | 109 | else |
113 | { | 110 | { |
114 | /* Wait on any previous thread in this location-- could be one not quite | 111 | /* Wait on any previous thread in this location-- could be one not |
115 | * finished exiting but has just unlocked the mutex. If it's NULL, the | 112 | * quite finished exiting but has just unlocked the mutex. If it's |
116 | * call returns immediately. | 113 | * NULL, the call returns immediately. |
117 | * | 114 | * |
118 | * See remove_thread below for more information. */ | 115 | * See thread_exit below for more information. */ |
119 | SDL_WaitThread(thread->context.told, NULL); | 116 | SDL_WaitThread(thread->context.told, NULL); |
120 | } | 117 | } |
121 | } | 118 | } |
@@ -126,103 +123,6 @@ void sim_thread_shutdown(void) | |||
126 | threads_status = THREADS_EXIT_COMMAND_DONE; | 123 | threads_status = THREADS_EXIT_COMMAND_DONE; |
127 | } | 124 | } |
128 | 125 | ||
129 | static void new_thread_id(unsigned int slot_num, | ||
130 | struct thread_entry *thread) | ||
131 | { | ||
132 | unsigned int version = | ||
133 | (thread->id + (1u << THREAD_ID_VERSION_SHIFT)) | ||
134 | & THREAD_ID_VERSION_MASK; | ||
135 | |||
136 | if (version == 0) | ||
137 | version = 1u << THREAD_ID_VERSION_SHIFT; | ||
138 | |||
139 | thread->id = version | (slot_num & THREAD_ID_SLOT_MASK); | ||
140 | } | ||
141 | |||
142 | static struct thread_entry * find_empty_thread_slot(void) | ||
143 | { | ||
144 | struct thread_entry *thread = NULL; | ||
145 | int n; | ||
146 | |||
147 | for (n = 0; n < MAXTHREADS; n++) | ||
148 | { | ||
149 | int state = threads[n].state; | ||
150 | |||
151 | if (state == STATE_KILLED) | ||
152 | { | ||
153 | thread = &threads[n]; | ||
154 | break; | ||
155 | } | ||
156 | } | ||
157 | |||
158 | return thread; | ||
159 | } | ||
160 | |||
161 | |||
162 | /* Initialize SDL threading */ | ||
163 | void init_threads(void) | ||
164 | { | ||
165 | static uintptr_t main_stack[] = { DEADBEEF, 0 }; | ||
166 | struct thread_entry *thread; | ||
167 | int n; | ||
168 | |||
169 | memset(cores, 0, sizeof(cores)); | ||
170 | memset(threads, 0, sizeof(threads)); | ||
171 | |||
172 | m = SDL_CreateMutex(); | ||
173 | |||
174 | if (SDL_LockMutex(m) == -1) | ||
175 | { | ||
176 | fprintf(stderr, "Couldn't lock mutex\n"); | ||
177 | return; | ||
178 | } | ||
179 | |||
180 | /* Initialize all IDs */ | ||
181 | for (n = 0; n < MAXTHREADS; n++) | ||
182 | threads[n].id = THREAD_ID_INIT(n); | ||
183 | |||
184 | /* Slot 0 is reserved for the main thread - initialize it here and | ||
185 | then create the SDL thread - it is possible to have a quick, early | ||
186 | shutdown try to access the structure. */ | ||
187 | thread = &threads[0]; | ||
188 | thread->stack = main_stack; | ||
189 | thread->stack_size = sizeof (main_stack); | ||
190 | thread->name = "main"; | ||
191 | thread->state = STATE_RUNNING; | ||
192 | thread->context.s = SDL_CreateSemaphore(0); | ||
193 | thread->context.t = NULL; /* NULL for the implicit main thread */ | ||
194 | cores[CURRENT_CORE].running = thread; | ||
195 | |||
196 | if (thread->context.s == NULL) | ||
197 | { | ||
198 | fprintf(stderr, "Failed to create main semaphore\n"); | ||
199 | return; | ||
200 | } | ||
201 | |||
202 | /* Tell all threads jump back to their start routines, unlock and exit | ||
203 | gracefully - we'll check each one in turn for it's status. Threads | ||
204 | _could_ terminate via remove_thread or multiple threads could exit | ||
205 | on each unlock but that is safe. */ | ||
206 | |||
207 | /* Setup jump for exit */ | ||
208 | if (setjmp(thread_jmpbufs[0]) == 0) | ||
209 | { | ||
210 | THREAD_SDL_DEBUGF("Main thread: %p\n", thread); | ||
211 | return; | ||
212 | } | ||
213 | |||
214 | SDL_UnlockMutex(m); | ||
215 | |||
216 | /* Set to 'COMMAND_DONE' when other rockbox threads have exited. */ | ||
217 | while (threads_status < THREADS_EXIT_COMMAND_DONE) | ||
218 | SDL_Delay(10); | ||
219 | |||
220 | SDL_DestroyMutex(m); | ||
221 | |||
222 | /* We're the main thead - perform exit - doesn't return. */ | ||
223 | sim_do_exit(); | ||
224 | } | ||
225 | |||
226 | void sim_thread_exception_wait(void) | 126 | void sim_thread_exception_wait(void) |
227 | { | 127 | { |
228 | while (1) | 128 | while (1) |
@@ -237,7 +137,7 @@ void sim_thread_exception_wait(void) | |||
237 | void sim_thread_lock(void *me) | 137 | void sim_thread_lock(void *me) |
238 | { | 138 | { |
239 | SDL_LockMutex(m); | 139 | SDL_LockMutex(m); |
240 | cores[CURRENT_CORE].running = (struct thread_entry *)me; | 140 | __running_self_entry() = (struct thread_entry *)me; |
241 | 141 | ||
242 | if (threads_status != THREADS_RUN) | 142 | if (threads_status != THREADS_RUN) |
243 | thread_exit(); | 143 | thread_exit(); |
@@ -245,70 +145,14 @@ void sim_thread_lock(void *me) | |||
245 | 145 | ||
246 | void * sim_thread_unlock(void) | 146 | void * sim_thread_unlock(void) |
247 | { | 147 | { |
248 | struct thread_entry *current = cores[CURRENT_CORE].running; | 148 | struct thread_entry *current = __running_self_entry(); |
249 | SDL_UnlockMutex(m); | 149 | SDL_UnlockMutex(m); |
250 | return current; | 150 | return current; |
251 | } | 151 | } |
252 | 152 | ||
253 | struct thread_entry * thread_id_entry(unsigned int thread_id) | ||
254 | { | ||
255 | return &threads[thread_id & THREAD_ID_SLOT_MASK]; | ||
256 | } | ||
257 | |||
258 | static void add_to_list_l(struct thread_entry **list, | ||
259 | struct thread_entry *thread) | ||
260 | { | ||
261 | if (*list == NULL) | ||
262 | { | ||
263 | /* Insert into unoccupied list */ | ||
264 | thread->l.next = thread; | ||
265 | thread->l.prev = thread; | ||
266 | *list = thread; | ||
267 | } | ||
268 | else | ||
269 | { | ||
270 | /* Insert last */ | ||
271 | thread->l.next = *list; | ||
272 | thread->l.prev = (*list)->l.prev; | ||
273 | thread->l.prev->l.next = thread; | ||
274 | (*list)->l.prev = thread; | ||
275 | } | ||
276 | } | ||
277 | |||
278 | static void remove_from_list_l(struct thread_entry **list, | ||
279 | struct thread_entry *thread) | ||
280 | { | ||
281 | if (thread == thread->l.next) | ||
282 | { | ||
283 | /* The only item */ | ||
284 | *list = NULL; | ||
285 | return; | ||
286 | } | ||
287 | |||
288 | if (thread == *list) | ||
289 | { | ||
290 | /* List becomes next item */ | ||
291 | *list = thread->l.next; | ||
292 | } | ||
293 | |||
294 | /* Fix links to jump over the removed entry. */ | ||
295 | thread->l.prev->l.next = thread->l.next; | ||
296 | thread->l.next->l.prev = thread->l.prev; | ||
297 | } | ||
298 | |||
299 | unsigned int thread_self(void) | ||
300 | { | ||
301 | return cores[CURRENT_CORE].running->id; | ||
302 | } | ||
303 | |||
304 | struct thread_entry* thread_self_entry(void) | ||
305 | { | ||
306 | return cores[CURRENT_CORE].running; | ||
307 | } | ||
308 | |||
309 | void switch_thread(void) | 153 | void switch_thread(void) |
310 | { | 154 | { |
311 | struct thread_entry *current = cores[CURRENT_CORE].running; | 155 | struct thread_entry *current = __running_self_entry(); |
312 | 156 | ||
313 | enable_irq(); | 157 | enable_irq(); |
314 | 158 | ||
@@ -346,17 +190,7 @@ void switch_thread(void) | |||
346 | 190 | ||
347 | oldlevel = disable_irq_save(); | 191 | oldlevel = disable_irq_save(); |
348 | 192 | ||
349 | if (current->state == STATE_BLOCKED_W_TMO) | 193 | current->state = STATE_RUNNING; |
350 | { | ||
351 | /* Timed out */ | ||
352 | remove_from_list_l(current->bqp, current); | ||
353 | |||
354 | #ifdef HAVE_WAKEUP_EXT_CB | ||
355 | if (current->wakeup_ext_cb != NULL) | ||
356 | current->wakeup_ext_cb(current); | ||
357 | #endif | ||
358 | current->state = STATE_RUNNING; | ||
359 | } | ||
360 | 194 | ||
361 | if (result == SDL_MUTEX_TIMEDOUT) | 195 | if (result == SDL_MUTEX_TIMEDOUT) |
362 | { | 196 | { |
@@ -384,7 +218,7 @@ void switch_thread(void) | |||
384 | #ifdef DEBUG | 218 | #ifdef DEBUG |
385 | core_check_valid(); | 219 | core_check_valid(); |
386 | #endif | 220 | #endif |
387 | cores[CURRENT_CORE].running = current; | 221 | __running_self_entry() = current; |
388 | 222 | ||
389 | if (threads_status != THREADS_RUN) | 223 | if (threads_status != THREADS_RUN) |
390 | thread_exit(); | 224 | thread_exit(); |
@@ -392,7 +226,7 @@ void switch_thread(void) | |||
392 | 226 | ||
393 | void sleep_thread(int ticks) | 227 | void sleep_thread(int ticks) |
394 | { | 228 | { |
395 | struct thread_entry *current = cores[CURRENT_CORE].running; | 229 | struct thread_entry *current = __running_self_entry(); |
396 | int rem; | 230 | int rem; |
397 | 231 | ||
398 | current->state = STATE_SLEEPING; | 232 | current->state = STATE_SLEEPING; |
@@ -404,7 +238,7 @@ void sleep_thread(int ticks) | |||
404 | current->tmo_tick = (1000/HZ) * ticks + ((1000/HZ)-1) - rem; | 238 | current->tmo_tick = (1000/HZ) * ticks + ((1000/HZ)-1) - rem; |
405 | } | 239 | } |
406 | 240 | ||
407 | void block_thread(struct thread_entry *current, int ticks) | 241 | void block_thread_(struct thread_entry *current, int ticks) |
408 | { | 242 | { |
409 | if (ticks < 0) | 243 | if (ticks < 0) |
410 | current->state = STATE_BLOCKED; | 244 | current->state = STATE_BLOCKED; |
@@ -414,24 +248,19 @@ void block_thread(struct thread_entry *current, int ticks) | |||
414 | current->tmo_tick = (1000/HZ)*ticks; | 248 | current->tmo_tick = (1000/HZ)*ticks; |
415 | } | 249 | } |
416 | 250 | ||
417 | add_to_list_l(current->bqp, current); | 251 | wait_queue_register(current); |
418 | } | 252 | } |
419 | 253 | ||
420 | unsigned int wakeup_thread_(struct thread_entry **list) | 254 | unsigned int wakeup_thread_(struct thread_entry *thread) |
421 | { | 255 | { |
422 | struct thread_entry *thread = *list; | 256 | switch (thread->state) |
423 | |||
424 | if (thread != NULL) | ||
425 | { | 257 | { |
426 | switch (thread->state) | 258 | case STATE_BLOCKED: |
427 | { | 259 | case STATE_BLOCKED_W_TMO: |
428 | case STATE_BLOCKED: | 260 | wait_queue_remove(thread); |
429 | case STATE_BLOCKED_W_TMO: | 261 | thread->state = STATE_RUNNING; |
430 | remove_from_list_l(list, thread); | 262 | SDL_SemPost(thread->context.s); |
431 | thread->state = STATE_RUNNING; | 263 | return THREAD_OK; |
432 | SDL_SemPost(thread->context.s); | ||
433 | return THREAD_OK; | ||
434 | } | ||
435 | } | 264 | } |
436 | 265 | ||
437 | return THREAD_NONE; | 266 | return THREAD_NONE; |
@@ -439,7 +268,7 @@ unsigned int wakeup_thread_(struct thread_entry **list) | |||
439 | 268 | ||
440 | void thread_thaw(unsigned int thread_id) | 269 | void thread_thaw(unsigned int thread_id) |
441 | { | 270 | { |
442 | struct thread_entry *thread = thread_id_entry(thread_id); | 271 | struct thread_entry *thread = __thread_id_entry(thread_id); |
443 | 272 | ||
444 | if (thread->id == thread_id && thread->state == STATE_FROZEN) | 273 | if (thread->id == thread_id && thread->state == STATE_FROZEN) |
445 | { | 274 | { |
@@ -450,15 +279,14 @@ void thread_thaw(unsigned int thread_id) | |||
450 | 279 | ||
451 | int runthread(void *data) | 280 | int runthread(void *data) |
452 | { | 281 | { |
453 | struct thread_entry *current; | ||
454 | jmp_buf *current_jmpbuf; | ||
455 | |||
456 | /* Cannot access thread variables before locking the mutex as the | 282 | /* Cannot access thread variables before locking the mutex as the |
457 | data structures may not be filled-in yet. */ | 283 | data structures may not be filled-in yet. */ |
458 | SDL_LockMutex(m); | 284 | SDL_LockMutex(m); |
459 | cores[CURRENT_CORE].running = (struct thread_entry *)data; | 285 | |
460 | current = cores[CURRENT_CORE].running; | 286 | struct thread_entry *current = (struct thread_entry *)data; |
461 | current_jmpbuf = &thread_jmpbufs[current - threads]; | 287 | __running_self_entry() = current; |
288 | |||
289 | jmp_buf *current_jmpbuf = &thread_jmpbufs[THREAD_ID_SLOT(current->id)]; | ||
462 | 290 | ||
463 | /* Setup jump for exit */ | 291 | /* Setup jump for exit */ |
464 | if (setjmp(*current_jmpbuf) == 0) | 292 | if (setjmp(*current_jmpbuf) == 0) |
@@ -469,14 +297,15 @@ int runthread(void *data) | |||
469 | SDL_UnlockMutex(m); | 297 | SDL_UnlockMutex(m); |
470 | SDL_SemWait(current->context.s); | 298 | SDL_SemWait(current->context.s); |
471 | SDL_LockMutex(m); | 299 | SDL_LockMutex(m); |
472 | cores[CURRENT_CORE].running = current; | 300 | __running_self_entry() = current; |
473 | } | 301 | } |
474 | 302 | ||
475 | if (threads_status == THREADS_RUN) | 303 | if (threads_status == THREADS_RUN) |
476 | { | 304 | { |
477 | current->context.start(); | 305 | current->context.start(); |
478 | THREAD_SDL_DEBUGF("Thread Done: %d (%s)\n", | 306 | THREAD_SDL_DEBUGF("Thread Done: %d (%s)\n", |
479 | current - threads, THREAD_SDL_GET_NAME(current)); | 307 | THREAD_ID_SLOT(current->id), |
308 | THREAD_SDL_GET_NAME(current)); | ||
480 | /* Thread routine returned - suicide */ | 309 | /* Thread routine returned - suicide */ |
481 | } | 310 | } |
482 | 311 | ||
@@ -495,27 +324,23 @@ unsigned int create_thread(void (*function)(void), | |||
495 | void* stack, size_t stack_size, | 324 | void* stack, size_t stack_size, |
496 | unsigned flags, const char *name) | 325 | unsigned flags, const char *name) |
497 | { | 326 | { |
498 | struct thread_entry *thread; | ||
499 | SDL_Thread* t; | ||
500 | SDL_sem *s; | ||
501 | |||
502 | THREAD_SDL_DEBUGF("Creating thread: (%s)\n", name ? name : ""); | 327 | THREAD_SDL_DEBUGF("Creating thread: (%s)\n", name ? name : ""); |
503 | 328 | ||
504 | thread = find_empty_thread_slot(); | 329 | struct thread_entry *thread = thread_alloc(); |
505 | if (thread == NULL) | 330 | if (thread == NULL) |
506 | { | 331 | { |
507 | DEBUGF("Failed to find thread slot\n"); | 332 | DEBUGF("Failed to find thread slot\n"); |
508 | return 0; | 333 | return 0; |
509 | } | 334 | } |
510 | 335 | ||
511 | s = SDL_CreateSemaphore(0); | 336 | SDL_sem *s = SDL_CreateSemaphore(0); |
512 | if (s == NULL) | 337 | if (s == NULL) |
513 | { | 338 | { |
514 | DEBUGF("Failed to create semaphore\n"); | 339 | DEBUGF("Failed to create semaphore\n"); |
515 | return 0; | 340 | return 0; |
516 | } | 341 | } |
517 | 342 | ||
518 | t = SDL_CreateThread(runthread, thread); | 343 | SDL_Thread *t = SDL_CreateThread(runthread, thread); |
519 | if (t == NULL) | 344 | if (t == NULL) |
520 | { | 345 | { |
521 | DEBUGF("Failed to create SDL thread\n"); | 346 | DEBUGF("Failed to create SDL thread\n"); |
@@ -523,12 +348,6 @@ unsigned int create_thread(void (*function)(void), | |||
523 | return 0; | 348 | return 0; |
524 | } | 349 | } |
525 | 350 | ||
526 | unsigned int stack_words = stack_size / sizeof (uintptr_t); | ||
527 | for (unsigned int i = stack_words; i-- > 0;) | ||
528 | ((uintptr_t *)stack)[i] = DEADBEEF; | ||
529 | |||
530 | thread->stack = stack; | ||
531 | thread->stack_size = stack_size; | ||
532 | thread->name = name; | 351 | thread->name = name; |
533 | thread->state = (flags & CREATE_THREAD_FROZEN) ? | 352 | thread->state = (flags & CREATE_THREAD_FROZEN) ? |
534 | STATE_FROZEN : STATE_RUNNING; | 353 | STATE_FROZEN : STATE_RUNNING; |
@@ -536,27 +355,22 @@ unsigned int create_thread(void (*function)(void), | |||
536 | thread->context.t = t; | 355 | thread->context.t = t; |
537 | thread->context.s = s; | 356 | thread->context.s = s; |
538 | 357 | ||
539 | THREAD_SDL_DEBUGF("New Thread: %d (%s)\n", | 358 | THREAD_SDL_DEBUGF("New Thread: %lu (%s)\n", |
540 | thread - threads, THREAD_SDL_GET_NAME(thread)); | 359 | (unsigned long)thread->id, |
360 | THREAD_SDL_GET_NAME(thread)); | ||
541 | 361 | ||
542 | return thread->id; | 362 | return thread->id; |
363 | (void)stack; (void)stack_size; | ||
543 | } | 364 | } |
544 | 365 | ||
545 | static void remove_thread(unsigned int thread_id) | 366 | void thread_exit(void) |
546 | { | 367 | { |
547 | struct thread_entry *current = cores[CURRENT_CORE].running; | 368 | struct thread_entry *current = __running_self_entry(); |
548 | struct thread_entry *thread = thread_id_entry(thread_id); | ||
549 | |||
550 | SDL_Thread *t; | ||
551 | SDL_sem *s; | ||
552 | |||
553 | if (thread->id != thread_id) | ||
554 | return; | ||
555 | 369 | ||
556 | int oldlevel = disable_irq_save(); | 370 | int oldlevel = disable_irq_save(); |
557 | 371 | ||
558 | t = thread->context.t; | 372 | SDL_Thread *t = current->context.t; |
559 | s = thread->context.s; | 373 | SDL_sem *s = current->context.s; |
560 | 374 | ||
561 | /* Wait the last thread here and keep this one or SDL will leak it since | 375 | /* Wait the last thread here and keep this one or SDL will leak it since |
562 | * it doesn't free its own library allocations unless a wait is performed. | 376 | * it doesn't free its own library allocations unless a wait is performed. |
@@ -566,59 +380,27 @@ static void remove_thread(unsigned int thread_id) | |||
566 | * | 380 | * |
567 | * However, see more below about SDL_KillThread. | 381 | * However, see more below about SDL_KillThread. |
568 | */ | 382 | */ |
569 | SDL_WaitThread(thread->context.told, NULL); | 383 | SDL_WaitThread(current->context.told, NULL); |
570 | 384 | ||
571 | thread->context.t = NULL; | 385 | current->context.t = NULL; |
572 | thread->context.s = NULL; | 386 | current->context.s = NULL; |
573 | thread->context.told = t; | 387 | current->context.told = t; |
574 | 388 | ||
575 | if (thread != current) | 389 | unsigned int id = current->id; |
576 | { | 390 | new_thread_id(current); |
577 | switch (thread->state) | 391 | current->state = STATE_KILLED; |
578 | { | 392 | wait_queue_wake(¤t->queue); |
579 | case STATE_BLOCKED: | ||
580 | case STATE_BLOCKED_W_TMO: | ||
581 | /* Remove thread from object it's waiting on */ | ||
582 | remove_from_list_l(thread->bqp, thread); | ||
583 | |||
584 | #ifdef HAVE_WAKEUP_EXT_CB | ||
585 | if (thread->wakeup_ext_cb != NULL) | ||
586 | thread->wakeup_ext_cb(thread); | ||
587 | #endif | ||
588 | break; | ||
589 | } | ||
590 | |||
591 | SDL_SemPost(s); | ||
592 | } | ||
593 | |||
594 | THREAD_SDL_DEBUGF("Removing thread: %d (%s)\n", | ||
595 | thread - threads, THREAD_SDL_GET_NAME(thread)); | ||
596 | |||
597 | new_thread_id(thread->id, thread); | ||
598 | thread->state = STATE_KILLED; | ||
599 | thread_queue_wake(&thread->queue); | ||
600 | 393 | ||
601 | SDL_DestroySemaphore(s); | 394 | SDL_DestroySemaphore(s); |
602 | 395 | ||
603 | if (thread == current) | 396 | /* Do a graceful exit - perform the longjmp back into the thread |
604 | { | 397 | function to return */ |
605 | /* Do a graceful exit - perform the longjmp back into the thread | ||
606 | function to return */ | ||
607 | restore_irq(oldlevel); | ||
608 | longjmp(thread_jmpbufs[current - threads], 1); | ||
609 | } | ||
610 | |||
611 | /* SDL_KillThread frees the old pointer too because it uses SDL_WaitThread | ||
612 | * to wait for the host to remove it. */ | ||
613 | thread->context.told = NULL; | ||
614 | SDL_KillThread(t); | ||
615 | restore_irq(oldlevel); | 398 | restore_irq(oldlevel); |
616 | } | ||
617 | 399 | ||
618 | void thread_exit(void) | 400 | thread_free(current); |
619 | { | 401 | |
620 | unsigned int id = thread_self(); | 402 | longjmp(thread_jmpbufs[THREAD_ID_SLOT(id)], 1); |
621 | remove_thread(id); | 403 | |
622 | /* This should never and must never be reached - if it is, the | 404 | /* This should never and must never be reached - if it is, the |
623 | * state is corrupted */ | 405 | * state is corrupted */ |
624 | THREAD_PANICF("thread_exit->K:*R (ID: %d)", id); | 406 | THREAD_PANICF("thread_exit->K:*R (ID: %d)", id); |
@@ -627,44 +409,73 @@ void thread_exit(void) | |||
627 | 409 | ||
628 | void thread_wait(unsigned int thread_id) | 410 | void thread_wait(unsigned int thread_id) |
629 | { | 411 | { |
630 | struct thread_entry *current = cores[CURRENT_CORE].running; | 412 | struct thread_entry *current = __running_self_entry(); |
631 | struct thread_entry *thread = thread_id_entry(thread_id); | 413 | struct thread_entry *thread = __thread_id_entry(thread_id); |
632 | 414 | ||
633 | if (thread->id == thread_id && thread->state != STATE_KILLED) | 415 | if (thread->id == thread_id && thread->state != STATE_KILLED) |
634 | { | 416 | { |
635 | current->bqp = &thread->queue; | 417 | block_thread(current, TIMEOUT_BLOCK, &thread->queue); |
636 | block_thread(current, TIMEOUT_BLOCK); | ||
637 | switch_thread(); | 418 | switch_thread(); |
638 | } | 419 | } |
639 | } | 420 | } |
640 | 421 | ||
641 | /*--------------------------------------------------------------------------- | 422 | /* Initialize SDL threading */ |
642 | * Suspends a thread's execution for at least the specified number of ticks. | 423 | void init_threads(void) |
643 | * | ||
644 | * May result in CPU core entering wait-for-interrupt mode if no other thread | ||
645 | * may be scheduled. | ||
646 | * | ||
647 | * NOTE: sleep(0) sleeps until the end of the current tick | ||
648 | * sleep(n) that doesn't result in rescheduling: | ||
649 | * n <= ticks suspended < n + 1 | ||
650 | * n to n+1 is a lower bound. Other factors may affect the actual time | ||
651 | * a thread is suspended before it runs again. | ||
652 | *--------------------------------------------------------------------------- | ||
653 | */ | ||
654 | unsigned sleep(unsigned ticks) | ||
655 | { | 424 | { |
656 | disable_irq(); | 425 | m = SDL_CreateMutex(); |
657 | sleep_thread(ticks); | ||
658 | switch_thread(); | ||
659 | return 0; | ||
660 | } | ||
661 | 426 | ||
662 | /*--------------------------------------------------------------------------- | 427 | if (SDL_LockMutex(m) == -1) |
663 | * Elects another thread to run or, if no other thread may be made ready to | 428 | { |
664 | * run, immediately returns control back to the calling thread. | 429 | fprintf(stderr, "Couldn't lock mutex\n"); |
665 | *--------------------------------------------------------------------------- | 430 | return; |
666 | */ | 431 | } |
667 | void yield(void) | 432 | |
668 | { | 433 | thread_alloc_init(); |
669 | switch_thread(); | 434 | |
435 | struct thread_entry *thread = thread_alloc(); | ||
436 | if (thread == NULL) | ||
437 | { | ||
438 | fprintf(stderr, "Main thread alloc failed\n"); | ||
439 | return; | ||
440 | } | ||
441 | |||
442 | /* Slot 0 is reserved for the main thread - initialize it here and | ||
443 | then create the SDL thread - it is possible to have a quick, early | ||
444 | shutdown try to access the structure. */ | ||
445 | thread->name = __main_thread_name; | ||
446 | thread->state = STATE_RUNNING; | ||
447 | thread->context.s = SDL_CreateSemaphore(0); | ||
448 | thread->context.t = NULL; /* NULL for the implicit main thread */ | ||
449 | __running_self_entry() = thread; | ||
450 | |||
451 | if (thread->context.s == NULL) | ||
452 | { | ||
453 | fprintf(stderr, "Failed to create main semaphore\n"); | ||
454 | return; | ||
455 | } | ||
456 | |||
457 | /* Tell all threads jump back to their start routines, unlock and exit | ||
458 | gracefully - we'll check each one in turn for it's status. Threads | ||
459 | _could_ terminate via thread_exit or multiple threads could exit | ||
460 | on each unlock but that is safe. */ | ||
461 | |||
462 | /* Setup jump for exit */ | ||
463 | if (setjmp(thread_jmpbufs[THREAD_ID_SLOT(thread->id)]) == 0) | ||
464 | { | ||
465 | THREAD_SDL_DEBUGF("Main Thread: %lu (%s)\n", | ||
466 | (unsigned long)thread->id, | ||
467 | THREAD_SDL_GET_NAME(thread)); | ||
468 | return; | ||
469 | } | ||
470 | |||
471 | SDL_UnlockMutex(m); | ||
472 | |||
473 | /* Set to 'COMMAND_DONE' when other rockbox threads have exited. */ | ||
474 | while (threads_status < THREADS_EXIT_COMMAND_DONE) | ||
475 | SDL_Delay(10); | ||
476 | |||
477 | SDL_DestroyMutex(m); | ||
478 | |||
479 | /* We're the main thead - perform exit - doesn't return. */ | ||
480 | sim_do_exit(); | ||
670 | } | 481 | } |