schedulersmpimpl.h

Go to the documentation of this file.

/* SPDX-License-Identifier: BSD-2-Clause */
 
/*
 * Copyright (C) 2013, 2021 embedded brains GmbH & Co. KG
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef _RTEMS_SCORE_SCHEDULERSMPIMPL_H
#define _RTEMS_SCORE_SCHEDULERSMPIMPL_H
 
#include <rtems/score/schedulersmp.h>
#include <rtems/score/assert.h>
#include <rtems/score/chainimpl.h>
#include <rtems/score/schedulersimpleimpl.h>
#include <rtems/bspIo.h>
 
#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */
 
typedef bool ( *Scheduler_SMP_Has_ready )(
  Scheduler_Context *context
);
 
typedef Scheduler_Node *( *Scheduler_SMP_Get_highest_ready )(
  Scheduler_Context *context,
  Scheduler_Node    *filter
);
 
typedef Scheduler_Node *( *Scheduler_SMP_Get_lowest_ready )(
  Scheduler_Context *context
);
 
typedef Scheduler_Node *( *Scheduler_SMP_Get_lowest_scheduled )(
  Scheduler_Context *context,
  Scheduler_Node    *filter
);
 
typedef void ( *Scheduler_SMP_Extract )(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_extract
);
 
typedef void ( *Scheduler_SMP_Insert )(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_insert,
  Priority_Control   insert_priority
);
 
typedef void ( *Scheduler_SMP_Move )(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_move
);
 
typedef bool ( *Scheduler_SMP_Ask_for_help )(
  Scheduler_Context *context,
  Thread_Control    *thread,
  Scheduler_Node    *node
);
 
typedef void ( *Scheduler_SMP_Update )(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_update,
  Priority_Control   new_priority
);
 
typedef void ( *Scheduler_SMP_Set_affinity )(
  Scheduler_Context *context,
  Scheduler_Node    *node,
  void              *arg
);
 
typedef bool ( *Scheduler_SMP_Enqueue )(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_enqueue,
  Priority_Control   priority
);
 
typedef void ( *Scheduler_SMP_Enqueue_scheduled )(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_enqueue,
  Priority_Control   priority
);
 
typedef void ( *Scheduler_SMP_Allocate_processor )(
  Scheduler_Context *context,
  Scheduler_Node    *scheduled,
  Per_CPU_Control   *cpu
);
 
typedef void ( *Scheduler_SMP_Register_idle )(
  Scheduler_Context *context,
  Scheduler_Node    *idle,
  Per_CPU_Control   *cpu
);
 
static inline void _Scheduler_SMP_Do_nothing_register_idle(
  Scheduler_Context *context,
  Scheduler_Node    *idle,
  Per_CPU_Control   *cpu
)
{
  (void) context;
  (void) idle;
  (void) cpu;
}
 
static inline bool _Scheduler_SMP_Priority_less_equal(
  const void       *key,
  const Chain_Node *to_insert,
  const Chain_Node *next
)
{
  const Priority_Control   *priority_to_insert;
  const Scheduler_SMP_Node *node_next;
 
  (void) to_insert;
  priority_to_insert = (const Priority_Control *) key;
  node_next = (const Scheduler_SMP_Node *) next;
 
  return *priority_to_insert <= node_next->priority;
}
 
static inline Scheduler_SMP_Context *_Scheduler_SMP_Get_self(
  Scheduler_Context *context
)
{
  return (Scheduler_SMP_Context *) context;
}
 
static inline void _Scheduler_SMP_Initialize(
  Scheduler_SMP_Context *self
)
{
  _Chain_Initialize_empty( &self->Scheduled );
}
 
static inline Scheduler_SMP_Node *_Scheduler_SMP_Thread_get_node(
  Thread_Control *thread
)
{
  return (Scheduler_SMP_Node *) _Thread_Scheduler_get_home_node( thread );
}
 
static inline Scheduler_SMP_Node *_Scheduler_SMP_Thread_get_own_node(
  Thread_Control *thread
)
{
  return (Scheduler_SMP_Node *) _Thread_Scheduler_get_home_node( thread );
}
 
static inline Scheduler_SMP_Node *_Scheduler_SMP_Node_downcast(
  Scheduler_Node *node
)
{
  return (Scheduler_SMP_Node *) node;
}
 
static inline Scheduler_SMP_Node_state _Scheduler_SMP_Node_state(
  const Scheduler_Node *node
)
{
  return ( (const Scheduler_SMP_Node *) node )->state;
}
 
static inline Priority_Control _Scheduler_SMP_Node_priority(
  const Scheduler_Node *node
)
{
  return ( (const Scheduler_SMP_Node *) node )->priority;
}
 
static inline void _Scheduler_SMP_Node_initialize(
  const Scheduler_Control *scheduler,
  Scheduler_SMP_Node      *node,
  Thread_Control          *thread,
  Priority_Control         priority
)
{
  _Scheduler_Node_do_initialize( scheduler, &node->Base, thread, priority );
  node->state = SCHEDULER_SMP_NODE_BLOCKED;
  node->priority = priority;
}
 
static inline void _Scheduler_SMP_Node_update_priority(
  Scheduler_SMP_Node *node,
  Priority_Control    new_priority
)
{
  node->priority = new_priority;
}
 
static inline void _Scheduler_SMP_Node_change_state(
  Scheduler_Node           *node,
  Scheduler_SMP_Node_state  new_state
)
{
  Scheduler_SMP_Node *the_node;
 
  the_node = _Scheduler_SMP_Node_downcast( node );
  the_node->state = new_state;
}
 
