view src/nucleus/qus.c @ 221:5bf097aeaad7

LLS: when turning off all LEDs on boot, skip LED-C Having LLS turn off LED-A and LED-B on boot is normally unnecessary (they should already be off in Iota), but it is harmless, hence this logic is kept for robustness. However, having LLS read-modify-write the BCICTL2 register (to turn off LED-C) creates a potential race condition with FCHG writes to this register, especially in the case when baseband switch-on is caused by VCHG and charging is expected to start right away. Furthermore, control of the charging LED itself (on those hw targets that have it) is the responsibility of the FCHG SWE, hence LLS should leave it alone.
author Mychaela Falconia <falcon@freecalypso.org>
date Mon, 26 Apr 2021 21:55:13 +0000
parents 4e78acac3d88
children
line wrap: on
line source

/*************************************************************************/
/*                                                                       */
/*               Copyright Mentor Graphics Corporation 2002              */
/*                         All Rights Reserved.                          */
/*                                                                       */
/* THIS WORK CONTAINS TRADE SECRET AND PROPRIETARY INFORMATION WHICH IS  */
/* THE PROPERTY OF MENTOR GRAPHICS CORPORATION OR ITS LICENSORS AND IS   */
/* SUBJECT TO LICENSE TERMS.                                             */
/*                                                                       */
/*************************************************************************/

/*************************************************************************/
/*                                                                       */
/* FILE NAME                                               VERSION       */
/*                                                                       */
/*      qus.c                                          Nucleus PLUS 1.14 */
/*                                                                       */
/* COMPONENT                                                             */
/*                                                                       */
/*      QU - Queue Management                                            */
/*                                                                       */
/* DESCRIPTION                                                           */
/*                                                                       */
/*      This file contains supplemental routines for the Queue           */
/*      Management component.                                            */
/*                                                                       */
/* DATA STRUCTURES                                                       */
/*                                                                       */
/*      None                                                             */
/*                                                                       */
/* FUNCTIONS                                                             */
/*                                                                       */
/*      QUS_Reset_Queue                     Reset a queue                */
/*      QUS_Send_To_Front_Of_Queue          Send message to queue's front*/
/*      QUS_Broadcast_To_Queue              Broadcast a message to queue */
/*                                                                       */
/* DEPENDENCIES                                                          */
/*                                                                       */
/*      cs_extr.h                           Common Service functions     */
/*      tc_extr.h                           Thread Control functions     */
/*      qu_extr.h                           Queue functions              */
/*      hi_extr.h                           History functions            */
/*                                                                       */
/* HISTORY                                                               */
/*                                                                       */
/*         DATE                    REMARKS                               */
/*                                                                       */
/*      03-01-1994      Created initial version 1.1 from                 */
/*                      routines originally in core                      */
/*                                                                       */
/*      03-18-1994      Verified version 1.1                             */
/*      04-17-1996      updated to version 1.2                           */
/*      02-04-1998      Corrected SPR434 resulting in                    */
/*                      version 1.2a.                                    */
/*      03-24-1998      Released version 1.3                             */
/*      03-26-1999      Released 1.11m (new release                      */
/*                        numbering scheme)                              */
/*      04-17-2002      Released version 1.13m                           */
/*      11-07-2002      Released version 1.14                            */
/*************************************************************************/
#define         NU_SOURCE_FILE


#include        "cs_extr.h"                 /* Common service functions  */
#include        "tc_extr.h"                 /* Thread control functions  */
#include        "qu_extr.h"                 /* Queue functions           */
#include        "hi_extr.h"                 /* History functions         */
#include        "profiler.h"                /* ProView interface         */


/* Define internal component function prototypes.  */

VOID    QUC_Cleanup(VOID *information);


