void _Chain_Initialize(
Chain_Control *the_chain,
void *starting_address,
size_t number_nodes,
size_t node_size
)
{
size_t count;
Chain_Node *current;
Chain_Node *next;
count = number_nodes;
current = _Chain_Head( the_chain );
the_chain->permanent_null = NULL;
next = starting_address;
while ( count-- ) {
current->next = next;
next->previous = current;
current = next;
next = (Chain_Node *)
_Addresses_Add_offset( (void *) next, node_size );
}
current->next = _Chain_Tail( the_chain );
the_chain->last = current;
}
void _Chain_Initialize(
Chain_Control *the_chain,
void *starting_address,
size_t number_nodes,
size_t node_size
)
{
size_t count = number_nodes;
Chain_Node *head = _Chain_Head( the_chain );
Chain_Node *tail = _Chain_Tail( the_chain );
Chain_Node *current = head;
Chain_Node *next = starting_address;
head->previous = NULL;
while ( count-- ) {
current->next = next;
next->previous = current;
current = next;
next = (Chain_Node *)
_Addresses_Add_offset( (void *) next, node_size );
}
current->next = tail;
tail->previous = current;
}
void _POSIX_Threads_cancel_run(
Thread_Control *the_thread
)
{
POSIX_Cancel_Handler_control *handler;
Chain_Control *handler_stack;
POSIX_API_Control *thread_support;
ISR_Level level;
thread_support = the_thread->API_Extensions[ THREAD_API_POSIX ];
handler_stack = &thread_support->Cancellation_Handlers;
thread_support->cancelability_state = PTHREAD_CANCEL_DISABLE;
while ( !_Chain_Is_empty( handler_stack ) ) {
_ISR_Disable( level );
handler = (POSIX_Cancel_Handler_control *)
_Chain_Tail( handler_stack )->previous;
_Chain_Extract_unprotected( &handler->Node );
_ISR_Enable( level );
(*handler->routine)( handler->arg );
_Workspace_Free( handler );
}
}
static void test_chain_control_layout(void)
{
Chain_Control chain;
puts( "INIT - Verify rtems_chain_control layout" );
rtems_test_assert(
sizeof(Chain_Control)
== sizeof(Chain_Node) + sizeof(Chain_Node *)
);
rtems_test_assert(
sizeof(Chain_Control)
== 3 * sizeof(Chain_Node *)
);
rtems_test_assert(
_Chain_Previous( _Chain_Head( &chain ) )
== _Chain_Next( _Chain_Tail( &chain ) )
);
#if !defined( RTEMS_SMP )
rtems_test_assert(
sizeof(Chain_Control)
== sizeof(rtems_chain_control)
);
#endif
}
void _Watchdog_Report_chain(
const char *name,
Chain_Control *header
)
{
ISR_Level level;
Chain_Node *node;
_ISR_Disable( level );
printk( "Watchdog Chain: %s %p\n", name, header );
if ( !_Chain_Is_empty( header ) ) {
for ( node = _Chain_First( header ) ;
node != _Chain_Tail(header) ;
node = node->next )
{
Watchdog_Control *watch = (Watchdog_Control *) node;
_Watchdog_Report( NULL, watch );
}
printk( "== end of %s \n", name );
} else {
printk( "Chain is empty\n" );
}
_ISR_Enable( level );
}
void _Watchdog_Insert_locked(
Watchdog_Header *header,
Watchdog_Control *the_watchdog,
ISR_lock_Context *lock_context
)
{
if ( the_watchdog->state == WATCHDOG_INACTIVE ) {
Watchdog_Iterator iterator;
Chain_Node *current;
Chain_Node *next;
Watchdog_Interval delta;
the_watchdog->state = WATCHDOG_BEING_INSERTED;
_Chain_Append_unprotected( &header->Iterators, &iterator.Node );
delta = the_watchdog->initial;
current = _Chain_Head( &header->Watchdogs );
while (
( next = _Chain_Next( current ) ) != _Chain_Tail( &header->Watchdogs )
) {
Watchdog_Control *next_watchdog;
Watchdog_Interval delta_next;
next_watchdog = (Watchdog_Control *) next;
delta_next = next_watchdog->delta_interval;
if ( delta < delta_next ) {
_Watchdog_Insert_fixup( header, next_watchdog, delta );
break;
}
iterator.delta_interval = delta - delta_next;
iterator.current = next;
_Watchdog_Flash( header, lock_context );
if ( the_watchdog->state != WATCHDOG_BEING_INSERTED ) {
goto abort_insert;
}
delta = iterator.delta_interval;
current = iterator.current;
}
the_watchdog->delta_interval = delta;
the_watchdog->start_time = _Watchdog_Ticks_since_boot;
_Watchdog_Activate( the_watchdog );
_Chain_Insert_unprotected( current, &the_watchdog->Node );
abort_insert:
_Chain_Extract_unprotected( &iterator.Node );
}
}
static void _Watchdog_Remove_it(
Watchdog_Header *header,
Watchdog_Control *the_watchdog
)
{
Chain_Node *next;
Watchdog_Interval delta;
const Chain_Node *iterator_tail;
Chain_Node *iterator_node;
_Assert( the_watchdog->state == WATCHDOG_ACTIVE );
the_watchdog->state = WATCHDOG_INACTIVE;
the_watchdog->stop_time = _Watchdog_Ticks_since_boot;
next = _Chain_Next( &the_watchdog->Node );
delta = the_watchdog->delta_interval;
if ( next != _Chain_Tail( &header->Watchdogs ) ) {
Watchdog_Control *next_watchdog;
next_watchdog = (Watchdog_Control *) next;
next_watchdog->delta_interval += delta;
}
_Chain_Extract_unprotected( &the_watchdog->Node );
iterator_node = _Chain_First( &header->Iterators );
iterator_tail = _Chain_Immutable_tail( &header->Iterators );
while ( iterator_node != iterator_tail ) {
Watchdog_Iterator *iterator;
iterator = (Watchdog_Iterator *) iterator_node;
if ( iterator->current == next ) {
iterator->delta_interval += delta;
}
if ( iterator->current == &the_watchdog->Node ) {
Chain_Node *previous = _Chain_Previous( &the_watchdog->Node );
iterator->current = previous;
if ( previous != _Chain_Head( &header->Watchdogs ) ) {
Watchdog_Control *previous_watchdog;
previous_watchdog = (Watchdog_Control *) previous;
iterator->delta_interval += previous_watchdog->delta_interval;
}
}
iterator_node = _Chain_Next( iterator_node );
}
}
void pthread_cleanup_pop(
int execute
)
{
POSIX_Cancel_Handler_control *handler;
POSIX_Cancel_Handler_control tmp_handler;
Chain_Control *handler_stack;
POSIX_API_Control *thread_support;
ISR_Level level;
thread_support = _Thread_Executing->API_Extensions[ THREAD_API_POSIX ];
handler_stack = &thread_support->Cancellation_Handlers;
/*
* We need interrupts disabled to safely check the chain and pull
* the last element off. But we also need dispatching disabled to
* ensure that we do not get prempted and deleted while we are holding
* memory that needs to be freed.
*/
_Thread_Disable_dispatch();
_ISR_Disable( level );
if ( _Chain_Is_empty( handler_stack ) ) {
_Thread_Enable_dispatch();
_ISR_Enable( level );
return;
}
handler = (POSIX_Cancel_Handler_control *)
_Chain_Tail( handler_stack )->previous;
_Chain_Extract_unprotected( &handler->Node );
_ISR_Enable( level );
tmp_handler = *handler;
_Workspace_Free( handler );
_Thread_Enable_dispatch();
if ( execute )
(*tmp_handler.routine)( tmp_handler.arg );
}
Thread_Control *_Thread_queue_Dequeue_priority(
Thread_queue_Control *the_thread_queue
)
{
uint32_t index;
ISR_Level level;
Thread_Control *the_thread = NULL; /* just to remove warnings */
Thread_Control *new_first_thread;
Chain_Node *head;
Chain_Node *tail;
Chain_Node *new_first_node;
Chain_Node *new_second_node;
Chain_Node *last_node;
Chain_Node *next_node;
Chain_Node *previous_node;
_ISR_Disable( level );
for( index=0 ;
index < TASK_QUEUE_DATA_NUMBER_OF_PRIORITY_HEADERS ;
index++ ) {
if ( !_Chain_Is_empty( &the_thread_queue->Queues.Priority[ index ] ) ) {
the_thread = (Thread_Control *) _Chain_First(
&the_thread_queue->Queues.Priority[ index ]
);
goto dequeue;
}
}
/*
* We did not find a thread to unblock.
*/
_ISR_Enable( level );
return NULL;
dequeue:
the_thread->Wait.queue = NULL;
new_first_node = _Chain_First( &the_thread->Wait.Block2n );
new_first_thread = (Thread_Control *) new_first_node;
next_node = the_thread->Object.Node.next;
previous_node = the_thread->Object.Node.previous;
if ( !_Chain_Is_empty( &the_thread->Wait.Block2n ) ) {
last_node = _Chain_Last( &the_thread->Wait.Block2n );
new_second_node = new_first_node->next;
previous_node->next = new_first_node;
next_node->previous = new_first_node;
new_first_node->next = next_node;
new_first_node->previous = previous_node;
if ( !_Chain_Has_only_one_node( &the_thread->Wait.Block2n ) ) {
/* > two threads on 2-n */
head = _Chain_Head( &new_first_thread->Wait.Block2n );
tail = _Chain_Tail( &new_first_thread->Wait.Block2n );
new_second_node->previous = head;
head->next = new_second_node;
tail->previous = last_node;
last_node->next = tail;
}
} else {
previous_node->next = next_node;
next_node->previous = previous_node;
}
if ( !_Watchdog_Is_active( &the_thread->Timer ) ) {
_ISR_Enable( level );
_Thread_Unblock( the_thread );
} else {
_Watchdog_Deactivate( &the_thread->Timer );
_ISR_Enable( level );
(void) _Watchdog_Remove( &the_thread->Timer );
_Thread_Unblock( the_thread );
}
#if defined(RTEMS_MULTIPROCESSING)
if ( !_Objects_Is_local_id( the_thread->Object.id ) )
_Thread_MP_Free_proxy( the_thread );
#endif
return( the_thread );
}
Thread_blocking_operation_States _Thread_queue_Enqueue_priority (
Thread_queue_Control *the_thread_queue,
Thread_Control *the_thread,
ISR_Level *level_p
)
{
Priority_Control search_priority;
Thread_Control *search_thread;
ISR_Level level;
Chain_Control *header;
uint32_t header_index;
Chain_Node *the_node;
Chain_Node *next_node;
Chain_Node *previous_node;
Chain_Node *search_node;
Priority_Control priority;
States_Control block_state;
_Chain_Initialize_empty( &the_thread->Wait.Block2n );
priority = the_thread->current_priority;
header_index = _Thread_queue_Header_number( priority );
header = &the_thread_queue->Queues.Priority[ header_index ];
block_state = the_thread_queue->state;
if ( _Thread_queue_Is_reverse_search( priority ) )
goto restart_reverse_search;
restart_forward_search:
search_priority = PRIORITY_MINIMUM - 1;
_ISR_Disable( level );
search_thread = (Thread_Control *) header->first;
while ( !_Chain_Is_tail( header, (Chain_Node *)search_thread ) ) {
search_priority = search_thread->current_priority;
if ( priority <= search_priority )
break;
#if ( CPU_UNROLL_ENQUEUE_PRIORITY == TRUE )
search_thread = (Thread_Control *) search_thread->Object.Node.next;
if ( _Chain_Is_tail( header, (Chain_Node *)search_thread ) )
break;
search_priority = search_thread->current_priority;
if ( priority <= search_priority )
break;
#endif
_ISR_Flash( level );
if ( !_States_Are_set( search_thread->current_state, block_state) ) {
_ISR_Enable( level );
goto restart_forward_search;
}
search_thread =
(Thread_Control *)search_thread->Object.Node.next;
}
if ( the_thread_queue->sync_state !=
THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED )
goto synchronize;
the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
if ( priority == search_priority )
goto equal_priority;
search_node = (Chain_Node *) search_thread;
previous_node = search_node->previous;
the_node = (Chain_Node *) the_thread;
the_node->next = search_node;
the_node->previous = previous_node;
previous_node->next = the_node;
search_node->previous = the_node;
the_thread->Wait.queue = the_thread_queue;
_ISR_Enable( level );
return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
restart_reverse_search:
search_priority = PRIORITY_MAXIMUM + 1;
_ISR_Disable( level );
search_thread = (Thread_Control *) header->last;
while ( !_Chain_Is_head( header, (Chain_Node *)search_thread ) ) {
search_priority = search_thread->current_priority;
if ( priority >= search_priority )
break;
#if ( CPU_UNROLL_ENQUEUE_PRIORITY == TRUE )
search_thread = (Thread_Control *) search_thread->Object.Node.previous;
if ( _Chain_Is_head( header, (Chain_Node *)search_thread ) )
break;
search_priority = search_thread->current_priority;
if ( priority >= search_priority )
break;
#endif
_ISR_Flash( level );
if ( !_States_Are_set( search_thread->current_state, block_state) ) {
_ISR_Enable( level );
goto restart_reverse_search;
}
search_thread = (Thread_Control *)
search_thread->Object.Node.previous;
}
if ( the_thread_queue->sync_state !=
THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED )
goto synchronize;
the_thread_queue->sync_state = THREAD_BLOCKING_OPERATION_SYNCHRONIZED;
if ( priority == search_priority )
goto equal_priority;
search_node = (Chain_Node *) search_thread;
next_node = search_node->next;
the_node = (Chain_Node *) the_thread;
the_node->next = next_node;
the_node->previous = search_node;
search_node->next = the_node;
next_node->previous = the_node;
the_thread->Wait.queue = the_thread_queue;
_ISR_Enable( level );
return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
equal_priority: /* add at end of priority group */
search_node = _Chain_Tail( &search_thread->Wait.Block2n );
previous_node = search_node->previous;
the_node = (Chain_Node *) the_thread;
the_node->next = search_node;
the_node->previous = previous_node;
previous_node->next = the_node;
search_node->previous = the_node;
the_thread->Wait.queue = the_thread_queue;
_ISR_Enable( level );
return THREAD_BLOCKING_OPERATION_NOTHING_HAPPENED;
synchronize:
/*
* An interrupt completed the thread's blocking request.
* For example, the blocking thread could have been given
* the mutex by an ISR or timed out.
*
* WARNING! Returning with interrupts disabled!
*/
*level_p = level;
return the_thread_queue->sync_state;
}
static int _Condition_Wake( struct _Condition_Control *_condition, int count )
{
Condition_Control *condition;
ISR_lock_Context lock_context;
Thread_queue_Heads *heads;
Chain_Control unblock;
Chain_Node *node;
Chain_Node *tail;
int woken;
condition = _Condition_Get( _condition );
_ISR_lock_ISR_disable( &lock_context );
_Condition_Queue_acquire_critical( condition, &lock_context );
/*
* In common uses cases of condition variables there are normally no threads
* on the queue, so check this condition early.
*/
heads = condition->Queue.Queue.heads;
if ( __predict_true( heads == NULL ) ) {
_Condition_Queue_release( condition, &lock_context );
return 0;
}
woken = 0;
_Chain_Initialize_empty( &unblock );
while ( count > 0 && heads != NULL ) {
const Thread_queue_Operations *operations;
Thread_Control *first;
bool do_unblock;
operations = CONDITION_TQ_OPERATIONS;
first = ( *operations->first )( heads );
do_unblock = _Thread_queue_Extract_locked(
&condition->Queue.Queue,
operations,
first
);
if (do_unblock) {
_Chain_Append_unprotected( &unblock, &first->Wait.Node.Chain );
}
++woken;
--count;
heads = condition->Queue.Queue.heads;
}
node = _Chain_First( &unblock );
tail = _Chain_Tail( &unblock );
if ( node != tail ) {
Per_CPU_Control *cpu_self;
cpu_self = _Thread_Dispatch_disable_critical( &lock_context );
_Condition_Queue_release( condition, &lock_context );
do {
Thread_Control *thread;
Chain_Node *next;
next = _Chain_Next( node );
thread = THREAD_CHAIN_NODE_TO_THREAD( node );
_Watchdog_Remove_ticks( &thread->Timer );
_Thread_Unblock( thread );
node = next;
} while ( node != tail );
_Thread_Dispatch_enable( cpu_self );
} else {
_Condition_Queue_release( condition, &lock_context );
}
return woken;
}
static void test_chain_iterator( void )
{
Chain_Control chain;
Chain_Iterator_registry reg;
Chain_Iterator fit;
Chain_Iterator bit;
Chain_Node a;
Chain_Node b;
Chain_Node c;
puts( "INIT - Verify Chain_Iterator" );
rtems_test_assert( _Chain_Is_empty( &static_reg.Iterators ));
_Chain_Initialize_empty( &chain );
_Chain_Iterator_registry_initialize( ® );
_Chain_Iterator_initialize( &chain, ®, &fit, CHAIN_ITERATOR_FORWARD );
_Chain_Iterator_initialize( &chain, ®, &bit, CHAIN_ITERATOR_BACKWARD );
rtems_test_assert( _Chain_Iterator_next( &fit ) == _Chain_Tail( &chain ));
rtems_test_assert( _Chain_Iterator_next( &bit ) == _Chain_Head( &chain ));
_Chain_Iterator_set_position( &fit, _Chain_Head( &chain ) );
_Chain_Iterator_set_position( &bit, _Chain_Tail( &chain ) );
rtems_test_assert( _Chain_Iterator_next( &fit ) == _Chain_Tail( &chain ));
rtems_test_assert( _Chain_Iterator_next( &bit ) == _Chain_Head( &chain ));
_Chain_Append_unprotected( &chain, &a );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &a );
_Chain_Append_unprotected( &chain, &b );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &b );
_Chain_Append_unprotected( &chain, &c );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &b );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
_Chain_Insert_unprotected( &a, &b );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &c );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &b );
_Chain_Append_unprotected( &chain, &c );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &a );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &b );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
_Chain_Prepend_unprotected( &chain, &a );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &a );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &b );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &b );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &c );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &c );
rtems_test_assert( _Chain_Iterator_next( &fit ) == _Chain_Tail( &chain ));
rtems_test_assert( _Chain_Iterator_next( &bit ) == _Chain_Head( &chain ));
_Chain_Append_unprotected( &chain, &a );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &a );
_Chain_Append_unprotected( &chain, &b );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &b );
_Chain_Append_unprotected( &chain, &c );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &c );
update_registry_and_extract( ®, &c );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &b );
update_registry_and_extract( ®, &b );
rtems_test_assert( _Chain_Iterator_next( &fit ) == &a );
rtems_test_assert( _Chain_Iterator_next( &bit ) == &a );
update_registry_and_extract( ®, &a );
rtems_test_assert( _Chain_Iterator_next( &fit ) == _Chain_Tail( &chain ));
rtems_test_assert( _Chain_Iterator_next( &bit ) == _Chain_Head( &chain ));
rtems_test_assert( !_Chain_Is_empty( ®.Iterators ));
_Chain_Iterator_destroy( &fit );
rtems_test_assert( !_Chain_Is_empty( ®.Iterators ));
_Chain_Iterator_destroy( &bit );
rtems_test_assert( _Chain_Is_empty( ®.Iterators ));
}