static inline bool _Scheduler_SMP_Is_processor_owned_by_us(
  const Scheduler_Context *context,
  const Per_CPU_Control   *cpu
)
{
  return cpu->Scheduler.context == context;
}
 
void _Scheduler_SMP_Remove_ask_for_help_from_processor(
  Thread_Control  *thread,
  Per_CPU_Control *cpu
);
 
static inline void _Scheduler_SMP_Cancel_ask_for_help( Thread_Control *thread )
{
  Per_CPU_Control *cpu;
 
  _Assert( _ISR_lock_Is_owner( &thread->Scheduler.Lock ) );
  cpu = thread->Scheduler.ask_for_help_cpu;
 
  if ( RTEMS_PREDICT_FALSE( cpu != NULL ) ) {
    _Scheduler_SMP_Remove_ask_for_help_from_processor( thread, cpu );
  }
}
 
static inline void _Scheduler_SMP_Request_ask_for_help( Thread_Control *thread )
{
  ISR_lock_Context lock_context;
  Per_CPU_Control *cpu_self;
 
  cpu_self = _Per_CPU_Get();
 
  _Assert( thread->Scheduler.ask_for_help_cpu == NULL );
  thread->Scheduler.ask_for_help_cpu = cpu_self;
  cpu_self->dispatch_necessary = true;
 
  _Per_CPU_Acquire( cpu_self, &lock_context );
  _Chain_Append_unprotected(
    &cpu_self->Threads_in_need_for_help,
    &thread->Scheduler.Help_node
  );
  _Per_CPU_Release( cpu_self, &lock_context );
}
 
typedef enum {
  SCHEDULER_SMP_DO_SCHEDULE,
  SCHEDULER_SMP_DO_NOT_SCHEDULE
} Scheduler_SMP_Action;
 
static inline Scheduler_SMP_Action _Scheduler_SMP_Try_to_schedule(
  Scheduler_Node          *node,
  Scheduler_Get_idle_node  get_idle_node,
  void                    *arg
)
{
  ISR_lock_Context        lock_context;
  Thread_Control         *owner;
  Thread_Scheduler_state  owner_state;
  int                     owner_sticky_level;
 
  owner = _Scheduler_Node_get_owner( node );
  _Assert( _Scheduler_Node_get_idle( node ) == NULL );
 
  _Thread_Scheduler_acquire_critical( owner, &lock_context );
  owner_state = owner->Scheduler.state;
  owner_sticky_level = node->sticky_level;
 
  if ( RTEMS_PREDICT_TRUE( owner_state == THREAD_SCHEDULER_READY ) ) {
    _Scheduler_SMP_Cancel_ask_for_help( owner );
    _Scheduler_Thread_change_state( owner, THREAD_SCHEDULER_SCHEDULED );
    _Thread_Scheduler_release_critical( owner, &lock_context );
    return SCHEDULER_SMP_DO_SCHEDULE;
  }
 
  _Thread_Scheduler_release_critical( owner, &lock_context );
 
  if (
    ( owner_state == THREAD_SCHEDULER_SCHEDULED && owner_sticky_level <= 1 ) ||
    owner_sticky_level == 0
  ) {
    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_BLOCKED );
 
    return SCHEDULER_SMP_DO_NOT_SCHEDULE;
  }
 
  (void) _Scheduler_Use_idle_thread( node, get_idle_node, arg );
 
  return SCHEDULER_SMP_DO_SCHEDULE;
}
 
static inline void _Scheduler_SMP_Allocate_processor_lazy(
  Scheduler_Context *context,
  Scheduler_Node    *scheduled,
  Per_CPU_Control   *cpu
)
{
  Thread_Control *scheduled_thread = _Scheduler_Node_get_user( scheduled );
  Per_CPU_Control *scheduled_cpu = _Thread_Get_CPU( scheduled_thread );
  Per_CPU_Control *cpu_self = _Per_CPU_Get();
 
  _Assert( _ISR_Get_level() != 0 );
 
  if ( cpu == scheduled_cpu ) {
    _Thread_Set_CPU( scheduled_thread, cpu );
    _Thread_Dispatch_update_heir( cpu_self, cpu, scheduled_thread );
 
    return;
  }
 
  if (
    _Thread_Is_executing_on_a_processor( scheduled_thread ) &&
    _Scheduler_SMP_Is_processor_owned_by_us( context, scheduled_cpu )
  ) {
    Thread_Control *heir = scheduled_cpu->heir;
    _Thread_Dispatch_update_heir( cpu_self, scheduled_cpu, scheduled_thread );
    _Thread_Set_CPU( heir, cpu );
    _Thread_Dispatch_update_heir( cpu_self, cpu, heir );
 
    return;
  }
 
  _Thread_Set_CPU( scheduled_thread, cpu );
  _Thread_Dispatch_update_heir( cpu_self, cpu, scheduled_thread );
}
 
static inline void _Scheduler_SMP_Allocate_processor_exact(
  Scheduler_Context *context,
  Scheduler_Node    *scheduled,
  Per_CPU_Control   *cpu
)
{
  Thread_Control *scheduled_thread = _Scheduler_Node_get_user( scheduled );
  Per_CPU_Control *cpu_self = _Per_CPU_Get();
 
  (void) context;
 
  _Thread_Set_CPU( scheduled_thread, cpu );
  _Thread_Dispatch_update_heir( cpu_self, cpu, scheduled_thread );
}
 
static inline void _Scheduler_SMP_Allocate_processor(
  Scheduler_Context                *context,
  Scheduler_Node                   *scheduled,
  Per_CPU_Control                  *cpu,
  Scheduler_SMP_Allocate_processor  allocate_processor
)
{
  _Scheduler_SMP_Node_change_state( scheduled, SCHEDULER_SMP_NODE_SCHEDULED );
  ( *allocate_processor )( context, scheduled, cpu );
}
 