/*************************************************************************/
/*                                                                       */
/* FUNCTION                                                              */
/*                                                                       */
/*      QUS_Reset_Queue                                                  */
/*                                                                       */
/* DESCRIPTION                                                           */
/*                                                                       */
/*      This function resets the specified queue back to the original    */
/*      state.  Any messages in the queue are discarded.  Also, any      */
/*      tasks currently suspended on the queue are resumed with the      */
/*      reset status.                                                    */
/*                                                                       */
/* CALLED BY                                                             */
/*                                                                       */
/*      Application                                                      */
/*      QUSE_Reset_Queue                    Error checking shell         */
/*                                                                       */
/* CALLS                                                                 */
/*                                                                       */
/*      [HIC_Make_History_Entry]            Make entry in history log    */
/*      TCC_Resume_Task                     Resume a suspended task      */
/*      [TCT_Check_Stack]                   Stack checking function      */
/*      TCT_Control_To_System               Transfer control to system   */
/*      TCT_System_Protect                  Protect queue data structures*/
/*      TCT_Unprotect                       Release protection           */
/*                                                                       */
/* INPUTS                                                                */
/*                                                                       */
/*      queue_ptr                           Queue control block pointer  */
/*                                                                       */
/* OUTPUTS                                                               */
/*                                                                       */
/*      NU_SUCCESS                                                       */
/*                                                                       */
/* HISTORY                                                               */
/*                                                                       */
/*        DATE                    REMARKS                                */
/*                                                                       */
/*      03-01-1993      Created initial version 1.0                      */
/*      04-19-1993      Verified version 1.0                             */
/*      03-01-1994      Changed function interfaces to                   */
/*                      match those in prototype,                        */
/*                      added register options, changed                  */
/*                      protection logic to reduce                       */
/*                      overhead, fixed read and write                   */
/*                      pointers to both point at the                    */
/*                      start, resulting in version 1.1                  */
/*                                                                       */
/*      03-18-1994      Verified version 1.1                             */
/*      02-04-1998      Corrected SPR434.                                */
/*                                                                       */
/*************************************************************************/
STATUS  QUS_Reset_Queue(NU_QUEUE *queue_ptr)
{

R1 QU_QCB      *queue;                      /* Queue control block ptr   */
QU_SUSPEND     *suspend_ptr;                /* Suspend block pointer     */
QU_SUSPEND     *next_ptr;                   /* Next suspend block pointer*/
STATUS          preempt;                    /* Status for resume call    */
NU_SUPERV_USER_VARIABLES

    /* Move input queue pointer into internal pointer.  */
    queue =  (QU_QCB *) queue_ptr;

    /* Switch to supervisor mode */
    NU_SUPERVISOR_MODE();

#ifdef  NU_ENABLE_STACK_CHECK

    /* Call stack checking function to check for an overflow condition.  */
    TCT_Check_Stack();

#endif

#ifdef  NU_ENABLE_HISTORY

    /* Make an entry that corresponds to this function in the system history
       log.  */
    HIC_Make_History_Entry(NU_RESET_QUEUE_ID, (UNSIGNED) queue,
                                        (UNSIGNED) 0, (UNSIGNED) 0);

#endif

    /* Protect against access to the queue.  */
    TCT_System_Protect();

    /* Pickup the suspended task suspension list.  */
    suspend_ptr =  queue -> qu_suspension_list;

    /* Walk the chain task(s) currently suspended on the queue.  */
    preempt =  0;
    while (suspend_ptr)
    {

        /* Resume the suspended task.  Insure that the status returned is
           NU_QUEUE_RESET.  */
        suspend_ptr -> qu_return_status =  NU_QUEUE_RESET;

        /* Point to the next suspend structure in the link.  */
        next_ptr =  (QU_SUSPEND *) (suspend_ptr -> qu_suspend_link.cs_next);

        /* Resume the specified task.  */
        preempt =  preempt |
            TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
                                                NU_QUEUE_SUSPEND);

        /* Determine if the next is the same as the head pointer.  */
        if (next_ptr == queue -> qu_suspension_list)

            /* Clear the suspension pointer to signal the end of the list
               traversal.  */
            suspend_ptr =  NU_NULL;
        else

            /* Position the suspend pointer to the next suspend block.  */
            suspend_ptr =  next_ptr;
    }

    /* Pickup the urgent message suspension list.  */
    suspend_ptr =  queue -> qu_urgent_list;

    /* Walk the chain task(s) currently suspended on the queue.  */
    while (suspend_ptr)
    {

        /* Resume the suspended task.  Insure that the status returned is
           NU_QUEUE_RESET.  */
        suspend_ptr -> qu_return_status =  NU_QUEUE_RESET;

        /* Point to the next suspend structure in the link.  */
        next_ptr =  (QU_SUSPEND *) (suspend_ptr -> qu_suspend_link.cs_next);

        /* Resume the specified task.  */
        preempt =  preempt |
            TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
                                                NU_QUEUE_SUSPEND);

        /* Determine if the next is the same as the head pointer.  */
        if (next_ptr == queue -> qu_urgent_list)

            /* Clear the suspension pointer to signal the end of the list
               traversal.  */
            suspend_ptr =  NU_NULL;
        else

            /* Position suspend pointer to the next suspend block.  */
            suspend_ptr =  next_ptr;
    }

    /* Initialize various elements of the queue.  */
    queue -> qu_available =             queue -> qu_end - queue -> qu_start;
    queue -> qu_messages =              0;
    queue -> qu_read =                  queue -> qu_start;
    queue -> qu_write =                 queue -> qu_start;
    queue -> qu_tasks_waiting =         0;
    queue -> qu_suspension_list =       NU_NULL;
    queue -> qu_urgent_list =           NU_NULL;

