void Logging_task(uint_32 param)
{
_queue_id log_qid;
LOG_MESSAGE_PTR msg_ptr;
#if DEMOCFG_ENABLE_USB_FILESYSTEM
MQX_FILE_PTR log_fp;
#endif
// create a pool of logging messages
log_pool = _msgpool_create(sizeof(LOG_MESSAGE), LOG_POOL_SIZE, 0, 0);
// open a message queue to receive log message on
log_qid = _msgq_open(LOG_QUEUE, 0);
// signal that initialization is complete
_lwsem_post(&Logging_init_sem);
while (TRUE) {
// wait for a message
msg_ptr = _msgq_receive(log_qid, 0);
if (msg_ptr) {
#if DEMOCFG_ENABLE_USB_FILESYSTEM
// check to see if a filesystem is available
if (_lwsem_poll(&USB_Stick)) {
// Open the log file and position to the end
log_fp = fopen(LOG_FILE,"a");
if (log_fp) {
fseek(log_fp,0,IO_SEEK_END);
do {
// Write the message to the log file
write(log_fp,msg_ptr->MESSAGE, strlen(msg_ptr->MESSAGE));
// Return the message back to the message pool
_msg_free(msg_ptr);
// check for another message
msg_ptr = _msgq_poll(log_qid);
} while (msg_ptr != NULL);
// close the file
fclose(log_fp);
}
// Indicate that the USB stick is no longer in use
_lwsem_post(&USB_Stick);
} else {
_msg_free(msg_ptr);
}
#else
printf(msg_ptr->MESSAGE);
_msg_free(msg_ptr);
#endif
}
}
}
void msg_on_shutdown(void)
{
int i;
for (i = 0; i < _msg_max; i++)
_msg_free(_msgs[i]);
free(_msgs);
}
void client_task
(
uint32_t index
)
{
SERVER_MESSAGE_PTR msg_ptr;
_queue_id client_qid;
bool result;
client_qid = _msgq_open((_queue_number)(CLIENT_QUEUE_BASE +
index), 0);
if (client_qid == 0) {
printf("\nCould not open a client message queue\n");
_task_block();
}
while (TRUE) {
/*allocate a message*/
msg_ptr = (SERVER_MESSAGE_PTR)_msg_alloc(message_pool);
if (msg_ptr == NULL) {
printf("\nCould not allocate a message\n");
_task_block();
}
msg_ptr->HEADER.SOURCE_QID = client_qid;
msg_ptr->HEADER.TARGET_QID = _msgq_get_id(0, SERVER_QUEUE);
msg_ptr->HEADER.SIZE = sizeof(MESSAGE_HEADER_STRUCT) +
strlen((char *)msg_ptr->DATA) + 1;
msg_ptr->DATA[0] = ('A'+ index);
printf("Client Task %ld\n", index);
result = _msgq_send(msg_ptr);
if (result != TRUE) {
printf("\nCould not send a message\n");
_task_block();
}
/* wait for a return message */
msg_ptr = _msgq_receive(client_qid, 0);
if (msg_ptr == NULL) {
printf("\nCould not receive a message\n");
_task_block();
}
/* free the message */
_msg_free(msg_ptr);
}
}
void runSerialHandler(os_task_param_t task_init_data)
{
printf("[Serial Handler] Task started.\n");
// Initialize queues and message pools
_queue_id interruptQueue = _initializeQueue(HANDLER_INTERRUPT_QUEUE_ID);
_queue_id inputQueue = _initializeQueue(HANDLER_INPUT_QUEUE_ID);
_initializeHandlerMessagePools();
// Initialize Handler
Handler handler;
g_Handler = &handler;
_initializeHandlerMutex(&g_HandlerMutex);
_initializeHandler(g_Handler, interruptQueue, inputQueue, myUART_IDX);
#ifdef PEX_USE_RTOS
while (1) {
#endif
// Wait for any incoming messages
GenericMessagePtr receivedMessage = (GenericMessagePtr) _msgq_receive(MSGQ_ANY_QUEUE, 0);
if (receivedMessage == NULL){
printf("[Serial Handler] Failed to receive a message.\n");
_task_block();
}
// Lock access to the handler while it processses the message
if(_mutex_lock(&g_HandlerMutex) != MQX_OK){
printf("[Serial Handler] Mutex lock failed.\n");
_task_block();
}
// If the message is an serial message from the current writer, handle the writer input
if(receivedMessage->HEADER.TARGET_QID == inputQueue){
_handleWriteMessage((SerialMessagePtr) receivedMessage, g_Handler);
}
// If the message is an interrupt message from the UART event handler, handle the character press
else if (receivedMessage->HEADER.TARGET_QID == interruptQueue){
_handleInterruptMessage((InterruptMessagePtr) receivedMessage, g_Handler);
}
_msg_free(receivedMessage);
// Unlock the handler for user access
_mutex_unlock(&g_HandlerMutex);
#ifdef PEX_USE_RTOS
}
#endif
}
void main_task
(
uint32_t dummy
)
{
_pool_id msgpool;
THE_MESSAGE_PTR msg_ptr;
_queue_id qid;
_queue_id my_qid;
uint32_t test_number = 0;
my_qid = _msgq_open(MAIN_QUEUE,0);
qid = _msgq_get_id(TEST2_ID,RESPONDER_QUEUE);
msgpool = _msgpool_create(sizeof(THE_MESSAGE), 8, 8, 16);
while (test_number < 1000) {
msg_ptr = (THE_MESSAGE_PTR)_msg_alloc(msgpool);
msg_ptr->HEADER.TARGET_QID = qid;
msg_ptr->HEADER.SOURCE_QID = my_qid;
msg_ptr->DATA = test_number++;
putchar('-');
_msgq_send(msg_ptr);
msg_ptr = _msgq_receive(MSGQ_ANY_QUEUE, 1000);
if (msg_ptr == NULL) {
puts("Receive failed\n");
//_task_block();
} else {
if (msg_ptr->HEADER.SIZE != sizeof(THE_MESSAGE)) {
puts("Message wrong size\n");
//_task_block();
} else {
if (MSG_MUST_CONVERT_DATA_ENDIAN(msg_ptr->HEADER.CONTROL)) {
_mem_swap_endian(global_endian_swap_def, &msg_ptr->DATA);
} /* Endif */
if (msg_ptr->DATA != test_number) {
puts("Message data incorrect\n");
//_task_block();
} else {
puts("Message received\n");
}
}
_msg_free(msg_ptr);
}
}
puts("All complete\n");
_task_block();
}
void runScheduler(os_task_param_t task_init_data)
{
printf("[Scheduler] Task started.\n");
_queue_id requestQueue = _initializeQueue(SCHEDULER_INTERFACE_QUEUE_ID);
_initializeScheduler(requestQueue, USER_TASKS, USER_TASK_COUNT);
MQX_TICK_STRUCT nextDeadline;
SchedulerRequestMessagePtr requestMessage;
#ifdef PEX_USE_RTOS
while (1) {
#endif
requestMessage = NULL;
bool deadlineExists = _getNextDeadline(&nextDeadline);
// If the scheduler currently has tasks, wait for a new message or the next deadline
if(deadlineExists){
requestMessage = _msgq_receive_until(requestQueue, &nextDeadline);
// Handle reached deadlines
if(requestMessage == NULL){
_handleDeadlineReached();
continue;
}
}
// Otherwise wait indefinitely for a scheduler request
else{
requestMessage = _msgq_receive(requestQueue, 0);
}
// Handle scheduler requests
_handleSchedulerRequest(requestMessage);
_msg_free(requestMessage);
#ifdef PEX_USE_RTOS
}
#endif
}
/*!
* \brief This function is called when a task is being destroyed.
*
* Closes all open message queues and then free all messages owned by this task.
*
* \param[in] td_ptr The task descriptor of the task that is being destroyed.
*/
void _msg_cleanup
(
TD_STRUCT_PTR td_ptr
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
MSG_COMPONENT_STRUCT_PTR msg_component_ptr;
MSGQ_STRUCT_PTR msgq_ptr;
MSGQ_STRUCT_PTR qprev_ptr;
MSGPOOL_STRUCT_PTR msgpool_ptr;
MSGPOOL_BLOCK_STRUCT_PTR msgpool_block_ptr;
INTERNAL_MESSAGE_STRUCT_PTR imsg_ptr;
INTERNAL_MESSAGE_STRUCT_PTR tmp_imsg_ptr;
_mqx_uint i,j,raw_message_size;
_GET_KERNEL_DATA(kernel_data);
msg_component_ptr = _GET_MSG_COMPONENT_STRUCT_PTR(kernel_data);
#if MQX_CHECK_ERRORS
if (msg_component_ptr == NULL)
{
return;
} /* Endif */
#endif
/* Delete the message queues owned by the task, and free the messages
* on the queues
*/
msgq_ptr = (MSGQ_STRUCT_PTR)td_ptr->MSG_QUEUE_HEAD;
while (msgq_ptr != NULL)
{
if (msgq_ptr->TD_PTR != NULL)
{
msgq_ptr->TD_PTR->MESSAGES_AVAILABLE -= msgq_ptr->NO_OF_ENTRIES;
} /* Endif */
imsg_ptr = msgq_ptr->FIRST_MSG_PTR;
while ( imsg_ptr != NULL )
{
tmp_imsg_ptr = imsg_ptr->NEXT;
imsg_ptr->QUEUED = FALSE;
_msg_free((pointer)&imsg_ptr->MESSAGE);
imsg_ptr = tmp_imsg_ptr;
} /* Endwhile */
qprev_ptr = msgq_ptr;
msgq_ptr = msgq_ptr->NEXT_MSGQ_PTR;
qprev_ptr->FIRST_MSG_PTR = NULL;
qprev_ptr->NEXT_MSGQ_PTR = NULL;
qprev_ptr->QUEUE = 0;
} /* Endwhile */
td_ptr->MSG_QUEUE_HEAD = NULL;
td_ptr->MSG_QUEUE_TAIL = NULL;
/* Search through all of the message pools, looking for any messages
* owned by this task. If found free them.
*/
msgpool_ptr = msg_component_ptr->MSGPOOLS_PTR;
i = msg_component_ptr->MAX_MSGPOOLS + 1;
while (--i)
{
if (msgpool_ptr->VALID == MSG_VALID)
{
/* Search through all of the message pool blocks for this pool */
msgpool_block_ptr = msgpool_ptr->MSGPOOL_BLOCK_PTR;
while (msgpool_block_ptr != NULL)
{
raw_message_size = msgpool_block_ptr->RAW_MESSAGE_SIZE;
imsg_ptr = (INTERNAL_MESSAGE_STRUCT_PTR)
((uchar _PTR_)msgpool_block_ptr + sizeof(MSGPOOL_BLOCK_STRUCT));
/* if the message is not free, not queued and the
* owner is the task being destroyed, then Free the message
*/
j = msgpool_block_ptr->NUM_MESSAGES + 1;
while (--j)
{
if ( (imsg_ptr->TD_PTR == td_ptr) &&
(imsg_ptr->FREE == FALSE) &&
(imsg_ptr->QUEUED == FALSE) )
{
_msg_free(&imsg_ptr->MESSAGE);
} /* Endif */
imsg_ptr =(INTERNAL_MESSAGE_STRUCT_PTR)
((uchar _PTR_)imsg_ptr + raw_message_size);
} /* Endwhile */
msgpool_block_ptr = msgpool_block_ptr->NEXT_BLOCK_PTR;
} /* Endwhile */
} /* Endif */
msgpool_ptr++;
} /* Endwhile */
} /* Endbody */
void main_task
(
uint_32 parameter
)
{
MESSAGE_HEADER_STRUCT_PTR msg_ptr;
_task_id created_task;
_mqx_uint log_result;
_mqx_uint event_result;
_mqx_uint sem_result;
_int_install_unexpected_isr();
printf("\nMQX %s\n",_mqx_version);
printf("Hello from main_task().\n");
/* create the log component */
log_result = _log_create_component();
if (log_result != MQX_OK) {
/* log component could not be created */
} /* endif */
/* Create the mutex component */
if (_mutex_create_component() != MQX_OK) {
/* an error has been detected */
}
/* create the event component */
event_result = _event_create_component(EVENT_INITIAL_NUMBER, EVENT_GROWTH,
EVENT_MAXIMUM);
if (event_result != MQX_OK) {
/* event component could not be created */
printf("Error: Cannot create event component\n");
_task_block();
} /* endif */
/* create the semaphore component */
sem_result = _sem_create_component(SEM_INITIAL_NUMBER, SEM_GROWTH,
SEM_MAXIMUM);
if (sem_result != MQX_OK) {
/* semaphore component could not be created */
printf("Error: Cannot create semaphore component\n");
_task_block();
} /* endif */
MsgPool_pool_id = _msgpool_create ( 8, 10, 0, 0);
if (MsgPool_pool_id == MSGPOOL_NULL_POOL_ID) {
/* _msgpool_create did not succeed */
printf("Error: Cannot create message pool\n");
_task_block();
}
Main_Queue_qid = _msgq_open( MSGQ_FREE_QUEUE, SIZE_UNLIMITED);
if (Main_Queue_qid == (_queue_id)0){
/* queue could not be opened */
printf("Error: Cannot open message pool\n");
_task_block();
}
created_task = _task_create(0, SENDER, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
created_task = _task_create(0, MUTEXA, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
created_task = _task_create(0, MUTEXB, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
created_task = _task_create(0, SEMA, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
created_task = _task_create(0, SEMB, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
created_task = _task_create(0, EVENTA, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
created_task = _task_create(0, EVENTB, 0);
if (created_task == MQX_NULL_TASK_ID) {
/* task creation failed */
printf("Error: Cannot create task\n");
_task_block();
}
#if MQX_KERNEL_LOGGING
/* create log number 0 */
log_result = _klog_create(200, 0);
if (log_result != MQX_OK) {
/* log 0 could not be created */
printf("Error: Cannot create kernel log\n");
_task_block();
} /* endif */
/* define kernel logging */
_klog_control(0xFFFFFFFF, FALSE);
_klog_control(
KLOG_ENABLED |
KLOG_FUNCTIONS_ENABLED |
KLOG_INTERRUPTS_ENABLED |
KLOG_SYSTEM_CLOCK_INT_ENABLED |
KLOG_CONTEXT_ENABLED |
KLOG_TASKING_FUNCTIONS |
KLOG_ERROR_FUNCTIONS |
KLOG_MESSAGE_FUNCTIONS |
KLOG_INTERRUPT_FUNCTIONS |
KLOG_MEMORY_FUNCTIONS |
KLOG_TIME_FUNCTIONS |
KLOG_EVENT_FUNCTIONS |
KLOG_NAME_FUNCTIONS |
KLOG_MUTEX_FUNCTIONS |
KLOG_SEMAPHORE_FUNCTIONS |
KLOG_WATCHDOG_FUNCTIONS,
TRUE
);
#endif
/*
** LOOP -
*/
while ( TRUE ) {
msg_ptr = _msg_alloc((_pool_id) MsgPool_pool_id );
if (msg_ptr == NULL) {
/* No available message buffer */
}
msg_ptr->SIZE = sizeof(MESSAGE_HEADER_STRUCT);
msg_ptr->SOURCE_QID = msg_ptr->TARGET_QID;
msg_ptr->TARGET_QID = Sender_Queue_qid;
_msgq_send(msg_ptr);
/*
* Service the message queue - Main_Queue
*/
msg_ptr = _msgq_receive_ticks(Main_Queue_qid, NO_TIMEOUT);
/* process message End_msg */
_msg_free(msg_ptr);
} /* endwhile */
} /*end of task*/
boolean _msgq_send_internal
(
/* [IN] pointer to the message being sent by application */
MESSAGE_HEADER_STRUCT_PTR msg_ptr,
/* [IN] is the calling task to be blocked after the call */
boolean blocking,
/* [IN] the queue to put the message onto */
_queue_id target_qid
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
MSG_COMPONENT_STRUCT_PTR msg_component_ptr;
register INTERNAL_MESSAGE_STRUCT_PTR imsg_ptr;
register MSGQ_STRUCT_PTR msgq_ptr;
register TD_STRUCT_PTR td_ptr;
MESSAGE_HEADER_STRUCT_PTR tmp_msg_ptr;
register _mqx_uint state;
register _queue_number queue;
register _processor_number pnum;
/* Start CR 2191 */
boolean swapped_msg;
/* End CR 2191 */
_GET_KERNEL_DATA(kernel_data);
msg_component_ptr = _GET_MSG_COMPONENT_STRUCT_PTR(kernel_data);
#if MQX_CHECK_ERRORS
if (msg_component_ptr == NULL){
_task_set_error(MQX_COMPONENT_DOES_NOT_EXIST);
return(FALSE);
} /* Endif */
if (msg_ptr == NULL) {
_task_set_error(MSGQ_INVALID_MESSAGE);
return(FALSE);
} /* Endif */
#endif
imsg_ptr = GET_INTERNAL_MESSAGE_PTR(msg_ptr);
#if MQX_CHECK_ERRORS
if (imsg_ptr->VALID != MSG_VALID){
/* An invalid message was input by the application. */
_task_set_error(MSGQ_INVALID_MESSAGE);
return FALSE;
} /* Endif */
#endif
#if MQX_CHECK_ERRORS
if (imsg_ptr->FREE || imsg_ptr->QUEUED){
/* Trying to send a free message, or one on a message queue. */
_task_set_error(MSGQ_INVALID_MESSAGE);
return FALSE;
} /* Endif */
#endif
pnum = PROC_NUMBER_FROM_QID(target_qid);
/* If processor number is zero then the message is for this processor */
if (pnum == 0) {
/* Fix up the target QID in the message header */
msg_ptr->TARGET_QID = BUILD_QID(kernel_data->INIT.PROCESSOR_NUMBER,
msg_ptr->TARGET_QID);
} else if (pnum != kernel_data->INIT.PROCESSOR_NUMBER) {
#if MQX_IS_MULTI_PROCESSOR
IPC_MSG_ROUTING_COMPONENT_STRUCT_PTR ipc_msg_comp_ptr;
ipc_msg_comp_ptr = (IPC_MSG_ROUTING_COMPONENT_STRUCT_PTR)
kernel_data->KERNEL_COMPONENTS[KERNEL_IPC_MSG_ROUTING];
if (ipc_msg_comp_ptr && ipc_msg_comp_ptr->MSG_ROUTER) {
return( (*ipc_msg_comp_ptr->MSG_ROUTER)(pnum, msg_ptr, blocking));
} else {
#endif
_msg_free(msg_ptr);
_task_set_error(MSGQ_INVALID_QUEUE_ID);
return FALSE;
#if MQX_IS_MULTI_PROCESSOR
}/* Endif */
#endif
} /* Endif */
queue = QUEUE_FROM_QID(target_qid);
#if MQX_CHECK_ERRORS
if ( ! VALID_QUEUE(queue)) {
_msg_free(msg_ptr);
_task_set_error(MSGQ_INVALID_QUEUE_ID);
return FALSE;
} /* Endif */
#endif
msgq_ptr = &msg_component_ptr->MSGQS_PTR[queue];
if (msgq_ptr->QUEUE != queue) {
msgq_ptr = NULL;
} /* Endif */
#if MQX_CHECK_ERRORS
if (msgq_ptr == NULL) {
_msg_free(msg_ptr);
_task_set_error(MSGQ_QUEUE_IS_NOT_OPEN);
return FALSE;
} /* Endif */
#endif
_INT_DISABLE();
if ((msgq_ptr->MAX_ENTRIES == 0) ||
(msgq_ptr->NO_OF_ENTRIES < msgq_ptr->MAX_ENTRIES))
{
/* End CR 2265 */
/* There is room on the queue, so add the msg. We
need to check for room here even if the msg ends up
being short-cutted to the receiver (via td_ptr->MESSAGE)
in case msg_receive needs to enqueue the msg.
*/
if (msgq_ptr->TYPE == MSG_QUEUE) {
/* THIS MESSAGE QUEUE IS ATTACHED TO A TASK */
/* check for pending receive
** if a receive is pending then satisfy the request
** and add the receiving task onto the ready-to-run queue
*/
td_ptr = msgq_ptr->TD_PTR;
state = td_ptr->STATE & STATE_MASK;
if ( (state == RCV_ANY_BLOCKED) ||
((state == RCV_SPECIFIC_BLOCKED) && (td_ptr->INFO == queue)))
{
/* The task is blocked, waiting for a message */
td_ptr->MESSAGE = &imsg_ptr->MESSAGE;
imsg_ptr->TD_PTR = td_ptr;
_TIME_DEQUEUE(td_ptr,kernel_data);
_TASK_READY(td_ptr,kernel_data);
/* Now run the notification function */
if (msgq_ptr->NOTIFICATION_FUNCTION != NULL) {
(*msgq_ptr->NOTIFICATION_FUNCTION)(msgq_ptr->NOTIFICATION_FUNCTION_PARAMETER);
} /* Endif */
if (blocking) {
if ( ! kernel_data->IN_ISR) {
td_ptr = kernel_data->ACTIVE_PTR;
td_ptr->STATE = SEND_BLOCKED;
_task_block();
} /* Endif */
} else {
/*
** if the highest priority ready task is not the
** same priority as the sending task, then a higher
** priority task was made ready and it has to be allowed
** to run.
*/
_CHECK_RUN_SCHEDULER(); /* Let a higher priority task run */
} /* Endif */
} else {
/* The task is ready to run and pre-empted OR blocked and
** on a different queue.
*/
/* Start CR 2191 */
swapped_msg = FALSE;
/* End CR 2191 */
if ((msg_ptr->CONTROL & MSG_PRIORITY_MASK) &&
(td_ptr->MESSAGE != NULL))
{
/* Check the message in the TD */
tmp_msg_ptr = (MESSAGE_HEADER_STRUCT_PTR)td_ptr->MESSAGE;
if ( (msg_ptr->CONTROL & MSG_HDR_URGENT) ||
/* Urgent messages first */
( (! (tmp_msg_ptr->CONTROL & MSG_HDR_URGENT)) &&
/* Start CR 621 */
( (_mqx_uint)(tmp_msg_ptr->CONTROL & MSG_HDR_PRIORITY_MASK) <
(_mqx_uint)(msg_ptr->CONTROL & MSG_HDR_PRIORITY_MASK))
/* End CR 621 */
)
)
/* Higher priority messages first */
{
/* Put new message into TD */
td_ptr->MESSAGE = msg_ptr;
/* Start CR 2193 */
/* Set the new message's ownership to the receiving queue's TD */
imsg_ptr = GET_INTERNAL_MESSAGE_PTR(msg_ptr);
imsg_ptr->TD_PTR = td_ptr;
/* Old message which we pulled from TD, need to add to queue, below */
/* End CR 2193 */
msg_ptr = tmp_msg_ptr;
imsg_ptr = GET_INTERNAL_MESSAGE_PTR(msg_ptr);
/* Don't know the sender's TD for the swapped out msg,
so set it to NULL; */
imsg_ptr->TD_PTR = NULL;
/* Start CR 2191 */
/* Indicate that a swap occurred */
swapped_msg = TRUE;
/* Set the queue to the swapped msg's queue. */
if (target_qid != msg_ptr->TARGET_QID)
{
queue = QUEUE_FROM_QID(msg_ptr->TARGET_QID);
msgq_ptr = &msg_component_ptr->MSGQS_PTR[queue];
/* This msg's queue was not full when it was short-cut,
so we should not get here. Check anyway. */
if ((msgq_ptr->MAX_ENTRIES != 0) &&
(msgq_ptr->NO_OF_ENTRIES >= msgq_ptr->MAX_ENTRIES))
{
/* Queue full, error */
_INT_ENABLE();
_msg_free(msg_ptr);
_task_set_error(MSGQ_QUEUE_FULL);
return FALSE;
} /* Endif */
} /* Endif */
} /* Endif */
} /* Endif */
/* add the message */
_msgq_insert_message_internal(msgq_ptr, imsg_ptr, swapped_msg);
if (msgq_ptr->TD_PTR){
++(msgq_ptr->TD_PTR->MESSAGES_AVAILABLE);
} /* Endif */
/* Now run the notification function */
if (msgq_ptr->NOTIFICATION_FUNCTION != NULL) {
(*msgq_ptr->NOTIFICATION_FUNCTION)(msgq_ptr->NOTIFICATION_FUNCTION_PARAMETER);
} /* Endif */
if (blocking && ! kernel_data->IN_ISR ) {
td_ptr = kernel_data->ACTIVE_PTR;
td_ptr->STATE = SEND_BLOCKED;
_task_block();
} /* Endif */
} /* Endif */
} else {
/* THIS IS A SYSTEM QUEUE NOT ATTACHED TO A TASK */
/* add the message to the queue */
_msgq_insert_message_internal(msgq_ptr, imsg_ptr, FALSE);
/* Run the notification function. */
if ( msgq_ptr->NOTIFICATION_FUNCTION != NULL ) {
(*msgq_ptr->NOTIFICATION_FUNCTION)(msgq_ptr->NOTIFICATION_FUNCTION_PARAMETER);
} /* Endif */
} /* Endif */
} else {
/* Queue full, error */
_INT_ENABLE();
_task_set_error(MSGQ_QUEUE_FULL);
_msg_free(&imsg_ptr->MESSAGE);
return FALSE;
} /* Endif */
_INT_ENABLE();
return TRUE; /* Message sent MQX_OK */
} /* Endbody */