static inline void _Scheduler_SMP_Preempt(
  Scheduler_Context                *context,
  Scheduler_Node                   *scheduled,
  Scheduler_Node                   *victim,
  Thread_Control                   *victim_idle,
  Scheduler_SMP_Allocate_processor  allocate_processor
)
{
  Thread_Control   *victim_owner;
  ISR_lock_Context  lock_context;
  Per_CPU_Control  *cpu;
 
  _Scheduler_SMP_Node_change_state( victim, SCHEDULER_SMP_NODE_READY );
 
  victim_owner = _Scheduler_Node_get_owner( victim );
  _Thread_Scheduler_acquire_critical( victim_owner, &lock_context );
 
  if ( RTEMS_PREDICT_TRUE( victim_idle == NULL ) ) {
    if ( victim_owner->Scheduler.state == THREAD_SCHEDULER_SCHEDULED ) {
      _Scheduler_Thread_change_state( victim_owner, THREAD_SCHEDULER_READY );
 
      if ( victim_owner->Scheduler.helping_nodes > 0 ) {
        _Scheduler_SMP_Request_ask_for_help( victim_owner );
      }
    }
 
    cpu = _Thread_Get_CPU( victim_owner );
  } else {
    cpu = _Thread_Get_CPU( victim_idle );
  }
 
  _Thread_Scheduler_release_critical( victim_owner, &lock_context );
 
  _Scheduler_SMP_Allocate_processor(
    context,
    scheduled,
    cpu,
    allocate_processor
  );
}
 
static inline Scheduler_Node *_Scheduler_SMP_Get_lowest_scheduled(
  Scheduler_Context *context,
  Scheduler_Node    *filter
)
{
  Scheduler_SMP_Context *self;
  Scheduler_Node        *lowest_scheduled;
 
  (void) filter;
 
  self = _Scheduler_SMP_Get_self( context );
 
  _Assert( !_Chain_Is_empty( &self->Scheduled ) );
  lowest_scheduled = (Scheduler_Node *) _Chain_Last( &self->Scheduled );
 
  _Assert(
    _Chain_Next( &lowest_scheduled->Node.Chain ) ==
      _Chain_Tail( &self->Scheduled )
  );
 
  return lowest_scheduled;
}
 
static inline void _Scheduler_SMP_Enqueue_to_scheduled(
  Scheduler_Context                *context,
  Scheduler_Node                   *node,
  Priority_Control                  priority,
  Scheduler_Node                   *lowest_scheduled,
  Scheduler_SMP_Insert              insert_scheduled,
  Scheduler_SMP_Move                move_from_scheduled_to_ready,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node,
  Scheduler_Release_idle_node       release_idle_node
)
{
  Thread_Control      *lowest_scheduled_idle;
  Scheduler_SMP_Action action;
 
  lowest_scheduled_idle = _Scheduler_Release_idle_thread_if_necessary(
    lowest_scheduled,
    release_idle_node,
    context
  );
 
  ( *move_from_scheduled_to_ready )( context, lowest_scheduled );
 
  action = _Scheduler_SMP_Try_to_schedule( node, get_idle_node, context );
 
  if ( RTEMS_PREDICT_TRUE( action == SCHEDULER_SMP_DO_SCHEDULE ) ) {
    _Scheduler_SMP_Preempt(
      context,
      node,
      lowest_scheduled,
      lowest_scheduled_idle,
      allocate_processor
    );
 
    ( *insert_scheduled )( context, node, priority );
  } else {
    _Assert( action == SCHEDULER_SMP_DO_NOT_SCHEDULE );
 
    if ( lowest_scheduled_idle != NULL ) {
      (void) _Scheduler_Use_idle_thread( lowest_scheduled, get_idle_node, context );
    }
 
    ( *move_from_ready_to_scheduled )( context, lowest_scheduled );
  }
}
 
static inline bool _Scheduler_SMP_Enqueue(
  Scheduler_Context                  *context,
  Scheduler_Node                     *node,
  Priority_Control                    insert_priority,
  Chain_Node_order                    order,
  Scheduler_SMP_Insert                insert_ready,
  Scheduler_SMP_Insert                insert_scheduled,
  Scheduler_SMP_Move                  move_from_scheduled_to_ready,
  Scheduler_SMP_Move                  move_from_ready_to_scheduled,
  Scheduler_SMP_Get_lowest_scheduled  get_lowest_scheduled,
  Scheduler_SMP_Allocate_processor    allocate_processor,
  Scheduler_Get_idle_node             get_idle_node,
  Scheduler_Release_idle_node         release_idle_node
)
{
  bool            needs_help;
  Scheduler_Node *lowest_scheduled;
 
  lowest_scheduled = ( *get_lowest_scheduled )( context, node );
 
  if (
    ( *order )(
      &insert_priority,
      &node->Node.Chain,
      &lowest_scheduled->Node.Chain
    )
  ) {
    _Scheduler_SMP_Enqueue_to_scheduled(
      context,
      node,
      insert_priority,
      lowest_scheduled,
      insert_scheduled,
      move_from_scheduled_to_ready,
      move_from_ready_to_scheduled,
      allocate_processor,
      get_idle_node,
      release_idle_node
    );
    needs_help = false;
  } else {
    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
    ( *insert_ready )( context, node, insert_priority );
    needs_help = true;
  }
 
  return needs_help;
}
 