#ifdef INCLUDE_PROVIEW
    _RTProf_DumpQueue(RT_PROF_RESET_QUEUE,queue,RT_PROF_OK);
#endif

    /* Determine if preemption needs to occur.  */
    if (preempt)

        /* Transfer control to system to facilitate preemption.  */
        TCT_Control_To_System();

    /* Release protection against access to the queue.  */
    TCT_Unprotect();

    /* Return to user mode */
    NU_USER_MODE();

    /* Return a successful completion.  */
    return(NU_SUCCESS);
}


/*************************************************************************/
/*                                                                       */
/* FUNCTION                                                              */
/*                                                                       */
/*      QUS_Send_To_Front_Of_Queue                                       */
/*                                                                       */
/* DESCRIPTION                                                           */
/*                                                                       */
/*      This function sends a message to the front of the specified      */
/*      message queue.  The message length is determined by the caller.  */
/*      If there are any tasks suspended on the queue for a message, the */
/*      message is copied into the message area of the first waiting     */
/*      task and that task is resumed.  If there is enough room in the   */
/*      queue, the message is copied in front of all other messages.     */
/*      If there is not enough room in the queue, suspension of the      */
/*      caller is possible.                                              */
/*                                                                       */
/* CALLED BY                                                             */
/*                                                                       */
/*      Application                                                      */
/*      QUSE_Send_To_Front_Of_Queue         Error checking shell         */
/*                                                                       */
/* CALLS                                                                 */
/*                                                                       */
/*      CSC_Place_On_List                   Place on suspend list        */
/*      CSC_Remove_From_List                Remove from suspend list     */
/*      [HIC_Make_History_Entry]            Make entry in history log    */
/*      TCC_Resume_Task                     Resume a suspended task      */
/*      TCC_Suspend_Task                    Suspend calling task         */
/*      [TCT_Check_Stack]                   Stack checking function      */
/*      TCT_Control_To_System               Transfer control to system   */
/*      TCT_Current_Thread                  Pickup current thread pointer*/
/*      TCT_System_Protect                  Protect queue                */
/*      TCT_Unprotect                       Release protection           */
/*                                                                       */
/* INPUTS                                                                */
/*                                                                       */
/*      queue_ptr                           Queue control block pointer  */
/*      message                             Pointer to message to send   */
/*      size                                Size of message to send      */
/*      suspend                             Suspension option if full    */
/*                                                                       */
/* OUTPUTS                                                               */
/*                                                                       */
/*      NU_SUCCESS                          If service is successful     */
/*      NU_QUEUE_FULL                       If queue is currently full   */
/*      NU_TIMEOUT                          If timeout on service expires*/
/*      NU_QUEUE_DELETED                    If queue was deleted during  */
/*                                            suspension                 */
/*      NU_QUEUE_RESET                      If queue was reset during    */
/*                                            suspension                 */
/*                                                                       */
/* HISTORY                                                               */
/*                                                                       */
/*         DATE                    REMARKS                               */
/*                                                                       */
/*      03-01-1993      Created initial version 1.0                      */
/*      04-19-1993      Verified version 1.0                             */
/*      03-01-1994      Changed function interfaces to                   */
/*                      match those in prototype,                        */
/*                      added register options, changed                  */
/*                      protection logic to reduce                       */
/*                      overhead, optimized copy loop,                   */
/*                      resulting in version 1.1                         */
/*                                                                       */
/*      03-18-1994      Verified version 1.1                             */
/*                                                                       */
/*************************************************************************/
STATUS  QUS_Send_To_Front_Of_Queue(NU_QUEUE *queue_ptr, VOID *message,
                                        UNSIGNED size, UNSIGNED suspend)
{

R1 QU_QCB      *queue;                      /* Queue control block ptr   */
QU_SUSPEND      suspend_block;              /* Allocate suspension block */
QU_SUSPEND     *suspend_ptr;                /* Pointer to suspend block  */
R3 UNSIGNED_PTR source;                     /* Pointer to source         */
R4 UNSIGNED_PTR destination;                /* Pointer to destination    */
UNSIGNED        copy_size;                  /* Partial copy size         */
R2 INT          i;                          /* Working counter           */
TC_TCB         *task;                       /* Task pointer              */
STATUS          preempt;                    /* Preempt flag              */
STATUS          status;                     /* Completion status         */
NU_SUPERV_USER_VARIABLES

    /* Switch to supervisor mode */
    NU_SUPERVISOR_MODE();

    /* Move input queue pointer into internal pointer.  */
    queue =  (QU_QCB *) queue_ptr;


#ifdef  NU_ENABLE_STACK_CHECK

    /* Call stack checking function to check for an overflow condition.  */
    TCT_Check_Stack();

#endif

#ifdef  NU_ENABLE_HISTORY

    /* Make an entry that corresponds to this function in the system history
       log.  */
    HIC_Make_History_Entry(NU_SEND_TO_FRONT_OF_QUEUE_ID, (UNSIGNED) queue,
                                        (UNSIGNED) message, (UNSIGNED) size);

#endif

    /* Initialize the status as successful.  */
    status =  NU_SUCCESS;

    /* Protect against simultaneous access to the queue.  */
    TCT_System_Protect();

    /* Determine if an extra word of overhead needs to be added to the
       calculation.  */
    if (queue -> qu_fixed_size)

        /* No overhead.  */
        i =  0;
    else
    {
        /* Variable messages have one additional word of overhead.  */
        i =  1;

        /* Make special check to see if a suspension needs to be
           forced for a variable length message.  */
        if ((queue -> qu_suspension_list) && (queue -> qu_messages))
        {

            /* Pickup task control block pointer.  */
            task =  (TC_TCB *) TCT_Current_Thread();

            /* Now we know that there are other task(s) are suspended trying
               to send a variable length message.  Determine whether or not
               a suspension should be forced.  */
            if ((queue -> qu_fifo_suspend) ||
                (suspend == NU_NO_SUSPEND) ||
                ((queue -> qu_suspension_list) -> qu_suspend_link.cs_priority <=
                                                    TCC_Task_Priority(task)))

                /* Bump the computed size to avoid placing the new variable
                   length message ahead of the suspended tasks.  */
                i =  (INT) queue -> qu_available;
        }
    }

    /* Determine if there is enough room in the queue for the message.  */
    if (queue -> qu_available < (size + i))
    {

        /* Queue does not have room for the message.  Determine if
           suspension is required.  */
        if (suspend)
        {

            /* Suspension is requested.   */

            /* Increment the number of tasks waiting.  */
            queue -> qu_tasks_waiting++;

#ifdef INCLUDE_PROVIEW
            _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_WAIT);
#endif

            /* Setup the suspend block and suspend the calling task.  */
            suspend_ptr =  &suspend_block;
            suspend_ptr -> qu_queue =                    queue;
            suspend_ptr -> qu_suspend_link.cs_next =     NU_NULL;
            suspend_ptr -> qu_suspend_link.cs_previous = NU_NULL;
            suspend_ptr -> qu_message_area =             (UNSIGNED_PTR) message;
            suspend_ptr -> qu_message_size =             size;
            task =                            (TC_TCB *) TCT_Current_Thread();
            suspend_ptr -> qu_suspended_task =           task;

            /* Place the task on the urgent message suspension list.  */
            CSC_Place_On_List((CS_NODE **) &(queue -> qu_urgent_list),
                                        &(suspend_ptr -> qu_suspend_link));

            /* Move the head pointer of the list to make this suspension the
               first in the list.  */
            queue -> qu_urgent_list =  (QU_SUSPEND *)
                (queue -> qu_urgent_list) -> qu_suspend_link.cs_previous;

            /* Finally, suspend the calling task. Note that the suspension call
               automatically clears the protection on the queue.  */
            TCC_Suspend_Task((NU_TASK *) task, NU_QUEUE_SUSPEND,
                                        QUC_Cleanup, suspend_ptr, suspend);

            /* Pickup the return status.  */
            status =  suspend_ptr -> qu_return_status;
        }
        else
        {

            /* Return a status of NU_QUEUE_FULL because there is no
               room in the queue for the message.  */
            status =  NU_QUEUE_FULL;

#ifdef INCLUDE_PROVIEW
            _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_FAIL);
#endif
        }
    }
    else
    {

        /* Determine if a task is waiting on an empty queue.  */
        if ((queue -> qu_suspension_list) && (queue -> qu_messages == 0))
        {

            /* Task is waiting on queue for a message.  */

            /* Decrement the number of tasks waiting on queue.  */
            queue -> qu_tasks_waiting--;

#ifdef INCLUDE_PROVIEW
            _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_OK);
#endif

            /* Remove the first suspended block from the list.  */
            suspend_ptr =  queue -> qu_suspension_list;
            CSC_Remove_From_List((CS_NODE **) &(queue -> qu_suspension_list),
                                          &(suspend_ptr -> qu_suspend_link));

            /* Setup the source and destination pointers.  */
            source =       (UNSIGNED_PTR) message;
            destination =  suspend_ptr -> qu_message_area;

            /* Initialize the return status.  */
            suspend_ptr -> qu_return_status =  NU_SUCCESS;

            /* Loop to actually copy the message.  */
            i =  (INT) size;
            do
            {
                *(destination++) =  *(source);
                if ((--i) == 0)
                    break;
                source++;
            } while (1);

            /* Return the size of the message copied.  */
            suspend_ptr -> qu_actual_size =  size;

            /* Wakeup the waiting task and check for preemption.  */
            preempt =
                TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
                                                           NU_QUEUE_SUSPEND);

            /* Determine if preemption needs to take place. */
            if (preempt)

                 /* Transfer control to the system if the resumed task function
                   detects a preemption condition.  */
                TCT_Control_To_System();
        }
        else
        {

            /* There is enough room in the queue and no task is waiting.  */

            /* Setup the source pointer.  */
            source =       (UNSIGNED_PTR) message;
            destination =  queue -> qu_read;

            /* Process according to the type of message supported.  */
            if (queue -> qu_fixed_size)
            {

                /* Fixed-size message queue.  */

                /* Determine if the read pointer is at the top of the queue
                   area.  */
                if (destination == queue -> qu_start)

                    /* Prepare to place the message in the lower part
                       of the queue area.  */
                    destination =  queue -> qu_end - size;
                else

                    /* Backup the length of a message from the current
                       read pointer.  */
                    destination =  destination - size;

                /* Adjust the actual read pointer before the copy is done.  */
                queue -> qu_read =  destination;

                /* Copy the message into the queue area.  */
                i =  (INT) size;
                do
                {
                   *(destination++) =  *(source);
                    if ((--i) == 0)
                        break;
                    source++;
                } while (1);
            }
            else
            {

                /* Variable-size message queue.  */

                /* Calculate the number of words remaining at the top of the
                   queue.  */
                copy_size =  destination - queue -> qu_start;

                /* Determine if part of the message needs to be placed at the
                   bottom of the queue area.  */
                if (copy_size < (size + i))

                    /* Compute the starting location for the message.  */
                    destination =  queue -> qu_end - ((size +i) - copy_size);
                else

                    /* Compute the starting location for the message.  */
                    destination =  destination - (size + i);

                /* Adjust the actual queue read pointer also.  */
                queue -> qu_read =  destination;

                /* Place message size in first location.  */
                *(destination++) =  size;

                /* Check for a wrap-around condition on the queue.  */
                if (destination >= queue -> qu_end)

                    /* Wrap the write pointer back to the top of the queue
                       area.  */
                    destination =  queue -> qu_start;

                /* Decrement the number of words remaining by 1 for this
                   extra word of overhead.  */
                queue -> qu_available--;

                /* Calculate the number of words remaining from the
                   destination pointer to the bottom of the queue.  */
                copy_size =  queue -> qu_end - destination;

                /* Determine if the message needs to be wrapped around the
                   edge of the queue area.  */
                if (copy_size >= size)
                {

                    /* Copy the whole message at once.  */
                    i =  (INT) size;
                    do
                    {
                        *(destination++) =  *(source);
                        if ((--i) == 0)
                            break;
                        source++;
                    } while (1);
                }
                else
                {

                    /* Copy the first half of the message.  */
                    i =  (INT) copy_size;
                    do
                    {
                        *(destination) =  *(source++);
                        if ((--i) == 0)
                            break;
                        destination++;
                    } while (1);

                    /* Copy the second half of the message.  */
                    destination =  queue -> qu_start;
                    i =  (INT) (size - copy_size);
                    do
                    {
                        *(destination++) =  *(source);
                        if ((--i) == 0)
                            break;
                        source++;
                    } while (1);
                }
            }

            /* Decrement the number of available words.  */
            queue -> qu_available =  queue -> qu_available - size;

            /* Increment the number of messages in the queue.  */
            queue -> qu_messages++;

#ifdef INCLUDE_PROVIEW
            _RTProf_DumpQueue(RT_PROF_SEND_TO_FRONT_OF_QUEUE,queue,RT_PROF_OK);
#endif

        }
    }

    /* Release protection against access to the queue.  */
    TCT_Unprotect();

    /* Return to user mode */
    NU_USER_MODE();

    /* Return the completion status.  */
    return(status);
}


