qm.c

NecluesRTX RTOS的源码

        {
        
            /* We are deleting the tail node, adjust the tail pointer.  */
            *ptr_tail_ptr =  suspend_ptr -> qm_prev_susp;

            /* Adjust the tail of the list.  */
            if (*ptr_tail_ptr != NU_NULL)
               (*ptr_tail_ptr) -> qm_next_susp =  NU_NULL;
        }
  
        /* Reset the pointers of the removed suspension block.  */        
        suspend_ptr -> qm_next_susp =    NU_NULL;
        suspend_ptr -> qm_prev_susp =    NU_NULL;
    }           
}                                           /* end  QM_Remove_From_List */



/************************************************************************/
/*                                                                      */
/*  FUNCTION                                "QM_Retrieve_Item"          */
/*                                                                      */
/*                                                                      */
/*  DESCRIPTION                                                         */
/*                                                                      */
/*      This function retrieves an item from the specified queue.       */
/*                                                                      */
/*  AUTHOR                                                              */
/*                                                                      */
/*      William E. Lamie,  Accelerated Technology                       */
/*                                                                      */
/*  CALLED FROM                                                         */
/*                                                                      */
/*      Service routines that request queue items                       */
/*                                                                      */
/*  ROUTINES CALLED                                                     */
/*                                                                      */
/*      QM_Remove_From_List                 Remove from queue full list */
/*      SKP_Ready_Task                      Place the task on ready lst */
/*      SKD_Leave_Task                      Leave task if required      */
/*                                                                      */
/*  INPUTS                                                              */
/*                                                                      */
/*      queue_id                            Identification of the queue */
/*      destination_ptr                     Pointer to destination      */
/*                                                                      */
/*  OUTPUTS                                                             */
/*                                                                      */
/*      return(status)                                                  */
/*                                                                      */
/************************************************************************/
signed int  QM_Retrieve_Item(signed int queue_id,
             unsigned int *destination_ptr)
{

struct QM_QUEUE_CONTROL_STRUCT  
                  *QCB_ptr;                 /* Pointer to QCB entry     */
struct QM_SUSPENSION_STRUCT  
                  *suspend_ptr;             /* Suspension pointer       */
unsigned int       i;                       /* Working variable         */
signed int         status;                  /* Status of request        */
    

    /* Build a pointer to the queue control block.  */
    QCB_ptr =  &QM_QCB_List[queue_id];

    /* See if there is anything in the queue.  */
    if ((QCB_ptr -> qm_queue_front != QCB_ptr -> qm_queue_back) ||
        (QCB_ptr -> qm_queue_full))
    {

        /* Yes something is in the queue.  */
        
        /* Decrement the queued count.  */
        QCB_ptr -> qm_queued--;

        /* Copy the item to the destination buffer.  */
        for (i = 0; i < QCB_ptr -> qm_item_size; i++)
        {
      
            *destination_ptr++ =  *(QCB_ptr -> qm_queue_front++);
        }
        
        /* Check for a wrap condition.  */
        if (QCB_ptr -> qm_queue_front >= QCB_ptr -> qm_queue_end)
           QCB_ptr -> qm_queue_front =  QCB_ptr -> qm_queue_start;
        
        /* Check for a previous queue full condition.  */
        if (QCB_ptr -> qm_queue_full)
        {
        
            /* Check for a suspension block on the queue full.  */
            if (QCB_ptr -> qm_full_wait_head)
            {
            
                /* Point to the queue suspend block.  */
                suspend_ptr =  QCB_ptr -> qm_full_wait_head;
                
                /* Remove the suspend pointer from the suspend list.  */
                QM_Remove_From_List(&(QCB_ptr -> qm_full_wait_head),
                                     &(QCB_ptr -> qm_full_wait_tail),
                                     suspend_ptr);
                                    
                /* Place the item in the suspended block at the back of
                   the queue.  */
                *(suspend_ptr -> qm_return_addr) =  NU_SUCCESS;
                
                /* Copy the item from the source buffer.  */
                for (i = 0; i < QCB_ptr -> qm_item_size; i++)
                {
      
                    *(QCB_ptr -> qm_queue_back++) =  
                                      *(suspend_ptr -> qm_item_ptr++);
                }
        
                /* Check for a wrap condition.  */
                if (QCB_ptr -> qm_queue_back >= QCB_ptr -> qm_queue_end)
                    QCB_ptr -> qm_queue_back =  QCB_ptr -> qm_queue_start;
                
                /* Increment the queued count.  */
                QCB_ptr -> qm_queued++;

                /* Lift the suspension on the task that was waiting to 
                   place an item on the queue.  Context switch if 
                   required. */
                if (SKP_Ready_Task(suspend_ptr -> qm_task_id))
                {
                
                    /* Leave this task temporarily.  */
                    SKD_Leave_Task();
                }
            }
            else
            {
            
                /* The queue is not full any longer.  */
                
                /* Clear the queue full condition.  */
                QCB_ptr -> qm_queue_full =  NU_FALSE;
            }
        }
        
        /* Yes, there is something in this queue.  Return the queue id
           as the successful status.  */
        status =  QCB_ptr -> qm_queue_id;
    }
    else
    {
    
        /* Return an appropriate status.  */
        status =  NU_QUEUE_EMPTY;
    }
    
    /* Return the status to the caller.  */
    return(status);
}                                           /* end of  QM_Retrieve_Item */