static inline void _Scheduler_SMP_Enqueue_scheduled(
  Scheduler_Context                *context,
  Scheduler_Node                   *const node,
  Priority_Control                  insert_priority,
  Chain_Node_order                  order,
  Scheduler_SMP_Extract             extract_from_ready,
  Scheduler_SMP_Get_highest_ready   get_highest_ready,
  Scheduler_SMP_Insert              insert_ready,
  Scheduler_SMP_Insert              insert_scheduled,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node,
  Scheduler_Release_idle_node       release_idle_node
)
{
  Thread_Control *node_idle;
 
  node_idle = _Scheduler_Release_idle_thread_if_necessary(
    node,
    release_idle_node,
    context
  );
 
  while ( true ) {
    Scheduler_Node       *highest_ready;
    Scheduler_SMP_Action  action;
 
    highest_ready = ( *get_highest_ready )( context, node );
 
    /*
     * The node has been extracted from the scheduled chain.  We have to place
     * it now on the scheduled or ready set.
     */
    if (
      node->sticky_level > 0 && ( *order )(
        &insert_priority,
        &node->Node.Chain,
        &highest_ready->Node.Chain
      )
    ) {
      if ( node_idle != NULL ) {
        Thread_Control   *owner;
        ISR_lock_Context  lock_context;
 
        owner = _Scheduler_Node_get_owner( node );
        _Thread_Scheduler_acquire_critical( owner, &lock_context );
 
        if ( owner->Scheduler.state == THREAD_SCHEDULER_READY ) {
          Per_CPU_Control *cpu;
 
          _Scheduler_SMP_Cancel_ask_for_help( owner );
          _Scheduler_Thread_change_state( owner, THREAD_SCHEDULER_SCHEDULED );
          cpu = _Thread_Get_CPU( node_idle );
          _Thread_Set_CPU( owner, cpu );
          _Thread_Scheduler_release_critical( owner, &lock_context );
          _Thread_Dispatch_update_heir( _Per_CPU_Get(), cpu, owner );
        } else {
          Thread_Control *new_idle;
 
          _Thread_Scheduler_release_critical( owner, &lock_context );
          new_idle = _Scheduler_Use_idle_thread( node, get_idle_node, context );
          _Assert_Unused_variable_equals( new_idle, node_idle );
        }
      }
 
      ( *insert_scheduled )( context, node, insert_priority );
 
      return;
    }
 
    action = _Scheduler_SMP_Try_to_schedule(
      highest_ready,
      get_idle_node,
      context
    );
 
    if ( RTEMS_PREDICT_TRUE( action == SCHEDULER_SMP_DO_SCHEDULE ) ) {
      _Scheduler_SMP_Preempt(
        context,
        highest_ready,
        node,
        node_idle,
        allocate_processor
      );
 
      ( *move_from_ready_to_scheduled )( context, highest_ready );
      ( *insert_ready )( context, node, insert_priority );
      return;
    }
 
    _Assert( action == SCHEDULER_SMP_DO_NOT_SCHEDULE );
    ( *extract_from_ready )( context, highest_ready );
  }
}
 
static inline void _Scheduler_SMP_Extract_from_scheduled(
  Scheduler_Context *context,
  Scheduler_Node    *node
)
{
  (void) context;
  _Chain_Extract_unprotected( &node->Node.Chain );
}
 
static inline void _Scheduler_SMP_Schedule_highest_ready(
  Scheduler_Context                *context,
  Scheduler_Node                   *victim,
  Per_CPU_Control                  *cpu,
  Scheduler_SMP_Extract             extract_from_scheduled,
  Scheduler_SMP_Extract             extract_from_ready,
  Scheduler_SMP_Get_highest_ready   get_highest_ready,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node
)
{
  Scheduler_SMP_Action action;
 
  _Scheduler_SMP_Node_change_state( victim, SCHEDULER_SMP_NODE_BLOCKED );
  ( *extract_from_scheduled )( context, victim );
 
  while ( true ) {
    Scheduler_Node *highest_ready = ( *get_highest_ready )( context, victim );
 
    action = _Scheduler_SMP_Try_to_schedule(
      highest_ready,
      get_idle_node,
      context
    );
 
    if ( RTEMS_PREDICT_TRUE( action == SCHEDULER_SMP_DO_SCHEDULE ) ) {
      _Scheduler_SMP_Allocate_processor(
        context,
        highest_ready,
        cpu,
        allocate_processor
      );
 
      ( *move_from_ready_to_scheduled )( context, highest_ready );
      return;
    }
 
    _Assert( action == SCHEDULER_SMP_DO_NOT_SCHEDULE );
    ( *extract_from_ready )( context, highest_ready );
  }
}
 
static inline void _Scheduler_SMP_Preempt_and_schedule_highest_ready(
  Scheduler_Context                *context,
  Scheduler_Node                   *victim,
  Scheduler_SMP_Extract             extract_from_ready,
  Scheduler_SMP_Get_highest_ready   get_highest_ready,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node,
  Scheduler_Release_idle_node       release_idle_node
)
{
  Thread_Control      *victim_idle;
  Scheduler_SMP_Action action;
 
  _Scheduler_SMP_Node_change_state( victim, SCHEDULER_SMP_NODE_READY );
  victim_idle = _Scheduler_Release_idle_thread_if_necessary(
    victim,
    release_idle_node,
    context
  );
 
  while ( true ) {
    Scheduler_Node *highest_ready = ( *get_highest_ready )( context, victim );
 
    action = _Scheduler_SMP_Try_to_schedule(
      highest_ready,
      get_idle_node,
      context
    );
 
    if ( RTEMS_PREDICT_TRUE( action == SCHEDULER_SMP_DO_SCHEDULE ) ) {
      _Scheduler_SMP_Preempt(
        context,
        highest_ready,
        victim,
        victim_idle,
        allocate_processor
      );
 
      ( *move_from_ready_to_scheduled )( context, highest_ready );
      return;
    }
 
    _Assert( action == SCHEDULER_SMP_DO_NOT_SCHEDULE );
    ( *extract_from_ready )( context, highest_ready );
  }
}
 
