unified: Add support for semaphore groups
Semaphore groups are enabled by default. Disabling them will both
decrease the footprint as well as improve the performance of the
k_sem_give() routine.
Change-Id: If6c1b0e2e1f71afd43e620f05f17068039d12b05
Signed-off-by: Peter Mitsis <peter.mitsis@windriver.com>
diff --git a/include/kernel.h b/include/kernel.h
index 74dcd2f..c779301 100644
--- a/include/kernel.h
+++ b/include/kernel.h
@@ -644,10 +644,53 @@
return sem->count;
}
-extern struct k_sem *k_sem_group_take(struct k_sem **sem_array,
- int32_t timeout);
-extern void k_sem_group_give(struct k_sem **sem_array);
-extern void k_sem_group_reset(struct k_sem **sem_array);
+#ifdef CONFIG_SEMAPHORE_GROUPS
+/**
+ * @brief Take the first available semaphore
+ *
+ * Given a list of semaphore pointers, this routine will attempt to take one
+ * of them, waiting up to a maximum of @a timeout ms to do so. The taken
+ * semaphore is identified by @a sem (set to NULL on error).
+ *
+ * Be aware that the more semaphores specified in the group, the more stack
+ * space is required by the waiting thread.
+ *
+ * @param sem_array Array of semaphore pointers terminated by a K_END entry
+ * @param sem Identifies the semaphore that was taken
+ * @param timeout Maximum number of milliseconds to wait
+ *
+ * @retval 0 A semaphore was successfully taken
+ * @retval -EBUSY No semaphore was available (@a timeout = K_NO_WAIT)
+ * @retval -EAGAIN Time out occurred while waiting for semaphore
+ */
+
+extern int k_sem_group_take(struct k_sem *sem_array[], struct k_sem **sem,
+ int32_t timeout);
+
+/**
+ * @brief Give all the semaphores in the group
+ *
+ * This routine will give each semaphore in the array of semaphore pointers.
+ *
+ * @param sem_array Array of semaphore pointers terminated by a K_END entry
+ *
+ * @return N/A
+ */
+extern void k_sem_group_give(struct k_sem *sem_array[]);
+
+/**
+ * @brief Reset the count to zero on each semaphore in the array
+ *
+ * This routine resets the count of each semaphore in the group to zero.
+ * Note that it does NOT have any impact on any thread that might have
+ * been previously pending on any of the semaphores.
+ *
+ * @param sem_array Array of semaphore pointers terminated by a K_END entry
+ *
+ * @return N/A
+ */
+extern void k_sem_group_reset(struct k_sem *sem_array[]);
+#endif
#define K_SEM_INITIALIZER(obj, initial_count, count_limit) \
{ \
diff --git a/include/legacy.h b/include/legacy.h
index b16fb68..0759e32 100644
--- a/include/legacy.h
+++ b/include/legacy.h
@@ -273,15 +273,21 @@
#define task_sem_reset k_sem_reset
#define task_sem_count_get k_sem_count_get
+#ifdef CONFIG_SEMAPHORE_GROUPS
typedef ksem_t *ksemg_t;
static inline ksem_t task_sem_group_take(ksemg_t group, int32_t timeout)
{
- return k_sem_group_take(group, _ticks_to_ms(timeout));
+ struct k_sem *sem;
+
+ (void)k_sem_group_take(group, &sem, _ticks_to_ms(timeout));
+
+ return sem;
}
#define task_sem_group_give k_sem_group_give
#define task_sem_group_reset k_sem_group_reset
+#endif
#define DEFINE_SEMAPHORE(name) \
K_SEM_DEFINE(_k_sem_obj_##name, 0, UINT_MAX); \
diff --git a/kernel/unified/Kconfig b/kernel/unified/Kconfig
index 5db434e..39cbadc 100644
--- a/kernel/unified/Kconfig
+++ b/kernel/unified/Kconfig
@@ -277,4 +277,14 @@
not subject to time slicing.
endmenu
+
+config SEMAPHORE_GROUPS
+ bool "Enable semaphore groups"
+ default y
+ help
+ This option enables support for semaphore groups. Threads that use
+ semaphore groups require more stack space. Disabling this option will
+ both decrease the footprint as well as improve the performance of
+ the k_sem_give() routine.
+
endmenu
diff --git a/kernel/unified/sem.c b/kernel/unified/sem.c
index 43ef159..b142059 100644
--- a/kernel/unified/sem.c
+++ b/kernel/unified/sem.c
@@ -35,6 +35,19 @@
#include <misc/dlist.h>
#include <sched.h>
+#ifdef CONFIG_SEMAPHORE_GROUPS
+struct _sem_desc {
+ sys_dnode_t semg_node; /* Node in list of semaphores */
+ struct k_thread *thread; /* Thread waiting for semaphores */
+ struct k_sem *sem; /* Semaphore on which to wait */
+};
+
+struct _sem_thread {
+ struct tcs_base dummy;
+ struct _sem_desc desc;
+};
+#endif
+
void k_sem_init(struct k_sem *sem, unsigned int initial_count,
unsigned int limit)
{
@@ -46,29 +59,233 @@
SYS_TRACING_OBJ_INIT(nano_sem, sem);
}
-void k_sem_give(struct k_sem *sem)
+#ifdef CONFIG_SEMAPHORE_GROUPS
+int k_sem_group_take(struct k_sem *sem_array[], struct k_sem **sem,
+ int32_t timeout)
{
- int key = irq_lock();
- struct tcs *first_pending_thread = _unpend_first_thread(&sem->wait_q);
+ unsigned int key;
+ struct k_sem *item = *sem_array;
+ int num = 0;
- if (first_pending_thread) {
- _timeout_abort(first_pending_thread);
- _ready_thread(first_pending_thread);
+ __ASSERT(sem_array[0] != K_END, "Empty semaphore list");
- _set_thread_return_value(first_pending_thread, 0);
+ key = irq_lock();
- if (!_is_in_isr() && _must_switch_threads()) {
- _Swap(key);
- return;
+ do {
+ if (item->count > 0) {
+ item->count--; /* Available semaphore found */
+ irq_unlock(key);
+ *sem = item;
+ return 0;
}
- } else {
- if (likely(sem->count != sem->limit)) {
- sem->count++;
- }
+ num++;
+ item = sem_array[num];
+ } while (item != K_END);
+
+ if (timeout == K_NO_WAIT) {
+ irq_unlock(key);
+ *sem = NULL;
+ return -EBUSY;
}
+ struct _sem_thread wait_objects[num];
+ int32_t priority = k_thread_priority_get(_current);
+ sys_dlist_t list;
+
+ sys_dlist_init(&list);
+ _current->swap_data = &list;
+
+ for (int i = 0; i < num; i++) {
+ wait_objects[i].dummy.flags = K_DUMMY;
+ wait_objects[i].dummy.prio = priority;
+
+ _timeout_tcs_init((struct k_thread *) &wait_objects[i].dummy);
+
+ sys_dlist_append(&list, &wait_objects[i].desc.semg_node);
+ wait_objects[i].desc.thread = _current;
+ wait_objects[i].desc.sem = sem_array[i];
+
+ _pend_thread((struct k_thread *)&wait_objects[i].dummy,
+ &sem_array[i]->wait_q, timeout);
+ }
+
+ /* Pend the current thread on a dummy wait queue */
+
+ _wait_q_t wait_q;
+
+ sys_dlist_init(&wait_q);
+ _pend_current_thread(&wait_q, timeout);
+
+ if (_Swap(key) != 0) {
+ *sem = NULL;
+ return -EAGAIN;
+ }
+
+ /* The accepted semaphore is the only one left on the list */
+
+ struct _sem_desc *desc = (struct _sem_desc *)sys_dlist_get(&list);
+
+ *sem = desc->sem;
+ return 0;
+}
+
+/**
+ * @brief Cancel all but specified semaphore in list if part of a semphore group
+ *
+ * Interrupts are locked prior to calling this routine
+ *
+ * @return 0 if not part of semaphore group, 1 if it is
+ */
+static int handle_sem_group(struct k_sem *sem, struct k_thread *thread)
+{
+ struct _sem_thread *dummy = (struct _sem_thread *)thread;
+ struct _sem_thread *sem_thread;
+ struct _sem_desc *desc = NULL;
+ sys_dlist_t *list;
+ sys_dnode_t *node;
+ sys_dnode_t *next;
+
+ if (!(thread->flags & K_DUMMY)) {
+ /*
+ * The awakened thread is a real thread and thus was not
+ * involved in a semaphore group operation.
+ */
+ return 0;
+ }
+
+ /*
+ * The awakened thread is a dummy thread and thus was involved
+ * in a semaphore group operation.
+ */
+
+ list = (sys_dlist_t *)dummy->desc.thread->swap_data;
+ node = sys_dlist_peek_head(list);
+
+ __ASSERT(node != NULL, "");
+
+ do {
+ next = sys_dlist_peek_next(list, node);
+
+ desc = (struct _sem_desc *)node;
+
+ if (desc->sem != sem) {
+ sem_thread = CONTAINER_OF(desc, struct _sem_thread,
+ desc);
+
+ _timeout_abort((struct k_thread *)&sem_thread->dummy);
+ _unpend_thread((struct k_thread *)&sem_thread->dummy);
+
+ sys_dlist_remove(node);
+ }
+ node = next;
+ } while (node != NULL);
+
+ /*
+ * If 'desc' is NULL, then the user-supplied 'sem_array' had only
+ * one semaphore in it. This is considered a user error as
+ * k_sem_give() should have been called instead.
+ */
+
+ __ASSERT(desc != NULL, "");
+
+ /*
+ * As this code may be executed several times by a semaphore group give
+ * operation, it is important to ensure that the attempt to ready the
+ * master thread is done only once.
+ */
+
+ if (!_is_thread_ready(desc->thread)) {
+ _reset_thread_states(desc->thread, K_PENDING | K_TIMING);
+ _timeout_abort(desc->thread);
+ if (_is_thread_ready(desc->thread)) {
+ _add_thread_to_ready_q(desc->thread);
+ }
+ }
+ _set_thread_return_value(desc->thread, 0);
+
+ return 1;
+}
+
+#else
+#define handle_sem_group(sem, thread) 0
+#endif
+
+/**
+ * @brief Common semaphore give code
+ *
+ * @return true if _Swap() will need to be invoked; false if not
+ */
+static bool sem_give_common(struct k_sem *sem)
+{
+ struct k_thread *thread;
+
+ thread = _unpend_first_thread(&sem->wait_q);
+ if (!thread) {
+ /*
+ * No thread is waiting on the semaphore.
+ * Increment the semaphore's count unless
+ * its limit has already been reached.
+ */
+ sem->count += (sem->count != sem->limit);
+ return false;
+ }
+
+ _timeout_abort(thread);
+
+ if (!handle_sem_group(sem, thread)) {
+ /* Handle the non-group case */
+ _ready_thread(thread);
+ _set_thread_return_value(thread, 0);
+ }
+
+ return !_is_in_isr() && _must_switch_threads();
+}
+
+#ifdef CONFIG_SEMAPHORE_GROUPS
+void k_sem_group_give(struct k_sem *sem_array[])
+{
+ unsigned int key;
+ bool swap_needed = false;
+
+ __ASSERT(sem_array[0] != K_END, "Empty semaphore list");
+
+ key = irq_lock();
+
+ for (int i = 0; sem_array[i] != K_END; i++) {
+ swap_needed |= sem_give_common(sem_array[i]);
+ }
+
+ if (swap_needed) {
+ _Swap(key);
+ } else {
+ irq_unlock(key);
+ }
+}
+
+void k_sem_group_reset(struct k_sem *sem_array[])
+{
+ unsigned int key;
+
+ key = irq_lock();
+ for (int i = 0; sem_array[i] != K_END; i++) {
+ sem_array[i]->count = 0;
+ }
irq_unlock(key);
}
+#endif
+
+void k_sem_give(struct k_sem *sem)
+{
+ unsigned int key;
+
+ key = irq_lock();
+
+ if (sem_give_common(sem)) {
+ _Swap(key);
+ } else {
+ irq_unlock(key);
+ }
+}
int k_sem_take(struct k_sem *sem, int32_t timeout)
{
