From acb5161d7384e6b6352c0a9f3cc067621fbf28ad Mon Sep 17 00:00:00 2001 From: Alex AUVOLAT Date: Fri, 28 Mar 2014 17:16:09 +0100 Subject: Import and compile article 6.75 --- sos-code-article6.75/sos/kwaitq.c | 321 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 321 insertions(+) create mode 100644 sos-code-article6.75/sos/kwaitq.c (limited to 'sos-code-article6.75/sos/kwaitq.c') diff --git a/sos-code-article6.75/sos/kwaitq.c b/sos-code-article6.75/sos/kwaitq.c new file mode 100644 index 0000000..2b49a04 --- /dev/null +++ b/sos-code-article6.75/sos/kwaitq.c @@ -0,0 +1,321 @@ +/* Copyright (C) 2004 David Decotigny + + 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 program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, + USA. +*/ + +#include +#include +#include +#include + +#include "kwaitq.h" + + +sos_ret_t sos_kwaitq_init(struct sos_kwaitq *kwq, + const char *name, + sos_kwaitq_ordering_t ordering) +{ + memset(kwq, 0x0, sizeof(struct sos_kwaitq)); + +#ifdef SOS_KWQ_DEBUG + if (! name) + name = ""; + strzcpy(kwq->name, name, SOS_KWQ_DEBUG_MAX_NAMELEN); +#endif + kwq->ordering = ordering; + list_init_named(kwq->waiting_list, + prev_entry_in_kwaitq, next_entry_in_kwaitq); + + return SOS_OK; +} + + +sos_ret_t sos_kwaitq_dispose(struct sos_kwaitq *kwq) +{ + sos_ui32_t flags; + sos_ret_t retval; + + sos_disable_IRQs(flags); + if (list_is_empty_named(kwq->waiting_list, + prev_entry_in_kwaitq, next_entry_in_kwaitq)) + retval = SOS_OK; + else + retval = -SOS_EBUSY; + + sos_restore_IRQs(flags); + return retval; +} + + +sos_bool_t sos_kwaitq_is_empty(const struct sos_kwaitq *kwq) +{ + sos_ui32_t flags; + sos_ret_t retval; + + sos_disable_IRQs(flags); + retval = list_is_empty_named(kwq->waiting_list, + prev_entry_in_kwaitq, next_entry_in_kwaitq); + + sos_restore_IRQs(flags); + return retval; +} + + +sos_ret_t sos_kwaitq_init_entry(struct sos_kwaitq_entry *kwq_entry) +{ + memset(kwq_entry, 0x0, sizeof(struct sos_kwaitq_entry)); + kwq_entry->thread = sos_thread_get_current(); + return SOS_OK; +} + + +/** Internal helper function equivalent to sos_kwaitq_add_entry(), but + without interrupt protection scheme */ +inline static sos_ret_t +_kwaitq_add_entry(struct sos_kwaitq *kwq, + struct sos_kwaitq_entry *kwq_entry, + sos_sched_priority_t prio) +{ + struct sos_kwaitq_entry *next_entry = NULL, *entry; + int nb_entries; + + /* This entry is already added in the kwaitq ! */ + SOS_ASSERT_FATAL(NULL == kwq_entry->kwaitq); + + /* sos_kwaitq_init_entry() has not been called ?! */ + SOS_ASSERT_FATAL(NULL != kwq_entry->thread); + + /* (Re-)Initialize wakeup status of the entry */ + kwq_entry->wakeup_triggered = FALSE; + kwq_entry->wakeup_status = SOS_OK; + + /* Insert this entry in the kwaitq waiting list */ + switch (kwq->ordering) + { + case SOS_KWQ_ORDER_FIFO: + /* Insertion in the list in FIFO order */ + { + /* Add the thread in the list */ + list_add_tail_named(kwq->waiting_list, kwq_entry, + prev_entry_in_kwaitq, next_entry_in_kwaitq); + } + break; + + case SOS_KWQ_ORDER_PRIO: + /* Priority-driven insertion in the list */ + { + /* Look for the place where to insert the thread in the queue (we + want to order them in order of increasing priorities) */ + list_foreach_forward_named(kwq->waiting_list, entry, nb_entries, + prev_entry_in_kwaitq, next_entry_in_kwaitq) + { + /* Does the thread we want to insert have higher priority than + the given thread in the queue ? */ + if (SOS_SCHED_PRIO_CMP(prio, + sos_thread_get_priority(entry->thread)) + > 0) + { + /* Yes: we insert before this given thread */ + next_entry = entry; + break; + } + } + + /* Actually insert the entry in the list */ + if (next_entry != NULL) + { + list_insert_before_named(kwq->waiting_list, kwq_entry, next_entry, + prev_entry_in_kwaitq, + next_entry_in_kwaitq); + } + else + { + /* The thread we want to insert has less priority than any + other in the list */ + list_add_tail_named(kwq->waiting_list, kwq_entry, + prev_entry_in_kwaitq, next_entry_in_kwaitq); + } + } + break; + + default: + SOS_FATAL_ERROR("Invalid kwq ordering %d !\n", kwq->ordering); + break; + } + + /* Update the list of waitqueues for the thread */ + list_add_tail_named(kwq_entry->thread->kwaitq_list, kwq_entry, + prev_entry_for_thread, next_entry_for_thread); + + kwq_entry->kwaitq = kwq; + + return SOS_OK; +} + + +sos_ret_t sos_kwaitq_add_entry(struct sos_kwaitq *kwq, + struct sos_kwaitq_entry *kwq_entry) +{ + sos_ui32_t flags; + sos_ret_t retval; + + sos_disable_IRQs(flags); + retval = _kwaitq_add_entry(kwq, kwq_entry, + sos_thread_get_priority(kwq_entry->thread)); + sos_restore_IRQs(flags); + + return retval; +} + + +/** Internal helper function equivalent to sos_kwaitq_remove_entry(), + but without interrupt protection scheme */ +inline static sos_ret_t +_kwaitq_remove_entry(struct sos_kwaitq *kwq, + struct sos_kwaitq_entry *kwq_entry) +{ + SOS_ASSERT_FATAL(kwq_entry->kwaitq == kwq); + + list_delete_named(kwq->waiting_list, kwq_entry, + prev_entry_in_kwaitq, next_entry_in_kwaitq); + + list_delete_named(kwq_entry->thread->kwaitq_list, kwq_entry, + prev_entry_for_thread, next_entry_for_thread); + + kwq_entry->kwaitq = NULL; + return SOS_OK; +} + + +sos_ret_t sos_kwaitq_remove_entry(struct sos_kwaitq *kwq, + struct sos_kwaitq_entry *kwq_entry) +{ + sos_ui32_t flags; + sos_ret_t retval; + + sos_disable_IRQs(flags); + retval = _kwaitq_remove_entry(kwq, kwq_entry); + sos_restore_IRQs(flags); + + return retval; +} + + +sos_ret_t sos_kwaitq_wait(struct sos_kwaitq *kwq, + struct sos_time *timeout) +{ + sos_ui32_t flags; + sos_ret_t retval; + struct sos_kwaitq_entry kwq_entry; + + sos_kwaitq_init_entry(& kwq_entry); + + sos_disable_IRQs(flags); + + retval = _kwaitq_add_entry(kwq, & kwq_entry, + sos_thread_get_priority(kwq_entry.thread)); + + /* Wait for wakeup or timeout */ + sos_thread_sleep(timeout); + /* Woken up ! */ + + /* Sleep delay elapsed ? */ + if (! kwq_entry.wakeup_triggered) + { + /* Yes (timeout occured, or wakeup on another waitqueue): remove + the waitq entry by ourselves */ + _kwaitq_remove_entry(kwq, & kwq_entry); + retval = -SOS_EINTR; + } + else + { + retval = kwq_entry.wakeup_status; + } + + sos_restore_IRQs(flags); + + /* We were correctly awoken: position return status */ + return retval; +} + + +sos_ret_t sos_kwaitq_wakeup(struct sos_kwaitq *kwq, + unsigned int nb_threads, + sos_ret_t wakeup_status) +{ + sos_ui32_t flags; + + sos_disable_IRQs(flags); + + /* Wake up as much threads waiting in waitqueue as possible (up to + nb_threads), scanning the list in FIFO/decreasing priority order + (depends on the kwaitq ordering) */ + while (! list_is_empty_named(kwq->waiting_list, + prev_entry_in_kwaitq, next_entry_in_kwaitq)) + { + struct sos_kwaitq_entry *kwq_entry + = list_get_head_named(kwq->waiting_list, + prev_entry_in_kwaitq, next_entry_in_kwaitq); + + /* Enough threads woken up ? */ + if (nb_threads <= 0) + break; + + /* + * Ok: wake up the thread for this entry + */ + + /* Thread already woken up ? */ + if (SOS_THR_RUNNING == sos_thread_get_state(kwq_entry->thread)) + { + /* Yes => Do nothing because WE are that woken-up thread. In + particular: don't call set_ready() here because this + would result in an inconsistent configuration (currently + running thread marked as "waiting for CPU"...). */ + continue; + } + else + { + /* No => wake it up now. */ + sos_sched_set_ready(kwq_entry->thread); + } + + /* Remove this waitq entry */ + _kwaitq_remove_entry(kwq, kwq_entry); + kwq_entry->wakeup_triggered = TRUE; + kwq_entry->wakeup_status = wakeup_status; + + /* Next iteration... */ + nb_threads --; + } + + sos_restore_IRQs(flags); + + return SOS_OK; +} + + +/* Internal function (callback for thread subsystem) */ +sos_ret_t sos_kwaitq_change_priority(struct sos_kwaitq *kwq, + struct sos_kwaitq_entry *kwq_entry, + sos_sched_priority_t priority) +{ + /* Reorder the waiting list */ + _kwaitq_remove_entry(kwq, kwq_entry); + _kwaitq_add_entry(kwq, kwq_entry, priority); + + return SOS_OK; +} -- cgit v1.2.3