static inline void _Scheduler_SMP_Block(
  Scheduler_Context                *context,
  Thread_Control                   *thread,
  Scheduler_Node                   *node,
  Scheduler_SMP_Extract             extract_from_scheduled,
  Scheduler_SMP_Extract             extract_from_ready,
  Scheduler_SMP_Get_highest_ready   get_highest_ready,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node
)
{
  int                       sticky_level;
  ISR_lock_Context          lock_context;
  Scheduler_SMP_Node_state  node_state;
  Per_CPU_Control          *cpu;
 
  sticky_level = node->sticky_level;
  --sticky_level;
  node->sticky_level = sticky_level;
  _Assert( sticky_level >= 0 );
 
  _Thread_Scheduler_acquire_critical( thread, &lock_context );
  _Scheduler_SMP_Cancel_ask_for_help( thread );
  cpu = _Thread_Get_CPU( thread );
  _Scheduler_Thread_change_state( thread, THREAD_SCHEDULER_BLOCKED );
  _Thread_Scheduler_release_critical( thread, &lock_context );
 
  node_state = _Scheduler_SMP_Node_state( node );
 
  if ( RTEMS_PREDICT_FALSE( sticky_level > 0 ) ) {
    if (
      node_state == SCHEDULER_SMP_NODE_SCHEDULED &&
      _Scheduler_Node_get_idle( node ) == NULL
    ) {
      Thread_Control *idle;
 
      idle = _Scheduler_Use_idle_thread( node, get_idle_node, context );
      _Thread_Set_CPU( idle, cpu );
      _Thread_Dispatch_update_heir( _Per_CPU_Get(), cpu, idle );
    }
 
    return;
  }
 
  _Assert( _Scheduler_Node_get_user( node ) == thread );
  _Assert( _Scheduler_Node_get_idle( node ) == NULL );
 
  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
    _Scheduler_SMP_Schedule_highest_ready(
      context,
      node,
      cpu,
      extract_from_scheduled,
      extract_from_ready,
      get_highest_ready,
      move_from_ready_to_scheduled,
      allocate_processor,
      get_idle_node
    );
  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_BLOCKED );
    ( *extract_from_ready )( context, node );
  }
}
 
static inline void _Scheduler_SMP_Unblock(
  Scheduler_Context          *context,
  Thread_Control             *thread,
  Scheduler_Node             *node,
  Scheduler_SMP_Update        update,
  Scheduler_SMP_Enqueue       enqueue,
  Scheduler_Release_idle_node release_idle_node
)
{
  Scheduler_SMP_Node_state  node_state;
  Priority_Control          priority;
 
  _Assert( _Chain_Is_node_off_chain( &thread->Scheduler.Help_node ) );
 
  ++node->sticky_level;
  _Assert( node->sticky_level > 0 );
 
  node_state = _Scheduler_SMP_Node_state( node );
 
  if ( RTEMS_PREDICT_FALSE( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) ) {
    _Scheduler_Thread_change_state( thread, THREAD_SCHEDULER_SCHEDULED );
    _Scheduler_Discard_idle_thread(
      thread,
      node,
      release_idle_node,
      context
    );
 
    return;
  }
 
  _Scheduler_Thread_change_state( thread, THREAD_SCHEDULER_READY );
 
  priority = _Scheduler_Node_get_priority( node );
  priority = SCHEDULER_PRIORITY_PURIFY( priority );
 
  if ( priority != _Scheduler_SMP_Node_priority( node ) ) {
    ( *update )( context, node, priority );
  }
 
  if ( node_state == SCHEDULER_SMP_NODE_BLOCKED ) {
    Priority_Control insert_priority;
    bool             needs_help;
 
    insert_priority = SCHEDULER_PRIORITY_APPEND( priority );
    needs_help = ( *enqueue )( context, node, insert_priority );
 
    if ( needs_help && thread->Scheduler.helping_nodes > 0 ) {
      _Scheduler_SMP_Request_ask_for_help( thread );
    }
  } else {
    _Assert( node_state == SCHEDULER_SMP_NODE_READY );
    _Assert( node->sticky_level > 0 );
    _Assert( node->idle == NULL );
    _Scheduler_SMP_Request_ask_for_help( thread );
  }
}
 
static inline void _Scheduler_SMP_Update_priority(
  Scheduler_Context              *context,
  Thread_Control                 *thread,
  Scheduler_Node                 *node,
  Scheduler_SMP_Extract           extract_from_scheduled,
  Scheduler_SMP_Extract           extract_from_ready,
  Scheduler_SMP_Update            update,
  Scheduler_SMP_Enqueue           enqueue,
  Scheduler_SMP_Enqueue_scheduled enqueue_scheduled,
  Scheduler_SMP_Ask_for_help      ask_for_help
)
{
  Priority_Control         priority;
  Priority_Control         insert_priority;
  Scheduler_SMP_Node_state node_state;
 
  insert_priority = _Scheduler_Node_get_priority( node );
  priority = SCHEDULER_PRIORITY_PURIFY( insert_priority );
 
  if ( priority == _Scheduler_SMP_Node_priority( node ) ) {
    if ( _Thread_Is_ready( thread ) ) {
      ( *ask_for_help )( context, thread, node );
    }
 
    return;
  }
 
  node_state = _Scheduler_SMP_Node_state( node );
 
  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
    ( *extract_from_scheduled )( context, node );
    ( *update )( context, node, priority );
    ( *enqueue_scheduled )( context, node, insert_priority );
  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
    ( *extract_from_ready )( context, node );
    ( *update )( context, node, priority );
    ( *enqueue )( context, node, insert_priority );
  } else {
    ( *update )( context, node, priority );
 
    if ( _Thread_Is_ready( thread ) ) {
      ( *ask_for_help )( context, thread, node );
    }
  }
}
 
