/* 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)
 {
   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
   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, and explicit priority
     ordering */
 inline static sos_ret_t _kwaitq_add_entry(struct sos_kwaitq *kwq,
                                           struct sos_kwaitq_entry *kwq_entry)
 {
   /* 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;
 
   /* Add the thread in the list */
   list_add_tail_named(kwq->waiting_list, kwq_entry,
                       prev_entry_in_kwaitq, next_entry_in_kwaitq);
 
   /* 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_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);
 
   /* 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 order */
   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;
 }