_mqx_uint _lwsem_wait_timed_internal
(
/* [IN] the semaphore address */
LWSEM_STRUCT_PTR sem_ptr,
/* [IN] the task descriptor waiting */
TD_STRUCT_PTR td_ptr
)
{ /* Body */
td_ptr->STATE = LWSEM_BLOCKED;
td_ptr->INFO = (_mqx_uint)&sem_ptr->TD_QUEUE;
_QUEUE_UNLINK(td_ptr);
_QUEUE_ENQUEUE(&sem_ptr->TD_QUEUE, &td_ptr->AUX_QUEUE);
_time_delay_internal(td_ptr);
if (td_ptr->INFO != 0) {
/* Start CR 544 */
/*_QUEUE_REMOVE(&sem_ptr->TD_QUEUE, &td_ptr->AUX_QUEUE);*/
/* End CR 544 */
return(MQX_LWSEM_WAIT_TIMEOUT);
} /* Endif */
return(MQX_OK);
} /* Endbody */
void _int_default_isr
(
/* [IN] the parameter passed to the ISR by the kernel */
pointer vector_number
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
_GET_KERNEL_DATA(kernel_data);
td_ptr = kernel_data->ACTIVE_PTR;
_KLOGE5(KLOG_int_default_isr, td_ptr, vector_number,
&vector_number, vector_number);
_int_disable();
if (td_ptr->STATE != UNHANDLED_INT_BLOCKED) {
td_ptr->STATE = UNHANDLED_INT_BLOCKED;
td_ptr->INFO = (_mqx_uint)vector_number;
_task_set_error_td_internal(td_ptr, MQX_UNHANDLED_INTERRUPT);
_QUEUE_UNLINK(td_ptr);
} /* Endif */
_int_enable();
} /* Endbody */
_mqx_uint _taskq_suspend
(
/* [IN] the task queue handle */
pointer users_task_queue_ptr
)
{ /* Body */
register KERNEL_DATA_STRUCT_PTR kernel_data;
register TD_STRUCT_PTR td_ptr;
register TASK_QUEUE_STRUCT_PTR task_queue_ptr =
(TASK_QUEUE_STRUCT_PTR)users_task_queue_ptr;
_GET_KERNEL_DATA(kernel_data);
_KLOGE2(KLOG_taskq_suspend, users_task_queue_ptr);
#if MQX_CHECK_ERRORS
if (task_queue_ptr == NULL){
_KLOGX2(KLOG_taskq_suspend, MQX_INVALID_PARAMETER);
return(MQX_INVALID_PARAMETER);
} /* Endif */
if (kernel_data->IN_ISR) {
_KLOGX2(KLOG_taskq_suspend, MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
return(MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
}/* Endif */
#endif
td_ptr = kernel_data->ACTIVE_PTR;
_INT_DISABLE();
#if MQX_CHECK_VALIDITY
if (task_queue_ptr->VALID != TASK_QUEUE_VALID) {
_int_enable();
_KLOGX2(KLOG_taskq_suspend, MQX_INVALID_TASK_QUEUE);
return(MQX_INVALID_TASK_QUEUE);
} /* Endif */
#endif
td_ptr->STATE = TASK_QUEUE_BLOCKED;
_QUEUE_UNLINK(td_ptr); /* Remove task from ready to run queue */
td_ptr->INFO = (_mqx_uint)&task_queue_ptr->TD_QUEUE;
if (task_queue_ptr->POLICY & MQX_TASK_QUEUE_BY_PRIORITY) {
_sched_insert_priorityq_internal(&task_queue_ptr->TD_QUEUE, td_ptr);
} else {
_QUEUE_ENQUEUE(&task_queue_ptr->TD_QUEUE, td_ptr);
} /* Endif */
_sched_execute_scheduler_internal(); /* Let the other tasks run */
_INT_ENABLE();
_KLOGX2(KLOG_taskq_suspend, MQX_OK);
return( MQX_OK );
} /* Endbody */
/*!
* \brief An MQX-provided default ISR for unhandled interrupts. The function
* depends on the PSP.
*
* The function changes the state of the active task to UNHANDLED_INT_BLOCKED and
* blocks the task.
* \n The function uses the default I/O channel to display at least:
* \li Vector number that caused the unhandled exception.
* \li Task ID and task descriptor of the active task.
*
* \n Depending on the PSP, more information might be displayed.
*
* \param[in] parameter Parameter passed to the default ISR.
*
* \note
* Since the ISR uses printf() to display information to the default I/O channel,
* default I/O must not be on a channel that uses interrupt-driven I/O or the
* debugger.
*
* \warning Blocks the active task.
*
* \see _int_install_unexpected_isr
*/
void _int_unexpected_isr
(
void *parameter
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
_GET_KERNEL_DATA(kernel_data);
td_ptr = kernel_data->ACTIVE_PTR;
#if MQXCFG_PRINT_MEM_DUMP
#error "MQXCFG_PRINT_MEM_DUMP functionality is currently not supported. This feature will be supported again in future release.\
Please set MQXCFG_PRINT_MEM_DUMP to zero."
#endif
#if MQXCFG_PRINT_MEM_DUMP
{
unsigned int psp, msp, i;
printf("\n\r*** UNHANDLED INTERRUPT ***\n\r");
printf("Vector #: 0x%02x Task Id: 0x%0x Td_ptr 0x%x\n\r",
(unsigned int)parameter, (unsigned int)td_ptr->TASK_ID, (unsigned int)td_ptr);
psp = __get_PSP();
msp = __get_MSP();
printf("PSP: 0x%08x MSP: 0x%08x PSR: 0x%08x\n\r", psp, msp, (unsigned int)__get_xPSR());
printf("\n\r\n\rMemory dump:\n\r");
for (i = 0; i < 32; i += 4) {
printf("0x%08x : 0x%08x 0x%08x 0x%08x 0x%08x\n\r", psp + i * 4, ((unsigned int*)psp)[i], ((unsigned int*)psp)[i + 1], ((unsigned int*)psp)[i + 2], ((unsigned int*)psp)[i + 3]);
}
printf("\n\r\n\rMemory dump:\n\r");
for (i = 0; i < 32; i += 4) {
printf("0x%08x : 0x%08x 0x%08x 0x%08x 0x%08x\n\r", msp + i * 4, ((unsigned int*)msp)[i], ((unsigned int*)msp)[i + 1], ((unsigned int*)msp)[i + 2], ((unsigned int*)msp)[i + 3]);
}
}
#endif /* MQXCFG_PRINT_MEM_DUMP */
_INT_DISABLE();
if (td_ptr->STATE != UNHANDLED_INT_BLOCKED) {
td_ptr->STATE = UNHANDLED_INT_BLOCKED;
td_ptr->INFO = (_mqx_uint)parameter;
_QUEUE_UNLINK(td_ptr);
} /* Endif */
_INT_ENABLE();
} /* Endbody */
void _time_delay_internal
(
/* [IN] the task to delay */
register TD_STRUCT_PTR td_ptr
)
{ /* Body */
register KERNEL_DATA_STRUCT_PTR kernel_data;
register TD_STRUCT_PTR td2_ptr;
register TD_STRUCT_PTR tdprev_ptr;
register _mqx_uint count;
register _mqx_int result;
_GET_KERNEL_DATA(kernel_data);
/* Remove task from ready to run queue */
tdprev_ptr = (TD_STRUCT_PTR)((pointer)&kernel_data->TIMEOUT_QUEUE);
if ( _QUEUE_GET_SIZE(&kernel_data->TIMEOUT_QUEUE) ) {
/* Perform insertion sort by time */
td2_ptr = (TD_STRUCT_PTR)((pointer)kernel_data->TIMEOUT_QUEUE.NEXT);
/* SPR P171-0023-01 use pre-decrement on while loop */
count = _QUEUE_GET_SIZE(&kernel_data->TIMEOUT_QUEUE) + 1;
while ( --count ) {
/* END SPR */
result = PSP_CMP_TICKS(&td2_ptr->TIMEOUT, &td_ptr->TIMEOUT);
if (MQX_DELAY_ENQUEUE_POLICY(result)) { /* CR171 */
/* Enqueue before td2_ptr */
break;
} /* Endif */
tdprev_ptr = td2_ptr;
td2_ptr = td2_ptr->TD_NEXT;
} /* Endwhile */
} /* Endif */
/* Remove from ready queue */
_QUEUE_UNLINK(td_ptr);
/* Insert into timeout queue */
_QUEUE_INSERT(&kernel_data->TIMEOUT_QUEUE,tdprev_ptr,td_ptr);
td_ptr->STATE |= IS_ON_TIMEOUT_Q;
_sched_execute_scheduler_internal();
} /* Endbody */
void _int_unexpected_isr
(
/* [IN] the parameter passed to the default ISR, the vector */
pointer parameter
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
//PSP_INT_CONTEXT_STRUCT_PTR exception_frame_ptr;
uint_32 psp, msp, i;
_GET_KERNEL_DATA(kernel_data);
td_ptr = kernel_data->ACTIVE_PTR;
//exception_frame_ptr = kernel_data->INTERRUPT_CONTEXT_PTR;
printf("\n\r*** UNHANDLED INTERRUPT ***\n\r");
printf("Vector #: 0x%02x Task Id: 0x%0x Td_ptr 0x%x\n\r",
(uint_32)parameter, (uint_32)td_ptr->TASK_ID, (uint_32)td_ptr);
psp = __get_PSP();
msp = __get_MSP();
printf("PC: 0x%08x LR: 0x%08x PSP: 0x%08x MSP: 0x%08x PSR: 0x%08x\n\r", __get_PC(), __get_LR(), psp, msp, __get_PSR());
printf("\n\r\n\rMemory dump:\n\r");
for (i = 0; i < 32; i += 4) {
printf("0x%08x : 0x%08x 0x%08x 0x%08x 0x%08x\n\r", psp + i * 4, ((uint_32*)psp)[i], ((uint_32*)psp)[i + 1], ((uint_32*)psp)[i + 2], ((uint_32*)psp)[i + 3]);
}
printf("\n\r\n\rMemory dump:\n\r");
for (i = 0; i < 32; i += 4) {
printf("0x%08x : 0x%08x 0x%08x 0x%08x 0x%08x\n\r", msp + i * 4, ((uint_32*)msp)[i], ((uint_32*)msp)[i + 1], ((uint_32*)msp)[i + 2], ((uint_32*)msp)[i + 3]);
}
_INT_ENABLE();
if (td_ptr->STATE != UNHANDLED_INT_BLOCKED) {
td_ptr->STATE = UNHANDLED_INT_BLOCKED;
td_ptr->INFO = (_mqx_uint)parameter;
_QUEUE_UNLINK(td_ptr);
} /* Endif */
_INT_DISABLE();
} /* Endbody */
void _time_notify_kernel
(
void
)
{ /* Body */
register KERNEL_DATA_STRUCT_PTR kernel_data;
register TD_STRUCT_PTR td_ptr;
register TD_STRUCT_PTR next_td_ptr;
register _mqx_uint count;
register _mqx_int result;
_GET_KERNEL_DATA(kernel_data);
/*
** Update the current time.
*/
PSP_INC_TICKS(&kernel_data->TIME);
_INT_DISABLE();
if (kernel_data->GET_HWTICKS) {
// The hardware clock may have counted passed it's reference
// and have an interrupt pending. Thus, HW_TICKS may exceed
// kernel_data->HW_TICKS_PER_TICK and this tick_ptr may need
// normalizing. This is done in a moment.
kernel_data->TIME.HW_TICKS = (*kernel_data->GET_HWTICKS)
(kernel_data->GET_HWTICKS_PARAM);
} /* Endif */
// The tick_ptr->HW_TICKS value might exceed the
// kernel_data->HW_TICKS_PER_TICK and need to be
// normalized for the PSP.
PSP_NORMALIZE_TICKS(&kernel_data->TIME);
/*
** Check for tasks on the timeout queue, and wake the appropriate
** ones up. The timeout queue is a time-priority queue.
*/
count = _QUEUE_GET_SIZE(&kernel_data->TIMEOUT_QUEUE);
if (count) {
td_ptr = (TD_STRUCT_PTR)((pointer)kernel_data->TIMEOUT_QUEUE.NEXT);
++count;
while ( --count ) {
next_td_ptr = td_ptr->TD_NEXT;
result = PSP_CMP_TICKS(&kernel_data->TIME, &td_ptr->TIMEOUT);
if (result >= 0) {
--kernel_data->TIMEOUT_QUEUE.SIZE;
_QUEUE_UNLINK(td_ptr);
td_ptr->STATE &= ~IS_ON_TIMEOUT_Q;
if (td_ptr->STATE & TD_IS_ON_AUX_QUEUE) {
td_ptr->STATE &= ~TD_IS_ON_AUX_QUEUE;
_QUEUE_REMOVE(td_ptr->INFO, &td_ptr->AUX_QUEUE);
} /* Endif */
_TASK_READY(td_ptr, kernel_data);
} else {
break; /* No more to do */
} /* Endif */
td_ptr = next_td_ptr;
} /* Endwhile */
} /* Endif */
#if MQX_HAS_TIME_SLICE
/*
** Check if the currently running task is a time slice task
** and if its time has expired, put it at the end of its queue
*/
td_ptr = kernel_data->ACTIVE_PTR;
if ( td_ptr->FLAGS & MQX_TIME_SLICE_TASK ) {
PSP_INC_TICKS(&td_ptr->CURRENT_TIME_SLICE);
if (! (td_ptr->FLAGS & TASK_PREEMPTION_DISABLED) ) {
result = PSP_CMP_TICKS(&td_ptr->CURRENT_TIME_SLICE, &td_ptr->TIME_SLICE);
if ( result >= 0 ) {
_QUEUE_UNLINK(td_ptr);
_TASK_READY(td_ptr,kernel_data);
} /* Endif */
} /* Endif */
} /* Endif */
#endif
_INT_ENABLE();
#if MQX_USE_TIMER
/* If the timer component needs servicing, call its ISR function */
if (kernel_data->TIMER_COMPONENT_ISR != NULL) {
(*kernel_data->TIMER_COMPONENT_ISR)();
}/* Endif */
#endif
#if MQX_USE_LWTIMER
/* If the lwtimer needs servicing, call its ISR function */
if (kernel_data->LWTIMER_ISR != NULL) {
(*kernel_data->LWTIMER_ISR)();
}/* Endif */
#endif
} /* Endbody */
_mqx_uint _lwsem_wait_ticks
(
/* [IN] the semaphore address */
LWSEM_STRUCT_PTR sem_ptr,
/* [IN] the number of ticks to delay, if 0, delay forever */
_mqx_uint time_in_ticks
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
_mqx_uint result;
#if MQX_ENABLE_USER_MODE && MQX_ENABLE_USER_STDAPI
if (MQX_RUN_IN_USER_MODE) {
return _usr_lwsem_wait_ticks(sem_ptr, time_in_ticks);
}
#endif
_GET_KERNEL_DATA(kernel_data);
_KLOGE3(KLOG_lwsem_wait_ticks, sem_ptr, time_in_ticks);
#if MQX_CHECK_ERRORS
if (kernel_data->IN_ISR) {
_KLOGX2(KLOG_lwsem_wait_ticks, MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
return(MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
} /* Endif */
#endif
#if MQX_CHECK_VALIDITY
if (sem_ptr->VALID != LWSEM_VALID) {
_KLOGX2(KLOG_lwsem_wait_ticks, MQX_INVALID_LWSEM);
return(MQX_INVALID_LWSEM);
} /* Endif */
#endif
_INT_DISABLE();
if (sem_ptr->VALUE <= 0) {
td_ptr = kernel_data->ACTIVE_PTR;
if (time_in_ticks == 0) {
td_ptr->STATE = LWSEM_BLOCKED;
td_ptr->INFO = (_mqx_uint)&sem_ptr->TD_QUEUE;
_QUEUE_UNLINK(td_ptr);
_QUEUE_ENQUEUE(&sem_ptr->TD_QUEUE, &td_ptr->AUX_QUEUE);
_sched_execute_scheduler_internal(); /* Let the other tasks run */
/* Another task has posted a semaphore, and it has been tranfered to this
** task.
*/
result = MQX_OK;
} else {
PSP_ADD_TICKS_TO_TICK_STRUCT(&kernel_data->TIME, time_in_ticks,
&td_ptr->TIMEOUT);
result = _lwsem_wait_timed_internal(sem_ptr, td_ptr);
} /* Endif */
} else {
--sem_ptr->VALUE;
/* Start CR 788 */
result = MQX_OK;
/* End CR 788 */
} /* Endif */
//#if MQX_COMPONENT_DESTRUCTION
/* We must check for component destruction */
if (sem_ptr->VALID != LWSEM_VALID) {
_int_enable();
/* The semaphore has been deleted */
_KLOGX2(KLOG_lwsem_wait_ticks, MQX_INVALID_LWSEM);
return(MQX_INVALID_LWSEM);
} /* Endif */
//#endif
_INT_ENABLE();
_KLOGX2(KLOG_lwsem_wait_ticks, result);
return(result);
}
_mqx_uint _lwmsgq_receive
(
/* Handle to the queue */
pointer handle,
/* location of message to copy to */
_mqx_max_type_ptr message,
/* flags for blocking on empty */
_mqx_uint flags,
/* Timeout for receive if using ticks if 0, ignored */
_mqx_uint ticks,
/* Timeout if receive timout using tick struct must have flags set */
MQX_TICK_STRUCT_PTR tick_ptr
)
{/* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
LWMSGQ_STRUCT_PTR q_ptr = (LWMSGQ_STRUCT_PTR)handle;
_mqx_uint i;
_mqx_max_type_ptr from_ptr;
_mqx_max_type_ptr to_ptr;
#if MQX_ENABLE_USER_MODE && MQX_ENABLE_USER_STDAPI
if (MQX_RUN_IN_USER_MODE) {
return _usr_lwmsgq_receive(handle, message, flags, ticks, tick_ptr);
}
#endif
/* Start CR 1944 */
_GET_KERNEL_DATA(kernel_data);
_KLOGE6(KLOG_lwmsgq_receive, handle, message, flags, ticks, tick_ptr);
/* End CR 1944 */
_int_disable();
#if MQX_CHECK_VALIDITY
if (q_ptr->VALID != LWMSGQ_VALID){
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_send, LWMSGQ_INVALID);
/* End CR 1944 */
return LWMSGQ_INVALID;
} /* Endif */
#endif
if (LWMSGQ_IS_EMPTY(q_ptr)) {
if (flags & LWMSGQ_RECEIVE_BLOCK_ON_EMPTY) {
td_ptr = kernel_data->ACTIVE_PTR;
while (LWMSGQ_IS_EMPTY(q_ptr)) {
td_ptr->STATE = LWMSGQ_READ_BLOCKED;
td_ptr->INFO = (_mqx_uint)&q_ptr->WAITING_READERS;
_QUEUE_UNLINK(td_ptr);
_QUEUE_ENQUEUE(&q_ptr->WAITING_READERS, &td_ptr->AUX_QUEUE);
if (ticks || (flags & (LWMSGQ_TIMEOUT_UNTIL | LWMSGQ_TIMEOUT_FOR))){
if (ticks) {
PSP_ADD_TICKS_TO_TICK_STRUCT(&kernel_data->TIME, ticks,
&td_ptr->TIMEOUT);
} else if (flags & LWMSGQ_TIMEOUT_UNTIL){
td_ptr->TIMEOUT = *tick_ptr;
} else {
PSP_ADD_TICKS(tick_ptr, &kernel_data->TIME, &td_ptr->TIMEOUT);
} /* Endif */
_time_delay_internal(td_ptr);
if (td_ptr->INFO != 0) {
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_receive, LWMSGQ_TIMEOUT);
/* End CR 1944 */
return LWMSGQ_TIMEOUT;
} /* Endif */
} else {
_sched_execute_scheduler_internal(); /* Let other tasks run */
} /* Endif */
} /* Endwhile */
} else {
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_receive, LWMSGQ_EMPTY);
/* End CR 1944 */
return LWMSGQ_EMPTY;
} /* Endif */
}/* Endif */
from_ptr = q_ptr->MSG_READ_LOC;
to_ptr = message;
i = q_ptr->MSG_SIZE+1;
while (--i) {
*to_ptr++ = *from_ptr++;
} /* Endwhile */
q_ptr->MSG_READ_LOC += q_ptr->MSG_SIZE;
if (q_ptr->MSG_READ_LOC >= q_ptr->MSG_END_LOC) {
q_ptr->MSG_READ_LOC = q_ptr->MSG_START_LOC;
} /* Endif */
q_ptr->CURRENT_SIZE--;
if (! _QUEUE_IS_EMPTY(&q_ptr->WAITING_WRITERS)) {
_QUEUE_DEQUEUE(&q_ptr->WAITING_WRITERS, td_ptr);
_BACKUP_POINTER(td_ptr, TD_STRUCT, AUX_QUEUE);
td_ptr->INFO = 0; /* Signal that post is activating the task */
_TASK_READY(td_ptr, kernel_data);
_CHECK_RUN_SCHEDULER(); /* Let higher priority task run */
} /* Endif */
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_receive, MQX_OK);
/* End CR 1944 */
return MQX_OK;
}/* Endbody */
_mqx_uint _lwsem_wait
(
/* [IN] the semaphore address */
LWSEM_STRUCT_PTR sem_ptr
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
#if MQX_ENABLE_USER_MODE && MQX_ENABLE_USER_STDAPI
if (MQX_RUN_IN_USER_MODE) {
return _usr_lwsem_wait(sem_ptr);
}
#endif
_GET_KERNEL_DATA(kernel_data);
_KLOGE2(KLOG_lwsem_wait, sem_ptr);
#if MQX_CHECK_ERRORS
if (kernel_data->IN_ISR) {
_KLOGX2(KLOG_lwsem_wait, MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
return(MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
}
#endif
#if MQX_CHECK_VALIDITY
if (sem_ptr->VALID != LWSEM_VALID) {
_KLOGX2(KLOG_lwsem_wait, MQX_INVALID_LWSEM);
return(MQX_INVALID_LWSEM);
}
#endif
_INT_DISABLE();
if (sem_ptr->VALUE <= 0) {
td_ptr = kernel_data->ACTIVE_PTR;
td_ptr->STATE = LWSEM_BLOCKED;
td_ptr->INFO = (_mqx_uint)&sem_ptr->TD_QUEUE;
_QUEUE_UNLINK(td_ptr);
_QUEUE_ENQUEUE(&sem_ptr->TD_QUEUE, &td_ptr->AUX_QUEUE);
_sched_execute_scheduler_internal(); /* Let the other tasks run */
/* Another task has posted a semaphore, and it has been tranfered to this
** task.
*/
} else {
--sem_ptr->VALUE;
}
//#if MQX_COMPONENT_DESTRUCTION
/* We must check for component destruction */
if (sem_ptr->VALID != LWSEM_VALID) {
_int_enable();
/* The semaphore has been deleted */
_KLOGX2(KLOG_lwsem_wait, MQX_INVALID_LWSEM);
return(MQX_INVALID_LWSEM);
} /* Endif */
//#endif
_INT_ENABLE();
_KLOGX2(KLOG_lwsem_wait, MQX_OK);
return(MQX_OK);
}
_mqx_uint _lwmsgq_send
(
/* Handle to the queue */
pointer handle,
/* location of message to copy in */
_mqx_max_type_ptr message,
/* flags for blocking on full, blocking on send */
_mqx_uint flags
)
{/* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
TD_STRUCT_PTR td_ptr;
LWMSGQ_STRUCT_PTR q_ptr = (LWMSGQ_STRUCT_PTR)handle;
_mqx_uint i;
_mqx_max_type_ptr from_ptr;
_mqx_max_type_ptr to_ptr;
#if MQX_ENABLE_USER_MODE && MQX_ENABLE_USER_STDAPI
if (MQX_RUN_IN_USER_MODE) {
return _usr_lwmsgq_send(handle, message, flags);
}
#endif
/* Start CR 1944 */
_GET_KERNEL_DATA(kernel_data);
_KLOGE4(KLOG_lwmsgq_send, handle, message, flags);
/* End CR 1944 */
_int_disable();
#if MQX_CHECK_VALIDITY
if (q_ptr->VALID != LWMSGQ_VALID){
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_send, LWMSGQ_INVALID);
/* End CR 1944 */
return LWMSGQ_INVALID;
} /* Endif */
#endif
if (LWMSGQ_IS_FULL(q_ptr)) {
if (flags & LWMSGQ_SEND_BLOCK_ON_FULL) {
td_ptr = kernel_data->ACTIVE_PTR;
while (LWMSGQ_IS_FULL(q_ptr)) {
td_ptr->STATE = LWMSGQ_WRITE_BLOCKED;
td_ptr->INFO = (_mqx_uint)&q_ptr->WAITING_WRITERS;
_QUEUE_UNLINK(td_ptr);
_QUEUE_ENQUEUE(&q_ptr->WAITING_WRITERS, &td_ptr->AUX_QUEUE);
_sched_execute_scheduler_internal(); /* Let other tasks run */
} /* Endwhile */
} else {
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_send, LWMSGQ_FULL);
/* End CR 1944 */
return LWMSGQ_FULL;
} /* Endif */
}/* Endif */
to_ptr = q_ptr->MSG_WRITE_LOC;
from_ptr = message;
i = q_ptr->MSG_SIZE+1;
while (--i) {
*to_ptr++ = *from_ptr++;
} /* Endwhile */
q_ptr->MSG_WRITE_LOC += q_ptr->MSG_SIZE;
if (q_ptr->MSG_WRITE_LOC >= q_ptr->MSG_END_LOC) {
q_ptr->MSG_WRITE_LOC = q_ptr->MSG_START_LOC;
} /* Endif */
q_ptr->CURRENT_SIZE++;
if (! _QUEUE_IS_EMPTY(&q_ptr->WAITING_READERS)) {
_QUEUE_DEQUEUE(&q_ptr->WAITING_READERS, td_ptr);
_BACKUP_POINTER(td_ptr, TD_STRUCT, AUX_QUEUE);
_TIME_DEQUEUE(td_ptr, kernel_data);
td_ptr->INFO = 0; /* Signal that post is activating the task */
_TASK_READY(td_ptr, kernel_data);
if (flags & LWMSGQ_SEND_BLOCK_ON_SEND) {
_task_block();
} else {
_CHECK_RUN_SCHEDULER(); /* Let higher priority task run */
}/* Endif */
} else {
if (flags & LWMSGQ_SEND_BLOCK_ON_SEND) {
_task_block();
}/* Endif */
} /* Endif */
_int_enable();
/* Start CR 1944 */
_KLOGX2(KLOG_lwmsgq_send, MQX_OK);
/* End CR 1944 */
return MQX_OK;
}/* Endbody */