static inline void _Scheduler_SMP_Yield(
  Scheduler_Context              *context,
  Thread_Control                 *thread,
  Scheduler_Node                 *node,
  Scheduler_SMP_Extract           extract_from_scheduled,
  Scheduler_SMP_Extract           extract_from_ready,
  Scheduler_SMP_Enqueue           enqueue,
  Scheduler_SMP_Enqueue_scheduled enqueue_scheduled
)
{
  Scheduler_SMP_Node_state node_state;
  Priority_Control         insert_priority;
 
  node_state = _Scheduler_SMP_Node_state( node );
  insert_priority = _Scheduler_SMP_Node_priority( node );
  insert_priority = SCHEDULER_PRIORITY_APPEND( insert_priority );
 
  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
    ( *extract_from_scheduled )( context, node );
    ( *enqueue_scheduled )( context, node, insert_priority );
  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
    ( *extract_from_ready )( context, node );
    (void) ( *enqueue )( context, node, insert_priority );
  }
}
 
static inline void _Scheduler_SMP_Insert_scheduled(
  Scheduler_Context *context,
  Scheduler_Node    *node_to_insert,
  Priority_Control   priority_to_insert
)
{
  Scheduler_SMP_Context *self;
 
  self = _Scheduler_SMP_Get_self( context );
 
  _Chain_Insert_ordered_unprotected(
    &self->Scheduled,
    &node_to_insert->Node.Chain,
    &priority_to_insert,
    _Scheduler_SMP_Priority_less_equal
  );
}
 
static inline bool _Scheduler_SMP_Ask_for_help(
  Scheduler_Context                  *context,
  Thread_Control                     *thread,
  Scheduler_Node                     *node,
  Chain_Node_order                    order,
  Scheduler_SMP_Insert                insert_ready,
  Scheduler_SMP_Insert                insert_scheduled,
  Scheduler_SMP_Move                  move_from_scheduled_to_ready,
  Scheduler_SMP_Get_lowest_scheduled  get_lowest_scheduled,
  Scheduler_SMP_Allocate_processor    allocate_processor,
  Scheduler_Release_idle_node         release_idle_node
)
{
  Scheduler_Node   *lowest_scheduled;
  ISR_lock_Context  lock_context;
  bool              success;
 
  if ( thread->Scheduler.pinned_scheduler != NULL ) {
    /*
     * Pinned threads are not allowed to ask for help.  Return success to break
     * the loop in _Thread_Ask_for_help() early.
     */
    return true;
  }
 
  lowest_scheduled = ( *get_lowest_scheduled )( context, node );
 
  _Thread_Scheduler_acquire_critical( thread, &lock_context );
 
  if ( thread->Scheduler.state == THREAD_SCHEDULER_READY ) {
    Scheduler_SMP_Node_state node_state;
 
    node_state = _Scheduler_SMP_Node_state( node );
 
    if ( node_state == SCHEDULER_SMP_NODE_BLOCKED ) {
      Priority_Control insert_priority;
 
      insert_priority = _Scheduler_SMP_Node_priority( node );
 
      if (
        ( *order )(
          &insert_priority,
          &node->Node.Chain,
          &lowest_scheduled->Node.Chain
        )
      ) {
        Thread_Control *lowest_scheduled_idle;
 
        _Scheduler_SMP_Cancel_ask_for_help( thread );
        _Scheduler_Thread_change_state( thread, THREAD_SCHEDULER_SCHEDULED );
        _Thread_Scheduler_release_critical( thread, &lock_context );
 
        lowest_scheduled_idle = _Scheduler_Release_idle_thread_if_necessary(
          lowest_scheduled,
          release_idle_node,
          context
        );
 
        _Scheduler_SMP_Preempt(
          context,
          node,
          lowest_scheduled,
          lowest_scheduled_idle,
          allocate_processor
        );
 
        ( *move_from_scheduled_to_ready )( context, lowest_scheduled );
        ( *insert_scheduled )( context, node, insert_priority );
 
        success = true;
      } else {
        _Thread_Scheduler_release_critical( thread, &lock_context );
 
        _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_READY );
        ( *insert_ready )( context, node, insert_priority );
        success = false;
      }
    } else if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
      _Scheduler_SMP_Cancel_ask_for_help( thread );
      _Scheduler_Thread_change_state( thread, THREAD_SCHEDULER_SCHEDULED );
      _Thread_Scheduler_release_critical( thread, &lock_context );
      _Scheduler_Discard_idle_thread(
        thread,
        node,
        release_idle_node,
        context
      );
      success = true;
    } else {
      _Thread_Scheduler_release_critical( thread, &lock_context );
      success = false;
    }
  } else {
    _Thread_Scheduler_release_critical( thread, &lock_context );
    success = false;
  }
 
  return success;
}
 