/*************************************************************************/
/*                                                                       */
/* FUNCTION                                                              */
/*                                                                       */
/*      QUS_Broadcast_To_Queue                                           */
/*                                                                       */
/* DESCRIPTION                                                           */
/*                                                                       */
/*      This function sends a message to all tasks waiting for a message */
/*      from the specified queue.  If there are no tasks waiting for a   */
/*      message the service performs like a standard send request.       */
/*                                                                       */
/* CALLED BY                                                             */
/*                                                                       */
/*      Application                                                      */
/*      QUSE_Broadcast_To_Queue             Error checking shell         */
/*                                                                       */
/* CALLS                                                                 */
/*                                                                       */
/*      CSC_Place_On_List                   Place on suspend list        */
/*      CSC_Priority_Place_On_List          Place on priority list       */
/*      CSC_Remove_From_List                Remove from suspend list     */
/*      [HIC_Make_History_Entry]            Make entry in history log    */
/*      TCC_Resume_Task                     Resume a suspended task      */
/*      TCC_Suspend_Task                    Suspend calling task         */
/*      TCC_Task_Priority                   Pickup task's priority       */
/*      [TCT_Check_Stack]                   Stack checking function      */
/*      TCT_Control_To_System               Transfer control to system   */
/*      TCT_Current_Thread                  Pickup current thread pointer*/
/*      TCT_System_Protect                  Protect queue                */
/*      TCT_Unprotect                       Release protection           */
/*                                                                       */
/* INPUTS                                                                */
/*                                                                       */
/*      queue_ptr                           Queue control block pointer  */
/*      message                             Pointer to message to send   */
/*      size                                Size of message to send      */
/*      suspend                             Suspension option if full    */
/*                                                                       */
/* OUTPUTS                                                               */
/*                                                                       */
/*      NU_SUCCESS                          If service is successful     */
/*      NU_QUEUE_FULL                       If queue is currently full   */
/*      NU_TIMEOUT                          If timeout on service expires*/
/*      NU_QUEUE_DELETED                    If queue was deleted during  */
/*                                            suspension                 */
/*      NU_QUEUE_RESET                      If queue was reset during    */
/*                                            suspension                 */
/*                                                                       */
/* HISTORY                                                               */
/*                                                                       */
/*        DATE                    REMARKS                                */
/*                                                                       */
/*      03-01-1993      Created initial version 1.0                      */
/*      04-19-1993      Verified version 1.0                             */
/*      03-01-1994      Changed function interfaces to                   */
/*                      match those in prototype,                        */
/*                      added register options, changed                  */
/*                      protection logic to reduce                       */
/*                      overhead, optimized copy loop,                   */
/*                      resulting in version 1.1                         */
/*                                                                       */
/*      03-18-1994      Verified version 1.1                             */
/*                                                                       */
/*************************************************************************/
STATUS  QUS_Broadcast_To_Queue(NU_QUEUE *queue_ptr, VOID *message,
                                        UNSIGNED size, UNSIGNED suspend)
{

R1 QU_QCB      *queue;                      /* Queue control block ptr   */
QU_SUSPEND      suspend_block;              /* Allocate suspension block */
QU_SUSPEND     *suspend_ptr;                /* Pointer to suspend block  */
R3 UNSIGNED_PTR source;                     /* Pointer to source         */
R4 UNSIGNED_PTR destination;                /* Pointer to destination    */
UNSIGNED        copy_size;                  /* Partial copy size         */
R2 INT          i;                          /* Working counter           */
TC_TCB         *task;                       /* Task pointer              */
STATUS          preempt;                    /* Preempt flag              */
STATUS          status;                     /* Completion status         */
NU_SUPERV_USER_VARIABLES

    /* Switch to supervisor mode */
    NU_SUPERVISOR_MODE();

    /* Move input queue pointer into internal pointer.  */
    queue =  (QU_QCB *) queue_ptr;


#ifdef  NU_ENABLE_STACK_CHECK

    /* Call stack checking function to check for an overflow condition.  */
    TCT_Check_Stack();

#endif

#ifdef  NU_ENABLE_HISTORY

    /* Make an entry that corresponds to this function in the system history
       log.  */
    HIC_Make_History_Entry(NU_BROADCAST_TO_QUEUE_ID, (UNSIGNED) queue,
                                        (UNSIGNED) message, (UNSIGNED) size);

#endif

    /* Initialize the status as successful.  */
    status =  NU_SUCCESS;

    /* Protect against simultaneous access to the queue.  */
    TCT_System_Protect();

    /* Determine if an extra word of overhead needs to be added to the
       calculation.  */
    if (queue -> qu_fixed_size)

        /* No overhead.  */
        i =  0;
    else
    {
        /* Variable messages have one additional word of overhead.  */
        i =  1;

        /* Make special check to see if a suspension needs to be
           forced for a variable length message.  */
        if ((queue -> qu_suspension_list) && (queue -> qu_messages))
        {

            /* Pickup task control block pointer.  */
            task =  (TC_TCB *) TCT_Current_Thread();

            /* Now we know that there are other task(s) are suspended trying
               to send a variable length message.  Determine whether or not
               a suspension should be forced.  */
            if ((queue -> qu_fifo_suspend) ||
                (suspend == NU_NO_SUSPEND) ||
                ((queue -> qu_suspension_list) -> qu_suspend_link.cs_priority <=
                                                    TCC_Task_Priority(task)))

                /* Bump the computed size to avoid placing the new variable
                   length message ahead of the suspended tasks.  */
                i =  (INT) queue -> qu_available;
        }
    }

    /* Determine if there is enough room in the queue for the message.  */
    if (queue -> qu_available < (size + i))
    {

        /* Queue does not have room for the message.  Determine if
           suspension is required.  */
        if (suspend)
        {

            /* Suspension is requested.   */

            /* Increment the number of tasks waiting.  */
            queue -> qu_tasks_waiting++;

#ifdef INCLUDE_PROVIEW
            _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_WAIT);
#endif

            /* Setup the suspend block and suspend the calling task.  */
            suspend_ptr = &suspend_block;
            suspend_ptr -> qu_queue =                    queue;
            suspend_ptr -> qu_suspend_link.cs_next =     NU_NULL;
            suspend_ptr -> qu_suspend_link.cs_previous = NU_NULL;
            suspend_ptr -> qu_message_area =           (UNSIGNED_PTR) message;
            suspend_ptr -> qu_message_size =             size;
            task =                            (TC_TCB *) TCT_Current_Thread();
            suspend_ptr -> qu_suspended_task =           task;

            /* Determine if priority or FIFO suspension is associated with the
               queue.  */
            if (queue -> qu_fifo_suspend)
            {

                /* FIFO suspension is required.  Link the suspend block into
                   the list of suspended tasks on this queue.  */
                CSC_Place_On_List((CS_NODE **) &(queue -> qu_suspension_list),
                                        &(suspend_ptr -> qu_suspend_link));
            }
            else
            {

                /* Get the priority of the current thread so the suspend block
                   can be placed in the appropriate place.  */
                suspend_ptr -> qu_suspend_link.cs_priority =
                                                      TCC_Task_Priority(task);

                CSC_Priority_Place_On_List((CS_NODE **)
                                &(queue -> qu_suspension_list),
                                        &(suspend_ptr -> qu_suspend_link));
            }

            /* Finally, suspend the calling task. Note that the suspension call
               automatically clears the protection on the queue.  */
            TCC_Suspend_Task((NU_TASK *) task, NU_QUEUE_SUSPEND,
                                        QUC_Cleanup, suspend_ptr, suspend);

            /* Pickup the return status.  */
            status =  suspend_ptr -> qu_return_status;
        }
        else
        {

            /* Return a status of NU_QUEUE_FULL because there is no
               room in the queue for the message.  */
            status =  NU_QUEUE_FULL;

#ifdef INCLUDE_PROVIEW
            _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_FAIL);
#endif

        }
    }
    else
    {

        /* Determine if a task is waiting on an empty queue.  */
        if ((queue -> qu_suspension_list) && (queue -> qu_messages == 0))
        {

#ifdef INCLUDE_PROVIEW
        _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_OK);
#endif

            /* Yes, one or more tasks are waiting for a message from this
               queue.  */
            preempt =  0;
            do
            {

                /* Decrement the number of tasks waiting on queue.  */
                queue -> qu_tasks_waiting--;

                /* Remove the first suspended block from the list.  */
                suspend_ptr =  queue -> qu_suspension_list;
                CSC_Remove_From_List((CS_NODE **)
                                &(queue -> qu_suspension_list),
                                          &(suspend_ptr -> qu_suspend_link));

                /* Setup the source and destination pointers.  */
                source =       (UNSIGNED_PTR) message;
                destination =  suspend_ptr -> qu_message_area;

                /* Initialize the return status.  */
                suspend_ptr -> qu_return_status =  NU_SUCCESS;

                /* Loop to actually copy the message.  */
                i =  (INT) size;
                do
                {
                    *(destination++) =  *(source);
                    if ((--i) == 0)
                        break;
                    source++;
                } while (1);

                /* Return the size of the message copied.  */
                suspend_ptr -> qu_actual_size =  size;

                /* Wakeup the waiting task and check for preemption.  */
                preempt =  preempt |
                 TCC_Resume_Task((NU_TASK *) suspend_ptr -> qu_suspended_task,
                                                         NU_QUEUE_SUSPEND);

                /* Move the suspend pointer to the next node, which is now
                   at the head of the list.  */
                suspend_ptr =  queue -> qu_suspension_list;
            } while (suspend_ptr);

            /* Determine if preemption needs to take place. */
            if (preempt)

                 /* Transfer control to the system if the resumed task function
                   detects a preemption condition.  */
                TCT_Control_To_System();
        }
        else
        {

            /* There is enough room in the queue and no task is waiting.  */

            /* Setup the source pointer.  */
            source =       (UNSIGNED_PTR) message;
            destination =  queue -> qu_write;

            /* Process according to the type of message supported.  */
            if (queue -> qu_fixed_size)
            {

                /* Fixed-size messages are supported by this queue.  */

                /* Loop to copy the message into the queue area.  */
                i =  (INT) size;
                do
                {
                   *(destination++) =  *(source);
                    if ((--i) == 0)
                        break;
                    source++;
                } while (1);
            }
            else
            {

                /* Variable-size messages are supported.  Processing must
                   check for queue wrap-around conditions.  */

                /* Place message size in first location.  */
                *(destination++) =  size;

                /* Check for a wrap-around condition on the queue.  */
                if (destination >= queue -> qu_end)

                    /* Wrap the write pointer back to the top of the queue
                       area.  */
                    destination =  queue -> qu_start;

                /* Decrement the number of words remaining by 1 for this
                   extra word of overhead.  */
                queue -> qu_available--;

                /* Calculate the number of words remaining from the write
                   pointer to the bottom of the queue.  */
                copy_size =  queue -> qu_end - destination;

                /* Determine if the message needs to be wrapped around the
                   edge of the queue area.  */
                if (copy_size >= size)
                {

                    /* Copy the whole message at once.  */
                    i =  (INT) size;
                    do
                    {
                        *(destination++) =  *(source);
                        if ((--i) == 0)
                            break;
                        source++;
                    } while(1);
                }
                else
                {

                    /* Copy the first half of the message.  */
                    i =  (INT) copy_size;
                    do
                    {
                        *(destination) =  *(source++);
                        if ((--i) == 0)
                            break;
                        destination++;
                    } while (1);

                    /* Copy the second half of the message.  */
                    destination =  queue -> qu_start;
                    i =  (INT) (size - copy_size);
                    do
                    {
                        *(destination++) =  *(source);
                        if ((--i) == 0)
                            break;
                        source++;
                    } while (1);
                }
            }

            /* Check again for wrap-around condition on the write pointer. */
            if (destination >= queue -> qu_end)

                /* Move the write pointer to the top of the queue area.  */
                queue -> qu_write =  queue -> qu_start;
            else

                /* Simply copy the last position of the destination pointer
                   into the write pointer.  */
                queue -> qu_write =  destination;

            /* Decrement the number of available words.  */
            queue -> qu_available =  queue -> qu_available - size;

            /* Increment the number of messages in the queue.  */
            queue -> qu_messages++;

#ifdef INCLUDE_PROVIEW
        _RTProf_DumpQueue(RT_PROF_BROADCAST_TO_QUEUE,queue,RT_PROF_OK);
#endif
        }
    }

    /* Release protection against access to the queue.  */
    TCT_Unprotect();

    /* Return to user mode */
    NU_USER_MODE();

    /* Return the completion status.  */
    return(status);
}