summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorMichael Sevakis <jethead71@rockbox.org>2014-08-08 06:33:51 -0400
committerMichael Sevakis <jethead71@rockbox.org>2014-08-16 05:15:37 -0400
commit6ed00870abd566d7267d2436c2693f5a281cda2f (patch)
tree6011c73e302254fc73f61a1b8b1f295ded1f5d56
parenteb63d8b4a2a7cbe4e98216b48a75391718fcebd7 (diff)
downloadrockbox-6ed00870abd566d7267d2436c2693f5a281cda2f.tar.gz
rockbox-6ed00870abd566d7267d2436c2693f5a281cda2f.zip
Base scheduler queues off linked lists and do cleanup/consolidation
Abstracts threading from itself a bit, changes the way its queues are handled and does type hiding for that as well. Do alot here due to already required major brain surgery. Threads may now be on a run queue and a wait queue simultaneously so that the expired timer only has to wake the thread but not remove it from the wait queue which simplifies the implicit wake handling. List formats change for wait queues-- doubly-linked, not circular. Timeout queue is now singly-linked. The run queue is still circular as before. Adds a better thread slot allocator that may keep the slot marked as used regardless of the thread state. Assists in dumping special tasks that switch_thread was tasked to perform (blocking tasks). Deletes alot of code yet surprisingly, gets larger than expected. Well, I'm not not minding that for the time being-- omlettes and break a few eggs and all that. Change-Id: I0834d7bb16b2aecb2f63b58886eeda6ae4f29d59
Diffstat
-rw-r--r--apps/debug_menu.c31
-rw-r--r--firmware/asm/m68k/thread.c5
-rw-r--r--firmware/export/system.h38
-rw-r--r--firmware/kernel/include/mrsw_lock.h7
-rw-r--r--firmware/kernel/include/mutex.h12
-rw-r--r--firmware/kernel/include/queue.h4
-rw-r--r--firmware/kernel/include/semaphore.h8
-rw-r--r--firmware/kernel/include/thread.h18
-rw-r--r--firmware/kernel/mrsw_lock.c103
-rw-r--r--firmware/kernel/mutex.c28
-rw-r--r--firmware/kernel/pthread/thread.c81
-rw-r--r--firmware/kernel/queue.c145
-rw-r--r--firmware/kernel/semaphore.c59
-rw-r--r--firmware/kernel/thread-common.c247
-rw-r--r--firmware/kernel/thread-internal.h407
-rw-r--r--firmware/kernel/thread.c1837
-rw-r--r--firmware/libc/errno.c2
-rw-r--r--firmware/target/arm/pp/app-pp.lds1
-rw-r--r--firmware/target/arm/pp/thread-pp.c95
-rw-r--r--firmware/target/hosted/sdl/thread-sdl.c429
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 */
40struct mrsw_lock 40struct 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
27struct mutex 27struct 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
109struct event_queue 109struct 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
27struct semaphore 27struct 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
35extern void semaphore_init(struct semaphore *s, int max, int start); 35extern 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 */
112struct blocker 117struct blocker
113{ 118{
@@ -168,6 +173,7 @@ int thread_get_priority(unsigned int thread_id);
168void thread_set_io_priority(unsigned int thread_id, int io_priority); 173void thread_set_io_priority(unsigned int thread_id, int io_priority);
169int thread_get_io_priority(unsigned int thread_id); 174int 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
172unsigned int switch_core(unsigned int new_core); 178unsigned 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
189struct thread_debug_info 203struct 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
35static FORCE_INLINE void 36static FORCE_INLINE void
36mrsw_reader_relinquish(struct mrsw_lock *mrsw, struct thread_entry *current, 37mrsw_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
74static FORCE_INLINE unsigned int 76static FORCE_INLINE unsigned int
75mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw, unsigned int slotnum) 77mrsw_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
81static FORCE_INLINE unsigned int 84static FORCE_INLINE unsigned int
82mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw) 85mrsw_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
87static FORCE_INLINE unsigned int 91static FORCE_INLINE unsigned int
88mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) 92mrsw_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
103static FORCE_INLINE unsigned int 107static FORCE_INLINE unsigned int
104mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw) 108mrsw_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
110static FORCE_INLINE unsigned int 114static FORCE_INLINE unsigned int
111mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw) 115mrsw_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
117static FORCE_INLINE unsigned int 121static FORCE_INLINE unsigned int
118mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) 122mrsw_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)
138void mrsw_init(struct mrsw_lock *mrsw) 146void 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. */
155void mrsw_read_acquire(struct mrsw_lock *mrsw) 160void 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. */
208void mrsw_read_release(struct mrsw_lock *mrsw) 210void 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. */
266void mrsw_write_acquire(struct mrsw_lock *mrsw) 269void 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 */
306void mrsw_write_release(struct mrsw_lock *mrsw) 306void 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 */
31void mutex_init(struct mutex *m) 31void 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 */
44void mutex_lock(struct mutex *m) 43void 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)
82void mutex_unlock(struct mutex *m) 77void 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
10static inline void timespec_add_ns(struct timespec *a, uint64_t ns) 10static 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
28struct thread_entry* thread_self_entry(void)
29{
30 return _current;
31}
32
33unsigned int thread_self(void) 28unsigned 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
156static 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
176static 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 */
199void block_thread_switch(struct thread_entry *t, struct corelock *cl) 151void 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
207void block_thread_switch_w_tmo(struct thread_entry *t, int timeout, struct corelock *cl) 160void 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
230unsigned int wakeup_thread(struct thread_entry **list) 182unsigned 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 */
68static void queue_release_sender(struct thread_entry * volatile * sender, 68static 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); 77static 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 */
108static 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 */
168static inline void queue_do_auto_reply(struct queue_sender_list *send) 147static 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
183static void queue_wake_waiter_inner(struct thread_entry *thread)
184{
185 wakeup_thread(thread, WAKEUP_DEFAULT);
186}
187
188static 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 */
209void queue_init(struct event_queue *q, bool register_queue) 196void 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. */
43int semaphore_wait(struct semaphore *s, int timeout) 44int 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)
93void semaphore_release(struct semaphore *s) 99void 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
32const 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
38static struct core_entry __core_entries[NUM_CORES] IBSS_ATTR;
39struct core_entry *__cores[NUM_CORES] IBSS_ATTR;
40#else
41struct core_entry __cores[NUM_CORES] IBSS_ATTR;
42#endif
43
44static struct thread_entry __thread_entries[MAXTHREADS] IBSS_ATTR;
45struct 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 */
32unsigned int thread_queue_wake(struct thread_entry **list) 55static 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 */
67void 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 */
100struct 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 */
122void 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 */
133void 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 */
149unsigned 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
164void 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 */
174unsigned 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 */
192unsigned 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 */
210void 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
223void 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 */
56static unsigned int stack_usage(uintptr_t *stackptr, size_t stack_size) 239static 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 */
79int core_get_debug_info(unsigned int core, struct core_debug_info *infop) 258int 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 */
84enum
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
91struct 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 */
99struct thread_entry; /* forward */
100struct 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 */
110struct thread_entry 91struct 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) 175static 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 */ 189void format_thread_name(char *buf, size_t bufsize,
190 const struct thread_entry *thread);
191
192static 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
207static 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
245void thread_alloc_init(void) INIT_ATTR;
246struct thread_entry * thread_alloc(void);
247void thread_free(struct thread_entry *thread);
248void new_thread_id(struct thread_entry *thread);
233 249
234/* Switch to next runnable thread */ 250/* Switch to next runnable thread */
235void switch_thread(void); 251void switch_thread(void);
@@ -237,7 +253,21 @@ void switch_thread(void);
237 * next tick) */ 253 * next tick) */
238void sleep_thread(int ticks); 254void 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) */
240void block_thread(struct thread_entry *current, int timeout); 256void 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. */
249unsigned int thread_queue_wake(struct thread_entry **list); 279unsigned 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 */
252enum wakeup_thread_protocol 282enum wakeup_thread_protocol
@@ -257,36 +287,139 @@ enum wakeup_thread_protocol
257 WAKEUP_TRANSFER_MULTI, 287 WAKEUP_TRANSFER_MULTI,
258}; 288};
259 289
260unsigned int wakeup_thread_(struct thread_entry **list 290unsigned 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
272void thread_set_io_priority(unsigned int thread_id, int io_priority); 302void profile_thread(void);
273int thread_get_io_priority(unsigned int thread_id);
274#endif /* HAVE_IO_PRIORITY */
275#if NUM_CORES > 1
276unsigned int switch_core(unsigned int new_core);
277#endif 303#endif
278 304
279/* Return the id of the calling thread. */ 305static inline void rtr_queue_init(struct __rtr_queue *rtrp)
280unsigned int thread_self(void); 306{
307 lldc_init(rtrp);
308}
309
310static 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 */ 316static inline void rtr_queue_add(struct __rtr_queue *rtrp,
283struct 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 */ 322static inline void rtr_queue_remove(struct __rtr_queue *rtrp,
286struct 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)
289void profile_thread(void); 329
330static inline bool tmo_is_queued(struct thread_entry *thread)
331{
332 return thread->tmo.next != TMO_NOT_QUEUED;
333}
334
335static inline void tmo_set_dequeued(struct thread_entry *thread)
336{
337 thread->tmo.next = TMO_NOT_QUEUED;
338}
339
340static inline void tmo_queue_init(struct __tmo_queue *tmop)
341{
342 ll_init(tmop);
343}
344
345static 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
353static 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
363static 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
370static inline void wait_queue_init(struct __wait_queue *wqp)
371{
372 lld_init(wqp);
373}
374
375static inline void wait_queue_register(struct thread_entry *thread)
376{
377 lld_insert_last(thread->wqp, &thread->wq);
378}
379
380static inline struct __wait_queue *
381 wait_queue_ptr(struct thread_entry *thread)
382{
383 return thread->wqp;
384}
385
386static 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
395static 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
408static 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
416static 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 171static 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 172static FORCE_INLINE void store_context(void* addr);
127 * (someday :). */ 173static FORCE_INLINE void load_context(const void* addr);
128static struct core_entry cores[NUM_CORES] IBSS_ATTR;
129struct thread_entry threads[MAXTHREADS] IBSS_ATTR;
130
131static const char main_thread_name[] = "main";
132#if (CONFIG_PLATFORM & PLATFORM_NATIVE)
133extern uintptr_t stackbegin[];
134extern uintptr_t stackend[];
135#else
136extern uintptr_t *stackbegin;
137extern uintptr_t *stackend;
138#endif
139
140static inline void core_sleep(IF_COP_VOID(unsigned int core))
141 __attribute__((always_inline));
142
143void check_tmo_threads(void)
144 __attribute__((noinline));
145
146static inline void block_thread_on_l(struct thread_entry *thread, unsigned state)
147 __attribute__((always_inline));
148
149static void add_to_list_tmo(struct thread_entry *thread)
150 __attribute__((noinline));
151
152static void core_schedule_wakeup(struct thread_entry *thread)
153 __attribute__((noinline));
154
155#if NUM_CORES > 1
156static inline void run_blocking_ops(
157 unsigned int core, struct thread_entry *thread)
158 __attribute__((always_inline));
159#endif
160
161static void thread_stkov(struct thread_entry *thread)
162 __attribute__((noinline));
163
164static inline void store_context(void* addr)
165 __attribute__((always_inline));
166
167static inline void load_context(const void* addr)
168 __attribute__((always_inline));
169
170#if NUM_CORES > 1
171static void thread_final_exit_do(struct thread_entry *current)
172 __attribute__((noinline)) NORETURN_ATTR USED_ATTR;
173#else
174static inline void thread_final_exit(struct thread_entry *current)
175 __attribute__((always_inline)) NORETURN_ATTR;
176#endif
177
178void 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
196static NO_INLINE 189static 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
209static void thread_stkov(struct thread_entry *thread) 199static 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
250static inline void thread_store_context(struct thread_entry *thread) 255static 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
258static inline void thread_load_context(struct thread_entry *thread) 263static 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
266static inline unsigned int should_switch_tasks(void) 271static FORCE_INLINE unsigned int
272should_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
292static inline struct thread_entry * lock_blocker_thread(struct blocker *bl)
293{
294 return bl->thread;
295}
296#else /* NUM_CORES > 1 */
297static 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
331static 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 */
361static 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 */
386static 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 */
429static 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 */
457static 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 */
578static void core_schedule_wakeup(struct thread_entry *thread) 343static 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 */
380static 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 */
397static 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 */
411static 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
429static inline struct thread_entry * lock_blocker_thread(struct blocker *bl)
430{
431 return bl->thread;
432}
433#else /* NUM_CORES > 1 */
434static 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
468static 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 */
602static inline void set_running_thread_priority( 484static 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 */
622static int find_highest_priority_in_list_l( 505static 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 */
741static void priority_disinherit_internal(struct thread_entry *thread, 615static 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
622static 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 */
633static 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
645static 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
754void priority_disinherit(struct thread_entry *thread, struct blocker *bl) 652void 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 */
976unsigned int wakeup_thread_(struct thread_entry **list 851unsigned 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 */
1040void check_tmo_threads(void) 906static 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
1153static 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 966static FORCE_INLINE void check_tmo_expired(struct core_entry *corep)
1177void 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 */
1189static inline void block_thread_on_l(struct thread_entry *thread, 976static 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 */
1245void switch_thread(void) 1012void 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 */
1404void sleep_thread(int ticks) 1118void 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 */
1424void block_thread(struct thread_entry *current, int timeout) 1133void 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 */
1470static 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 */
1489static 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 */
1523struct 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 */
1532unsigned 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 */
1543struct 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 */
1553void core_idle(void) 1173void 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 */
1663static 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
1672void trigger_cpu_boost(void)
1673{
1674 struct thread_entry *current = cores[CURRENT_CORE].running;
1675 boost_thread(current, true);
1676}
1677
1678void 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 */
1692void thread_wait(unsigned int thread_id) 1239void 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, 1264static USED_ATTR NORETURN_ATTR
1723 * such as optimizing subexpressions that put variables on the stack that 1265void thread_exit_final(struct thread_entry *current)
1724 * get used after switching stacks. */
1725#if NUM_CORES > 1
1726/* Called by ASM stub */
1727static void thread_final_exit_do(struct thread_entry *current)
1728#else
1729/* No special procedure is required before calling */
1730static 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(&current->queue); 1272 wait_queue_wake(&current->queue);
1273
1274 UNLOCK_THREAD(current);
1738 corelock_unlock(&current->waiter_cl); 1275 corelock_unlock(&current->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
1746void thread_exit(void) 1286void 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 */
1873int thread_get_priority(unsigned int thread_id) 1409int 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
1889int thread_get_io_priority(unsigned int thread_id) 1425int 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
1895void thread_set_io_priority(unsigned int thread_id,int io_priority) 1431void 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 */
1908void thread_thaw(unsigned int thread_id) 1444void 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 */
1465static USED_ATTR NORETURN_ATTR
1466void 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
1929unsigned int switch_core(unsigned int new_core) 1480unsigned 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 */
1512static 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 - 1521void 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 */ 1526void 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)
1998void INIT_ATTR init_threads(void) 1538void 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 */
2098unsigned 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 */
2116void 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"
2int * __errno(void) 2int * __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 */
85static inline void NORETURN_ATTR __attribute__((always_inline)) 85static 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 */
115static 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 */
139static void __attribute__((naked)) 115static 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__)
39static char __name[32]; 39static 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 */
51static struct core_entry cores[NUM_CORES];
52struct 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
129static 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
142static 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 */
163void 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
226void sim_thread_exception_wait(void) 126void sim_thread_exception_wait(void)
227{ 127{
228 while (1) 128 while (1)
@@ -237,7 +137,7 @@ void sim_thread_exception_wait(void)
237void sim_thread_lock(void *me) 137void 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
246void * sim_thread_unlock(void) 146void * 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
253struct thread_entry * thread_id_entry(unsigned int thread_id)
254{
255 return &threads[thread_id & THREAD_ID_SLOT_MASK];
256}
257
258static 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
278static 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
299unsigned int thread_self(void)
300{
301 return cores[CURRENT_CORE].running->id;
302}
303
304struct thread_entry* thread_self_entry(void)
305{
306 return cores[CURRENT_CORE].running;
307}
308
309void switch_thread(void) 153void 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
393void sleep_thread(int ticks) 227void 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
407void block_thread(struct thread_entry *current, int ticks) 241void 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
420unsigned int wakeup_thread_(struct thread_entry **list) 254unsigned 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
440void thread_thaw(unsigned int thread_id) 269void 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
451int runthread(void *data) 280int 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
545static void remove_thread(unsigned int thread_id) 366void 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(&current->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
618void 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
628void thread_wait(unsigned int thread_id) 410void 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. 423void 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 */
654unsigned 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 }
667void 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}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-16 00:48:21 +0000