static inline void _Scheduler_SMP_Reconsider_help_request(
  Scheduler_Context     *context,
  Thread_Control        *thread,
  Scheduler_Node        *node,
  Scheduler_SMP_Extract  extract_from_ready
)
{
  ISR_lock_Context lock_context;
 
  _Thread_Scheduler_acquire_critical( thread, &lock_context );
 
  if (
    thread->Scheduler.state == THREAD_SCHEDULER_SCHEDULED
      && _Scheduler_SMP_Node_state( node ) == SCHEDULER_SMP_NODE_READY
      && node->sticky_level == 1
  ) {
    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_BLOCKED );
    ( *extract_from_ready )( context, node );
  }
 
  _Thread_Scheduler_release_critical( thread, &lock_context );
}
 
static inline void _Scheduler_SMP_Withdraw_node(
  Scheduler_Context                *context,
  Thread_Control                   *thread,
  Scheduler_Node                   *node,
  Thread_Scheduler_state            next_state,
  Scheduler_SMP_Extract             extract_from_scheduled,
  Scheduler_SMP_Extract             extract_from_ready,
  Scheduler_SMP_Get_highest_ready   get_highest_ready,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node
)
{
  ISR_lock_Context         lock_context;
  Scheduler_SMP_Node_state node_state;
 
  _Thread_Scheduler_acquire_critical( thread, &lock_context );
 
  node_state = _Scheduler_SMP_Node_state( node );
 
  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
    Per_CPU_Control *cpu;
 
    _Assert( thread == _Scheduler_Node_get_user( node ) );
    cpu = _Thread_Get_CPU( thread );
    _Scheduler_Thread_change_state( thread, next_state );
    _Thread_Scheduler_release_critical( thread, &lock_context );
 
    _Assert( _Scheduler_Node_get_user( node ) == thread );
    _Assert( _Scheduler_Node_get_idle( node ) == NULL );
 
    _Scheduler_SMP_Schedule_highest_ready(
      context,
      node,
      cpu,
      extract_from_scheduled,
      extract_from_ready,
      get_highest_ready,
      move_from_ready_to_scheduled,
      allocate_processor,
      get_idle_node
    );
  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
    _Thread_Scheduler_release_critical( thread, &lock_context );
    _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_BLOCKED );
    ( *extract_from_ready )( context, node );
  } else {
    _Assert( node_state == SCHEDULER_SMP_NODE_BLOCKED );
    _Thread_Scheduler_release_critical( thread, &lock_context );
  }
}
 
static inline void _Scheduler_SMP_Make_sticky(
  const Scheduler_Control *scheduler,
  Thread_Control          *the_thread,
  Scheduler_Node          *node,
  Scheduler_SMP_Update     update,
  Scheduler_SMP_Enqueue    enqueue
)
{
  Scheduler_SMP_Node_state node_state;
 
  node_state = _Scheduler_SMP_Node_state( node );
 
  if ( node_state == SCHEDULER_SMP_NODE_BLOCKED ) {
    Scheduler_Context *context;
    Priority_Control   insert_priority;
    Priority_Control   priority;
 
    context = _Scheduler_Get_context( scheduler );
    priority = _Scheduler_Node_get_priority( node );
    priority = SCHEDULER_PRIORITY_PURIFY( priority );
 
    if ( priority != _Scheduler_SMP_Node_priority( node ) ) {
      ( *update )( context, node, priority );
    }
 
    insert_priority = SCHEDULER_PRIORITY_APPEND( priority );
    (void) ( *enqueue )( context, node, insert_priority );
  }
}
 
static inline void _Scheduler_SMP_Clean_sticky(
  const Scheduler_Control          *scheduler,
  Thread_Control                   *the_thread,
  Scheduler_Node                   *node,
  Scheduler_SMP_Extract             extract_from_scheduled,
  Scheduler_SMP_Extract             extract_from_ready,
  Scheduler_SMP_Get_highest_ready   get_highest_ready,
  Scheduler_SMP_Move                move_from_ready_to_scheduled,
  Scheduler_SMP_Allocate_processor  allocate_processor,
  Scheduler_Get_idle_node           get_idle_node,
  Scheduler_Release_idle_node       release_idle_node
)
{
  Scheduler_SMP_Node_state node_state;
 
  node_state = _Scheduler_SMP_Node_state( node );
 
  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
    Thread_Control *idle;
 
    idle = _Scheduler_Node_get_idle( node );
 
    if ( idle != NULL ) {
      Scheduler_Context *context;
 
      context = _Scheduler_Get_context( scheduler );
 
      _Scheduler_Release_idle_thread( node, idle, release_idle_node, context );
      _Scheduler_SMP_Schedule_highest_ready(
        context,
        node,
        _Thread_Get_CPU( idle ),
        extract_from_scheduled,
        extract_from_ready,
        get_highest_ready,
        move_from_ready_to_scheduled,
        allocate_processor,
        get_idle_node
      );
    }
  }
}
 
static inline void _Scheduler_SMP_Do_start_idle(
  Scheduler_Context           *context,
  Thread_Control              *idle,
  Per_CPU_Control             *cpu,
  Scheduler_SMP_Register_idle  register_idle
)
{
  Scheduler_SMP_Context *self;
  Scheduler_SMP_Node    *node;
 
  self = _Scheduler_SMP_Get_self( context );
  node = _Scheduler_SMP_Thread_get_node( idle );
 
  _Scheduler_Thread_change_state( idle, THREAD_SCHEDULER_SCHEDULED );
  node->state = SCHEDULER_SMP_NODE_SCHEDULED;
 
  _Thread_Set_CPU( idle, cpu );
  ( *register_idle )( context, &node->Base, cpu );
  _Chain_Append_unprotected( &self->Scheduled, &node->Base.Node.Chain );
}
 
