void _Semaphore_Post( struct _Semaphore_Control *_sem )
{
Semaphore_Control *sem;
Thread_queue_Context queue_context;
Thread_queue_Heads *heads;
sem = _Semaphore_Get( _sem );
_Thread_queue_Context_initialize( &queue_context );
_Semaphore_Queue_acquire( sem, &queue_context );
heads = sem->Queue.Queue.heads;
if ( heads == NULL ) {
_Assert( sem->count < UINT_MAX );
++sem->count;
_Semaphore_Queue_release( sem, &queue_context );
} else {
const Thread_queue_Operations *operations;
Thread_Control *first;
operations = SEMAPHORE_TQ_OPERATIONS;
first = ( *operations->first )( heads );
_Thread_queue_Extract_critical(
&sem->Queue.Queue,
operations,
first,
&queue_context
);
}
}
static void _Mutex_Release_slow(
Mutex_Control *mutex,
Thread_Control *executing,
Thread_queue_Heads *heads,
bool keep_priority,
ISR_lock_Context *lock_context
)
{
if (heads != NULL) {
const Thread_queue_Operations *operations;
Thread_Control *first;
operations = MUTEX_TQ_OPERATIONS;
first = ( *operations->first )( heads );
mutex->owner = first;
_Thread_queue_Extract_critical(
&mutex->Queue.Queue,
operations,
first,
lock_context
);
} else {
_Mutex_Queue_release( mutex, lock_context);
}
if ( !keep_priority ) {
Per_CPU_Control *cpu_self;
cpu_self = _Thread_Dispatch_disable();
_Thread_Restore_priority( executing );
_Thread_Dispatch_enable( cpu_self );
}
}
Thread_Control *_Thread_queue_Do_dequeue(
Thread_queue_Control *the_thread_queue,
const Thread_queue_Operations *operations
#if defined(RTEMS_MULTIPROCESSING)
,
Thread_queue_MP_callout mp_callout
#endif
)
{
Thread_queue_Context queue_context;
Thread_Control *the_thread;
_Thread_queue_Context_initialize( &queue_context );
_Thread_queue_Context_set_MP_callout( &queue_context, mp_callout );
_Thread_queue_Acquire( the_thread_queue, &queue_context );
the_thread = _Thread_queue_First_locked( the_thread_queue, operations );
if ( the_thread != NULL ) {
_Thread_queue_Extract_critical(
&the_thread_queue->Queue,
operations,
the_thread,
&queue_context
);
} else {
_Thread_queue_Release( the_thread_queue, &queue_context );
}
return the_thread;
}
int sem_post( sem_t *_sem )
{
Sem_Control *sem;
ISR_Level level;
Thread_queue_Context queue_context;
Thread_queue_Heads *heads;
unsigned int count;
POSIX_SEMAPHORE_VALIDATE_OBJECT( _sem );
sem = _Sem_Get( &_sem->_Semaphore );
_Thread_queue_Context_initialize( &queue_context );
_Thread_queue_Context_ISR_disable( &queue_context, level );
_Sem_Queue_acquire_critical( sem, &queue_context );
heads = sem->Queue.Queue.heads;
count = sem->count;
if ( __predict_true( heads == NULL && count < SEM_VALUE_MAX ) ) {
sem->count = count + 1;
_Sem_Queue_release( sem, level, &queue_context );
return 0;
}
if ( __predict_true( heads != NULL ) ) {
const Thread_queue_Operations *operations;
Thread_Control *first;
_Thread_queue_Context_set_ISR_level( &queue_context, level );
operations = SEMAPHORE_TQ_OPERATIONS;
first = ( *operations->first )( heads );
_Thread_queue_Extract_critical(
&sem->Queue.Queue,
operations,
first,
&queue_context
);
return 0;
}
_Sem_Queue_release( sem, level, &queue_context );
rtems_set_errno_and_return_minus_one( EOVERFLOW );
}
void _Thread_queue_Flush(
Thread_queue_Control *the_thread_queue,
#if defined(RTEMS_MULTIPROCESSING)
Thread_queue_Flush_callout remote_extract_callout,
#else
Thread_queue_Flush_callout remote_extract_callout RTEMS_UNUSED,
#endif
uint32_t status
)
{
ISR_lock_Context lock_context;
Thread_Control *the_thread;
_Thread_queue_Acquire( the_thread_queue, &lock_context );
while ( (the_thread = _Thread_queue_First_locked( the_thread_queue ) ) ) {
#if defined(RTEMS_MULTIPROCESSING)
if ( _Objects_Is_local_id( the_thread->Object.id ) )
#endif
the_thread->Wait.return_code = status;
_Thread_queue_Extract_critical(
&the_thread_queue->Queue,
the_thread_queue->operations,
the_thread,
&lock_context
);
#if defined(RTEMS_MULTIPROCESSING)
if ( !_Objects_Is_local_id( the_thread->Object.id ) )
( *remote_extract_callout )( the_thread );
#endif
_Thread_queue_Acquire( the_thread_queue, &lock_context );
}
_Thread_queue_Release( the_thread_queue, &lock_context );
}
