/******************************************************************************
 *
 * Copyright:
 *    (C) 2000 - 2005 Embedded Artists AB
 *
 * Description:
 *
 * ESIC:
 *    pre_emptive_os
 *
 * Version:
 *    1.4.0
 *
 * Generate date:
 *    2005-03-15 at 20:27:15
 *
 * NOTE:
 *    DO NOT EDIT THIS FILE. IT IS AUTO GENERATED.
 *    CHANGES TO THIS FILE WILL BE LOST IF THE FILE IS RE-GENERATED
 *
 * Signature:
 *   7072655F656D70746976655F6F73,312E342E302E30,020235
 *   ,35,10104021013134373435363030,07323838,3732,01013
 *   830,0101013138303030303030,3135,33,3135,0232323530
 *   ,01020130,0231343734353539,3238313831,020101100302
 *   103030310010133,0163130,3230,3330,3430,3530,3630,3
 *   730,3830,3930,313030,313130,313230,313330,313430,3
 *   13530,313630,,35,35,35,35,35,35,35,35,35,35,35,35,
 *   35,35,35,35,,,,1001001100011000000000]484152445741
 *   5245,4C5043323130365F32,545538,756E7369676E6564206
 *   3686172,414C49474E4D454E54,34,54424F4F4C,756E73696
 *   76E65642063686172,54553332,756E7369676E656420696E7
 *   4,544D505F46494C4553,2A2E656C663B2A2E6C73743B2A2E6
 *   D61703B2A2E6F3B2A2E6F626A3B2A2E64,454E4449414E,4C4
 *   954544C45,54533332,7369676E656420696E74,545338,736
 *   9676E65642063686172,54553136,756E7369676E656420736
 *   86F7274,54533136,7369676E65642073686F7274,44455343
 *   52495054494F4E,,44454255475F4C4556454C,30,434F4445
 *   5F524F4F54,,47454E5F52554C4553,,4C494E455F5445524D
 *   ,43524C46,4252414345,,43524541544F52,416E646572732
 *   0526F7376616C6C,4352454154494F4E5F44415445,3230303
 *   52D30332D31352032303A31373A3432,524F4F54,433A2F446
 *   F63756D656E747320616E642053657474696E67732F416E646
 *   5727320526F7376616C6C2F4D696E6120646F6B756D656E742
 *   F456D62656464656420417274697374732F50726F647563747
 *   32F4C50433231303620525332333220517569636B537461727
 *   420426F6172642F72746F732F]505245464958,,4445425547
 *   5F4C4556454C,30,555345525F434F4D4D454E54,]64656661
 *   756C74,
 *
 * Checksum:
 *    569771
 *
 *****************************************************************************/


/******************************************************************************
 * Includes
 *****************************************************************************/

#include "queue.h"
#include "hal.h"
#include "../api/general.h"
#include "event.h"
#include "kernel.h"
#include "pcb.h"
#include "error.h"
#include "../stub/osstub.h"

/******************************************************************************
 * Implementation of public functions
 *****************************************************************************/


/*****************************************************************************
 *
 * Description:
 *    This function initializes a queue structure. 
 *
 * Params:
 *    [in] pQueue     - A pointer to an allocated queue structure. 
 *    [in] pQueueArea - A pointer to the queue area. The user must allocate 
 *                      the memory area used by the queue. The queue area is 
 *                      an array of void pointers. 
 *    [in] size       - The size of the queue area. The size is given in 
 *                      number of void pointers in the area. 
 *
 ****************************************************************************/
void
osCreateQueue(tQueue* pQueue,
              void**  pQueueArea,
              tU16    size)
{
  eventInit((tOSEvent*)pQueue);

  pQueue->pQStart = pQueueArea;
  pQueue->pQEnd  = pQueueArea+size;
  pQueue->pQIn = pQueueArea;
  pQueue->pQOut = pQueueArea;
  pQueue->queueSize = size;
  pQueue->nEntries = 0;
}


