void _Thread_queue_Surrender_sticky(
Thread_queue_Queue *queue,
Thread_queue_Heads *heads,
Thread_Control *previous_owner,
Thread_queue_Context *queue_context,
const Thread_queue_Operations *operations
)
{
Thread_Control *new_owner;
Per_CPU_Control *cpu_self;
_Assert( heads != NULL );
_Thread_queue_Context_clear_priority_updates( queue_context );
new_owner = ( *operations->surrender )(
queue,
heads,
previous_owner,
queue_context
);
queue->owner = new_owner;
_Thread_queue_Make_ready_again( new_owner );
cpu_self = _Thread_Dispatch_disable_critical(
&queue_context->Lock_context.Lock_context
);
_Thread_queue_Queue_release(
queue,
&queue_context->Lock_context.Lock_context
);
_Thread_Priority_and_sticky_update( previous_owner, -1 );
_Thread_Priority_and_sticky_update( new_owner, 0 );
_Thread_Dispatch_enable( cpu_self );
}
Status_Control _Thread_queue_Enqueue_sticky(
Thread_queue_Queue *queue,
const Thread_queue_Operations *operations,
Thread_Control *the_thread,
Thread_queue_Context *queue_context
)
{
Per_CPU_Control *cpu_self;
_Thread_Wait_claim( the_thread, queue );
if ( !_Thread_queue_Path_acquire_critical( queue, the_thread, queue_context ) ) {
_Thread_queue_Path_release_critical( queue_context );
_Thread_Wait_restore_default( the_thread );
_Thread_queue_Queue_release( queue, &queue_context->Lock_context.Lock_context );
_Thread_Wait_tranquilize( the_thread );
( *queue_context->deadlock_callout )( the_thread );
return _Thread_Wait_get_status( the_thread );
}
_Thread_queue_Context_clear_priority_updates( queue_context );
_Thread_Wait_claim_finalize( the_thread, operations );
( *operations->enqueue )( queue, the_thread, queue_context );
_Thread_queue_Path_release_critical( queue_context );
the_thread->Wait.return_code = STATUS_SUCCESSFUL;
_Thread_Wait_flags_set( the_thread, THREAD_QUEUE_INTEND_TO_BLOCK );
cpu_self = _Thread_Dispatch_disable_critical(
&queue_context->Lock_context.Lock_context
);
_Thread_queue_Queue_release( queue, &queue_context->Lock_context.Lock_context );
if ( cpu_self->thread_dispatch_disable_level != 1 ) {
_Internal_error(
INTERNAL_ERROR_THREAD_QUEUE_ENQUEUE_STICKY_FROM_BAD_STATE
);
}
_Thread_queue_Timeout( the_thread, cpu_self, queue_context );
_Thread_Priority_update( queue_context );
_Thread_Priority_and_sticky_update( the_thread, 1 );
_Thread_Dispatch_enable( cpu_self );
while (
_Thread_Wait_flags_get_acquire( the_thread ) == THREAD_QUEUE_INTEND_TO_BLOCK
) {
/* Wait */
}
_Thread_Wait_tranquilize( the_thread );
_Thread_Timer_remove( the_thread );
return _Thread_Wait_get_status( the_thread );
}
bool _Thread_queue_Extract_locked(
Thread_queue_Queue *queue,
const Thread_queue_Operations *operations,
Thread_Control *the_thread,
Thread_queue_Context *queue_context
)
{
_Thread_queue_Context_clear_priority_updates( queue_context );
#if defined(RTEMS_MULTIPROCESSING)
_Thread_queue_MP_set_callout( the_thread, queue_context );
#endif
( *operations->extract )( queue, the_thread, queue_context );
return _Thread_queue_Make_ready_again( the_thread );
}
int pthread_setschedparam(
pthread_t thread,
int policy,
struct sched_param *param
)
{
Thread_CPU_budget_algorithms budget_algorithm;
Thread_CPU_budget_algorithm_callout budget_callout;
Thread_Control *the_thread;
Per_CPU_Control *cpu_self;
Thread_queue_Context queue_context;
int error;
if ( param == NULL ) {
return EINVAL;
}
error = _POSIX_Thread_Translate_sched_param(
policy,
param,
&budget_algorithm,
&budget_callout
);
if ( error != 0 ) {
return error;
}
_Thread_queue_Context_initialize( &queue_context );
_Thread_queue_Context_clear_priority_updates( &queue_context );
the_thread = _Thread_Get( thread, &queue_context.Lock_context.Lock_context );
if ( the_thread == NULL ) {
return ESRCH;
}
_Thread_Wait_acquire_critical( the_thread, &queue_context );
error = _POSIX_Set_sched_param(
the_thread,
policy,
param,
budget_algorithm,
budget_callout,
&queue_context
);
cpu_self = _Thread_queue_Dispatch_disable( &queue_context );
_Thread_Wait_release( the_thread, &queue_context );
_Thread_Priority_update( &queue_context );
_Thread_Dispatch_enable( cpu_self );
return error;
}
rtems_status_code rtems_task_set_priority(
rtems_id id,
rtems_task_priority new_priority,
rtems_task_priority *old_priority_p
)
{
Thread_Control *the_thread;
Thread_queue_Context queue_context;
const Scheduler_Control *scheduler;
Priority_Control old_priority;
rtems_status_code status;
if ( old_priority_p == NULL ) {
return RTEMS_INVALID_ADDRESS;
}
_Thread_queue_Context_initialize( &queue_context );
_Thread_queue_Context_clear_priority_updates( &queue_context );
the_thread = _Thread_Get( id, &queue_context.Lock_context.Lock_context );
if ( the_thread == NULL ) {
#if defined(RTEMS_MULTIPROCESSING)
return _RTEMS_tasks_MP_Set_priority( id, new_priority, old_priority_p );
#else
return RTEMS_INVALID_ID;
#endif
}
_Thread_Wait_acquire_critical( the_thread, &queue_context );
scheduler = _Thread_Scheduler_get_home( the_thread );
old_priority = _Thread_Get_priority( the_thread );
if ( new_priority != RTEMS_CURRENT_PRIORITY ) {
status = _RTEMS_tasks_Set_priority(
the_thread,
scheduler,
new_priority,
&queue_context
);
} else {
_Thread_Wait_release( the_thread, &queue_context );
status = RTEMS_SUCCESSFUL;
}
*old_priority_p = _RTEMS_Priority_From_core( scheduler, old_priority );
return status;
}
void _Thread_queue_Surrender(
Thread_queue_Queue *queue,
Thread_queue_Heads *heads,
Thread_Control *previous_owner,
Thread_queue_Context *queue_context,
const Thread_queue_Operations *operations
)
{
Thread_Control *new_owner;
bool unblock;
Per_CPU_Control *cpu_self;
_Assert( heads != NULL );
_Thread_queue_Context_clear_priority_updates( queue_context );
new_owner = ( *operations->surrender )(
queue,
heads,
previous_owner,
queue_context
);
queue->owner = new_owner;
#if defined(RTEMS_MULTIPROCESSING)
if ( !_Thread_queue_MP_set_callout( new_owner, queue_context ) )
#endif
{
_Thread_Resource_count_increment( new_owner );
}
unblock = _Thread_queue_Make_ready_again( new_owner );
cpu_self = _Thread_Dispatch_disable_critical(
&queue_context->Lock_context.Lock_context
);
_Thread_queue_Queue_release(
queue,
&queue_context->Lock_context.Lock_context
);
_Thread_Priority_update( queue_context );
if ( unblock ) {
_Thread_Remove_timer_and_unblock( new_owner, queue );
}
_Thread_Dispatch_enable( cpu_self );
}
void _Thread_Restart_self(
Thread_Control *executing,
const Thread_Entry_information *entry,
ISR_lock_Context *lock_context
)
{
Per_CPU_Control *cpu_self;
Thread_queue_Context queue_context;
_Assert(
_Watchdog_Get_state( &executing->Timer.Watchdog ) == WATCHDOG_INACTIVE
);
_Assert(
executing->current_state == STATES_READY
|| executing->current_state == STATES_SUSPENDED
);
_Thread_queue_Context_initialize( &queue_context );
_Thread_queue_Context_clear_priority_updates( &queue_context );
_Thread_State_acquire_critical( executing, lock_context );
executing->Start.Entry = *entry;
_Thread_Change_life_locked(
executing,
0,
THREAD_LIFE_RESTARTING,
THREAD_LIFE_PROTECTED | THREAD_LIFE_CHANGE_DEFERRED
);
cpu_self = _Thread_Dispatch_disable_critical( lock_context );
_Thread_State_release( executing, lock_context );
_Thread_Wait_acquire_default( executing, lock_context );
_Thread_Priority_change(
executing,
&executing->Real_priority,
executing->Start.initial_priority,
false,
&queue_context
);
_Thread_Wait_release_default( executing, lock_context );
_Thread_Priority_update( &queue_context );
_Thread_Dispatch_enable( cpu_self );
RTEMS_UNREACHABLE();
}
int pthread_setschedprio( pthread_t thread, int prio )
{
Thread_Control *the_thread;
Per_CPU_Control *cpu_self;
Thread_queue_Context queue_context;
const Scheduler_Control *scheduler;
Priority_Control new_priority;
bool valid;
the_thread = _Thread_Get( thread, &queue_context.Lock_context.Lock_context );
if ( the_thread == NULL ) {
return ESRCH;
}
_Thread_queue_Context_clear_priority_updates( &queue_context );
_Thread_Wait_acquire_critical( the_thread, &queue_context );
scheduler = _Scheduler_Get_own( the_thread );
new_priority = _POSIX_Priority_To_core( scheduler, prio, &valid );
if ( !valid ) {
_Thread_Wait_release( the_thread, &queue_context );
return EINVAL;
}
_Thread_Priority_change(
the_thread,
&the_thread->Real_priority,
new_priority,
true,
&queue_context
);
cpu_self = _Thread_Dispatch_disable_critical(
&queue_context.Lock_context.Lock_context
);
_Thread_Wait_release( the_thread, &queue_context );
_Thread_Priority_update( &queue_context );
_Thread_Dispatch_enable( cpu_self );
return 0;
}
static void _Thread_Raise_real_priority(
Thread_Control *the_thread,
Priority_Control priority
)
{
Thread_queue_Context queue_context;
_Thread_queue_Context_initialize( &queue_context );
_Thread_queue_Context_clear_priority_updates( &queue_context );
_Thread_Wait_acquire( the_thread, &queue_context );
if ( priority < the_thread->Real_priority.priority ) {
_Thread_Priority_change(
the_thread,
&the_thread->Real_priority,
priority,
false,
&queue_context
);
}
_Thread_Wait_release( the_thread, &queue_context );
_Thread_Priority_update( &queue_context );
}
void _Thread_queue_Enqueue(
Thread_queue_Queue *queue,
const Thread_queue_Operations *operations,
Thread_Control *the_thread,
Thread_queue_Context *queue_context
)
{
Per_CPU_Control *cpu_self;
bool success;
_Assert( queue_context->enqueue_callout != NULL );
_Assert( (uint8_t) queue_context->timeout_discipline != 0x7f );
#if defined(RTEMS_MULTIPROCESSING)
if ( _Thread_MP_Is_receive( the_thread ) && the_thread->receive_packet ) {
the_thread = _Thread_MP_Allocate_proxy( queue_context->thread_state );
}
#endif
_Thread_Wait_claim( the_thread, queue );
if ( !_Thread_queue_Path_acquire_critical( queue, the_thread, queue_context ) ) {
_Thread_queue_Path_release_critical( queue_context );
_Thread_Wait_restore_default( the_thread );
_Thread_queue_Queue_release( queue, &queue_context->Lock_context.Lock_context );
_Thread_Wait_tranquilize( the_thread );
_Assert( queue_context->deadlock_callout != NULL );
( *queue_context->deadlock_callout )( the_thread );
return;
}
_Thread_queue_Context_clear_priority_updates( queue_context );
_Thread_Wait_claim_finalize( the_thread, operations );
( *operations->enqueue )( queue, the_thread, queue_context );
_Thread_queue_Path_release_critical( queue_context );
the_thread->Wait.return_code = STATUS_SUCCESSFUL;
_Thread_Wait_flags_set( the_thread, THREAD_QUEUE_INTEND_TO_BLOCK );
cpu_self = _Thread_Dispatch_disable_critical(
&queue_context->Lock_context.Lock_context
);
_Thread_queue_Queue_release( queue, &queue_context->Lock_context.Lock_context );
( *queue_context->enqueue_callout )( queue, the_thread, queue_context );
/*
* Set the blocking state for this thread queue in the thread.
*/
_Thread_Set_state( the_thread, queue_context->thread_state );
/*
* If the thread wants to timeout, then schedule its timer.
*/
_Thread_queue_Timeout( the_thread, cpu_self, queue_context );
/*
* At this point thread dispatching is disabled, however, we already released
* the thread queue lock. Thus, interrupts or threads on other processors
* may already changed our state with respect to the thread queue object.
* The request could be satisfied or timed out. This situation is indicated
* by the thread wait flags. Other parties must not modify our thread state
* as long as we are in the THREAD_QUEUE_INTEND_TO_BLOCK thread wait state,
* thus we have to cancel the blocking operation ourself if necessary.
*/
success = _Thread_Wait_flags_try_change_acquire(
the_thread,
THREAD_QUEUE_INTEND_TO_BLOCK,
THREAD_QUEUE_BLOCKED
);
if ( !success ) {
_Thread_Remove_timer_and_unblock( the_thread, queue );
}
_Thread_Priority_update( queue_context );
_Thread_Dispatch_direct( cpu_self );
}
static rtems_status_code _Semaphore_Set_priority(
Semaphore_Control *the_semaphore,
const Scheduler_Control *scheduler,
rtems_task_priority new_priority,
rtems_task_priority *old_priority_p,
Thread_queue_Context *queue_context
)
{
rtems_status_code sc;
bool valid;
Priority_Control core_priority;
Priority_Control old_priority;
Per_CPU_Control *cpu_self;
core_priority = _RTEMS_Priority_To_core( scheduler, new_priority, &valid );
if ( new_priority != RTEMS_CURRENT_PRIORITY && !valid ) {
return RTEMS_INVALID_PRIORITY;
}
_Thread_queue_Context_clear_priority_updates( queue_context );
_Thread_queue_Acquire_critical(
&the_semaphore->Core_control.Wait_queue,
queue_context
);
switch ( the_semaphore->variant ) {
case SEMAPHORE_VARIANT_MUTEX_PRIORITY_CEILING:
sc = _Semaphore_Is_scheduler_valid(
&the_semaphore->Core_control.Mutex,
scheduler
);
old_priority = _CORE_ceiling_mutex_Get_priority(
&the_semaphore->Core_control.Mutex
);
if ( sc == RTEMS_SUCCESSFUL && new_priority != RTEMS_CURRENT_PRIORITY ) {
_CORE_ceiling_mutex_Set_priority(
&the_semaphore->Core_control.Mutex,
core_priority,
queue_context
);
}
break;
#if defined(RTEMS_SMP)
case SEMAPHORE_VARIANT_MRSP:
old_priority = _MRSP_Get_priority(
&the_semaphore->Core_control.MRSP,
scheduler
);
if ( new_priority != RTEMS_CURRENT_PRIORITY ) {
_MRSP_Set_priority(
&the_semaphore->Core_control.MRSP,
scheduler,
core_priority
);
}
sc = RTEMS_SUCCESSFUL;
break;
#endif
default:
_Assert(
the_semaphore->variant == SEMAPHORE_VARIANT_MUTEX_INHERIT_PRIORITY
|| the_semaphore->variant == SEMAPHORE_VARIANT_MUTEX_NO_PROTOCOL
|| the_semaphore->variant == SEMAPHORE_VARIANT_SIMPLE_BINARY
|| the_semaphore->variant == SEMAPHORE_VARIANT_COUNTING
);
old_priority = 0;
sc = RTEMS_NOT_DEFINED;
break;
}
cpu_self = _Thread_Dispatch_disable_critical(
&queue_context->Lock_context.Lock_context
);
_Thread_queue_Release(
&the_semaphore->Core_control.Wait_queue,
queue_context
);
_Thread_Priority_update( queue_context );
_Thread_Dispatch_enable( cpu_self );
*old_priority_p = _RTEMS_Priority_From_core( scheduler, old_priority );
return sc;
}
