static bool _CORE_message_queue_Order(
const Chain_Node *left,
const Chain_Node *right
)
{
const CORE_message_queue_Buffer_control *left_message;
const CORE_message_queue_Buffer_control *right_message;
left_message = (const CORE_message_queue_Buffer_control *) left;
right_message = (const CORE_message_queue_Buffer_control *) right;
return _CORE_message_queue_Get_message_priority( left_message ) <
_CORE_message_queue_Get_message_priority( right_message );
}
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
);
}