/*****************************************************************************
 *
 * Description:
 *    This function retrieves the first message from the queue. The message is 
 *    removed from the queue. If the queue is empty the function will block 
 *    until there is a message to retrieve or a timeout occurs. 
 *
 * Params:
 *    [in]  pQueue  - A pointer to an initialized queue structure. 
 *    [in]  timeout - The number of ticks to wait on a queue before returning. 
 *                    If a timeout of zero is specified the function will 
 *                    never timeout. 
 *    [out] pError  - The return status of the function. 
 *
 * Returns:
 *    The first message in the queue or NULL if timeout or error. 
 *
 * Error codes:
 *    OS_OK         - The function completed successfully. 
 *    OS_ERROR_ISR  - The function was called from an interrupt service 
 *                    routine. 
 *    OS_ERROR_NULL - A NULL pointer was supplied to the function where it was 
 *                    not allowed. 
 *
 ****************************************************************************/
void*
osPendQueue(tQueue* pQueue,
            tU16    timeout,
            tU8*    pError)
{
  volatile tSR localSR;  /* declare temporary local space for status word */
  tBool haveMsg = FALSE;
  void * msg = NULL;
  tU8 ret;
  *pError = OS_OK;
  if(pQueue == NULL)
  {
    *pError = OS_ERROR_NULL;

    return NULL;
  }

  m_os_dis_int();
  if(isrNesting > 0)
  {
    m_os_ena_int();
    *pError = OS_ERROR_ISR;

    return NULL;
  }
  m_os_ena_int();

  /* we need to loop since when eventWait return
     another process may preempt us and modify
     the queue (eventWait may result in interrupts enabled)*/
  do{
    m_os_dis_int();
    if(pQueue->nEntries == 0)
    {
      eventWait((tOSEvent*)pQueue, timeout);
      m_os_ena_int();
      schedule();
      m_os_dis_int();
      ret = eventWaitReturn((tOSEvent*)pQueue);
      if(ret == OS_ERROR_TIMEOUT)
      {
        m_os_ena_int();
        return NULL;
      }
    }
    else
    {
      msg = *pQueue->pQOut++;
      pQueue->nEntries--;
      if(pQueue->pQOut == pQueue->pQEnd)
      {
        pQueue->pQOut = pQueue->pQStart;
      }
      haveMsg = TRUE;
    }
    m_os_ena_int();
  }while(haveMsg == FALSE);

  return msg;
}


/*****************************************************************************
 *
 * Description:
 *    This function tries to receive the first message from the queue. If the 
 *    queue is empty the function returns immediately. This function can be 
 *    called from within an interrupt service routine (ISR). 
 *
 * Params:
 *    [in]  pQueue - A pointer to an initialized queue structure. 
 *    [out] pError - The return status of the function. 
 *
 * Returns:
 *    The retrieved message or NULL if the queue is empty. 
 *
 * Error codes:
 *    OS_OK         - The function completed successfully. 
 *    OS_ERROR_NULL - A NULL pointer was supplied to the function where it was 
 *                    not allowed. 
 *
 ****************************************************************************/
void*
osAcceptQueue(tQueue* pQueue,
              tU8*    pError)
{
  volatile tSR localSR;  /* declare temporary local space for status word */
  void *pMsg = NULL;
  *pError = OS_OK;
  if(pQueue == NULL)
  {
    *pError = OS_ERROR_NULL;

    return NULL;
  }

  m_os_dis_int();
  if(pQueue->nEntries > 0)
  {
    pMsg = *pQueue->pQOut++;
    pQueue->nEntries--;
    if(pQueue->pQOut == pQueue->pQEnd)
    {
      pQueue->pQOut = pQueue->pQStart;
    }
  }
  m_os_ena_int();
  return pMsg;
}


/*****************************************************************************
 *
 * Description:
 *    This function clears a queue from all messages. 
 *
 * Params:
 *    [in]  pQueue - A pointer to an initialized queue. 
 *    [out] pError - The return status of the function. 
 *
 * Error codes:
 *    OS_OK         - The function completed successfully. 
 *    OS_ERROR_NULL - A NULL pointer was supplied to the function where it was 
 *                    not allowed. 
 *
 ****************************************************************************/
