322 lines
8.1 KiB
C
322 lines
8.1 KiB
C
/* 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 <sos/klibc.h>
|
|
#include <sos/list.h>
|
|
#include <sos/assert.h>
|
|
#include <hwcore/irq.h>
|
|
|
|
#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 = "<unknown>";
|
|
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;
|
|
}
|