emos_msgq.c

来自「emos是一个新的类似于ucos的内核」· C语言 代码 · 共 772 行 · 第 1/2 页

        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
        {
            if (pq->osQOut == pq->osQStart) 
            {  
            	/* Wrap OUT ptr if we are at the 1st queue entry */
                pq->osQOut = pq->osQEnd;
            }
            
            pq->osQOut--;
            *pq->osQOut = msg; /* Insert message into queue */
            pq->osQEntries++;  /* Update the nbr of entries in the queue */
            EMOS_EXIT_CRITICAL();
        }
        
        return (EMOS_NO_ERR);
    }
}

/*********************************************************************************************************
* QUERY A MESSAGE QUEUE
* Description: This function obtains information about a message queue.
* Arguments  : pevent        is a pointer to the event control block associated with the desired mailbox
*              pdata         is a pointer to a structure that will contain information about the message
*                            queue.
* Returns    : EMOS_NO_ERR          The call was successful and the message was sent
*              EMOS_ERR_EVENT_TYPE  If you are attempting to obtain data from a non queue.
**********************************************************************************************************/
uint8 emosQQuery (EMOS_EVENT_T *pevent, EMOS_Q_DATA_T *pdata)
{
    EMOS_Q_T   *pq;
    uint8   i;
    uint8  *psrc;
    uint8  *pdest;
    
    
    EMOS_ENTER_CRITICAL();
    if (pevent->osEventType != EMOS_EVENT_TYPE_Q) 
    {  
    	/* Validate event block type */
        EMOS_EXIT_CRITICAL();
        return (EMOS_ERR_EVENT_TYPE);
    }
    pdata->osEventGrp = pevent->osEventGrp;   /* Copy message mailbox wait list           */
    psrc              = &pevent->osEventTbl[0];
    pdest             = &pdata->osEventTbl[0];
    for (i = 0; i < EMOS_EVENT_TBL_SIZE; i++) 
    {
        *pdest++ = *psrc++;   
    }
    
    pq = (EMOS_Q_T *)pevent->osEventPtr;
    if (pq->osQEntries > 0) 
    {
        pdata->osMsg = pq->osQOut;                         /* Get next message to return if available  */
    } 
    else
    {
        pdata->osMsg = (void *)0;
    }
    
    pdata->osNMsgs = pq->osQEntries;
    pdata->osQSize = pq->osQSize;
    EMOS_EXIT_CRITICAL();
    return (EMOS_NO_ERR);
}

#endif




#if EMOS_Q_EN && (EMOS_MAX_MSGQS >= 2)
/*The MsgQ Operation is much like the VxWorks msgQ library,
  notes:
  It will use the malloc() operation to allocate space in emosMsgQCreate,
  free() operation in emosMsgQDelete, if want to use in a limited space embedded
  os system, try to porting the malloc/free to pre-allocated operations*/
 typedef struct EMOS_MSGQ_t
 {
   void*      osEventPtr;    /* Pointer to message or queue structure   */                   
   uint8      osEventTbl[EMOS_EVENT_TBL_SIZE]; 
   uint8      osEventType;                   
   uint8      osEventGrp;                     
   uint16     osEventCnt;  /*4*4B*/
   
   struct EMOS_MSGQ_t* msgQPtr;
   uint16 option;
   uint16 size;
   uint16 maxMsg;    /*Max message numbers*/
   uint16 maxMsgLen; /*Max message len of each message*/
   uint16 numMsg;    /*present number of message in the msgq*/
   uint16 front;     /*the msg buffer operation front pointer*/
   uint16 rear;      /*the msg buffer operation rear pointer*/
   char* buf;        /*the buffer to keep message*/ /*7*4B*/
 } EMOS_MSGQ_T;
/*typedef struct EMOS_MSGQ_t* EMOS_MSGQ_ID;*/
static EMOS_MSGQ_T  gEmosMSGQFree[EMOS_MAX_MSGQS];
static EMOS_MSGQ_T* gEmosMsgQFreeList;
void emosMsgQInit();
EMOS_MSGQ_ID emosMsgQCreate(uint16 msgNum, uint16 msgLen);
EMOS_MSGQ_ID emosMsgQDelete(EMOS_MSGQ_ID ID );
int emosMsgQRecv(EMOS_MSGQ_ID ID, char* buf, uint16 size, int timeout);
int emosMsgQSend(EMOS_MSGQ_ID ID, char* buf, uint16 size);
#define MCpy(a,b,c)  memcpy(a,b,c)
/*
static char* emosByteCp(char* dest, const char* src, uint16 len)
{
  char *p1 = (char*)dest;
  char *p2 = (char*)src;
  
  if(NULL == dest || NULL == src)  return NULL;
  while(len--) 
  {
  	 *p1++ = *p2++;
  }

  return dest;
}
*/

