/* OTG,add for otg */ _mqx_uint _lwmsgq_deinit /* only for no user mode */ ( pointer location ) { KERNEL_DATA_STRUCT_PTR kernel_data; LWMSGQ_STRUCT_PTR q_ptr = (LWMSGQ_STRUCT_PTR) location; LWMSGQ_STRUCT_PTR lwmsg_chk_ptr; uint_32 found = 0; _GET_KERNEL_DATA(kernel_data); // _KLOGE4(KLOG_lwmsgq_init, location, num_messages, msg_size); //MASK by guoyifang for compile error lwmsg_chk_ptr = (LWMSGQ_STRUCT_PTR) ((pointer) kernel_data->LWMSGQS.NEXT); while (lwmsg_chk_ptr != (LWMSGQ_STRUCT_PTR) ((pointer) &kernel_data->LWMSGQS)) { if (lwmsg_chk_ptr == q_ptr) { found = 1; break; } lwmsg_chk_ptr = (LWMSGQ_STRUCT_PTR) ((pointer) lwmsg_chk_ptr->LINK.NEXT); } if(found){ _QUEUE_REMOVE(&kernel_data->LWMSGQS, &q_ptr->LINK); return MQX_OK; } return (MQX_EINVAL); }
/*! * \brief Cancels all the lightweight timers in the periodic queue. * * \param[in] period_ptr Pointer to the periodic queue to cancel. * * \return MQX_OK * \return MQX_LWTIMER_INVALID (Period_ptr points to an invalid periodic queue.) * * \see _lwtimer_add_timer_to_queue * \see _lwtimer_cancel_timer * \see _lwtimer_create_periodic_queue * \see LWTIMER_PERIOD_STRUCT */ _mqx_uint _lwtimer_cancel_period ( LWTIMER_PERIOD_STRUCT_PTR period_ptr ) { /* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; _GET_KERNEL_DATA(kernel_data); _KLOGE2(KLOG_lwtimer_cancel_period, period_ptr); #if MQX_CHECK_VALIDITY if (period_ptr->VALID != LWTIMER_VALID) { _KLOGX2(KLOG_lwtimer_cancel_period, MQX_LWTIMER_INVALID); return MQX_LWTIMER_INVALID; } /* Endif */ #endif _int_disable(); period_ptr->VALID = 0; _QUEUE_REMOVE(&kernel_data->LWTIMERS, period_ptr); _int_enable(); _KLOGX2(KLOG_lwtimer_cancel_period, MQX_OK); return (MQX_OK); } /* Endbody */
void _sem_cleanup ( /* [IN] the task being destroyed */ TD_STRUCT_PTR td_ptr ) { /* Body */ SEM_CONNECTION_STRUCT_PTR sem_connection_ptr; SEM_CONNECTION_STRUCT_PTR connection_ptr; SEM_STRUCT_PTR sem_ptr; connection_ptr = _mem_get_next_block_internal(td_ptr, NULL); while (connection_ptr) { if ((connection_ptr->VALID == SEM_VALID) && (connection_ptr->TD_PTR == td_ptr) ) { sem_ptr = connection_ptr->SEM_PTR; if (sem_ptr->VALID == SEM_VALID) { /* Check if the connection is on the queue */ _int_disable(); sem_connection_ptr = (SEM_CONNECTION_STRUCT_PTR) ((pointer)sem_ptr->WAITING_TASKS.NEXT); while (sem_connection_ptr != (pointer)&sem_ptr->WAITING_TASKS.NEXT){ if (sem_connection_ptr == connection_ptr) { /* Connection is queued, so dequeue it */ _QUEUE_REMOVE(&sem_ptr->WAITING_TASKS, connection_ptr); break; }/* Endif */ sem_connection_ptr = (SEM_CONNECTION_STRUCT_PTR) sem_connection_ptr->NEXT; } /* Endwhile */ if (sem_ptr->POLICY & SEM_STRICT) { while (connection_ptr->POST_STATE) { _sem_post(connection_ptr); } /* Endwhile */ }/* Endif */ _int_enable(); }/* Endif */ } /* Endif */ connection_ptr = (SEM_CONNECTION_STRUCT_PTR) _mem_get_next_block_internal(td_ptr, connection_ptr); } /* Endwhile */ } /* Endbody */
/*! * \brief Cancels an outstanding timer request. * * \param[in] timer_ptr Pointer to the lightweight timer to cancel. * * \return MQX_OK * \return MQX_LWTIMER_INVALID (Timer_ptr points to either an invalid timer or * to a timer with a periodic queue.) * * \see _lwtimer_add_timer_to_queue * \see _lwtimer_cancel_period * \see _lwtimer_create_periodic_queue * \see LWTIMER_STRUCT */ _mqx_uint _lwtimer_cancel_timer ( LWTIMER_STRUCT_PTR timer_ptr ) { /* Body */ _KLOGM(KERNEL_DATA_STRUCT_PTR kernel_data); LWTIMER_PERIOD_STRUCT_PTR period_ptr; _KLOGM(_GET_KERNEL_DATA(kernel_data)); _KLOGE2(KLOG_lwtimer_cancel_timer, timer_ptr); #if MQX_CHECK_VALIDITY if (timer_ptr->VALID != LWTIMER_VALID) { _KLOGX2(KLOG_lwtimer_cancel_timer, MQX_LWTIMER_INVALID); return MQX_LWTIMER_INVALID; } /* Endif */ #endif period_ptr = timer_ptr->PERIOD_PTR; _int_disable(); #if MQX_CHECK_VALIDITY if (period_ptr->VALID != LWTIMER_VALID) { _int_enable(); _KLOGX2(KLOG_lwtimer_cancel_timer, MQX_LWTIMER_INVALID); return MQX_LWTIMER_INVALID; } /* Endif */ #endif timer_ptr->VALID = 0; if (timer_ptr == period_ptr->TIMER_PTR) { period_ptr->TIMER_PTR = (void *) timer_ptr->LINK.PREV; } /* Endif */ _QUEUE_REMOVE(&period_ptr->TIMERS, timer_ptr); _int_enable(); _KLOGX2(KLOG_lwtimer_cancel_timer, MQX_OK); return (MQX_OK); } /* Endbody */
_mqx_uint _task_set_priority ( /* [IN] the task id to use */ _task_id task_id, /* [IN] the new task priority */ _mqx_uint new_priority, /* [OUT] the location where the old task priority is to be placed */ _mqx_uint_ptr priority_ptr ) { /* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; READY_Q_STRUCT_PTR ready_q_ptr; TD_STRUCT_PTR td_ptr; TASK_QUEUE_STRUCT_PTR task_queue_ptr; _GET_KERNEL_DATA(kernel_data); _KLOGE3(KLOG_task_set_priority, task_id, new_priority); #if MQX_CHECK_ERRORS if (new_priority > kernel_data->LOWEST_TASK_PRIORITY) { _KLOGX2(KLOG_task_set_priority, MQX_INVALID_PARAMETER); return(MQX_INVALID_PARAMETER); }/* Endif */ #endif td_ptr = (TD_STRUCT_PTR)_task_get_td(task_id); if (td_ptr == NULL) { _KLOGX2(KLOG_task_set_priority, MQX_INVALID_TASK_ID); return(MQX_INVALID_TASK_ID); } /* Endif */ _int_disable(); /* Return old priority */ *priority_ptr = td_ptr->HOME_QUEUE->PRIORITY; /* Make the change permanent */ ready_q_ptr = kernel_data->READY_Q_LIST; td_ptr->HOME_QUEUE = ready_q_ptr - new_priority; if (td_ptr->BOOSTED) { /* Can only change priority to a higher (lower value) */ if (new_priority < td_ptr->MY_QUEUE->PRIORITY) { /* Move the task to the correct priority level */ _sched_set_priority_internal(td_ptr, new_priority); } /* Endif */ } else { /* Move the task to the correct priority level */ _sched_set_priority_internal(td_ptr, new_priority); } /* Endif */ if (td_ptr->STATE == TASK_QUEUE_BLOCKED) { task_queue_ptr = (TASK_QUEUE_STRUCT_PTR) ((uchar_ptr)td_ptr->INFO - FIELD_OFFSET(TASK_QUEUE_STRUCT, TD_QUEUE)); if (task_queue_ptr->POLICY & MQX_TASK_QUEUE_BY_PRIORITY) { /* Requeue the td by priority */ _QUEUE_REMOVE(&task_queue_ptr->TD_QUEUE, td_ptr); _sched_insert_priorityq_internal(&task_queue_ptr->TD_QUEUE, td_ptr); }/* Endif */ }/* Endif */ /* Allow higher priority tasks to run */ _CHECK_RUN_SCHEDULER(); _int_enable(); _KLOGX2(KLOG_task_set_priority, 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 _task_abort ( /* [IN] the task id of the task to abort */ _task_id task_id ) { /* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; TD_STRUCT_PTR td_ptr; pointer stack_ptr; _processor_number processor; _GET_KERNEL_DATA(kernel_data); _KLOGE2(KLOG_task_abort, task_id); if (task_id != MQX_NULL_TASK_ID) { processor = PROC_NUMBER_FROM_TASKID(task_id); if (processor != (_processor_number)kernel_data->INIT.PROCESSOR_NUMBER ) { #if MQX_IS_MULTI_PROCESSOR if ( kernel_data->IPC != NULL ) { _KLOGX2(KLOG_task_abort, MQX_OK); return( (*kernel_data->IPC)(FALSE, processor, KERNEL_MESSAGES, IPC_TASK_ABORT, 1, (_mqx_uint)task_id) ); } else { #endif _KLOGX2(KLOG_task_abort, MQX_INVALID_TASK_ID); return(MQX_INVALID_TASK_ID); #if MQX_IS_MULTI_PROCESSOR } /* Endif */ #endif }/* Endif */ }/* Endif */ td_ptr = (TD_STRUCT_PTR)_task_get_td(task_id); #if MQX_CHECK_ERRORS if ( (td_ptr == NULL) || (td_ptr == SYSTEM_TD_PTR(kernel_data)) ) { _KLOGX2(KLOG_task_abort, MQX_INVALID_TASK_ID); return( MQX_INVALID_TASK_ID ); } /* Endif */ #endif if (td_ptr == kernel_data->ACTIVE_PTR) { if (kernel_data->IN_ISR) { stack_ptr = (pointer)td_ptr->STACK_PTR; _PSP_SET_PC_OF_INTERRUPTED_TASK(stack_ptr, _task_exit_function_internal); } else { _task_exit_function_internal(); }/* Endif */ } else { _int_disable(); /* Task is not running */ stack_ptr = (pointer)td_ptr->STACK_PTR; _PSP_SET_PC_OF_BLOCKED_TASK(stack_ptr, _task_exit_function_internal); /* Start CR 1222 */ if (td_ptr->STATE & IS_ON_TIMEOUT_Q){ /* Remove from time queue (uses NEXT, PREV field) */ _TIME_DEQUEUE(td_ptr, kernel_data); /* End CR 1222 */ } else if (td_ptr->STATE & TD_IS_ON_QUEUE) { _QUEUE_REMOVE(td_ptr->INFO, td_ptr); /* Begin CR 1223 */ } else if((td_ptr->STATE & BLOCKED_ON_AUX_QUEUE) ==BLOCKED_ON_AUX_QUEUE){ /* We need to remove it here because _task_ready() below will change its state to READY */ _QUEUE_REMOVE(td_ptr->INFO, &td_ptr->AUX_QUEUE); } /* Endif */ /* End CR 1223 */ if (td_ptr->STATE & IS_BLOCKED) { _task_ready(td_ptr); } /* Endif */ _int_enable(); }/* Endif */ _KLOGX2(KLOG_task_abort, MQX_OK); return(MQX_OK); } /* Endbody */
_mqx_uint _timer_cancel ( /* [IN] id of the alarm to be cancelled */ _timer_id id ) { /* Body */ KERNEL_DATA_STRUCT_PTR kernel_data; TIMER_COMPONENT_STRUCT_PTR timer_component_ptr; TIMER_ENTRY_STRUCT_PTR timer_entry_ptr; QUEUE_STRUCT_PTR queue_ptr; _GET_KERNEL_DATA(kernel_data); _KLOGE2(KLOG_timer_cancel, id); timer_component_ptr = kernel_data->KERNEL_COMPONENTS[KERNEL_TIMER]; #if MQX_CHECK_ERRORS if (timer_component_ptr == NULL) { _KLOGX2(KLOG_timer_cancel, MQX_COMPONENT_DOES_NOT_EXIST); return MQX_COMPONENT_DOES_NOT_EXIST; } /* Endif */ #endif #if MQX_CHECK_VALIDITY if (timer_component_ptr->VALID != TIMER_VALID) { _KLOGX2(KLOG_timer_cancel, MQX_INVALID_COMPONENT_BASE); return MQX_INVALID_COMPONENT_BASE; } /* Endif */ #endif #if MQX_CHECK_ERRORS if (id == 0) { _KLOGX2(KLOG_timer_cancel, MQX_INVALID_PARAMETER); return MQX_INVALID_PARAMETER; } /* Endif */ if (kernel_data->IN_ISR) { _KLOGX2(KLOG_timer_cancel, MQX_CANNOT_CALL_FUNCTION_FROM_ISR); return MQX_CANNOT_CALL_FUNCTION_FROM_ISR; } /* Endif */ #endif /* Gain exclusive access to the timer queues */ /* Start CR 1332 */ if (kernel_data->ACTIVE_PTR != timer_component_ptr->TIMER_TD_PTR) { if (_lwsem_wait(&timer_component_ptr->TIMER_ENTRIES_LWSEM) != MQX_OK) { _KLOGX2(KLOG_timer_cancel, MQX_INVALID_LWSEM); return(MQX_INVALID_LWSEM); } /* Endif */ } /*Endif */ timer_entry_ptr = _timer_find_entry_internal(timer_component_ptr, id); /* End CR 1332 */ if ((timer_entry_ptr == NULL) || (timer_entry_ptr->VALID != TIMER_VALID)) { if (kernel_data->ACTIVE_PTR != timer_component_ptr->TIMER_TD_PTR) { _lwsem_post(&timer_component_ptr->TIMER_ENTRIES_LWSEM); } /* Endif */ _KLOGX2(KLOG_timer_cancel, MQX_INVALID_PARAMETER); return MQX_INVALID_PARAMETER; } /* Endif */ if (timer_entry_ptr->MODE == TIMER_ELAPSED_TIME_MODE) { queue_ptr = &timer_component_ptr->ELAPSED_TIMER_ENTRIES; } else { queue_ptr = &timer_component_ptr->KERNEL_TIMER_ENTRIES; } /* Endif */ timer_entry_ptr->VALID = 0; timer_entry_ptr->ID = 0; if (kernel_data->ACTIVE_PTR != timer_component_ptr->TIMER_TD_PTR) { _QUEUE_REMOVE(queue_ptr, timer_entry_ptr); _mem_free(timer_entry_ptr); _lwsem_post(&timer_component_ptr->TIMER_ENTRIES_LWSEM); } /* Endif */ _KLOGX2(KLOG_timer_cancel, MQX_OK); 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 */