sos-code-article10/sos/kwaitq.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;
}