void emosMsgQInit()
{
  uint16 i = 0;

  for(i = 0; i < EMOS_MAX_MSGQS -1; i++)
  {
      gEmosMSGQFree[i].msgQPtr = &gEmosMSGQFree[i+1]; 
  }
  
  gEmosMSGQFree[EMOS_MAX_MSGQS -1].msgQPtr = NULL; 
  gEmosMsgQFreeList = &gEmosMSGQFree[0];

  EMOS_Print("%s %d Init sucessfully,sizeof=%d\r\n",__FILE__,__LINE__,sizeof(EMOS_MSGQ_T));
  return;
}

EMOS_MSGQ_ID emosMsgQCreate(uint16 msgNum, uint16 msgLen)
{
  uint32 tmp = 0;
  EMOS_MSGQ_ID  msgqId = NULL;

  EMOS_ENTER_CRITICAL();
  msgqId = gEmosMsgQFreeList;
  if(gEmosMsgQFreeList != (EMOS_MSGQ_ID)0)
  {
       gEmosMsgQFreeList = (EMOS_MSGQ_ID) gEmosMsgQFreeList->msgQPtr;
  }
  EMOS_EXIT_CRITICAL();
  
  if(msgqId != (EMOS_MSGQ_ID )0)
  {
         msgqId->front = 0;
         msgqId->rear = 0;
         msgqId->numMsg = 0;
         msgqId->option = 0;
         msgqId->maxMsg = msgNum;
         msgqId->maxMsgLen = msgLen;      
         msgqId->size = (msgNum) * msgLen;
         /*adjust the message*/
         tmp = msgqId->size + msgLen;
         
         /*This can be replaced by pre-allocated buffer space*/
         msgqId->buf = EMOS_MALLOC(tmp);
         if(NULL == msgqId->buf)
         {
           EMOS_ENTER_CRITICAL();
           msgqId->msgQPtr = gEmosMsgQFreeList;
           gEmosMsgQFreeList = msgqId;
           EMOS_EXIT_CRITICAL();	
           msgqId = NULL;     
         }  
  }
      
  return (msgqId);
}

EMOS_MSGQ_ID emosMsgQDelete(EMOS_MSGQ_ID ID)
{
   if(NULL == ID)       return NULL;
   if(ID->buf == NULL)  return NULL;
   EMOS_FREE((ID->buf));
	
   EMOS_ENTER_CRITICAL();
   ID->msgQPtr = gEmosMsgQFreeList;
   gEmosMsgQFreeList = ID;
   EMOS_EXIT_CRITICAL();	
   return ID;
}

int emosMsgQRecv(EMOS_MSGQ_ID ID, char* buf, uint16 size, int timeout)
{
  uint16 tSize = 0;
  char*  p = 0;
  
  if(NULL == ID || NULL == buf)  return EMOS_INVALID_INPARA;

  tSize = size;
  if(tSize >= ID->maxMsgLen)
  {
  	tSize = ID->maxMsgLen;
  }
  
  EMOS_ENTER_CRITICAL();
  if(ID->numMsg != 0)
  {
      ID->numMsg--;
      p = ID->buf;
      p = p + ID->rear * ID->maxMsgLen;
      MCpy(buf,p,tSize);
    
      ID->rear++;
      if(ID->rear >= ID->maxMsg)
      {
      	ID->rear = 0;
      } 	
      EMOS_EXIT_CRITICAL();     
  }
  else if (gEmosIntNesting > 0)
  {  
     /* See if called from ISR*/
     EMOS_EXIT_CRITICAL();  /*can't PEND from an ISR */
     return EMOS_ERR_PEND_ISR;
  } 
  else
  {
        gEmosTCBCur->osTCBStat    |= EMOS_STAT_MSGQ; /* Task will have to pend for a message to be posted  */
        gEmosTCBCur->osTCBDly      = timeout;       /* Load timeout into TCB*/
        emosEventTaskWait((EMOS_EVENT_T *)ID);      /* Suspend task until event or timeout occurs*/
        EMOS_EXIT_CRITICAL();
        emosSched();  /* Find next highest priority task ready to run*/
        
        EMOS_ENTER_CRITICAL();

        /*This branch will not be reached*/
        if (gEmosTCBCur->osTCBMsg != (void *)0)
        {
        	EMOS_Print("file:%s line=%d:-> this branch is not expected to reach\r\n",__FILE__,__LINE__);
            gEmosTCBCur->osTCBMsg      = (void *)0;      
            gEmosTCBCur->osTCBStat     = EMOS_STAT_RDY;
            gEmosTCBCur->osTCBEventPtr = (EMOS_EVENT_T *)0;  /* No longer waiting for event*/
            EMOS_EXIT_CRITICAL();
            return EMOS_NO_ERR;
        } 
        else if (gEmosTCBCur->osTCBStat & EMOS_STAT_MSGQ)
        {
        	/* Timed out if status indicates pending on Q    */
            emosEventTo((EMOS_EVENT_T *)ID);
            EMOS_EXIT_CRITICAL();
            return EMOS_TIMEOUT;  /* Indicate a timeout occured */
        } 
        else 
        {
          ID->numMsg--;
          p = ID->buf;
          p = p + ID->rear * ID->maxMsgLen;
          MCpy(buf,p,tSize);
          ID->rear++;
          if(ID->rear >= ID->maxMsg)
          {
              ID->rear = 0;
          } 
          
          gEmosTCBCur->osTCBEventPtr = (EMOS_EVENT_T *)0;
          EMOS_EXIT_CRITICAL();
          return EMOS_NO_ERR;
        }
  }
  
  return EMOS_NO_ERR;   
}

