/****************************************************************************
 *
 * (c) Copyright 2001,2008, EMB system, All Rights Reserved.
 * THIS IS UNPUBLISHED PROPRIETARY SOURCE CODE OF EMB SYSTEM, INC.
 * The copyright notice above does not evidence any actual or intended
 * publication of such source code. 
 *
 *  Subsystem:   EMOS 
 *  File:        emos_q.c
 *  Author:      zenf zhao
 *  Description: EMOS msg queue management implement
 *               EVENT_T is like a MsgQId, with is eventPtr point to a EMOS_Q_T 
 *               element, the element is inited by Create CallBack whitch init the 
 *               element point to void* msg[size]  base address, the msg[i] will keep
 *               to every input pend msg address
 ****************************************************************************/
#include "emos_cfg.h"
#include "emos_core.h"

#if EMOS_Q_EN && (EMOS_MAX_QS >= 2)

/*LOCAL GLOBAL VARIABLES*/
typedef struct os_q 
{   
  /* QUEUE CONTROL BLOCK*/
  struct os_q  *osQPtr;          /* Link to next queue control block in list of free blocks      */
   void    **osQStart;           /* Pointer to start of queue data                               */
   void    **osQEnd;             /* Pointer to end   of queue data                               */
   void    **osQIn;              /* Pointer to where next message will be inserted  in   the Q   */
   void    **osQOut;             /* Pointer to where next message will be extracted from the Q   */
   uint16    osQSize;            /* Size of queue (maximum number of entries)                    */
   uint16    osQEntries;         /* Current number of entries in the queue                       */
} EMOS_Q_T;

 static  EMOS_Q_T  *gEmosQFreeList;             /* Pointer to list of free QUEUE control blocks */
 static  EMOS_Q_T   gEmosQTbl[EMOS_MAX_QS];     /* Table of QUEUE control blocks */

/*******************************************************************************************************
* QUEUE MODULE INITIALIZATION
* Description : This function is called by EMOS to initialize the message queue module.  Your
*               application MUST NOT call this function.
* Arguments   :  none
* Returns     :  none
**********************************************************************************************************/
void emosQInit (void)
{
    uint16 i;

    for (i = 0; i < (EMOS_MAX_QS - 1); i++) 
    {      /* Init. list of free QUEUE control blocks            */
        gEmosQTbl[i].osQPtr = &gEmosQTbl[i+1];
    }
    
    gEmosQTbl[EMOS_MAX_QS - 1].osQPtr = (EMOS_Q_T *)0;
    gEmosQFreeList  = &gEmosQTbl[0];
}


/*********************************************************************************************************
*  CREATE A MESSAGE QUEUE
* Description: This function creates a message queue if free event control blocks are available.
* Arguments  : start         is a pointer to the base address of the message queue storage area.  The
*                            storage area MUST be declared as an array of pointers to 'void' as follows
*                            void *MessageStorage[size]
*              size          is the number of elements in the storage area
* Returns    : != (void *)0  is a pointer to the event control clock (EMOS_EVENT_T) associated with the
*                            created queue
*              == (void *)0  if no event control blocks were available
**********************************************************************************************************/
EMOS_EVENT_T* emosQCreate (void **start, uint16 size)
{
    EMOS_EVENT_T *pevent;
    EMOS_Q_T     *pq;

    EMOS_ENTER_CRITICAL();
    pevent = gEmosEventFreeList;/* Get next free event control block*/
    if (gEmosEventFreeList != (EMOS_EVENT_T *)0) 
    {   
    	/* See if pool of free ECB pool was empty*/
        gEmosEventFreeList = (EMOS_EVENT_T *)gEmosEventFreeList->osEventPtr;
    }
    EMOS_EXIT_CRITICAL();
    
    if (pevent != (EMOS_EVENT_T *)0) 
    {   
    	/* See if we have an event control block 
    	   Get a free queue control block */
        EMOS_ENTER_CRITICAL(); 
        pq = gEmosQFreeList;
        if (gEmosQFreeList != (EMOS_Q_T *)0) 
        {
            gEmosQFreeList = gEmosQFreeList->osQPtr;
        }
        EMOS_EXIT_CRITICAL();
        
        if (pq != (EMOS_Q_T *)0) 
        {  
        	/* See if we were able to get a queue control block   */
            pq->osQStart        = start; /* Yes, initialize the queue */
            pq->osQEnd          = &start[size];
            pq->osQIn           = start;
            pq->osQOut          = start;
            pq->osQSize         = size;
            pq->osQEntries      = 0;
            pevent->osEventType = EMOS_EVENT_TYPE_Q;
            pevent->osEventPtr  = pq;
            emosEventWaitListInit(pevent);
        }
        
        else
        {     
        	/* No free Q control block, since we couldn't get a queue control block 
               Return event control block on error */
            EMOS_ENTER_CRITICAL();        
            pevent->osEventPtr = (void *)gEmosEventFreeList;
            gEmosEventFreeList    = pevent;
            EMOS_EXIT_CRITICAL();
            pevent = (EMOS_EVENT_T *)0;
        }     
    }
    
    return (pevent);
}

