void _CORE_message_queue_Insert_message(
CORE_message_queue_Control *the_message_queue,
CORE_message_queue_Buffer_control *the_message,
CORE_message_queue_Submit_types submit_type
)
{
Chain_Control *pending_messages;
#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
bool notify;
#endif
_CORE_message_queue_Set_message_priority( the_message, submit_type );
pending_messages = &the_message_queue->Pending_messages;
#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
notify = ( the_message_queue->number_of_pending_messages == 0 );
#endif
++the_message_queue->number_of_pending_messages;
if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST ) {
_Chain_Append_unprotected( pending_messages, &the_message->Node );
#if defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
} else if ( submit_type != CORE_MESSAGE_QUEUE_URGENT_REQUEST ) {
_Chain_Insert_ordered_unprotected(
pending_messages,
&the_message->Node,
_CORE_message_queue_Order
);
#endif
} else {
_Chain_Prepend_unprotected( pending_messages, &the_message->Node );
}
#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
/*
* According to POSIX, does this happen before or after the message
* is actually enqueued. It is logical to think afterwards, because
* the message is actually in the queue at this point.
*/
if ( notify && the_message_queue->notify_handler )
(*the_message_queue->notify_handler)(the_message_queue->notify_argument);
#endif
}
void _CORE_message_queue_Insert_message(
CORE_message_queue_Control *the_message_queue,
CORE_message_queue_Buffer_control *the_message,
CORE_message_queue_Submit_types submit_type
)
{
ISR_Level level;
#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
bool notify = false;
#define SET_NOTIFY() \
do { \
if ( the_message_queue->number_of_pending_messages == 0 ) \
notify = true; \
} while (0)
#else
#define SET_NOTIFY()
#endif
_CORE_message_queue_Set_message_priority( the_message, submit_type );
#if !defined(RTEMS_SCORE_COREMSG_ENABLE_MESSAGE_PRIORITY)
_ISR_Disable( level );
SET_NOTIFY();
the_message_queue->number_of_pending_messages++;
if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST )
_CORE_message_queue_Append_unprotected(the_message_queue, the_message);
else
_CORE_message_queue_Prepend_unprotected(the_message_queue, the_message);
_ISR_Enable( level );
#else
if ( submit_type == CORE_MESSAGE_QUEUE_SEND_REQUEST ) {
_ISR_Disable( level );
SET_NOTIFY();
the_message_queue->number_of_pending_messages++;
_CORE_message_queue_Append_unprotected(the_message_queue, the_message);
_ISR_Enable( level );
} else if ( submit_type == CORE_MESSAGE_QUEUE_URGENT_REQUEST ) {
_ISR_Disable( level );
SET_NOTIFY();
the_message_queue->number_of_pending_messages++;
_CORE_message_queue_Prepend_unprotected(the_message_queue, the_message);
_ISR_Enable( level );
} else {
CORE_message_queue_Buffer_control *this_message;
Chain_Node *the_node;
Chain_Control *the_header;
int the_priority;
the_priority = _CORE_message_queue_Get_message_priority(the_message);
the_header = &the_message_queue->Pending_messages;
the_node = _Chain_First( the_header );
while ( !_Chain_Is_tail( the_header, the_node ) ) {
int this_priority;
this_message = (CORE_message_queue_Buffer_control *) the_node;
this_priority = _CORE_message_queue_Get_message_priority(this_message);
if ( this_priority <= the_priority ) {
the_node = the_node->next;
continue;
}
break;
}
_ISR_Disable( level );
SET_NOTIFY();
the_message_queue->number_of_pending_messages++;
_Chain_Insert_unprotected( the_node->previous, &the_message->Node );
_ISR_Enable( level );
}
#endif
#if defined(RTEMS_SCORE_COREMSG_ENABLE_NOTIFICATION)
/*
* According to POSIX, does this happen before or after the message
* is actually enqueued. It is logical to think afterwards, because
* the message is actually in the queue at this point.
*/
if ( notify && the_message_queue->notify_handler )
(*the_message_queue->notify_handler)(the_message_queue->notify_argument);
#endif
}
void _CORE_message_queue_Seize(
CORE_message_queue_Control *the_message_queue,
Thread_Control *executing,
Objects_Id id,
void *buffer,
size_t *size_p,
bool wait,
Watchdog_Interval timeout
)
{
ISR_Level level;
CORE_message_queue_Buffer_control *the_message;
executing->Wait.return_code = CORE_MESSAGE_QUEUE_STATUS_SUCCESSFUL;
_ISR_Disable( level );
the_message = _CORE_message_queue_Get_pending_message( the_message_queue );
if ( the_message != NULL ) {
the_message_queue->number_of_pending_messages -= 1;
_ISR_Enable( level );
*size_p = the_message->Contents.size;
executing->Wait.count =
_CORE_message_queue_Get_message_priority( the_message );
_CORE_message_queue_Copy_buffer(
the_message->Contents.buffer,
buffer,
*size_p
);
#if !defined(RTEMS_SCORE_COREMSG_ENABLE_BLOCKING_SEND)
/*
* There is not an API with blocking sends enabled.
* So return immediately.
*/
_CORE_message_queue_Free_message_buffer(the_message_queue, the_message);
return;
#else
{
Thread_Control *the_thread;
/*
* There could be a thread waiting to send a message. If there
* is not, then we can go ahead and free the buffer.
*
* NOTE: If we note that the queue was not full before this receive,
* then we can avoid this dequeue.
*/
the_thread = _Thread_queue_Dequeue( &the_message_queue->Wait_queue );
if ( !the_thread ) {
_CORE_message_queue_Free_message_buffer(
the_message_queue,
the_message
);
return;
}
/*
* There was a thread waiting to send a message. This code
* puts the messages in the message queue on behalf of the
* waiting task.
*/
_CORE_message_queue_Set_message_priority(
the_message,
the_thread->Wait.count
);
the_message->Contents.size = (size_t) the_thread->Wait.option;
_CORE_message_queue_Copy_buffer(
the_thread->Wait.return_argument_second.immutable_object,
the_message->Contents.buffer,
the_message->Contents.size
);
_CORE_message_queue_Insert_message(
the_message_queue,
the_message,
_CORE_message_queue_Get_message_priority( the_message )
);
return;
}
#endif
}
if ( !wait ) {
_ISR_Enable( level );
executing->Wait.return_code = CORE_MESSAGE_QUEUE_STATUS_UNSATISFIED_NOWAIT;
return;
}
_Thread_queue_Enter_critical_section( &the_message_queue->Wait_queue );
executing->Wait.queue = &the_message_queue->Wait_queue;
executing->Wait.id = id;
executing->Wait.return_argument_second.mutable_object = buffer;
executing->Wait.return_argument = size_p;
/* Wait.count will be filled in with the message priority */
_ISR_Enable( level );
_Thread_queue_Enqueue(
&the_message_queue->Wait_queue,
executing,
STATES_WAITING_FOR_MESSAGE,
timeout
);
}
CORE_message_queue_Status _CORE_message_queue_Submit(
CORE_message_queue_Control *the_message_queue,
Thread_Control *executing,
const void *buffer,
size_t size,
Objects_Id id,
#if defined(RTEMS_MULTIPROCESSING)
CORE_message_queue_API_mp_support_callout api_message_queue_mp_support,
#else
CORE_message_queue_API_mp_support_callout api_message_queue_mp_support RTEMS_UNUSED,
#endif
CORE_message_queue_Submit_types submit_type,
bool wait,
Watchdog_Interval timeout,
ISR_lock_Context *lock_context
)
{
CORE_message_queue_Buffer_control *the_message;
Thread_Control *the_thread;
if ( size > the_message_queue->maximum_message_size ) {
_ISR_lock_ISR_enable( lock_context );
return CORE_MESSAGE_QUEUE_STATUS_INVALID_SIZE;
}
_CORE_message_queue_Acquire_critical( the_message_queue, lock_context );
/*
* Is there a thread currently waiting on this message queue?
*/
the_thread = _CORE_message_queue_Dequeue_receiver(
the_message_queue,
buffer,
size,
submit_type,
lock_context
);
if ( the_thread != NULL ) {
#if defined(RTEMS_MULTIPROCESSING)
if ( !_Objects_Is_local_id( the_thread->Object.id ) )
(*api_message_queue_mp_support) ( the_thread, id );
_Thread_Dispatch_enable( _Per_CPU_Get() );
#endif
return CORE_MESSAGE_QUEUE_STATUS_SUCCESSFUL;
}
/*
* No one waiting on the message queue at this time, so attempt to
* queue the message up for a future receive.
*/
the_message =
_CORE_message_queue_Allocate_message_buffer( the_message_queue );
if ( the_message ) {
the_message->Contents.size = size;
_CORE_message_queue_Set_message_priority( the_message, submit_type );
_CORE_message_queue_Copy_buffer(
buffer,
the_message->Contents.buffer,
size
);
_CORE_message_queue_Insert_message(
the_message_queue,
the_message,
submit_type
);
_CORE_message_queue_Release( the_message_queue, lock_context );
return CORE_MESSAGE_QUEUE_STATUS_SUCCESSFUL;
}
#if !defined(RTEMS_SCORE_COREMSG_ENABLE_BLOCKING_SEND)
_CORE_message_queue_Release( the_message_queue, lock_context );
return CORE_MESSAGE_QUEUE_STATUS_TOO_MANY;
#else
/*
* No message buffers were available so we may need to return an
* overflow error or block the sender until the message is placed
* on the queue.
*/
if ( !wait ) {
_CORE_message_queue_Release( the_message_queue, lock_context );
return CORE_MESSAGE_QUEUE_STATUS_TOO_MANY;
}
/*
* Do NOT block on a send if the caller is in an ISR. It is
* deadly to block in an ISR.
*/
if ( _ISR_Is_in_progress() ) {
_CORE_message_queue_Release( the_message_queue, lock_context );
return CORE_MESSAGE_QUEUE_STATUS_UNSATISFIED;
}
/*
* WARNING!! executing should NOT be used prior to this point.
* Thus the unusual choice to open a new scope and declare
* it as a variable. Doing this emphasizes how dangerous it
* would be to use this variable prior to here.
*/
executing->Wait.id = id;
executing->Wait.return_argument_second.immutable_object = buffer;
executing->Wait.option = (uint32_t) size;
executing->Wait.count = submit_type;
_Thread_queue_Enqueue_critical(
&the_message_queue->Wait_queue.Queue,
the_message_queue->Wait_queue.operations,
executing,
STATES_WAITING_FOR_MESSAGE,
timeout,
CORE_MESSAGE_QUEUE_STATUS_TIMEOUT,
lock_context
);
#if defined(RTEMS_MULTIPROCESSING)
_Thread_Dispatch_enable( _Per_CPU_Get() );
#endif
return CORE_MESSAGE_QUEUE_STATUS_UNSATISFIED_WAIT;
#endif
}
