// CrossWorks Tasking Library.
//
// Copyright (c) 2004 Rowley Associates Limited.
//
// This file may be distributed under the terms of the License Agreement
// provided with this software.
//
// THIS FILE IS PROVIDED AS IS WITH NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.

#include "ctl_api.h"
#include "ctl_impl.h"

void
ctl_message_queue_init(CTL_MESSAGE_QUEUE_t *m,
                       void **queue,
                       unsigned queue_size)
{  
  int enabled = ctl_global_interrupts_disable();
  m->q = queue;
  m->s = queue_size;
  m->front = m->n = 0;
  ctl_global_interrupts_set(enabled);
}

#define ctl_private_message_queue_is_full(MQ) (MQ->n==MQ->s)
#define ctl_private_message_queue_is_empty(MQ) (MQ->n==0)

static void
ctl_private_message_queue_join(CTL_MESSAGE_QUEUE_t *m, void *msg)
{  
  m->q[(m->front+m->n)%(m->s)] = msg;
  m->n++;
}

static void *
ctl_private_message_queue_leave(CTL_MESSAGE_QUEUE_t *m)
{
  void *front = m->q[m->front];  
  m->front = (m->front+1)%(m->s);
  m->n--;
  return front;
}

unsigned 
ctl_message_queue_post(CTL_MESSAGE_QUEUE_t *mq, 
                       void *message, 
                       unsigned use_timeout, 
                       CTL_TIME_t timeout)
{
  ctl_global_interrupts_disable();  
  if (ctl_interrupt_count)
    ctl_handle_error(CTL_WAIT_CALLED_FROM_ISR);
  ctl_task_executing->timeout = 0;  
  if (ctl_private_message_queue_is_full(mq)) 
    {
      ctl_task_executing->timeout = timeout;  
      ctl_task_executing->wait_object = mq;
      ctl_task_executing->u.message = message;
      ctl_task_executing->state = use_timeout ? (CTL_STATE_MESSAGE_QUEUE_POST_WAIT | CTL_STATE_TIMER_WAIT) : CTL_STATE_MESSAGE_QUEUE_POST_WAIT;                
    }
  else // queue not full
    {     
      CTL_TASK_t *t = 0;
      if (ctl_private_message_queue_is_empty(mq)) // queue is empty - see if someone is waiting to receive a message
        {          
          for (t=ctl_task_list; t; t=t->next)
            if ((t->state & CTL_STATE_MESSAGE_QUEUE_RECEIVE_WAIT) && (t->wait_object == mq))
              {
                t->u.message = message;
                t->state = CTL_STATE_RUNNABLE;
                break;
              }
        }
      if (t == 0) // queue not empty || no one waiting to receive        
        ctl_private_message_queue_join(mq, message);  
      ctl_task_executing->timeout = 1;
    }
  ctl_task_reschedule();  
  return ctl_task_executing->timeout;
}

void *
ctl_message_queue_receive(CTL_MESSAGE_QUEUE_t *mq,
                          unsigned use_timeout,
                          CTL_TIME_t timeout)
{
  unsigned waitNeeded=0;
  ctl_global_interrupts_disable();  
  if (ctl_interrupt_count)
    ctl_handle_error(CTL_WAIT_CALLED_FROM_ISR);
  ctl_task_executing->u.message = 0;  
  if (ctl_private_message_queue_is_empty(mq))
    {
      ctl_task_executing->timeout = timeout;  
      ctl_task_executing->wait_object = mq;          
      ctl_task_executing->state = use_timeout ? (CTL_STATE_MESSAGE_QUEUE_RECEIVE_WAIT | CTL_STATE_TIMER_WAIT) : CTL_STATE_MESSAGE_QUEUE_RECEIVE_WAIT;                  
    }
  else // queue not empty
    {
      CTL_TASK_t *t = 0;
      unsigned wasFull = ctl_private_message_queue_is_full(mq);
      ctl_task_executing->u.message = ctl_private_message_queue_leave(mq);
      if (wasFull) // queue was full - see if someone is waiting to post a message
        {
          for (t=ctl_task_list; t; t=t->next)
            if ((t->state & CTL_STATE_MESSAGE_QUEUE_POST_WAIT) && (t->wait_object == mq))
              {
                ctl_private_message_queue_join(mq, t->u.message);
                t->state = CTL_STATE_RUNNABLE;
                break;
              }
        }      
    }
  ctl_task_reschedule();  
  return ctl_task_executing->u.message;
}

unsigned 
ctl_message_queue_post_nb(CTL_MESSAGE_QUEUE_t *mq, 
                          void *message)
{
  unsigned okay = 0;
  int enabled = ctl_global_interrupts_disable();  
  if (!ctl_private_message_queue_is_full(mq)) 
    {
      CTL_TASK_t *t = 0;
      if (ctl_private_message_queue_is_empty(mq)) // queue is empty - see if someone is waiting to receive a message
        {          
          for (t=ctl_task_list; t; t=t->next)
            if ((t->state & CTL_STATE_MESSAGE_QUEUE_RECEIVE_WAIT) && (t->wait_object == mq))
              {
                t->u.message = message;
                t->state = CTL_STATE_RUNNABLE;
                break;
              }
        }
      if (t == 0) // queue not empty || no one waiting to receive        
        ctl_private_message_queue_join(mq, message);        
      okay = 1;
    }  
  if (ctl_interrupt_count==0)
    ctl_task_reschedule();
  else
    ctl_global_interrupts_set(enabled);
  return okay;
}