/*********************************************************************************************************
* FLUSH QUEUE
* Description : This function is used to flush the contents of the message queue.
* Arguments   : none
* Returns     : EMOS_NO_ERR  upon success
*               EMOS_ERR_EVENT_TYPE  If you didn't pass a pointer to a queue
**********************************************************************************************************/
uint8 emosQFlush (EMOS_EVENT_T *pevent)
{
    EMOS_Q_T  *pq;
    
    EMOS_ENTER_CRITICAL();
    if (pevent->osEventType != EMOS_EVENT_TYPE_Q) 
    {     
    	/* Validate event block type */
        EMOS_EXIT_CRITICAL();
        return (EMOS_ERR_EVENT_TYPE);
    }

    /* Point to queue storage structure */
    pq             = pevent->osEventPtr; 
    pq->osQIn      = pq->osQStart;
    pq->osQOut     = pq->osQStart;
    pq->osQEntries = 0;
    EMOS_EXIT_CRITICAL();
    return (EMOS_NO_ERR);
}


/*********************************************************************************************************
* ACCEPT MESSAGE FROM QUEUE
* Description: This function checks the queue to see if a message is available.  Unlike osQPend(),
*              emosQAccept() does not suspend the calling task if a message is not available.
* Arguments  : pevent        is a pointer to the event control block
* Returns    : != (void *)0  is the message in the queue if one is available.  The message is removed
*                            from the so the next time osQAccept() is called, the queue will contain
*                            one less entry.
*              == (void *)0  if the queue is empty
*                            if you passed an invalid event type
**********************************************************************************************************/
void *emosQAccept (EMOS_EVENT_T* pevent)
{
    void* msg;
    EMOS_Q_T* pq;

    EMOS_ENTER_CRITICAL();
    if (pevent->osEventType != EMOS_EVENT_TYPE_Q)
    {    
    	/* Validate event block type */
        EMOS_EXIT_CRITICAL();
        return ((void *)0);
    }
    
    pq = pevent->osEventPtr;  /* Point at queue control block */
    if (pq->osQEntries != 0) 
    { 
    	/* See if any messages in the queue*/
        msg = *pq->osQOut++;   /* Yes, extract oldest message from the queue */
        pq->osQEntries--;      /* Update the number of entries in the queue */
        if (pq->osQOut == pq->osQEnd)
        {  
        	/* Wrap OUT pointer if we are at the end of the queue */
            pq->osQOut = pq->osQStart;
        }
    } 
    else
    {
        msg = (void *)0; /* Queue is empty */
    }
    
    EMOS_EXIT_CRITICAL();
    return (msg);        /* Return message received (or NULL) */
}

