_mqx_uint _taskq_resume
(
/* [IN] the task queue handle */
pointer users_task_queue_ptr,
/* [IN] TRUE if all tasks on the queue to be resumed */
boolean all_tasks
)
{ /* 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);
_KLOGE3(KLOG_taskq_resume, users_task_queue_ptr, all_tasks);
#if MQX_CHECK_ERRORS
if (task_queue_ptr == NULL){
_KLOGX2(KLOG_taskq_resume, MQX_INVALID_TASK_QUEUE);
return(MQX_INVALID_TASK_QUEUE);
} /* Endif */
#endif
_INT_DISABLE();
#if MQX_CHECK_VALIDITY
if (task_queue_ptr->VALID != TASK_QUEUE_VALID) {
_int_enable();
_KLOGX2(KLOG_taskq_resume, MQX_INVALID_TASK_QUEUE);
return(MQX_INVALID_TASK_QUEUE);
} /* Endif */
#endif
if (_QUEUE_GET_SIZE(&task_queue_ptr->TD_QUEUE) == 0) {
/* Task queue is empty */
_int_enable();
_KLOGX2(KLOG_taskq_resume, MQX_TASK_QUEUE_EMPTY);
return(MQX_TASK_QUEUE_EMPTY);
} /* Endif */
if (all_tasks) {
while (_QUEUE_GET_SIZE(&task_queue_ptr->TD_QUEUE)) {
_QUEUE_DEQUEUE(&task_queue_ptr->TD_QUEUE, td_ptr);
_TASK_READY(td_ptr, kernel_data);
} /* Endwhile */
} else {
_QUEUE_DEQUEUE(&task_queue_ptr->TD_QUEUE, td_ptr);
_TASK_READY(td_ptr, kernel_data);
} /* Endif */
_INT_ENABLE();
_CHECK_RUN_SCHEDULER();/* Let higher priority task run */
_KLOGX2(KLOG_taskq_resume, MQX_OK);
return( MQX_OK );
} /* 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 */
/*!
* \brief Used by a task to set the specified event bits in an event.
*
* \param[in] event_ptr Pointer to the lightweight event to set bits in.
* \param[in] bit_mask Bit mask. Each bit represents an event bit to be set.
*
* \return MQX_OK
* \return MQX_LWEVENT_INVALID (Lightweight event was invalid.)
*
* \see _lwevent_create
* \see _lwevent_destroy
* \see _lwevent_set_auto_clear
* \see _lwevent_clear
* \see _lwevent_test
* \see _lwevent_wait_for
* \see _lwevent_wait_ticks
* \see _lwevent_wait_until
* \see _lwevent_get_signalled
* \see LWEVENT_STRUCT
*/
_mqx_uint _lwevent_set
(
LWEVENT_STRUCT_PTR event_ptr,
_mqx_uint bit_mask
)
{
KERNEL_DATA_STRUCT_PTR kernel_data;
QUEUE_ELEMENT_STRUCT_PTR q_ptr;
QUEUE_ELEMENT_STRUCT_PTR next_q_ptr;
TD_STRUCT_PTR td_ptr;
_mqx_uint set_bits;
#if MQX_ENABLE_USER_MODE && MQX_ENABLE_USER_STDAPI
if (MQX_RUN_IN_USER_MODE)
{
return _usr_lwevent_set(event_ptr, bit_mask);
}
#endif
_GET_KERNEL_DATA(kernel_data);
_KLOGE3(KLOG_lwevent_set, event_ptr, bit_mask);
_INT_DISABLE();
#if MQX_CHECK_VALIDITY
if (event_ptr->VALID != LWEVENT_VALID)
{
_int_enable();
_KLOGX2(KLOG_lwevent_set, MQX_LWEVENT_INVALID);
return (MQX_LWEVENT_INVALID);
} /* Endif */
#endif
set_bits = event_ptr->VALUE | bit_mask;
if (_QUEUE_GET_SIZE(&event_ptr->WAITING_TASKS))
{
/* Schedule waiting task(s) to run if bits ok */
q_ptr = event_ptr->WAITING_TASKS.NEXT;
while (q_ptr != (QUEUE_ELEMENT_STRUCT_PTR) ((void *) &event_ptr->WAITING_TASKS))
{
td_ptr = (void *) q_ptr;
_BACKUP_POINTER(td_ptr, TD_STRUCT, AUX_QUEUE);
next_q_ptr = q_ptr->NEXT;
if (((td_ptr->FLAGS & TASK_LWEVENT_ALL_BITS_WANTED) && ((td_ptr->LWEVENT_BITS & set_bits)
== td_ptr->LWEVENT_BITS)) || ((!(td_ptr->FLAGS & TASK_LWEVENT_ALL_BITS_WANTED))
&& (td_ptr->LWEVENT_BITS & set_bits)))
{
_QUEUE_REMOVE(&event_ptr->WAITING_TASKS, q_ptr);
_TIME_DEQUEUE(td_ptr, kernel_data);
td_ptr->INFO = 0;
_TASK_READY(td_ptr, kernel_data);
/* store information about which bits caused task to be unblocked */
td_ptr->LWEVENT_BITS &= set_bits;
set_bits &= ~(event_ptr->AUTO & td_ptr->LWEVENT_BITS);
} /* Endif */
q_ptr = next_q_ptr;
} /* Endwhile */
} /* Endif */
event_ptr->VALUE = set_bits;
_INT_ENABLE();
/* May need to let higher priority task run */
_CHECK_RUN_SCHEDULER();
_KLOGX2(KLOG_lwevent_set, MQX_OK);
return (MQX_OK);
}
/*!
* \private
*
* \brief Used by a task to destroy an instance of a lightweight event.
*
* \param[in] event_ptr Pointer to the lightweight event to be deinitialized.
* \param[in] user User mode
*
* \return MQX_OK
* \return MQX_LWEVENT_INVALID (Lightweight event was not valid.)
* \return MQX_CANNOT_CALL_FUNCTION_FROM_ISR (Function cannot be called from an ISR.)
*
* \see _lwevent_destroy
* \see LWEVENT_STRUCT
*/
_mqx_uint _lwevent_destroy_internal
(
LWEVENT_STRUCT_PTR event_ptr,
bool user
)
{
KERNEL_DATA_STRUCT_PTR kernel_data;
#if MQX_COMPONENT_DESTRUCTION
TD_STRUCT_PTR td_ptr;
#endif
#if MQX_ENABLE_USER_MODE
if (user && !_psp_mem_check_access_mask((uint32_t)event_ptr,
sizeof(LWEVENT_STRUCT),
MPU_UM_R, MPU_UM_RW))
{
return MQX_LWEVENT_INVALID;
}
#endif
_GET_KERNEL_DATA(kernel_data);
_KLOGE2(KLOG_lwevent_destroy, event_ptr);
#if MQX_COMPONENT_DESTRUCTION
#if MQX_CHECK_ERRORS
if (kernel_data->IN_ISR)
{
_KLOGX2(KLOG_lwevent_destroy, MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
return (MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
} /* Endif */
#endif
_int_disable();
#if MQX_CHECK_VALIDITY
if (event_ptr->VALID != LWEVENT_VALID)
{
_int_enable();
_KLOGX2(KLOG_lwevent_destroy, MQX_LWEVENT_INVALID);
return (MQX_LWEVENT_INVALID);
} /* Endif */
#endif
/* Effectively stop all access to the event */
event_ptr->VALID = 0;
while (_QUEUE_GET_SIZE(&event_ptr->WAITING_TASKS))
{
_QUEUE_DEQUEUE(&event_ptr->WAITING_TASKS, td_ptr);
_BACKUP_POINTER(td_ptr, TD_STRUCT, AUX_QUEUE);
_TIME_DEQUEUE(td_ptr, kernel_data);
_TASK_READY(td_ptr, kernel_data);
} /* Endwhile */
/* remove event from kernel LWEVENTS queue */
#if MQX_ENABLE_USER_MODE
if (user)
{
_QUEUE_REMOVE(&kernel_data->USR_LWEVENTS, event_ptr);
}
else
#endif
{
_QUEUE_REMOVE(&kernel_data->LWEVENTS, event_ptr);
}
_int_enable();
/* May need to let higher priority task run */
_CHECK_RUN_SCHEDULER();
#endif
_KLOGX2(KLOG_lwevent_destroy, MQX_OK);
return (MQX_OK);
}
_mqx_uint _event_set
(
/* [IN] - An event handle returned from _event_open or _event_open_fast */
pointer users_event_ptr,
/* [IN] - bit mask, each bit of which represents an event. */
_mqx_uint bit_mask
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
EVENT_STRUCT_PTR event_ptr;
EVENT_COMPONENT_STRUCT_PTR event_component_ptr;
EVENT_CONNECTION_STRUCT_PTR conn_ptr;
EVENT_CONNECTION_STRUCT_PTR next_conn_ptr;
EVENT_CONNECTION_STRUCT_PTR event_connection_ptr;
TD_STRUCT_PTR new_td_ptr;
_mqx_uint set_bits;
_GET_KERNEL_DATA(kernel_data);
_KLOGE3(KLOG_event_set, users_event_ptr, bit_mask);
event_connection_ptr = (EVENT_CONNECTION_STRUCT_PTR)users_event_ptr;
#if MQX_CHECK_VALIDITY
if (event_connection_ptr->VALID != EVENT_VALID){
_KLOGX2(KLOG_event_set, EVENT_INVALID_EVENT_HANDLE);
return(EVENT_INVALID_EVENT_HANDLE);
} /* Endif */
#endif
event_component_ptr = (EVENT_COMPONENT_STRUCT_PTR)
kernel_data->KERNEL_COMPONENTS[KERNEL_EVENTS];
#if MQX_CHECK_ERRORS
if (event_component_ptr == NULL){
_KLOGX2(KLOG_event_set, MQX_COMPONENT_DOES_NOT_EXIST);
return(MQX_COMPONENT_DOES_NOT_EXIST);
} /* Endif */
#endif
#if MQX_CHECK_VALIDITY
if (event_component_ptr->VALID != EVENT_VALID){
_KLOGX2(KLOG_event_set, MQX_INVALID_COMPONENT_BASE);
return(MQX_INVALID_COMPONENT_BASE);
} /* Endif */
#endif
#if MQX_IS_MULTI_PROCESSOR
if (event_connection_ptr->REMOTE_CPU) {
if (kernel_data->IPC) {
/* This open is for a remote processor */
(*kernel_data->IPC)(TRUE, event_connection_ptr->REMOTE_CPU,
KERNEL_EVENTS, IPC_EVENT_SET, 2, event_connection_ptr->EVENT_PTR,
bit_mask);
_KLOGX2(KLOG_event_set, kernel_data->ACTIVE_PTR->TASK_ERROR_CODE);
return(kernel_data->ACTIVE_PTR->TASK_ERROR_CODE);
} else {
_KLOGX2(KLOG_event_set, EVENT_NOT_FOUND);
return(EVENT_NOT_FOUND);
}/* Endif */
}/* Endif */
#endif
_INT_DISABLE();
event_ptr = event_connection_ptr->EVENT_PTR;
#if MQX_CHECK_VALIDITY
if (event_ptr->VALID != EVENT_VALID) {
_INT_ENABLE();
_KLOGX2(KLOG_event_set, EVENT_INVALID_EVENT);
return(EVENT_INVALID_EVENT);
} /* Endif */
#endif
set_bits = event_ptr->EVENT | bit_mask;
if (_QUEUE_GET_SIZE(&event_ptr->WAITING_TASKS)) {
/* Schedule waiting task(s) to run if bits ok */
conn_ptr = (EVENT_CONNECTION_STRUCT_PTR)
((pointer)event_ptr->WAITING_TASKS.NEXT);
while (conn_ptr != (EVENT_CONNECTION_STRUCT_PTR)
((pointer)&event_ptr->WAITING_TASKS))
{
next_conn_ptr = (EVENT_CONNECTION_STRUCT_PTR)conn_ptr->NEXT;
if (((conn_ptr->FLAGS & EVENT_WANTS_ALL) &&
((conn_ptr->MASK & set_bits) == conn_ptr->MASK)) ||
((!(conn_ptr->FLAGS & EVENT_WANTS_ALL)) &&
(conn_ptr->MASK & set_bits)))
{
new_td_ptr = conn_ptr->TD_PTR;
if ((new_td_ptr->STATE & STATE_MASK) == EVENT_BLOCKED) {
/* He may have timed out */
conn_ptr->FLAGS |= EVENT_OCCURRED;
_TIME_DEQUEUE(new_td_ptr, kernel_data);
_TASK_READY(new_td_ptr, kernel_data);
/* Only ready one task if event is an auto clear event */
if (event_ptr->AUTO_CLEAR) {
set_bits &= ~conn_ptr->MASK;
break;
} /* Endif */
} /* Endif */
} /* Endif */
conn_ptr = next_conn_ptr;
} /* Endwhile */
} /* Endif */
event_ptr->EVENT = set_bits;
_INT_ENABLE();
/* May need to let higher priority task run */
_CHECK_RUN_SCHEDULER();
_KLOGX2(KLOG_event_set, MQX_OK);
return(MQX_OK);
} /* Endbody */
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 */
_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 _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 */
_mqx_uint _sem_post
(
/* [IN] - The semaphore handle returned by _sem_open. */
pointer users_sem_ptr
)
{ /* Body */
KERNEL_DATA_STRUCT_PTR kernel_data;
SEM_STRUCT_PTR sem_ptr;
#if MQX_COMPONENT_DESTRUCTION
SEM_COMPONENT_STRUCT_PTR sem_component_ptr;
#endif
SEM_CONNECTION_STRUCT_PTR new_sem_connection_ptr;
SEM_CONNECTION_STRUCT_PTR sem_connection_ptr;
TD_STRUCT_PTR new_td_ptr;
boolean task_added = FALSE;
boolean destroying_semaphore = FALSE;
_GET_KERNEL_DATA(kernel_data);
_KLOGE2(KLOG_sem_post, users_sem_ptr);
sem_connection_ptr = (SEM_CONNECTION_STRUCT_PTR)users_sem_ptr;
#if MQX_CHECK_VALIDITY
if (sem_connection_ptr->VALID != SEM_VALID) {
_KLOGX2(KLOG_sem_post, SEM_INVALID_SEMAPHORE_HANDLE);
return(SEM_INVALID_SEMAPHORE_HANDLE);
} /* Endif */
#endif
sem_ptr = sem_connection_ptr->SEM_PTR;
#if MQX_CHECK_ERRORS
if (sem_ptr->POLICY & SEM_STRICT) {
if (kernel_data->IN_ISR) {
_KLOGX2(KLOG_sem_post, MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
return(MQX_CANNOT_CALL_FUNCTION_FROM_ISR);
} /* Endif */
if (sem_connection_ptr->TD_PTR != kernel_data->ACTIVE_PTR) {
/* ONLY OPENING task can use the semaphore */
_KLOGX2(KLOG_sem_post, SEM_INVALID_SEMAPHORE_HANDLE);
return(SEM_INVALID_SEMAPHORE_HANDLE);
} /* Endif */
if (sem_connection_ptr->POST_STATE == 0) {
_KLOGX2(KLOG_sem_post, SEM_CANNOT_POST);
return(SEM_CANNOT_POST);
} /* Endif */
} /* Endif */
#endif
_INT_DISABLE();
#if MQX_CHECK_VALIDITY
if (sem_ptr->VALID != SEM_VALID) {
_int_enable();
_KLOGX2(KLOG_sem_post, SEM_INVALID_SEMAPHORE);
return(SEM_INVALID_SEMAPHORE);
} /* Endif */
#endif
if (sem_ptr->POLICY & SEM_STRICT) {
#if MQX_CHECK_ERRORS
if (sem_ptr->COUNT > sem_ptr->MAX_COUNT) {
/* Corruption somewhere */
_int_enable();
_KLOGX2(KLOG_sem_post, SEM_INVALID_SEMAPHORE_COUNT);
return(SEM_INVALID_SEMAPHORE_COUNT);
} /* Endif */
#endif
--sem_connection_ptr->POST_STATE;
if (sem_connection_ptr->POST_STATE == 0) {
_QUEUE_REMOVE(&sem_ptr->OWNING_TASKS, sem_connection_ptr);
} /* Endif */
} /* Endif */
if (_QUEUE_GET_SIZE(&sem_ptr->WAITING_TASKS)) {
/* Schedule a waiting task to run */
new_sem_connection_ptr = (SEM_CONNECTION_STRUCT_PTR)
((pointer)sem_ptr->WAITING_TASKS.NEXT);
while (new_sem_connection_ptr != (pointer)&sem_ptr->WAITING_TASKS) {
new_td_ptr = new_sem_connection_ptr->TD_PTR;
if ((new_td_ptr->STATE & STATE_MASK) == SEM_BLOCKED) {
_TIME_DEQUEUE(new_td_ptr, kernel_data);
_TASK_READY(new_td_ptr, kernel_data);
new_td_ptr->INFO = SEM_AVAILABLE;
task_added = TRUE;
break;
} /* Endif */
new_sem_connection_ptr = (SEM_CONNECTION_STRUCT_PTR)
new_sem_connection_ptr->NEXT;
} /* Endwhile */
} /* Endif */
if (!task_added) {
++sem_ptr->COUNT;
#if MQX_COMPONENT_DESTRUCTION
if ( sem_ptr->DELAYED_DESTROY ) {
if ( ( (sem_ptr->POLICY & SEM_STRICT) &&
(sem_ptr->COUNT == sem_ptr->MAX_COUNT) ) ||
( !(sem_ptr->POLICY & SEM_STRICT) ) )
{
/* Destroy the semaphore name */
sem_ptr->VALID = 0;
destroying_semaphore = TRUE;
} /* Endif */
} /* Endif */
#endif
} /* Endif */
if (sem_connection_ptr->BOOSTED && (sem_connection_ptr->POST_STATE == 0)) {
/* This task was boosted by a waiting task */
_sched_unboost_priority_internal(kernel_data->ACTIVE_PTR,
sem_connection_ptr->BOOSTED);
sem_connection_ptr->BOOSTED = 0;
} /* Endif */
_INT_ENABLE();
/* Let higher priority task run */
_CHECK_RUN_SCHEDULER();
#if MQX_COMPONENT_DESTRUCTION
if (destroying_semaphore) {
sem_component_ptr = (SEM_COMPONENT_STRUCT_PTR)
kernel_data->KERNEL_COMPONENTS[KERNEL_SEMAPHORES];
if (sem_component_ptr != NULL) {
_name_delete_internal(sem_component_ptr->NAME_TABLE_HANDLE,
sem_ptr->NAME);
} /* Endif */
_mem_free(sem_ptr);
} /* Endif */
#endif
_KLOGX2(KLOG_sem_post, MQX_OK);
return(MQX_OK);
} /* Endbody */