int emosMsgQSend(EMOS_MSGQ_ID ID, char* buf, uint16 size)
{
  uint16 tSize = 0;
  /*uint16 tNxtIdx = 0;*/
  char*  p = 0;
  
  if(NULL == ID || NULL == buf)  return EMOS_INVALID_INPARA;

  tSize = size;
  if(tSize >= ID->maxMsgLen)
  {
  	tSize = ID->maxMsgLen;
  }
  
  EMOS_ENTER_CRITICAL();
  if(ID->numMsg >= ID->maxMsgLen)
  {
    EMOS_EXIT_CRITICAL(); 
    return EMOS_Q_FULL;
  }
  else
  {
    ID->numMsg++;       
    p = ID->buf;
    p = p + ID->front * ID->maxMsgLen;
    MCpy(p,buf,tSize);
    
    ID->front++;
    if(ID->front >= ID->maxMsg)
    {
      ID->front= 0;
    } 	

  	/* See if any task pending on queue 
  	   Ready highest priority task waiting on event 
  	   Need to send the msg to ptcb->osTCBMsg directly, and Activate the Task,
  	   but different to Q and MBox operation, the msg ptr is not the global variable,
  	   so send (void*)0 to emosEventTaskRdy as secondary paramter*/ 
  	  /*EMOS_Print("==>Release MsgQ Pended task\r\n");*/
      emosEventTaskRdy((EMOS_EVENT_T *)ID,(void*)0, EMOS_STAT_MSGQ);
      EMOS_EXIT_CRITICAL();
      emosSched();     
      return (EMOS_NO_ERR); 
  }

 
  #if 0
  EMOS_ENTER_CRITICAL();
  if (ID->osEventGrp)
  {   
  	/* See if any task pending on queue 
  	   Ready highest priority task waiting on event 
  	   Need to send the msg to ptcb->osTCBMsg directly, and Activate the Task*/ 
      emosEventTaskRdy((EMOS_EVENT_T *)ID,(void*)ID, EMOS_STAT_Q); 
      EMOS_EXIT_CRITICAL();
      emosSched(); 
      return (EMOS_NO_ERR);
  } 
  else
  {  
  	 /*check the MSGQ is FULL by the index*/
  	 /*
  	 tNxtIdx = ID->front+1;
  	 if(tNxtIdx >= ID->maxMsgLen)
  	 {
        tNxtIdx = 0;
  	 }
  	 
  	 if(tNxtIdx == ID->rear)
  	 */
     if(ID->numMsg >= ID->maxMsgLen)
     {
       EMOS_EXIT_CRITICAL(); 
       return EMOS_Q_FULL;
     }
     else
     {
        ID->numMsg++;       
        p = ID->buf;
        p = p + ID->front * ID->maxMsgLen;
        MCpy(buf,p,tSize);
        
        ID->front++;
        if(ID->front >= ID->maxMsg)
        {
          ID->front= 0;
        } 	
        EMOS_EXIT_CRITICAL();     
        return (EMOS_NO_ERR);
     }
  }
  #endif
  
  return (EMOS_NO_ERR);
}


#endif

 /*
 * Please add "$" around "Id" and "Log".
 * $Id$
 * $Log$
 *
 */