/*********************************************************************************************************
* PEND ON A QUEUE FOR A MESSAGE
* Description: This function waits for a message to be sent to a queue
* Arguments  : pevent        is a pointer to the event control block associated with the desired queue
*
*              timeout       is an optional timeout period (in clock ticks).  If non-zero, your task will
*                            wait for a message to arrive at the queue up to the amount of time 
*                            specified by this argument.  If you specify 0, however, your task will wait 
*                            forever at the specified queue or, until a message arrives.
*              err           is a pointer to where an error message will be deposited.  Possible error
*                            messages are:
*                            EMOS_NO_ERR         The call was successful and your task received a message.
*                            EMOS_TIMEOUT        A message was not received within the specified timeout
*                            EMOS_ERR_EVENT_TYPE You didn't pass a pointer to a queue
*                            EMOS_ERR_PEND_ISR  If you called this function from an ISR and the result
*                                               would lead to a suspension.
* Returns    : != (void *)0  is a pointer to the message received
*              == (void *)0  if no message was received or you didn't pass a pointer to a queue.
**********************************************************************************************************/
void *emosQPend (EMOS_EVENT_T *pevent, uint16 timeout, uint8 *err)
{
    void  *msg;
    EMOS_Q_T  *pq;

    EMOS_ENTER_CRITICAL();
    if (pevent->osEventType != EMOS_EVENT_TYPE_Q) 
    {
    	/* Validate event block type*/
        EMOS_EXIT_CRITICAL();
        *err = EMOS_ERR_EVENT_TYPE;
        return ((void *)0);
    }
    
    pq = pevent->osEventPtr;  /* Point at queue control block */
    if (pq->osQEntries != 0) 
    {   
    	/* See if any messages in the queue */
        msg = *pq->osQOut++; /* Yes, extract oldest message from the queue */
        pq->osQEntries--;    /* Update the number of entries in the queue */

        if (pq->osQOut == pq->osQEnd) 
        {   
        	/* Wrap OUT pointer if we are at the end of the queue */
            pq->osQOut = pq->osQStart;
        }
        
        EMOS_EXIT_CRITICAL();
        *err = EMOS_NO_ERR;
    } 
    else if (gEmosIntNesting > 0)
    {  
    	/* See if called from ISR ...*/
         EMOS_EXIT_CRITICAL();                      /* ... can't PEND from an ISR */
         *err = EMOS_ERR_PEND_ISR;
     } 
    else 
    {
        gEmosTCBCur->osTCBStat    |= EMOS_STAT_Q;   /* Task will have to pend for a message to be posted  */
        gEmosTCBCur->osTCBDly      = timeout;       /* Load timeout into TCB*/
        emosEventTaskWait(pevent);                  /* Suspend task until event or timeout occurs*/
        EMOS_EXIT_CRITICAL();
        emosSched();                                /* Find next highest priority task ready to run*/
        EMOS_ENTER_CRITICAL();
        
        if ((msg = gEmosTCBCur->osTCBMsg) != (void *)0)
        {
        	/* Did we get a message,Extract message from TCB (Put there by QPost) */
            gEmosTCBCur->osTCBMsg      = (void *)0;      
            gEmosTCBCur->osTCBStat     = EMOS_STAT_RDY;
            gEmosTCBCur->osTCBEventPtr = (EMOS_EVENT_T *)0;  /* No longer waiting for event*/
            EMOS_EXIT_CRITICAL();
            *err = EMOS_NO_ERR;
        } 
        else if (gEmosTCBCur->osTCBStat & EMOS_STAT_Q)
        {
        	/* Timed out if status indicates pending on Q    */
            emosEventTo(pevent);
            EMOS_EXIT_CRITICAL();
            msg = (void *)0;      /* No message received */
            *err = EMOS_TIMEOUT;     /* Indicate a timeout occured */
        } 
        else 
        {
            msg = *pq->osQOut++;                      /* Extract message from queue */
            pq->osQEntries--;                         /* Update the number of entries in the queue */
            if (pq->osQOut == pq->osQEnd) 
            {  
            	/* Wrap OUT pointer if we are at the end of Q    */
                pq->osQOut = pq->osQStart;
            }
            
            gEmosTCBCur->osTCBEventPtr = (EMOS_EVENT_T *)0;
            EMOS_EXIT_CRITICAL();
            *err = EMOS_NO_ERR;
        }
    }  
    
    return (msg); /* Return message received (or NULL)  */
}

