summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--firmware/SOURCES3
-rw-r--r--firmware/include/bitarray.h231
-rw-r--r--firmware/kernel/include/kernel.h1
-rw-r--r--firmware/kernel/include/mutex.h20
-rw-r--r--firmware/kernel/include/queue.h2
-rw-r--r--firmware/kernel/include/thread.h81
-rw-r--r--firmware/kernel/mutex.c65
-rw-r--r--firmware/kernel/queue.c21
-rw-r--r--firmware/kernel/semaphore.c8
-rw-r--r--firmware/kernel/thread.c961
-rw-r--r--firmware/target/hosted/sdl/thread-sdl.c34
11 files changed, 853 insertions, 574 deletions
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 5e37892efe..584254a666 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -1825,6 +1825,9 @@ drivers/touchpad.c
#ifdef HAVE_CORELOCK_OBJECT
kernel/corelock.c
#endif
+#if 0 /* pending dependent code */
+kernel/mrsw_lock.c
+#endif
kernel/mutex.c
kernel/queue.c
#ifdef HAVE_SEMAPHORE_OBJECTS
diff --git a/firmware/include/bitarray.h b/firmware/include/bitarray.h
new file mode 100644
index 0000000000..4777ccb6a4
--- /dev/null
+++ b/firmware/include/bitarray.h
@@ -0,0 +1,231 @@
+/***************************************************************************
+ * __________ __ ___.
+ * Open \______ \ ____ ____ | | _\_ |__ _______ ___
+ * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
+ * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
+ * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
+ * \/ \/ \/ \/ \/
+ * $Id$
+ *
+ * Copyright (C) 2014 by Michael Sevakis
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+#ifndef BITARRAY_H
+#define BITARRAY_H
+
+/* Type-checked bit array definitions */
+
+/* All this stuff gets optimized into very simple object code */
+
+#define BITARRAY_WORD_BITS \
+ (sizeof (unsigned int) * 8)
+#define BITARRAY_NWORDS(bits) \
+ (((bits) + BITARRAY_WORD_BITS - 1) / BITARRAY_WORD_BITS)
+#define BITARRAY_BITWORD(bitnum) \
+ ((bitnum) / BITARRAY_WORD_BITS)
+#define BITARRAY_WORDBIT(bitnum) \
+ ((bitnum) % BITARRAY_WORD_BITS)
+#define BITARRAY_NBIT(word, bit) \
+ ((word)*BITARRAY_WORD_BITS + (bit))
+#define BITARRAY_BITS(bits) \
+ (BITARRAY_NWORDS(bits)*BITARRAY_WORD_BITS)
+#define BITARRAY_BITN(bitnum) \
+ (1u << BITARRAY_WORDBIT(bitnum))
+
+
+/** Iterators **/
+#include "config.h"
+#include <stdint.h>
+
+#if (defined(CPU_ARM) && ARM_ARCH >= 5) || UINT32_MAX < UINT_MAX
+#define __BITARRAY_CTZ(wval) __builtin_ctz(wval)
+#else
+#include "system.h"
+#define __BITARRAY_CTZ(wval) find_first_set_bit(wval)
+#endif
+#define __BITARRAY_POPCNT(wval) __builtin_popcount(wval)
+
+#ifndef BIT_N
+#define BIT_N(n) (1u << (n))
+#endif
+
+/* Enumerate each word index */
+#define FOR_EACH_BITARRAY_WORD_INDEX(nwords, index) \
+ for (unsigned int index = 0, _nwords = (nwords); \
+ index < _nwords; index++)
+
+/* Enumerate each word value */
+#define FOR_EACH_BITARRAY_WORD(a, wval) \
+ FOR_EACH_BITARRAY_WORD_INDEX(ARRAYLEN((a)->words), _w) \
+ for (unsigned int wval = (a)->words[_w], _ = 1; _; _--)
+
+/* Enumerate the bit number of each set bit of a word in sequence */
+#define FOR_EACH_BITARRAY_SET_WORD_BIT(wval, bit) \
+ for (unsigned int _wval = (wval), bit; \
+ _wval ? (((bit) = __BITARRAY_CTZ(_wval)), 1) : 0; \
+ _wval &= ~BIT_N(bit))
+
+/* Enumerate the bit number of each set bit in the bit array in sequence */
+#define FOR_EACH_BITARRAY_SET_BIT_ARR(nwords, words, nbit) \
+ FOR_EACH_BITARRAY_WORD_INDEX(nwords, _w) \
+ FOR_EACH_BITARRAY_SET_WORD_BIT(words[_w], _bit) \
+ for (unsigned int nbit = BITARRAY_NBIT(_w, _bit), _ = 1; _; _--)
+
+/* As above but takes an array type for an argument */
+#define FOR_EACH_BITARRAY_SET_BIT(a, nbit) \
+ FOR_EACH_BITARRAY_SET_BIT_ARR(ARRAYLEN((a)->words), (a)->words, nbit)
+
+
+/** Base functions (called by typed functions) **/
+
+/* Return the word associated with the bit */
+static inline unsigned int
+__bitarray_get_word(unsigned int words[], unsigned int bitnum)
+{
+ return words[BITARRAY_BITWORD(bitnum)];
+}
+
+/* Set the word associated with the bit */
+static inline void
+__bitarray_set_word(unsigned int words[], unsigned int bitnum,
+ unsigned int wordval)
+{
+ words[BITARRAY_BITWORD(bitnum)] = wordval;
+}
+
+/* Set the bit at index 'bitnum' to '1' */
+static inline void
+__bitarray_set_bit(unsigned int words[], unsigned int bitnum)
+{
+ unsigned int word = BITARRAY_BITWORD(bitnum);
+ unsigned int bit = BITARRAY_BITN(bitnum);
+ words[word] |= bit;
+}
+
+/* Set the bit at index 'bitnum' to '0' */
+static inline void
+__bitarray_clear_bit(unsigned int words[], unsigned int bitnum)
+{
+ unsigned int word = BITARRAY_BITWORD(bitnum);
+ unsigned int bit = BITARRAY_BITN(bitnum);
+ words[word] &= ~bit;
+}
+
+/* Return the value of the specified bit ('0' or '1') */
+static inline unsigned int
+__bitarray_test_bit(const unsigned int words[], unsigned int bitnum)
+{
+ unsigned int word = BITARRAY_BITWORD(bitnum);
+ unsigned int nbit = BITARRAY_WORDBIT(bitnum);
+ return (words[word] >> nbit) & 1u;
+}
+
+/* Check if all bits in the bit array are '0' */
+static inline bool
+__bitarray_is_clear(const unsigned int words[], unsigned int nbits)
+{
+ FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
+ {
+ if (words[word] != 0)
+ return false;
+ }
+
+ return true;
+}
+
+/* Set every bit in the array to '0' */
+static inline void
+__bitarray_clear(unsigned int words[], unsigned int nbits)
+{
+ FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
+ words[word] = 0;
+}
+
+/* Set every bit in the array to '1' */
+static inline void
+__bitarray_set(unsigned int words[], unsigned int nbits)
+{
+ FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
+ words[word] = ~0u;
+}
+
+/* Find the lowest-indexed '1' bit in the bit array, returning the size of
+ the array if none are set */
+static inline unsigned int
+__bitarray_ffs(const unsigned int words[], unsigned int nbits)
+{
+ FOR_EACH_BITARRAY_SET_BIT_ARR(BITARRAY_NWORDS(nbits), words, nbit)
+ return nbit;
+
+ return BITARRAY_BITS(nbits);
+}
+
+/* Return the number of bits currently set to '1' in the bit array */
+static inline unsigned int
+__bitarray_popcount(const unsigned int words[], unsigned int nbits)
+{
+ unsigned int count = 0;
+
+ FOR_EACH_BITARRAY_WORD_INDEX(BITARRAY_NWORDS(nbits), word)
+ count += __BITARRAY_POPCNT(words[word]);
+
+ return count;
+}
+
+/**
+ * Giant macro to define all the typed functions
+ * typename: The name of the type (e.g. myarr_t myarr;)
+ * fnprefix: The prefix all functions get (e.g. myarr_set_bit)
+ * nbits : The minimum number of bits the array is meant to hold
+ * (the implementation rounds this up to the word size
+ * and all words may be fully utilized)
+ *
+ * uses 'typedef' to freely change from, e.g., struct to union without
+ * changing source code
+ */
+#define BITARRAY_TYPE_DECLARE(typename, fnprefix, nbits) \
+typedef struct \
+{ \
+ unsigned int words[BITARRAY_NWORDS(nbits)]; \
+} typename; \
+static inline unsigned int \
+fnprefix##_get_word(typename *array, unsigned int bitnum) \
+ { return __bitarray_get_word(array->words, bitnum); } \
+static inline void \
+fnprefix##_set_word(typename *array, unsigned int bitnum, \
+ unsigned int wordval) \
+ { __bitarray_set_word(array->words, bitnum, wordval); } \
+static inline void \
+fnprefix##_set_bit(typename *array, unsigned int bitnum) \
+ { __bitarray_set_bit(array->words, bitnum); } \
+static inline void \
+fnprefix##_clear_bit(typename *array, unsigned int bitnum) \
+ { __bitarray_clear_bit(array->words, bitnum); } \
+static inline unsigned int \
+fnprefix##_test_bit(const typename *array, unsigned int bitnum) \
+ { return __bitarray_test_bit(array->words, bitnum); } \
+static inline bool \
+fnprefix##_is_clear(const typename *array) \
+ { return __bitarray_is_clear(array->words, nbits); } \
+static inline void \
+fnprefix##_clear(typename *array) \
+ { __bitarray_clear(array->words, nbits); } \
+static inline void \
+fnprefix##_set(typename *array) \
+ { __bitarray_set(array->words, nbits); } \
+static inline unsigned int \
+fnprefix##_ffs(const typename *array) \
+ { return __bitarray_ffs(array->words, nbits); } \
+static inline unsigned int \
+fnprefix##_popcount(const typename *array) \
+ { return __bitarray_popcount(array->words, nbits); }
+
+#endif /* BITARRAY_H */
diff --git a/firmware/kernel/include/kernel.h b/firmware/kernel/include/kernel.h
index fafff25ce4..d2ffffcda9 100644
--- a/firmware/kernel/include/kernel.h
+++ b/firmware/kernel/include/kernel.h
@@ -26,6 +26,7 @@
#include "system.h"
#include "queue.h"
#include "mutex.h"
+#include "mrsw_lock.h"
#include "tick.h"
#ifdef INCLUDE_TIMEOUT_API
diff --git a/firmware/kernel/include/mutex.h b/firmware/kernel/include/mutex.h
index bcf5701bd9..02b85f331f 100644
--- a/firmware/kernel/include/mutex.h
+++ b/firmware/kernel/include/mutex.h
@@ -28,20 +28,14 @@
struct mutex
{
- struct thread_entry *queue; /* waiter list */
- int recursion; /* lock owner recursion count */
+ struct thread_entry *queue; /* waiter list */
+ int recursion; /* lock owner recursion count */
+ struct blocker blocker; /* priority inheritance info
+ for waiters and owner*/
+ IF_COP( struct corelock cl; ) /* multiprocessor sync */
#ifdef HAVE_PRIORITY_SCHEDULING
- struct blocker blocker; /* priority inheritance info
- for waiters */
- bool no_preempt; /* don't allow higher-priority thread
- to be scheduled even if woken */
-#else
- struct thread_entry *thread; /* Indicates owner thread - an owner
- implies a locked state - same goes
- for priority scheduling
- (in blocker struct for that) */
+ bool no_preempt;
#endif
- IF_COP( struct corelock cl; ) /* multiprocessor sync */
};
extern void mutex_init(struct mutex *m);
@@ -56,7 +50,7 @@ static inline void mutex_set_preempt(struct mutex *m, bool preempt)
#else
/* Deprecated but needed for now - firmware/drivers/ata_mmc.c */
static inline bool mutex_test(const struct mutex *m)
- { return m->thread != NULL; }
+ { return m->blocker.thread != NULL; }
#endif /* HAVE_PRIORITY_SCHEDULING */
#endif /* MUTEX_H */
diff --git a/firmware/kernel/include/queue.h b/firmware/kernel/include/queue.h
index 1b404f8297..3f24598d5b 100644
--- a/firmware/kernel/include/queue.h
+++ b/firmware/kernel/include/queue.h
@@ -143,6 +143,8 @@ extern bool queue_peek(struct event_queue *q, struct queue_event *ev);
#define QPEEK_FILTER_COUNT_MASK (0xffu) /* 0x00=1 filter, 0xff=256 filters */
#define QPEEK_FILTER_HEAD_ONLY (1u << 8) /* Ignored if no filters */
#define QPEEK_REMOVE_EVENTS (1u << 9) /* Remove or discard events */
+#define QPEEK_FILTER1(a) QPEEK_FILTER2((a), (a))
+#define QPEEK_FILTER2(a, b) (&(const long [2]){ (a), (b) })
extern bool queue_peek_ex(struct event_queue *q,
struct queue_event *ev,
unsigned int flags,
diff --git a/firmware/kernel/include/thread.h b/firmware/kernel/include/thread.h
index 8c13b462e6..f181f867cb 100644
--- a/firmware/kernel/include/thread.h
+++ b/firmware/kernel/include/thread.h
@@ -28,6 +28,7 @@
#include <stdbool.h>
#include "gcc_extensions.h"
#include "corelock.h"
+#include "bitarray.h"
/* Priority scheduling (when enabled with HAVE_PRIORITY_SCHEDULING) works
* by giving high priority threads more CPU time than lower priority threads
@@ -80,6 +81,10 @@
#endif
#define MAXTHREADS (BASETHREADS+TARGET_EXTRA_THREADS)
+
+BITARRAY_TYPE_DECLARE(threadbit_t, threadbit, MAXTHREADS)
+BITARRAY_TYPE_DECLARE(priobit_t, priobit, NUM_PRIORITIES)
+
/*
* We need more stack when we run under a host
* maybe more expensive C lib functions?
@@ -134,32 +139,39 @@ struct thread_list
struct thread_entry *next; /* Next thread in a list */
};
-#ifdef HAVE_PRIORITY_SCHEDULING
+/* Basic structure describing the owner of an object */
struct blocker
{
struct thread_entry * volatile thread; /* thread blocking other threads
(aka. object owner) */
- int priority; /* highest priority waiter */
- struct thread_entry * (*wakeup_protocol)(struct thread_entry *thread);
+#ifdef HAVE_PRIORITY_SCHEDULING
+ int priority; /* highest priority waiter */
+#endif
};
-/* Choices of wakeup protocol */
-
-/* For transfer of object ownership by one thread to another thread by
- * the owning thread itself (mutexes) */
-struct thread_entry *
- wakeup_priority_protocol_transfer(struct thread_entry *thread);
+/* If a thread has a blocker but the blocker's registered thread is NULL,
+ then it references this and the struct blocker pointer may be
+ reinterpreted as such. */
+struct blocker_splay
+{
+ struct blocker blocker; /* blocker info (first!) */
+#ifdef HAVE_PRIORITY_SCHEDULING
+ threadbit_t mask; /* mask of nonzero tcounts */
+#if NUM_CORES > 1
+ struct corelock cl; /* mutual exclusion */
+#endif
+#endif /* HAVE_PRIORITY_SCHEDULING */
+};
-/* For release by owner where ownership doesn't change - other threads,
- * interrupts, timeouts, etc. (mutex timeout, queues) */
-struct thread_entry *
- wakeup_priority_protocol_release(struct thread_entry *thread);
+#ifdef HAVE_PRIORITY_SCHEDULING
+/* Quick-disinherit of priority elevation. Must be a running thread. */
+void priority_disinherit(struct thread_entry *thread, struct blocker *bl);
struct priority_distribution
{
- uint8_t hist[NUM_PRIORITIES]; /* Histogram: Frequency for each priority */
- uint32_t mask; /* Bitmask of hist entries that are not zero */
+ uint8_t hist[NUM_PRIORITIES]; /* Histogram: Frequency for each priority */
+ priobit_t mask; /* Bitmask of hist entries that are not zero */
};
#endif /* HAVE_PRIORITY_SCHEDULING */
@@ -210,6 +222,7 @@ struct thread_entry
volatile intptr_t retval; /* Return value from a blocked operation/
misc. use */
#endif
+ uint32_t id; /* Current slot id */
int __errno; /* Thread error number (errno tls) */
#ifdef HAVE_PRIORITY_SCHEDULING
/* Priority summary of owned objects that support inheritance */
@@ -226,7 +239,6 @@ struct thread_entry
unsigned char priority; /* Scheduled priority (higher of base or
all threads blocked by this one) */
#endif
- uint16_t id; /* Current slot id */
unsigned short stack_size; /* Size of stack in bytes */
unsigned char state; /* Thread slot state (STATE_*) */
#ifdef HAVE_SCHEDULER_BOOSTCTRL
@@ -238,11 +250,12 @@ struct thread_entry
};
/*** Macros for internal use ***/
-/* Thread ID, 16 bits = |VVVVVVVV|SSSSSSSS| */
-#define THREAD_ID_VERSION_SHIFT 8
-#define THREAD_ID_VERSION_MASK 0xff00
-#define THREAD_ID_SLOT_MASK 0x00ff
+/* Thread ID, 32 bits = |VVVVVVVV|VVVVVVVV|VVVVVVVV|SSSSSSSS| */
+#define THREAD_ID_VERSION_SHIFT 8
+#define THREAD_ID_VERSION_MASK 0xffffff00
+#define THREAD_ID_SLOT_MASK 0x000000ff
#define THREAD_ID_INIT(n) ((1u << THREAD_ID_VERSION_SHIFT) | (n))
+#define THREAD_ID_SLOT(id) ((id) & THREAD_ID_SLOT_MASK)
#ifdef HAVE_CORELOCK_OBJECT
/* Operations to be performed just before stopping a thread and starting
@@ -337,11 +350,8 @@ void switch_thread(void);
/* Blocks a thread for at least the specified number of ticks (0 = wait until
* next tick) */
void sleep_thread(int ticks);
-/* Indefinitely blocks the current thread on a thread queue */
-void block_thread(struct thread_entry *current);
-/* Blocks the current thread on a thread queue until explicitely woken or
- * the timeout is reached */
-void block_thread_w_tmo(struct thread_entry *current, int timeout);
+/* Blocks the current thread on a thread queue (< 0 == infinite) */
+void block_thread(struct thread_entry *current, int timeout);
/* Return bit flags for thread wakeup */
#define THREAD_NONE 0x0 /* No thread woken up (exclusive) */
@@ -350,15 +360,32 @@ void block_thread_w_tmo(struct thread_entry *current, int timeout);
higher priority than current were woken) */
/* A convenience function for waking an entire queue of threads. */
-unsigned int thread_queue_wake(struct thread_entry **list);
+unsigned int thread_queue_wake(struct thread_entry **list,
+ volatile int *count);
/* Wakeup a thread at the head of a list */
-unsigned int wakeup_thread(struct thread_entry **list);
+enum wakeup_thread_protocol
+{
+ WAKEUP_DEFAULT,
+ WAKEUP_TRANSFER,
+ WAKEUP_RELEASE,
+ WAKEUP_TRANSFER_MULTI,
+};
+
+unsigned int wakeup_thread_(struct thread_entry **list
+ IF_PRIO(, enum wakeup_thread_protocol proto));
#ifdef HAVE_PRIORITY_SCHEDULING
+#define wakeup_thread(list, proto) \
+ wakeup_thread_((list), (proto))
+
int thread_set_priority(unsigned int thread_id, int priority);
int thread_get_priority(unsigned int thread_id);
+#else /* !HAVE_PRIORITY_SCHEDULING */
+#define wakeup_thread(list, proto...) \
+ wakeup_thread_((list));
#endif /* HAVE_PRIORITY_SCHEDULING */
+
#ifdef HAVE_IO_PRIORITY
void thread_set_io_priority(unsigned int thread_id, int io_priority);
int thread_get_io_priority(unsigned int thread_id);
diff --git a/firmware/kernel/mutex.c b/firmware/kernel/mutex.c
index f1e4b3c722..2e90b0f4b1 100644
--- a/firmware/kernel/mutex.c
+++ b/firmware/kernel/mutex.c
@@ -27,31 +27,10 @@
#include <stdbool.h>
#include "config.h"
#include "system.h"
-#include "mutex.h"
-#include "corelock.h"
+#include "kernel.h"
#include "thread-internal.h"
#include "kernel-internal.h"
-static inline void __attribute__((always_inline))
-mutex_set_thread(struct mutex *mtx, struct thread_entry *td)
-{
-#ifdef HAVE_PRIORITY_SCHEDULING
- mtx->blocker.thread = td;
-#else
- mtx->thread = td;
-#endif
-}
-
-static inline struct thread_entry * __attribute__((always_inline))
-mutex_get_thread(volatile struct mutex *mtx)
-{
-#ifdef HAVE_PRIORITY_SCHEDULING
- return mtx->blocker.thread;
-#else
- return mtx->thread;
-#endif
-}
-
/* Initialize a mutex object - call before any use and do not call again once
* the object is available to other threads */
void mutex_init(struct mutex *m)
@@ -59,10 +38,9 @@ void mutex_init(struct mutex *m)
corelock_init(&m->cl);
m->queue = NULL;
m->recursion = 0;
- mutex_set_thread(m, NULL);
+ m->blocker.thread = NULL;
#ifdef HAVE_PRIORITY_SCHEDULING
m->blocker.priority = PRIORITY_IDLE;
- m->blocker.wakeup_protocol = wakeup_priority_protocol_transfer;
m->no_preempt = false;
#endif
}
@@ -72,7 +50,7 @@ void mutex_lock(struct mutex *m)
{
struct thread_entry *current = thread_self_entry();
- if(current == mutex_get_thread(m))
+ if(current == m->blocker.thread)
{
/* current thread already owns this mutex */
m->recursion++;
@@ -83,10 +61,10 @@ void mutex_lock(struct mutex *m)
corelock_lock(&m->cl);
/* must read thread again inside cs (a multiprocessor concern really) */
- if(LIKELY(mutex_get_thread(m) == NULL))
+ if(LIKELY(m->blocker.thread == NULL))
{
/* lock is open */
- mutex_set_thread(m, current);
+ m->blocker.thread = current;
corelock_unlock(&m->cl);
return;
}
@@ -97,7 +75,7 @@ void mutex_lock(struct mutex *m)
current->bqp = &m->queue;
disable_irq();
- block_thread(current);
+ block_thread(current, TIMEOUT_BLOCK);
corelock_unlock(&m->cl);
@@ -109,9 +87,9 @@ void mutex_lock(struct mutex *m)
void mutex_unlock(struct mutex *m)
{
/* unlocker not being the owner is an unlocking violation */
- KERNEL_ASSERT(mutex_get_thread(m) == thread_self_entry(),
+ KERNEL_ASSERT(m->blocker.thread == thread_self_entry(),
"mutex_unlock->wrong thread (%s != %s)\n",
- mutex_get_thread(m)->name,
+ m->blocker.thread->name,
thread_self_entry()->name);
if(m->recursion > 0)
@@ -128,25 +106,24 @@ void mutex_unlock(struct mutex *m)
if(LIKELY(m->queue == NULL))
{
/* no threads waiting - open the lock */
- mutex_set_thread(m, NULL);
+ m->blocker.thread = NULL;
corelock_unlock(&m->cl);
return;
}
- else
- {
- const int oldlevel = disable_irq_save();
- /* Tranfer of owning thread is handled in the wakeup protocol
- * if priorities are enabled otherwise just set it from the
- * queue head. */
- IFN_PRIO( mutex_set_thread(m, m->queue); )
- IF_PRIO( unsigned int result = ) wakeup_thread(&m->queue);
- restore_irq(oldlevel);
- corelock_unlock(&m->cl);
+ const int oldlevel = disable_irq_save();
+ /* Tranfer of owning thread is handled in the wakeup protocol
+ * if priorities are enabled otherwise just set it from the
+ * queue head. */
+ IFN_PRIO( m->blocker.thread = m->queue; )
+ unsigned int result = wakeup_thread(&m->queue, WAKEUP_TRANSFER);
+ restore_irq(oldlevel);
+
+ corelock_unlock(&m->cl);
#ifdef HAVE_PRIORITY_SCHEDULING
- if((result & THREAD_SWITCH) && !m->no_preempt)
- switch_thread();
+ if((result & THREAD_SWITCH) && !m->no_preempt)
+ switch_thread();
#endif
- }
+ (void)result;
}
diff --git a/firmware/kernel/queue.c b/firmware/kernel/queue.c
index 379e3f62c8..22a8da9bd3 100644
--- a/firmware/kernel/queue.c
+++ b/firmware/kernel/queue.c
@@ -84,7 +84,7 @@ static void queue_release_sender(struct thread_entry * volatile * sender,
*thread->bqp = thread; /* Move blocking queue head to thread since
wakeup_thread wakes the first thread in
the list. */
- wakeup_thread(thread->bqp);
+ wakeup_thread(thread->bqp, WAKEUP_RELEASE);
}
/* Releases any waiting threads that are queued with queue_send -
@@ -108,16 +108,16 @@ static void queue_release_all_senders(struct event_queue *q)
}
}
+#ifdef HAVE_WAKEUP_EXT_CB
/* Callback to do extra forced removal steps from sender list in addition
* to the normal blocking queue removal and priority dis-inherit */
static void queue_remove_sender_thread_cb(struct thread_entry *thread)
{
*((struct thread_entry **)thread->retval) = NULL;
-#ifdef HAVE_WAKEUP_EXT_CB
thread->wakeup_ext_cb = NULL;
-#endif
thread->retval = 0;
}
+#endif /* HAVE_WAKEUP_EXT_CB */
/* Enables queue_send on the specified queue - caller allocates the extra
* data structure. Only queues which are taken to be owned by a thread should
@@ -139,7 +139,6 @@ void queue_enable_queue_send(struct event_queue *q,
{
memset(send, 0, sizeof(*send));
#ifdef HAVE_PRIORITY_SCHEDULING
- send->blocker.wakeup_protocol = wakeup_priority_protocol_release;
send->blocker.priority = PRIORITY_IDLE;
if(owner_id != 0)
{
@@ -268,7 +267,7 @@ void queue_delete(struct event_queue *q)
corelock_unlock(&all_queues.cl);
/* Release thread(s) waiting on queue head */
- thread_queue_wake(&q->queue);
+ thread_queue_wake(&q->queue, NULL);
#ifdef HAVE_EXTENDED_MESSAGING_AND_NAME
if(q->send)
@@ -325,7 +324,7 @@ void queue_wait(struct event_queue *q, struct queue_event *ev)
IF_COP( current->obj_cl = &q->cl; )
current->bqp = &q->queue;
- block_thread(current);
+ block_thread(current, TIMEOUT_BLOCK);
corelock_unlock(&q->cl);
switch_thread();
@@ -386,7 +385,7 @@ void queue_wait_w_tmo(struct event_queue *q, struct queue_event *ev, int ticks)
IF_COP( current->obj_cl = &q->cl; )
current->bqp = &q->queue;
- block_thread_w_tmo(current, ticks);
+ block_thread(current, ticks);
corelock_unlock(&q->cl);
switch_thread();
@@ -443,7 +442,7 @@ void queue_post(struct event_queue *q, long id, intptr_t data)
queue_do_unblock_sender(q->send, wr);
/* Wakeup a waiting thread if any */
- wakeup_thread(&q->queue);
+ wakeup_thread(&q->queue, WAKEUP_DEFAULT);
corelock_unlock(&q->cl);
restore_irq(oldlevel);
@@ -481,7 +480,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
}
/* Wakeup a waiting thread if any */
- wakeup_thread(&q->queue);
+ wakeup_thread(&q->queue, WAKEUP_DEFAULT);
/* Save thread in slot, add to list and wait for reply */
*spp = current;
@@ -493,7 +492,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
current->retval = (intptr_t)spp;
current->bqp = &send->list;
- block_thread(current);
+ block_thread(current, TIMEOUT_BLOCK);
corelock_unlock(&q->cl);
switch_thread();
@@ -502,7 +501,7 @@ intptr_t queue_send(struct event_queue *q, long id, intptr_t data)
}
/* Function as queue_post if sending is not enabled */
- wakeup_thread(&q->queue);
+ wakeup_thread(&q->queue, WAKEUP_DEFAULT);
corelock_unlock(&q->cl);
restore_irq(oldlevel);
diff --git a/firmware/kernel/semaphore.c b/firmware/kernel/semaphore.c
index f9ff0ad987..b6ce7fd742 100644
--- a/firmware/kernel/semaphore.c
+++ b/firmware/kernel/semaphore.c
@@ -82,11 +82,7 @@ int semaphore_wait(struct semaphore *s, int timeout)
* explicit in semaphore_release */
current->retval = OBJ_WAIT_TIMEDOUT;
- if(timeout > 0)
- block_thread_w_tmo(current, timeout); /* ...or timed out... */
- else
- block_thread(current); /* -timeout = infinite */
-
+ block_thread(current, timeout);
corelock_unlock(&s->cl);
/* ...and turn control over to next thread */
@@ -118,7 +114,7 @@ void semaphore_release(struct semaphore *s)
KERNEL_ASSERT(s->count == 0,
"semaphore_release->threads queued but count=%d!\n", s->count);
s->queue->retval = OBJ_WAIT_SUCCEEDED; /* indicate explicit wake */
- result = wakeup_thread(&s->queue);
+ result = wakeup_thread(&s->queue, WAKEUP_DEFAULT);
}
else
{
diff --git a/firmware/kernel/thread.c b/firmware/kernel/thread.c
index 43ff584a68..0a47f97e93 100644
--- a/firmware/kernel/thread.c
+++ b/firmware/kernel/thread.c
@@ -246,13 +246,13 @@ static void thread_stkov(struct thread_entry *thread)
cores[_core].blk_ops.cl_p = &(thread)->slot_cl; })
#else
#define LOCK_THREAD(thread) \
- ({ })
+ ({ (void)(thread); })
#define TRY_LOCK_THREAD(thread) \
- ({ })
+ ({ (void)(thread); })
#define UNLOCK_THREAD(thread) \
- ({ })
+ ({ (void)(thread); })
#define UNLOCK_THREAD_AT_TASK_SWITCH(thread) \
- ({ })
+ ({ (void)(thread); })
#endif
/* RTR list */
@@ -279,6 +279,100 @@ static void thread_stkov(struct thread_entry *thread)
#define rtr_move_entry_inl(core, from, to)
#endif
+static inline void thread_store_context(struct thread_entry *thread)
+{
+#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
+ thread->__errno = errno;
+#endif
+ store_context(&thread->context);
+}
+
+static inline void thread_load_context(struct thread_entry *thread)
+{
+ load_context(&thread->context);
+#if (CONFIG_PLATFORM & PLATFORM_HOSTED)
+ errno = thread->__errno;
+#endif
+}
+
+static inline unsigned int should_switch_tasks(void)
+{
+ unsigned int result = THREAD_OK;
+
+#ifdef HAVE_PRIORITY_SCHEDULING
+ struct thread_entry *current = cores[CURRENT_CORE].running;
+ if (current &&
+ priobit_ffs(&cores[IF_COP_CORE(current->core)].rtr.mask)
+ < current->priority)
+ {
+ /* There is a thread ready to run of higher priority on the same
+ * core as the current one; recommend a task switch. */
+ result |= THREAD_SWITCH;
+ }
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
+ return result;
+}
+
+#ifdef HAVE_PRIORITY_SCHEDULING
+/*---------------------------------------------------------------------------
+ * Locks the thread registered as the owner of the block and makes sure it
+ * didn't change in the meantime
+ *---------------------------------------------------------------------------
+ */
+#if NUM_CORES == 1
+static inline struct thread_entry * lock_blocker_thread(struct blocker *bl)
+{
+ return bl->thread;
+}
+#else /* NUM_CORES > 1 */
+static struct thread_entry * lock_blocker_thread(struct blocker *bl)
+{
+ /* The blocker thread may change during the process of trying to
+ capture it */
+ while (1)
+ {
+ struct thread_entry *t = bl->thread;
+
+ /* TRY, or else deadlocks are possible */
+ if (!t)
+ {
+ struct blocker_splay *blsplay = (struct blocker_splay *)bl;
+ if (corelock_try_lock(&blsplay->cl))
+ {
+ if (!bl->thread)
+ return NULL; /* Still multi */
+
+ corelock_unlock(&blsplay->cl);
+ }
+ }
+ else
+ {
+ if (TRY_LOCK_THREAD(t))
+ {
+ if (bl->thread == t)
+ return t;
+
+ UNLOCK_THREAD(t);
+ }
+ }
+ }
+}
+#endif /* NUM_CORES */
+
+static inline void unlock_blocker_thread(struct blocker *bl)
+{
+#if NUM_CORES > 1
+ struct thread_entry *blt = bl->thread;
+ if (blt)
+ UNLOCK_THREAD(blt);
+ else
+ corelock_unlock(&((struct blocker_splay *)bl)->cl);
+#endif /* NUM_CORES > 1*/
+ (void)bl;
+}
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
/*---------------------------------------------------------------------------
* Thread list structure - circular:
* +------------------------------+
@@ -420,7 +514,6 @@ static void remove_from_list_tmo(struct thread_entry *thread)
}
}
-
#ifdef HAVE_PRIORITY_SCHEDULING
/*---------------------------------------------------------------------------
* Priority distribution structure (one category for each possible priority):
@@ -476,19 +569,9 @@ static void remove_from_list_tmo(struct thread_entry *thread)
static inline unsigned int prio_add_entry(
struct priority_distribution *pd, int priority)
{
- unsigned int count;
- /* Enough size/instruction count difference for ARM makes it worth it to
- * use different code (192 bytes for ARM). Only thing better is ASM. */
-#ifdef CPU_ARM
- count = pd->hist[priority];
- if (++count == 1)
- pd->mask |= 1 << priority;
- pd->hist[priority] = count;
-#else /* This one's better for Coldfire */
- if ((count = ++pd->hist[priority]) == 1)
- pd->mask |= 1 << priority;
-#endif
-
+ unsigned int count = ++pd->hist[priority];
+ if (count == 1)
+ priobit_set_bit(&pd->mask, priority);
return count;
}
@@ -499,18 +582,9 @@ static inline unsigned int prio_add_entry(
static inline unsigned int prio_subtract_entry(
struct priority_distribution *pd, int priority)
{
- unsigned int count;
-
-#ifdef CPU_ARM
- count = pd->hist[priority];
- if (--count == 0)
- pd->mask &= ~(1 << priority);
- pd->hist[priority] = count;
-#else
- if ((count = --pd->hist[priority]) == 0)
- pd->mask &= ~(1 << priority);
-#endif
-
+ unsigned int count = --pd->hist[priority];
+ if (count == 0)
+ priobit_clear_bit(&pd->mask, priority);
return count;
}
@@ -521,31 +595,38 @@ static inline unsigned int prio_subtract_entry(
static inline void prio_move_entry(
struct priority_distribution *pd, int from, int to)
{
- uint32_t mask = pd->mask;
+ if (--pd->hist[from] == 0)
+ priobit_clear_bit(&pd->mask, from);
+
+ if (++pd->hist[to] == 1)
+ priobit_set_bit(&pd->mask, to);
+}
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
+/*---------------------------------------------------------------------------
+ * Move a thread back to a running state on its core.
+ *---------------------------------------------------------------------------
+ */
+static void core_schedule_wakeup(struct thread_entry *thread)
+{
+ const unsigned int core = IF_COP_CORE(thread->core);
-#ifdef CPU_ARM
- unsigned int count;
+ RTR_LOCK(core);
- count = pd->hist[from];
- if (--count == 0)
- mask &= ~(1 << from);
- pd->hist[from] = count;
+ thread->state = STATE_RUNNING;
- count = pd->hist[to];
- if (++count == 1)
- mask |= 1 << to;
- pd->hist[to] = count;
-#else
- if (--pd->hist[from] == 0)
- mask &= ~(1 << from);
+ add_to_list_l(&cores[core].running, thread);
+ rtr_add_entry(core, thread->priority);
- if (++pd->hist[to] == 1)
- mask |= 1 << to;
-#endif
+ RTR_UNLOCK(core);
- pd->mask = mask;
+#if NUM_CORES > 1
+ if (core != CURRENT_CORE)
+ core_wake(core);
+#endif
}
+#ifdef HAVE_PRIORITY_SCHEDULING
/*---------------------------------------------------------------------------
* Change the priority and rtr entry for a running thread
*---------------------------------------------------------------------------
@@ -605,191 +686,211 @@ static int find_highest_priority_in_list_l(
* those are prevented, right? :-)
*---------------------------------------------------------------------------
*/
-static struct thread_entry *
- blocker_inherit_priority(struct thread_entry *current)
+static void inherit_priority(
+ struct blocker * const blocker0, struct blocker *bl,
+ struct thread_entry *blt, int newblpr)
{
- const int priority = current->priority;
- struct blocker *bl = current->blocker;
- struct thread_entry * const tstart = current;
- struct thread_entry *bl_t = bl->thread;
-
- /* Blocker cannot change since the object protection is held */
- LOCK_THREAD(bl_t);
+ int oldblpr = bl->priority;
- for (;;)
+ while (1)
{
- struct thread_entry *next;
- int bl_pr = bl->priority;
+ if (blt == NULL)
+ {
+ /* Multiple owners */
+ struct blocker_splay *blsplay = (struct blocker_splay *)bl;
+
+ /* Recurse down the all the branches of this; it's the only way.
+ We might meet the same queue several times if more than one of
+ these threads is waiting the same queue. That isn't a problem
+ for us since we early-terminate, just notable. */
+ FOR_EACH_BITARRAY_SET_BIT(&blsplay->mask, slotnum)
+ {
+ bl->priority = oldblpr; /* To see the change each time */
+ blt = &threads[slotnum];
+ LOCK_THREAD(blt);
+ inherit_priority(blocker0, bl, blt, newblpr);
+ }
- if (priority >= bl_pr)
- break; /* Object priority already high enough */
+ corelock_unlock(&blsplay->cl);
+ return;
+ }
- bl->priority = priority;
+ bl->priority = newblpr;
- /* Add this one */
- prio_add_entry(&bl_t->pdist, priority);
+ /* Update blocker thread inheritance record */
+ if (newblpr < PRIORITY_IDLE)
+ prio_add_entry(&blt->pdist, newblpr);
- if (bl_pr < PRIORITY_IDLE)
- {
- /* Not first waiter - subtract old one */
- prio_subtract_entry(&bl_t->pdist, bl_pr);
- }
+ if (oldblpr < PRIORITY_IDLE)
+ prio_subtract_entry(&blt->pdist, oldblpr);
- if (priority >= bl_t->priority)
- break; /* Thread priority high enough */
+ int oldpr = blt->priority;
+ int newpr = priobit_ffs(&blt->pdist.mask);
+ if (newpr == oldpr)
+ break; /* No blocker thread priority change */
- if (bl_t->state == STATE_RUNNING)
+ if (blt->state == STATE_RUNNING)
{
- /* Blocking thread is a running thread therefore there are no
- * further blockers. Change the "run queue" on which it
- * resides. */
- set_running_thread_priority(bl_t, priority);
- break;
+ set_running_thread_priority(blt, newpr);
+ break; /* Running: last in chain */
}
- bl_t->priority = priority;
+ /* Blocker is blocked */
+ blt->priority = newpr;
- /* If blocking thread has a blocker, apply transitive inheritance */
- bl = bl_t->blocker;
+ bl = blt->blocker;
+ if (LIKELY(bl == NULL))
+ break; /* Block doesn't support PIP */
- if (bl == NULL)
- break; /* End of chain or object doesn't support inheritance */
+ if (UNLIKELY(bl == blocker0))
+ break; /* Full circle - deadlock! */
- next = bl->thread;
+ /* Blocker becomes current thread and the process repeats */
+ struct thread_entry **bqp = blt->bqp;
+ struct thread_entry *t = blt;
+ blt = lock_blocker_thread(bl);
- if (UNLIKELY(next == tstart))
- break; /* Full-circle - deadlock! */
+ UNLOCK_THREAD(t);
- UNLOCK_THREAD(current);
+ /* Adjust this wait queue */
+ oldblpr = bl->priority;
+ if (newpr <= oldblpr)
+ newblpr = newpr;
+ else if (oldpr <= oldblpr)
+ newblpr = find_highest_priority_in_list_l(*bqp);
-#if NUM_CORES > 1
- for (;;)
- {
- LOCK_THREAD(next);
+ if (newblpr == oldblpr)
+ break; /* Queue priority not changing */
+ }
- /* Blocker could change - retest condition */
- if (LIKELY(bl->thread == next))
- break;
+ UNLOCK_THREAD(blt);
+}
- UNLOCK_THREAD(next);
- next = bl->thread;
- }
-#endif
- current = bl_t;
- bl_t = next;
+/*---------------------------------------------------------------------------
+ * Quick-disinherit of priority elevation. 'thread' must be a running thread.
+ *---------------------------------------------------------------------------
+ */
+static void priority_disinherit_internal(struct thread_entry *thread,
+ int blpr)
+{
+ if (blpr < PRIORITY_IDLE &&
+ prio_subtract_entry(&thread->pdist, blpr) == 0 &&
+ blpr <= thread->priority)
+ {
+ int priority = priobit_ffs(&thread->pdist.mask);
+ if (priority != thread->priority)
+ set_running_thread_priority(thread, priority);
}
+}
- UNLOCK_THREAD(bl_t);
-
- return current;
+void priority_disinherit(struct thread_entry *thread, struct blocker *bl)
+{
+ LOCK_THREAD(thread);
+ priority_disinherit_internal(thread, bl->priority);
+ UNLOCK_THREAD(thread);
}
/*---------------------------------------------------------------------------
- * Readjust priorities when waking a thread blocked waiting for another
- * in essence "releasing" the thread's effect on the object owner. Can be
- * performed from any context.
+ * Transfer ownership from a single owner to a multi-owner splay from a wait
+ * queue
*---------------------------------------------------------------------------
*/
-struct thread_entry *
- wakeup_priority_protocol_release(struct thread_entry *thread)
+static void wakeup_thread_queue_multi_transfer(struct thread_entry *thread)
{
- const int priority = thread->priority;
- struct blocker *bl = thread->blocker;
- struct thread_entry * const tstart = thread;
- struct thread_entry *bl_t = bl->thread;
+ /* All threads will have the same blocker and queue; only we are changing
+ it now */
+ struct thread_entry **bqp = thread->bqp;
+ struct blocker_splay *blsplay = (struct blocker_splay *)thread->blocker;
+ struct thread_entry *blt = blsplay->blocker.thread;
+
+ /* The first thread is already locked and is assumed tagged "multi" */
+ int count = 1;
+ struct thread_entry *temp_queue = NULL;
+
+ /* 'thread' is locked on entry */
+ while (1)
+ {
+ LOCK_THREAD(blt);
- /* Blocker cannot change since object will be locked */
- LOCK_THREAD(bl_t);
+ remove_from_list_l(bqp, thread);
+ thread->blocker = NULL;
- thread->blocker = NULL; /* Thread not blocked */
+ struct thread_entry *tnext = *bqp;
+ if (tnext == NULL || tnext->retval == 0)
+ break;
- for (;;)
- {
- struct thread_entry *next;
- int bl_pr = bl->priority;
+ add_to_list_l(&temp_queue, thread);
- if (priority > bl_pr)
- break; /* Object priority higher */
+ UNLOCK_THREAD(thread);
+ UNLOCK_THREAD(blt);
- next = *thread->bqp;
+ count++;
+ thread = tnext;
- if (next == NULL)
- {
- /* No more threads in queue */
- prio_subtract_entry(&bl_t->pdist, bl_pr);
- bl->priority = PRIORITY_IDLE;
- }
- else
- {
- /* Check list for highest remaining priority */
- int queue_pr = find_highest_priority_in_list_l(next);
+ LOCK_THREAD(thread);
+ }
- if (queue_pr == bl_pr)
- break; /* Object priority not changing */
+ int blpr = blsplay->blocker.priority;
+ priority_disinherit_internal(blt, blpr);
- /* Change queue priority */
- prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
- bl->priority = queue_pr;
- }
+ /* Locking order reverses here since the threads are no longer on the
+ queue side */
+ if (count > 1)
+ {
+ add_to_list_l(&temp_queue, thread);
+ UNLOCK_THREAD(thread);
+ corelock_lock(&blsplay->cl);
+
+ blpr = find_highest_priority_in_list_l(*bqp);
+ blsplay->blocker.thread = NULL;
- if (bl_pr > bl_t->priority)
- break; /* thread priority is higher */
+ thread = temp_queue;
+ LOCK_THREAD(thread);
+ }
+ else
+ {
+ /* Becomes a simple, direct transfer */
+ if (thread->priority <= blpr)
+ blpr = find_highest_priority_in_list_l(*bqp);
+ blsplay->blocker.thread = thread;
+ }
- bl_pr = find_first_set_bit(bl_t->pdist.mask);
+ blsplay->blocker.priority = blpr;
- if (bl_pr == bl_t->priority)
- break; /* Thread priority not changing */
+ while (1)
+ {
+ unsigned int slotnum = THREAD_ID_SLOT(thread->id);
+ threadbit_set_bit(&blsplay->mask, slotnum);
- if (bl_t->state == STATE_RUNNING)
+ if (blpr < PRIORITY_IDLE)
{
- /* No further blockers */
- set_running_thread_priority(bl_t, bl_pr);
- break;
+ prio_add_entry(&thread->pdist, blpr);
+ if (blpr < thread->priority)
+ thread->priority = blpr;
}
- bl_t->priority = bl_pr;
-
- /* If blocking thread has a blocker, apply transitive inheritance */
- bl = bl_t->blocker;
+ if (count > 1)
+ remove_from_list_l(&temp_queue, thread);
- if (bl == NULL)
- break; /* End of chain or object doesn't support inheritance */
-
- next = bl->thread;
-
- if (UNLIKELY(next == tstart))
- break; /* Full-circle - deadlock! */
+ core_schedule_wakeup(thread);
UNLOCK_THREAD(thread);
-#if NUM_CORES > 1
- for (;;)
- {
- LOCK_THREAD(next);
-
- /* Blocker could change - retest condition */
- if (LIKELY(bl->thread == next))
- break;
+ thread = temp_queue;
+ if (thread == NULL)
+ break;
- UNLOCK_THREAD(next);
- next = bl->thread;
- }
-#endif
- thread = bl_t;
- bl_t = next;
+ LOCK_THREAD(thread);
}
- UNLOCK_THREAD(bl_t);
+ UNLOCK_THREAD(blt);
-#if NUM_CORES > 1
- if (UNLIKELY(thread != tstart))
+ if (count > 1)
{
- /* Relock original if it changed */
- LOCK_THREAD(tstart);
+ corelock_unlock(&blsplay->cl);
}
-#endif
- return cores[CURRENT_CORE].running;
+ blt->retval = count;
}
/*---------------------------------------------------------------------------
@@ -801,67 +902,95 @@ struct thread_entry *
* it is the running thread is made.
*---------------------------------------------------------------------------
*/
-struct thread_entry *
- wakeup_priority_protocol_transfer(struct thread_entry *thread)
+static void wakeup_thread_transfer(struct thread_entry *thread)
{
- /* Waking thread inherits priority boost from object owner */
+ /* Waking thread inherits priority boost from object owner (blt) */
struct blocker *bl = thread->blocker;
- struct thread_entry *bl_t = bl->thread;
- struct thread_entry *next;
- int bl_pr;
+ struct thread_entry *blt = bl->thread;
- THREAD_ASSERT(cores[CURRENT_CORE].running == bl_t,
+ THREAD_ASSERT(cores[CURRENT_CORE].running == blt,
"UPPT->wrong thread", cores[CURRENT_CORE].running);
- LOCK_THREAD(bl_t);
+ LOCK_THREAD(blt);
+
+ struct thread_entry **bqp = thread->bqp;
+ remove_from_list_l(bqp, thread);
+ thread->blocker = NULL;
- bl_pr = bl->priority;
+ int blpr = bl->priority;
/* Remove the object's boost from the owning thread */
- if (prio_subtract_entry(&bl_t->pdist, bl_pr) == 0 &&
- bl_pr <= bl_t->priority)
+ if (prio_subtract_entry(&blt->pdist, blpr) == 0 && blpr <= blt->priority)
{
/* No more threads at this priority are waiting and the old level is
* at least the thread level */
- int priority = find_first_set_bit(bl_t->pdist.mask);
-
- if (priority != bl_t->priority)
- {
- /* Adjust this thread's priority */
- set_running_thread_priority(bl_t, priority);
- }
+ int priority = priobit_ffs(&blt->pdist.mask);
+ if (priority != blt->priority)
+ set_running_thread_priority(blt, priority);
}
- next = *thread->bqp;
+ struct thread_entry *tnext = *bqp;
- if (LIKELY(next == NULL))
+ if (LIKELY(tnext == NULL))
{
/* Expected shortcut - no more waiters */
- bl_pr = PRIORITY_IDLE;
+ blpr = PRIORITY_IDLE;
}
else
{
- if (thread->priority <= bl_pr)
- {
- /* Need to scan threads remaining in queue */
- bl_pr = find_highest_priority_in_list_l(next);
- }
+ /* If lowering, we need to scan threads remaining in queue */
+ int priority = thread->priority;
+ if (priority <= blpr)
+ blpr = find_highest_priority_in_list_l(tnext);
- if (prio_add_entry(&thread->pdist, bl_pr) == 1 &&
- bl_pr < thread->priority)
- {
- /* Thread priority must be raised */
- thread->priority = bl_pr;
- }
+ if (prio_add_entry(&thread->pdist, blpr) == 1 && blpr < priority)
+ thread->priority = blpr; /* Raise new owner */
}
- bl->thread = thread; /* This thread pwns */
- bl->priority = bl_pr; /* Save highest blocked priority */
- thread->blocker = NULL; /* Thread not blocked */
+ core_schedule_wakeup(thread);
+ UNLOCK_THREAD(thread);
+
+ bl->thread = thread; /* This thread pwns */
+ bl->priority = blpr; /* Save highest blocked priority */
+ UNLOCK_THREAD(blt);
+}
+
+/*---------------------------------------------------------------------------
+ * Readjust priorities when waking a thread blocked waiting for another
+ * in essence "releasing" the thread's effect on the object owner. Can be
+ * performed from any context.
+ *---------------------------------------------------------------------------
+ */
+static void wakeup_thread_release(struct thread_entry *thread)
+{
+ struct blocker *bl = thread->blocker;
+ struct thread_entry *blt = lock_blocker_thread(bl);
+ struct thread_entry **bqp = thread->bqp;
+ remove_from_list_l(bqp, thread);
+ thread->blocker = NULL;
+
+ /* Off to see the wizard... */
+ core_schedule_wakeup(thread);
+
+ if (thread->priority > bl->priority)
+ {
+ /* Queue priority won't change */
+ UNLOCK_THREAD(thread);
+ unlock_blocker_thread(bl);
+ return;
+ }
+
+ UNLOCK_THREAD(thread);
- UNLOCK_THREAD(bl_t);
+ int newblpr = find_highest_priority_in_list_l(*bqp);
+ if (newblpr == bl->priority)
+ {
+ /* Blocker priority won't change */
+ unlock_blocker_thread(bl);
+ return;
+ }
- return bl_t;
+ inherit_priority(bl, bl, blt, newblpr);
}
/*---------------------------------------------------------------------------
@@ -877,9 +1006,8 @@ static void __attribute__((noinline)) check_for_obj_waiters(
{
/* Only one bit in the mask should be set with a frequency on 1 which
* represents the thread's own base priority */
- uint32_t mask = thread->pdist.mask;
- if ((mask & (mask - 1)) != 0 ||
- thread->pdist.hist[find_first_set_bit(mask)] > 1)
+ if (priobit_popcount(&thread->pdist.mask) != 1 ||
+ thread->pdist.hist[priobit_ffs(&thread->pdist.mask)] > 1)
{
unsigned char name[32];
thread_get_name(name, 32, thread);
@@ -889,26 +1017,72 @@ static void __attribute__((noinline)) check_for_obj_waiters(
#endif /* HAVE_PRIORITY_SCHEDULING */
/*---------------------------------------------------------------------------
- * Move a thread back to a running state on its core.
+ * Explicitly wakeup a thread on a blocking queue. Only effects threads of
+ * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
+ *
+ * This code should be considered a critical section by the caller meaning
+ * that the object's corelock should be held.
+ *
+ * INTERNAL: Intended for use by kernel objects and not for programs.
*---------------------------------------------------------------------------
*/
-static void core_schedule_wakeup(struct thread_entry *thread)
+unsigned int wakeup_thread_(struct thread_entry **list
+ IF_PRIO(, enum wakeup_thread_protocol proto))
{
- const unsigned int core = IF_COP_CORE(thread->core);
+ struct thread_entry *thread = *list;
- RTR_LOCK(core);
+ /* Check if there is a blocked thread at all. */
+ if (*list == NULL)
+ return THREAD_NONE;
- thread->state = STATE_RUNNING;
+ LOCK_THREAD(thread);
- add_to_list_l(&cores[core].running, thread);
- rtr_add_entry(core, thread->priority);
+ /* Determine thread's current state. */
+ switch (thread->state)
+ {
+ case STATE_BLOCKED:
+ case STATE_BLOCKED_W_TMO:
+#ifdef HAVE_PRIORITY_SCHEDULING
+ /* Threads with PIP blockers cannot specify "WAKEUP_DEFAULT" */
+ if (thread->blocker != NULL)
+ {
+ static void (* const funcs[])(struct thread_entry *thread)
+ ICONST_ATTR =
+ {
+ [WAKEUP_DEFAULT] = NULL,
+ [WAKEUP_TRANSFER] = wakeup_thread_transfer,
+ [WAKEUP_RELEASE] = wakeup_thread_release,
+ [WAKEUP_TRANSFER_MULTI] = wakeup_thread_queue_multi_transfer,
+ };
+
+ /* Call the specified unblocking PIP (does the rest) */
+ funcs[proto](thread);
+ }
+ else
+#endif /* HAVE_PRIORITY_SCHEDULING */
+ {
+ /* No PIP - just boost the thread by aging */
+#ifdef HAVE_PRIORITY_SCHEDULING
+ IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
+ thread->skip_count = thread->priority;
+#endif /* HAVE_PRIORITY_SCHEDULING */
+ remove_from_list_l(list, thread);
+ core_schedule_wakeup(thread);
+ UNLOCK_THREAD(thread);
+ }
- RTR_UNLOCK(core);
+ return should_switch_tasks();
-#if NUM_CORES > 1
- if (core != CURRENT_CORE)
- core_wake(core);
+ /* Nothing to do. State is not blocked. */
+ default:
+#if THREAD_EXTRA_CHECKS
+ THREAD_PANICF("wakeup_thread->block invalid", thread);
+ case STATE_RUNNING:
+ case STATE_KILLED:
#endif
+ UNLOCK_THREAD(thread);
+ return THREAD_NONE;
+ }
}
/*---------------------------------------------------------------------------
@@ -990,8 +1164,6 @@ void check_tmo_threads(void)
}
#endif /* NUM_CORES */
- remove_from_list_l(curr->bqp, curr);
-
#ifdef HAVE_WAKEUP_EXT_CB
if (curr->wakeup_ext_cb != NULL)
curr->wakeup_ext_cb(curr);
@@ -999,8 +1171,11 @@ void check_tmo_threads(void)
#ifdef HAVE_PRIORITY_SCHEDULING
if (curr->blocker != NULL)
- wakeup_priority_protocol_release(curr);
+ wakeup_thread_release(curr);
+ else
#endif
+ remove_from_list_l(curr->bqp, curr);
+
corelock_unlock(ocl);
}
/* else state == STATE_SLEEPING */
@@ -1161,8 +1336,7 @@ void switch_thread(void)
/* Begin task switching by saving our current context so that we can
* restore the state of the current thread later to the point prior
* to this call. */
- store_context(&thread->context);
-
+ thread_store_context(thread);
#ifdef DEBUG
/* Check core_ctx buflib integrity */
core_check_valid();
@@ -1212,8 +1386,7 @@ void switch_thread(void)
/* Select the new task based on priorities and the last time a
* process got CPU time relative to the highest priority runnable
* task. */
- struct priority_distribution *pd = &cores[core].rtr;
- int max = find_first_set_bit(pd->mask);
+ int max = priobit_ffs(&cores[core].rtr.mask);
if (block == NULL)
{
@@ -1269,7 +1442,7 @@ void switch_thread(void)
}
/* And finally give control to the next thread. */
- load_context(&thread->context);
+ thread_load_context(thread);
#ifdef RB_PROFILE
profile_thread_started(thread->id & THREAD_ID_SLOT_MASK);
@@ -1291,140 +1464,59 @@ void sleep_thread(int ticks)
LOCK_THREAD(current);
/* Set our timeout, remove from run list and join timeout list. */
- current->tmo_tick = current_tick + ticks + 1;
+ current->tmo_tick = current_tick + MAX(ticks, 0) + 1;
block_thread_on_l(current, STATE_SLEEPING);
UNLOCK_THREAD(current);
}
/*---------------------------------------------------------------------------
- * Indefinitely block a thread on a blocking queue for explicit wakeup.
+ * Block a thread on a blocking queue for explicit wakeup. If timeout is
+ * negative, the block is infinite.
*
* INTERNAL: Intended for use by kernel objects and not for programs.
*---------------------------------------------------------------------------
*/
-void block_thread(struct thread_entry *current)
+void block_thread(struct thread_entry *current, int timeout)
{
- /* Set the state to blocked and take us off of the run queue until we
- * are explicitly woken */
LOCK_THREAD(current);
- /* Set the list for explicit wakeup */
- block_thread_on_l(current, STATE_BLOCKED);
-
+ struct blocker *bl = NULL;
#ifdef HAVE_PRIORITY_SCHEDULING
- if (current->blocker != NULL)
+ bl = current->blocker;
+ struct thread_entry *blt = bl ? lock_blocker_thread(bl) : NULL;
+#endif /* HAVE_PRIORITY_SCHEDULING */
+
+ if (LIKELY(timeout < 0))
{
- /* Object supports PIP */
- current = blocker_inherit_priority(current);
+ /* Block until explicitly woken */
+ block_thread_on_l(current, STATE_BLOCKED);
}
-#endif
-
- UNLOCK_THREAD(current);
-}
-
-/*---------------------------------------------------------------------------
- * Block a thread on a blocking queue for a specified time interval or until
- * explicitly woken - whichever happens first.
- *
- * INTERNAL: Intended for use by kernel objects and not for programs.
- *---------------------------------------------------------------------------
- */
-void block_thread_w_tmo(struct thread_entry *current, int timeout)
-{
- /* Get the entry for the current running thread. */
- LOCK_THREAD(current);
-
- /* Set the state to blocked with the specified timeout */
- current->tmo_tick = current_tick + timeout;
-
- /* Set the list for explicit wakeup */
- block_thread_on_l(current, STATE_BLOCKED_W_TMO);
-
-#ifdef HAVE_PRIORITY_SCHEDULING
- if (current->blocker != NULL)
+ else
{
- /* Object supports PIP */
- current = blocker_inherit_priority(current);
+ /* Set the state to blocked with the specified timeout */
+ current->tmo_tick = current_tick + timeout;
+ block_thread_on_l(current, STATE_BLOCKED_W_TMO);
}
-#endif
- UNLOCK_THREAD(current);
-}
-
-/*---------------------------------------------------------------------------
- * Explicitly wakeup a thread on a blocking queue. Only effects threads of
- * STATE_BLOCKED and STATE_BLOCKED_W_TMO.
- *
- * This code should be considered a critical section by the caller meaning
- * that the object's corelock should be held.
- *
- * INTERNAL: Intended for use by kernel objects and not for programs.
- *---------------------------------------------------------------------------
- */
-unsigned int wakeup_thread(struct thread_entry **list)
-{
- struct thread_entry *thread = *list;
- unsigned int result = THREAD_NONE;
-
- /* Check if there is a blocked thread at all. */
- if (thread == NULL)
- return result;
-
- LOCK_THREAD(thread);
-
- /* Determine thread's current state. */
- switch (thread->state)
+ if (bl == NULL)
{
- case STATE_BLOCKED:
- case STATE_BLOCKED_W_TMO:
- remove_from_list_l(list, thread);
-
- result = THREAD_OK;
+ UNLOCK_THREAD(current);
+ return;
+ }
#ifdef HAVE_PRIORITY_SCHEDULING
- struct thread_entry *current;
- struct blocker *bl = thread->blocker;
-
- if (bl == NULL)
- {
- /* No inheritance - just boost the thread by aging */
- IF_NO_SKIP_YIELD( if (thread->skip_count != -1) )
- thread->skip_count = thread->priority;
- current = cores[CURRENT_CORE].running;
- }
- else
- {
- /* Call the specified unblocking PIP */
- current = bl->wakeup_protocol(thread);
- }
-
- if (current != NULL &&
- find_first_set_bit(cores[IF_COP_CORE(current->core)].rtr.mask)
- < current->priority)
- {
- /* There is a thread ready to run of higher or same priority on
- * the same core as the current one; recommend a task switch.
- * Knowing if this is an interrupt call would be helpful here. */
- result |= THREAD_SWITCH;
- }
-#endif /* HAVE_PRIORITY_SCHEDULING */
-
- core_schedule_wakeup(thread);
- break;
+ int newblpr = current->priority;
+ UNLOCK_THREAD(current);
- /* Nothing to do. State is not blocked. */
-#if THREAD_EXTRA_CHECKS
- default:
- THREAD_PANICF("wakeup_thread->block invalid", thread);
- case STATE_RUNNING:
- case STATE_KILLED:
- break;
-#endif
+ if (newblpr >= bl->priority)
+ {
+ unlock_blocker_thread(bl);
+ return; /* Queue priority won't change */
}
- UNLOCK_THREAD(thread);
- return result;
+ inherit_priority(bl, bl, blt, newblpr);
+#endif /* HAVE_PRIORITY_SCHEDULING */
}
/*---------------------------------------------------------------------------
@@ -1435,25 +1527,31 @@ unsigned int wakeup_thread(struct thread_entry **list)
* INTERNAL: Intended for use by kernel objects and not for programs.
*---------------------------------------------------------------------------
*/
-unsigned int thread_queue_wake(struct thread_entry **list)
+unsigned int thread_queue_wake(struct thread_entry **list,
+ volatile int *count)
{
+ int num = 0;
unsigned result = THREAD_NONE;
for (;;)
{
- unsigned int rc = wakeup_thread(list);
+ unsigned int rc = wakeup_thread(list, WAKEUP_DEFAULT);
if (rc == THREAD_NONE)
break; /* No more threads */
result |= rc;
+ num++;
}
+ if (count)
+ *count = num;
+
return result;
}
/*---------------------------------------------------------------------------
- * Assign the thread slot a new ID. Version is 1-255.
+ * Assign the thread slot a new ID. Version is 0x00000100..0xffffff00.
*---------------------------------------------------------------------------
*/
static void new_thread_id(unsigned int slot_num,
@@ -1693,7 +1791,7 @@ void thread_wait(unsigned int thread_id)
current->bqp = &thread->queue;
disable_irq();
- block_thread(current);
+ block_thread(current, TIMEOUT_BLOCK);
corelock_unlock(&thread->waiter_cl);
@@ -1723,7 +1821,7 @@ static inline void thread_final_exit(struct thread_entry *current)
* execution except the slot itself. */
/* Signal this thread */
- thread_queue_wake(&current->queue);
+ thread_queue_wake(&current->queue, NULL);
corelock_unlock(&current->waiter_cl);
switch_thread();
/* This should never and must never be reached - if it is, the
@@ -1912,20 +2010,18 @@ IF_COP( retry_state: )
}
}
#endif
- remove_from_list_l(thread->bqp, thread);
-
#ifdef HAVE_WAKEUP_EXT_CB
if (thread->wakeup_ext_cb != NULL)
thread->wakeup_ext_cb(thread);
#endif
#ifdef HAVE_PRIORITY_SCHEDULING
+ /* Remove thread's priority influence from its chain if needed */
if (thread->blocker != NULL)
- {
- /* Remove thread's priority influence from its chain */
wakeup_priority_protocol_release(thread);
- }
+ else
#endif
+ remove_from_list_l(thread->bqp, thread);
#if NUM_CORES > 1
if (ocl != NULL)
@@ -1970,130 +2066,77 @@ thread_killed: /* Thread was already killed */
*/
int thread_set_priority(unsigned int thread_id, int priority)
{
+ if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
+ return -1; /* Invalid priority argument */
+
int old_base_priority = -1;
struct thread_entry *thread = thread_id_entry(thread_id);
- /* A little safety measure */
- if (priority < HIGHEST_PRIORITY || priority > LOWEST_PRIORITY)
- return -1;
-
/* Thread could be on any list and therefore on an interrupt accessible
one - disable interrupts */
- int oldlevel = disable_irq_save();
-
+ const int oldlevel = disable_irq_save();
LOCK_THREAD(thread);
- /* Make sure it's not killed */
- if (thread->id == thread_id && thread->state != STATE_KILLED)
- {
- int old_priority = thread->priority;
-
- old_base_priority = thread->base_priority;
- thread->base_priority = priority;
-
- prio_move_entry(&thread->pdist, old_base_priority, priority);
- priority = find_first_set_bit(thread->pdist.mask);
-
- if (old_priority == priority)
- {
- /* No priority change - do nothing */
- }
- else if (thread->state == STATE_RUNNING)
- {
- /* This thread is running - change location on the run
- * queue. No transitive inheritance needed. */
- set_running_thread_priority(thread, priority);
- }
- else
- {
- thread->priority = priority;
-
- if (thread->blocker != NULL)
- {
- /* Bubble new priority down the chain */
- struct blocker *bl = thread->blocker; /* Blocker struct */
- struct thread_entry *bl_t = bl->thread; /* Blocking thread */
- struct thread_entry * const tstart = thread; /* Initial thread */
- const int highest = MIN(priority, old_priority); /* Higher of new or old */
-
- for (;;)
- {
- struct thread_entry *next; /* Next thread to check */
- int bl_pr; /* Highest blocked thread */
- int queue_pr; /* New highest blocked thread */
-#if NUM_CORES > 1
- /* Owner can change but thread cannot be dislodged - thread
- * may not be the first in the queue which allows other
- * threads ahead in the list to be given ownership during the
- * operation. If thread is next then the waker will have to
- * wait for us and the owner of the object will remain fixed.
- * If we successfully grab the owner -- which at some point
- * is guaranteed -- then the queue remains fixed until we
- * pass by. */
- for (;;)
- {
- LOCK_THREAD(bl_t);
-
- /* Double-check the owner - retry if it changed */
- if (LIKELY(bl->thread == bl_t))
- break;
-
- UNLOCK_THREAD(bl_t);
- bl_t = bl->thread;
- }
-#endif
- bl_pr = bl->priority;
-
- if (highest > bl_pr)
- break; /* Object priority won't change */
+ if (thread->id != thread_id || thread->state == STATE_KILLED)
+ goto done; /* Invalid thread */
- /* This will include the thread being set */
- queue_pr = find_highest_priority_in_list_l(*thread->bqp);
+ old_base_priority = thread->base_priority;
+ if (priority == old_base_priority)
+ goto done; /* No base priority change */
- if (queue_pr == bl_pr)
- break; /* Object priority not changing */
+ thread->base_priority = priority;
- /* Update thread boost for this object */
- bl->priority = queue_pr;
- prio_move_entry(&bl_t->pdist, bl_pr, queue_pr);
- bl_pr = find_first_set_bit(bl_t->pdist.mask);
+ /* Adjust the thread's priority influence on itself */
+ prio_move_entry(&thread->pdist, old_base_priority, priority);
- if (bl_t->priority == bl_pr)
- break; /* Blocking thread priority not changing */
+ int old_priority = thread->priority;
+ int new_priority = priobit_ffs(&thread->pdist.mask);
- if (bl_t->state == STATE_RUNNING)
- {
- /* Thread not blocked - we're done */
- set_running_thread_priority(bl_t, bl_pr);
- break;
- }
+ if (old_priority == new_priority)
+ goto done; /* No running priority change */
- bl_t->priority = bl_pr;
- bl = bl_t->blocker; /* Blocking thread has a blocker? */
+ if (thread->state == STATE_RUNNING)
+ {
+ /* This thread is running - just change location on the run queue.
+ Also sets thread->priority. */
+ set_running_thread_priority(thread, new_priority);
+ goto done;
+ }
- if (bl == NULL)
- break; /* End of chain */
+ /* Thread is blocked */
+ struct blocker *bl = thread->blocker;
+ if (bl == NULL)
+ {
+ thread->priority = new_priority;
+ goto done; /* End of transitive blocks */
+ }
- next = bl->thread;
+ struct thread_entry *blt = lock_blocker_thread(bl);
+ struct thread_entry **bqp = thread->bqp;
- if (UNLIKELY(next == tstart))
- break; /* Full-circle */
+ thread->priority = new_priority;
- UNLOCK_THREAD(thread);
+ UNLOCK_THREAD(thread);
+ thread = NULL;
- thread = bl_t;
- bl_t = next;
- } /* for (;;) */
+ int oldblpr = bl->priority;
+ int newblpr = oldblpr;
+ if (new_priority < oldblpr)
+ newblpr = new_priority;
+ else if (old_priority <= oldblpr)
+ newblpr = find_highest_priority_in_list_l(*bqp);
- UNLOCK_THREAD(bl_t);
- }
- }
+ if (newblpr == oldblpr)
+ {
+ unlock_blocker_thread(bl);
+ goto done;
}
- UNLOCK_THREAD(thread);
-
+ inherit_priority(bl, bl, blt, newblpr);
+done:
+ if (thread)
+ UNLOCK_THREAD(thread);
restore_irq(oldlevel);
-
return old_base_priority;
}
diff --git a/firmware/target/hosted/sdl/thread-sdl.c b/firmware/target/hosted/sdl/thread-sdl.c
index c17e793833..eaf59e245d 100644
--- a/firmware/target/hosted/sdl/thread-sdl.c
+++ b/firmware/target/hosted/sdl/thread-sdl.c
@@ -406,20 +406,20 @@ void sleep_thread(int ticks)
current->tmo_tick = (1000/HZ) * ticks + ((1000/HZ)-1) - rem;
}
-void block_thread(struct thread_entry *current)
+void block_thread(struct thread_entry *current, int ticks)
{
- current->state = STATE_BLOCKED;
- add_to_list_l(current->bqp, current);
-}
+ if (ticks < 0)
+ current->state = STATE_BLOCKED;
+ else
+ {
+ current->state = STATE_BLOCKED_W_TMO;
+ current->tmo_tick = (1000/HZ)*ticks;
+ }
-void block_thread_w_tmo(struct thread_entry *current, int ticks)
-{
- current->state = STATE_BLOCKED_W_TMO;
- current->tmo_tick = (1000/HZ)*ticks;
add_to_list_l(current->bqp, current);
}
-unsigned int wakeup_thread(struct thread_entry **list)
+unsigned int wakeup_thread_(struct thread_entry **list)
{
struct thread_entry *thread = *list;
@@ -439,20 +439,26 @@ unsigned int wakeup_thread(struct thread_entry **list)
return THREAD_NONE;
}
-unsigned int thread_queue_wake(struct thread_entry **list)
+unsigned int thread_queue_wake(struct thread_entry **list,
+ volatile int *count)
{
unsigned int result = THREAD_NONE;
+ int num = 0;
for (;;)
{
- unsigned int rc = wakeup_thread(list);
+ unsigned int rc = wakeup_thread_(list);
if (rc == THREAD_NONE)
break;
- result |= rc;
+ result |= rc;
+ num++;
}
+ if (count)
+ *count = num;
+
return result;
}
@@ -615,7 +621,7 @@ void remove_thread(unsigned int thread_id)
new_thread_id(thread->id, thread);
thread->state = STATE_KILLED;
- thread_queue_wake(&thread->queue);
+ thread_queue_wake(&thread->queue, NULL);
SDL_DestroySemaphore(s);
@@ -652,7 +658,7 @@ void thread_wait(unsigned int thread_id)
if (thread->id == thread_id && thread->state != STATE_KILLED)
{
current->bqp = &thread->queue;
- block_thread(current);
+ block_thread(current, TIMEOUT_BLOCK);
switch_thread();
}
}