void
osFlushQueue(tQueue* pQueue,
             tU8*    pError)
{
  volatile tSR localSR;  /* declare temporary local space for status word */
  *pError = OS_OK;
  if(pQueue == NULL)
  {
    *pError = OS_ERROR_NULL;

    return;
  }

  m_os_dis_int();
  pQueue->pQIn = pQueue->pQStart;
  pQueue->pQOut = pQueue->pQStart;
  pQueue->nEntries = 0;
  m_os_ena_int();
}


/*****************************************************************************
 *
 * Description:
 *    This function posts a new message to the end of the queue. 
 *
 * Params:
 *    [in]  pQueue - A pointer to an initialized queue structure. 
 *    [in]  msg    - The message to post. 
 *    [out] pError - The return status of the function. 
 *
 * Error codes:
 *    OS_ERROR_QUEUE_FULL - The queue is full. 
 *    OS_OK               - The function completed successfully. 
 *    OS_ERROR_NULL       - A NULL pointer was supplied to the function where 
 *                          it was not allowed. 
 *
 ****************************************************************************/
void
osPostQueue(tQueue* pQueue,
            void*   msg,
            tU8*    pError)
{
  volatile tSR localSR;  /* declare temporary local space for status word */
  *pError = OS_OK;
  if(pQueue == NULL)
  {
    *pError = OS_ERROR_NULL;

    return;
  }

  m_os_dis_int();
  
  if(pQueue->nEntries == pQueue->queueSize)
  {
    m_os_ena_int();
    *pError = OS_ERROR_QUEUE_FULL;

    return;
  }

  *(pQueue->pQIn) = msg;
  pQueue->pQIn++;
  pQueue->nEntries++;
  if(pQueue->pQIn == pQueue->pQEnd)
  {
    pQueue->pQIn = pQueue->pQStart;
  }
  if(eventIsEmpty((tOSEvent*)pQueue) == FALSE)
  {
    eventSignal((tOSEvent*)pQueue);
    if(isrNesting == 0)
    {
      m_os_ena_int();
      schedule();
      m_os_dis_int();
    }
  }
  m_os_ena_int();
}


/*****************************************************************************
 *
 * Description:
 *    This function posts a new message to the front of the queue. 
 *
 * Params:
 *    [in]  pQueue - 
 *    [in]  msg    - The message to post. 
 *    [out] pError - The return status of the function. 
 *
 * Error codes:
 *    OS_ERROR_QUEUE_FULL - The queue is full. 
 *    OS_OK               - The function completed successfully. 
 *    OS_ERROR_NULL       - A NULL pointer was supplied to the function where 
 *                          it was not allowed. 
 *
 ****************************************************************************/
void
osPostFrontQueue(tQueue* pQueue,
                 void*   msg,
                 tU8*    pError)
{
  volatile tSR localSR;  /* declare temporary local space for status word */
  *pError = OS_OK;
  if(pQueue == NULL)
  {
    *pError = OS_ERROR_NULL;

    return;
  }

  m_os_dis_int();
  
  if(pQueue->nEntries == pQueue->queueSize)
  {
    m_os_ena_int();
    *pError = OS_ERROR_QUEUE_FULL;

    return;
  }

  if(pQueue->pQOut == pQueue->pQStart)
  {
    pQueue->pQOut = pQueue->pQEnd;
  }
  pQueue->pQOut--;
  *(pQueue->pQOut) = msg;  
  pQueue->nEntries++;

  if(eventIsEmpty((tOSEvent*)pQueue) == FALSE)
  {
    eventSignal((tOSEvent*)pQueue);
    if(isrNesting == 0)
    {
      m_os_ena_int();
      schedule();
      m_os_dis_int();
    }
  }
  m_os_ena_int();
}