/*********************************************************************************************************
* POST MESSAGE TO A QUEUE
* Description: This function sends a message to a queue
* Arguments  : pevent        is a pointer to the event control block associated with the desired queue
*              msg           is a pointer to the message to send.  You MUST NOT send a NULL pointer.  
* Returns    : EMOS_NO_ERR          The call was successful and the message was sent
*              EMOS_Q_T_FULL          If the queue cannot accept any more messages because it is full.
*              EMOS_ERR_EVENT_TYPE  If you didn't pass a pointer to a queue.
**********************************************************************************************************/
uint8 emosQPost (EMOS_EVENT_T *pevent, void *msg)
{
    EMOS_Q_T   *pq;

    EMOS_ENTER_CRITICAL();
    if (pevent->osEventType != EMOS_EVENT_TYPE_Q)
    {   
    	/* Validate event block type */
        EMOS_EXIT_CRITICAL();
        return (EMOS_ERR_EVENT_TYPE);
    }
    
    if (pevent->osEventGrp)
    {   
    	/* See if any task pending on queue, then directly send the msg to ptcb->osTCBMsg,
    	   and the emosQPend will recv of the first if((msg = gEmosTCBCur->osTCBMsg) != (void *)0),
    	   don't need the msg storing operation*/
        emosEventTaskRdy(pevent, msg, EMOS_STAT_Q); /* Ready highest priority task waiting on event  */
        EMOS_EXIT_CRITICAL();
        emosSched(); 
        return (EMOS_NO_ERR);
    } 
    else
    {
        pq = pevent->osEventPtr;   /* Point to queue control block*/
        if (pq->osQEntries >= pq->osQSize) 
        {   
        	/* Make sure queue is not full*/
            EMOS_EXIT_CRITICAL();
            return (EMOS_Q_FULL);
        } 
        else 
        {
            *pq->osQIn++ = msg;                       /* Insert message into queue*/
            pq->osQEntries++;                         /* Update the nbr of entries in the queue*/
            if (pq->osQIn == pq->osQEnd)
            {  
            	/* Wrap IN ptr if we are at end of queue*/
                pq->osQIn = pq->osQStart;
            }
            EMOS_EXIT_CRITICAL();
        }
        
        return (EMOS_NO_ERR);
    }
}

/**********************************************************************************************************
* POST MESSAGE TO THE FRONT OF A QUEUE
* Description: This function sends a message to a queue but unlike osQPost(), the message is posted at
*              the front instead of the end of the queue.  Using osQPostFront() allows you to send
*              'priority' messages.  
* Arguments  : pevent        is a pointer to the event control block associated with the desired queue
*              msg           is a pointer to the message to send.  You MUST NOT send a NULL pointer.  
* Returns    : EMOS_NO_ERR          The call was successful and the message was sent
*              EMOS_Q_T_FULL          If the queue cannot accept any more messages because it is full.
*              EMOS_ERR_EVENT_TYPE  If you didn't pass a pointer to a queue.
**********************************************************************************************************/
uint8 emosQPostFront (EMOS_EVENT_T *pevent, void *msg)
{
    EMOS_Q_T   *pq;

    EMOS_ENTER_CRITICAL();
    if (pevent->osEventType != EMOS_EVENT_TYPE_Q) 
    {  
    	EMOS_EXIT_CRITICAL();
        return (EMOS_ERR_EVENT_TYPE);
    }
    
    if (pevent->osEventGrp)
    {   
    	/* See if any task pending on queue*/
        emosEventTaskRdy(pevent, msg, EMOS_STAT_Q);       /* Ready highest priority task waiting on event  */
        EMOS_EXIT_CRITICAL();
        emosSched(); 
        return (EMOS_NO_ERR);
    } 
    else 
    {