/************************************************************************/
/*                                                                      */
/*  FUNCTION                                "QM_Place_Item"             */
/*                                                                      */
/*                                                                      */
/*  DESCRIPTION                                                         */
/*                                                                      */
/*      This function places an item into the specified queue.          */
/*                                                                      */
/*  AUTHOR                                                              */
/*                                                                      */
/*      William E. Lamie,  Accelerated Technology                       */
/*                                                                      */
/*  CALLED FROM                                                         */
/*                                                                      */
/*      Service routines that request queue items                       */
/*                                                                      */
/*  ROUTINES CALLED                                                     */
/*                                                                      */
/*      QM_Remove_From_List                 Remove from queue empty list*/
/*      SKP_Ready_Task                      Place the task on ready lst */
/*      SKD_Leave_Task                      Leave the task if necessary */
/*                                                                      */
/*  INPUTS                                                              */
/*                                                                      */
/*      queue_id                            Queue identification        */
/*      source_ptr                          Pointer to source item      */
/*                                                                      */
/*  OUTPUTS                                                             */
/*                                                                      */
/*      return(status)                                                  */
/*                                                                      */
/************************************************************************/
signed int  QM_Place_Item(signed int queue_id, unsigned int *source_ptr)
{

struct QM_QUEUE_CONTROL_STRUCT  
                  *QCB_ptr;                 /* Pointer to QCB entry     */
struct QM_SUSPENSION_STRUCT  
                  *suspend_ptr;             /* Suspension pointer       */
unsigned int       i;                       /* Working variable         */
signed   int       task_id;                 /* Suspended task ID        */
signed   int       status;                  /* Status of request        */
    

    /* Build a pointer to the queue control block.  */
    QCB_ptr =  &QM_QCB_List[queue_id];

    /* Check for task(s) suspended on an empty queue.  */
    if (QCB_ptr -> qm_empty_wait_head != NU_NULL)
    {
        
        /* Return a successful status.  */
        status =  NU_SUCCESS;

        /* Point to the queue suspend block.  */
        suspend_ptr =  QCB_ptr -> qm_empty_wait_head;
                                    
        /* Return the queue id.  */
        *(suspend_ptr -> qm_return_addr) =  queue_id;

        /* Copy the item from the source buffer into the waiting task's
           buffer.  */
        for (i = 0; i < QCB_ptr -> qm_item_size; i++)
        {
      
            *(suspend_ptr -> qm_item_ptr++) =  *(source_ptr++);
        }

        /* Get the task ID to wake up later.  */
        task_id =  suspend_ptr -> qm_task_id;

        /* Determine if the suspend is one of many or just one.  */
        if (suspend_ptr -> qm_susp_type == QM_SINGLE)
        {
            
            /* Single QCB.  Remove it from the list.  */
            QM_Remove_From_List( &(QCB_ptr -> qm_empty_wait_head),
                                 &(QCB_ptr -> qm_empty_wait_tail), 
                                 suspend_ptr);
        }
        else
        {


            /* Remove the suspend pointers from the suspend list.  The 
               loop is necessary to remove suspend blocks in multiple
               suspension on empty queues.  Adjust the suspend pointer to
               the first element of the suspend array for this task, which
               was allocated off of this task's stack.  */
            suspend_ptr =  suspend_ptr - suspend_ptr -> qm_susp_id;
            
            for (i = 0; i < MAX_Q_SUSPENSIONS; i++)
            {
            
                /* Remove the suspend block, if it was associated with a 
                   valid queue id.  */
                if (suspend_ptr -> qm_QCB_ptr != NU_NULL)
                {

                    /* Remove it from the list.  */
                    QM_Remove_From_List(
                       &((suspend_ptr -> qm_QCB_ptr) -> qm_empty_wait_head),
                       &((suspend_ptr -> qm_QCB_ptr) -> qm_empty_wait_tail), 
                       suspend_ptr);
                }
                       
                /* Position to the next suspend block.  */
                suspend_ptr++;
            }
        }
            
        /* Lift the suspension on the task that was waiting to 
           for an item on the queue and context switch if necessary.  */
        if (SKP_Ready_Task(task_id))
        {
                
            /* Leave this task temporarily.  */
            SKD_Leave_Task();
        }
    }
    /* See if there is any room in the queue.  */
    else if (QCB_ptr -> qm_queue_full != NU_TRUE)
    {

        /* Yes there is room in the queue.  Place the item at the back.  */
        status =  NU_SUCCESS;

        /* Increment the queued count.  */
        QCB_ptr -> qm_queued++;

        /* Copy the item from the source buffer.  */
        for (i = 0; i < QCB_ptr -> qm_item_size; i++)
        {
      
            *(QCB_ptr -> qm_queue_back++) =  *source_ptr++;
        }
        
        /* Check for a wrap condition.  */
        if (QCB_ptr -> qm_queue_back >= QCB_ptr -> qm_queue_end)
           QCB_ptr -> qm_queue_back =  QCB_ptr -> qm_queue_start;
        
        /* Check for a queue full condition.  */