static inline void _Scheduler_SMP_Add_processor(
  Scheduler_Context              *context,
  Thread_Control                 *idle,
  Scheduler_SMP_Has_ready         has_ready,
  Scheduler_SMP_Enqueue_scheduled enqueue_scheduled,
  Scheduler_SMP_Register_idle     register_idle
)
{
  Scheduler_SMP_Context *self;
  Scheduler_Node        *node;
 
  self = _Scheduler_SMP_Get_self( context );
  idle->Scheduler.state = THREAD_SCHEDULER_SCHEDULED;
  node = _Thread_Scheduler_get_home_node( idle );
  _Scheduler_SMP_Node_change_state( node, SCHEDULER_SMP_NODE_SCHEDULED );
  ( *register_idle )( context, node, _Thread_Get_CPU( idle ) );
 
  if ( ( *has_ready )( &self->Base ) ) {
    Priority_Control insert_priority;
 
    insert_priority = _Scheduler_SMP_Node_priority( node );
    insert_priority = SCHEDULER_PRIORITY_APPEND( insert_priority );
    ( *enqueue_scheduled )( &self->Base, node, insert_priority );
  } else {
    _Chain_Append_unprotected( &self->Scheduled, &node->Node.Chain );
  }
}
 
static inline Thread_Control *_Scheduler_SMP_Remove_processor(
  Scheduler_Context          *context,
  Per_CPU_Control            *cpu,
  Scheduler_SMP_Extract       extract_from_scheduled,
  Scheduler_SMP_Extract       extract_from_ready,
  Scheduler_SMP_Enqueue       enqueue,
  Scheduler_Get_idle_node     get_idle_node,
  Scheduler_Release_idle_node release_idle_node
)
{
  Scheduler_SMP_Context *self;
  Chain_Node            *chain_node;
  Scheduler_Node        *victim_node;
  Thread_Control        *victim_user;
  Thread_Control        *victim_owner;
  Thread_Control        *idle;
 
  self = _Scheduler_SMP_Get_self( context );
  chain_node = _Chain_First( &self->Scheduled );
 
  do {
    _Assert( chain_node != _Chain_Immutable_tail( &self->Scheduled ) );
    victim_node = (Scheduler_Node *) chain_node;
    victim_user = _Scheduler_Node_get_user( victim_node );
    chain_node = _Chain_Next( chain_node );
  } while ( _Thread_Get_CPU( victim_user ) != cpu );
 
  ( *extract_from_scheduled )( &self->Base, victim_node );
  victim_owner = _Scheduler_Node_get_owner( victim_node );
 
  if ( !victim_owner->is_idle ) {
    Thread_Control  *victim_idle;
    Scheduler_Node  *idle_node;
    Priority_Control insert_priority;
 
    victim_idle = _Scheduler_Release_idle_thread_if_necessary(
      victim_node,
      release_idle_node,
      &self->Base
    );
    idle_node = ( *get_idle_node )( &self->Base );
    idle = _Scheduler_Node_get_owner( idle_node );
    _Scheduler_SMP_Preempt(
      &self->Base,
      idle_node,
      victim_node,
      victim_idle,
      _Scheduler_SMP_Allocate_processor_exact
    );
 
    _Assert( !_Chain_Is_empty( &self->Scheduled ) );
    insert_priority = _Scheduler_SMP_Node_priority( victim_node );
    insert_priority = SCHEDULER_PRIORITY_APPEND( insert_priority );
    ( *enqueue )( &self->Base, victim_node, insert_priority );
  } else {
    _Assert( victim_owner == victim_user );
    _Assert( _Scheduler_Node_get_idle( victim_node ) == NULL );
    idle = victim_owner;
  }
 
  return idle;
}
 
static inline void _Scheduler_SMP_Set_affinity(
  Scheduler_Context               *context,
  Thread_Control                  *thread,
  Scheduler_Node                  *node,
  void                            *arg,
  Scheduler_SMP_Set_affinity       set_affinity,
  Scheduler_SMP_Extract            extract_from_scheduled,
  Scheduler_SMP_Extract            extract_from_ready,
  Scheduler_SMP_Get_highest_ready  get_highest_ready,
  Scheduler_SMP_Move               move_from_ready_to_scheduled,
  Scheduler_SMP_Enqueue            enqueue,
  Scheduler_SMP_Allocate_processor allocate_processor,
  Scheduler_Get_idle_node          get_idle_node,
  Scheduler_Release_idle_node      release_idle_node
)
{
  Scheduler_SMP_Node_state node_state;
  Priority_Control         insert_priority;
 
  node_state = _Scheduler_SMP_Node_state( node );
  insert_priority = _Scheduler_SMP_Node_priority( node );
  insert_priority = SCHEDULER_PRIORITY_APPEND( insert_priority );
 
  if ( node_state == SCHEDULER_SMP_NODE_SCHEDULED ) {
    ( *extract_from_scheduled )( context, node );
    _Scheduler_SMP_Preempt_and_schedule_highest_ready(
      context,
      node,
      extract_from_ready,
      get_highest_ready,
      move_from_ready_to_scheduled,
      allocate_processor,
      get_idle_node,
      release_idle_node
    );
    ( *set_affinity )( context, node, arg );
    ( *enqueue )( context, node, insert_priority );
  } else if ( node_state == SCHEDULER_SMP_NODE_READY ) {
    ( *extract_from_ready )( context, node );
    ( *set_affinity )( context, node, arg );
    ( *enqueue )( context, node, insert_priority );
  } else {
    _Assert( node_state == SCHEDULER_SMP_NODE_BLOCKED );
    ( *set_affinity )( context, node, arg );
  }
}
 
#ifdef __cplusplus
}
#endif /* __cplusplus */
 
#endif /* _RTEMS_SCORE_SCHEDULERSMPIMPL_H */