score: Add thread pin/unpin support

Add support to temporarily pin a thread to its current processor.  This
may be used to access per-processor data structures in critical sections
with enabled thread dispatching, e.g. a pinned thread is allowed to
block.

Update #3508.

11 files changed, 197 insertions, 25 deletions

diff --git a/cpukit/include/rtems/score/scheduler.h b/cpukit/include/rtems/score/scheduler.h
index a6066c8e4a..fe6315daef 100644
--- a/cpukit/include/rtems/score/scheduler.h
+++ b/cpukit/include/rtems/score/scheduler.h
@@ -137,6 +137,36 @@ typedef struct {
   );
 
   /**
+   * @brief Pin thread operation.
+   *
+   * @param[in] scheduler The scheduler instance of the specified processor.
+   * @param[in] the_thread The thread to pin.
+   * @param[in] node The scheduler node of the thread.
+   * @param[in] cpu The processor to pin the thread.
+   */
+  void ( *pin )(
+    const Scheduler_Control *scheduler,
+    Thread_Control          *the_thread,
+    Scheduler_Node          *node,
+    struct Per_CPU_Control  *cpu
+  );
+
+  /**
+   * @brief Unpin thread operation.
+   *
+   * @param[in] scheduler The scheduler instance of the specified processor.
+   * @param[in] the_thread The thread to unpin.
+   * @param[in] node The scheduler node of the thread.
+   * @param[in] cpu The processor to unpin the thread.
+   */
+  void ( *unpin )(
+    const Scheduler_Control *scheduler,
+    Thread_Control          *the_thread,
+    Scheduler_Node          *node,
+    struct Per_CPU_Control  *cpu
+  );
+
+  /**
    * @brief Add processor operation.
    *
    * @param[in] scheduler The scheduler instance to add the processor.
@@ -405,10 +435,28 @@ Priority_Control _Scheduler_default_Unmap_priority(
     Thread_Scheduler_state   next_state
   );
 
+  /**
+   * @brief Does nothing in a single processor system, otherwise a fatal error
+   * is issued.
+   *
+   * @param[in] scheduler Unused.
+   * @param[in] the_thread Unused.
+   * @param[in] node Unused.
+   * @param[in] cpu Unused.
+   */
+  void _Scheduler_default_Pin_or_unpin(
+    const Scheduler_Control *scheduler,
+    Thread_Control          *the_thread,
+    Scheduler_Node          *node,
+    struct Per_CPU_Control  *cpu
+  );
+
   #define SCHEDULER_OPERATION_DEFAULT_ASK_FOR_HELP \
     _Scheduler_default_Ask_for_help, \
     _Scheduler_default_Reconsider_help_request, \
     _Scheduler_default_Withdraw_node, \
+    _Scheduler_default_Pin_or_unpin, \
+    _Scheduler_default_Pin_or_unpin, \
     NULL, \
     NULL,
 #else
diff --git a/cpukit/include/rtems/score/scheduleredfsmp.h b/cpukit/include/rtems/score/scheduleredfsmp.h
index 018568190e..72c906eefe 100644
--- a/cpukit/include/rtems/score/scheduleredfsmp.h
+++ b/cpukit/include/rtems/score/scheduleredfsmp.h
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2017 embedded brains GmbH.
+ * Copyright (c) 2017, 2018 embedded brains GmbH.
  *
  * The license and distribution terms for this file may be
  * found in the file LICENSE in this distribution or at
@@ -43,13 +43,24 @@ typedef struct {
   int64_t generation;
 
   /**
-   * @brief The ready queue index depending on the processor affinity of the thread.
+   * @brief The ready queue index depending on the processor affinity and
+   * pinning of the thread.
    *
    * The ready queue index zero is used for threads with a one-to-all thread
    * processor affinity.  Threads with a one-to-one processor affinity use the
    * processor index plus one as the ready queue index.
    */
-  uint32_t ready_queue_index;
+  uint8_t ready_queue_index;
+
+  /**
+   * @brief Ready queue index according to thread affinity.
+   */
+  uint8_t affinity_ready_queue_index;
+
+  /**
+   * @brief Ready queue index according to thread pinning.
+   */
+  uint8_t pinning_ready_queue_index;
 } Scheduler_EDF_SMP_Node;
 
 typedef struct {
@@ -105,6 +116,8 @@ typedef struct {
     _Scheduler_EDF_SMP_Ask_for_help, \
     _Scheduler_EDF_SMP_Reconsider_help_request, \
     _Scheduler_EDF_SMP_Withdraw_node, \
+    _Scheduler_EDF_SMP_Pin, \
+    _Scheduler_EDF_SMP_Unpin, \
     _Scheduler_EDF_SMP_Add_processor, \
     _Scheduler_EDF_SMP_Remove_processor, \
     _Scheduler_EDF_SMP_Node_initialize, \
@@ -162,6 +175,20 @@ void _Scheduler_EDF_SMP_Withdraw_node(
   Thread_Scheduler_state   next_state
 );
 
+void _Scheduler_EDF_SMP_Pin(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  Scheduler_Node          *node,
+  struct Per_CPU_Control  *cpu
+);
+
+void _Scheduler_EDF_SMP_Unpin(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  Scheduler_Node          *node,
+  struct Per_CPU_Control  *cpu
+);
+
 void _Scheduler_EDF_SMP_Add_processor(
   const Scheduler_Control *scheduler,
   Thread_Control          *idle
diff --git a/cpukit/include/rtems/score/schedulerimpl.h b/cpukit/include/rtems/score/schedulerimpl.h
index 2fe30b3d6b..fda043ea5a 100644
--- a/cpukit/include/rtems/score/schedulerimpl.h
+++ b/cpukit/include/rtems/score/schedulerimpl.h
@@ -1095,17 +1095,13 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
     &the_thread->Real_priority
   );
 
-  if ( !_Priority_Is_empty( &old_scheduler_node->Wait.Priority ) ) {
-    _Priority_Plain_insert(
-      &old_scheduler_node->Wait.Priority,
-      &the_thread->Real_priority,
-      the_thread->Real_priority.priority
-    );
-    return STATUS_RESOURCE_IN_USE;
-  }
-
+  if (
+    !_Priority_Is_empty( &old_scheduler_node->Wait.Priority )
 #if defined(RTEMS_SMP)
-  if ( !_Chain_Has_only_one_node( &the_thread->Scheduler.Wait_nodes ) ) {
+      || !_Chain_Has_only_one_node( &the_thread->Scheduler.Wait_nodes )
+      || the_thread->Scheduler.pin_level != 0
+#endif
+  ) {
     _Priority_Plain_insert(
       &old_scheduler_node->Wait.Priority,
       &the_thread->Real_priority,
@@ -1114,6 +1110,7 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
     return STATUS_RESOURCE_IN_USE;
   }
 
+#if defined(RTEMS_SMP)
   old_scheduler = _Thread_Scheduler_get_home( the_thread );
   new_scheduler_node = _Thread_Scheduler_get_node_by_index(
     the_thread,
@@ -1140,7 +1137,8 @@ RTEMS_INLINE_ROUTINE Status_Control _Scheduler_Set(
     return STATUS_UNSATISFIED;
   }
 
-  the_thread->Scheduler.home = new_scheduler;
+  _Assert( the_thread->Scheduler.pinned_scheduler == NULL );
+  the_thread->Scheduler.home_scheduler = new_scheduler;
 
   _Scheduler_Release_critical( new_scheduler, &lock_context );
 
diff --git a/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h b/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h
index d988d5752a..e8ee528c90 100644
--- a/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h
+++ b/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h
@@ -61,6 +61,8 @@ extern "C" {
     _Scheduler_priority_affinity_SMP_Ask_for_help, \
     _Scheduler_priority_affinity_SMP_Reconsider_help_request, \
     _Scheduler_priority_affinity_SMP_Withdraw_node, \
+    _Scheduler_default_Pin_or_unpin, \
+    _Scheduler_default_Pin_or_unpin, \
     _Scheduler_priority_affinity_SMP_Add_processor, \
     _Scheduler_priority_affinity_SMP_Remove_processor, \
     _Scheduler_priority_affinity_SMP_Node_initialize, \
diff --git a/cpukit/include/rtems/score/schedulerprioritysmp.h b/cpukit/include/rtems/score/schedulerprioritysmp.h
index 6671da5b7a..7d579f1846 100644
--- a/cpukit/include/rtems/score/schedulerprioritysmp.h
+++ b/cpukit/include/rtems/score/schedulerprioritysmp.h
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2013, 2016 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2013, 2018 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -89,6 +89,8 @@ typedef struct {
     _Scheduler_priority_SMP_Ask_for_help, \
     _Scheduler_priority_SMP_Reconsider_help_request, \
     _Scheduler_priority_SMP_Withdraw_node, \
+    _Scheduler_default_Pin_or_unpin, \
+    _Scheduler_default_Pin_or_unpin, \
     _Scheduler_priority_SMP_Add_processor, \
     _Scheduler_priority_SMP_Remove_processor, \
     _Scheduler_priority_SMP_Node_initialize, \
diff --git a/cpukit/include/rtems/score/schedulersimplesmp.h b/cpukit/include/rtems/score/schedulersimplesmp.h
index bc75b205d5..d9b4c1d46c 100644
--- a/cpukit/include/rtems/score/schedulersimplesmp.h
+++ b/cpukit/include/rtems/score/schedulersimplesmp.h
@@ -9,7 +9,7 @@
 /*
  *  Copyright (C) 2011 On-Line Applications Research Corporation (OAR).
  *
- *  Copyright (c) 2013, 2016 embedded brains GmbH.
+ *  Copyright (c) 2013, 2018 embedded brains GmbH.
  *
  *  The license and distribution terms for this file may be
  *  found in the file LICENSE in this distribution or at
@@ -72,6 +72,8 @@ typedef struct {
     _Scheduler_simple_SMP_Ask_for_help, \
     _Scheduler_simple_SMP_Reconsider_help_request, \
     _Scheduler_simple_SMP_Withdraw_node, \
+    _Scheduler_default_Pin_or_unpin, \
+    _Scheduler_default_Pin_or_unpin, \
     _Scheduler_simple_SMP_Add_processor, \
     _Scheduler_simple_SMP_Remove_processor, \
     _Scheduler_simple_SMP_Node_initialize, \
diff --git a/cpukit/include/rtems/score/schedulersmpimpl.h b/cpukit/include/rtems/score/schedulersmpimpl.h
index 09c95662b6..0abe82b1f0 100644
--- a/cpukit/include/rtems/score/schedulersmpimpl.h
+++ b/cpukit/include/rtems/score/schedulersmpimpl.h
@@ -1185,6 +1185,14 @@ static inline bool _Scheduler_SMP_Ask_for_help(
   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 );
@@ -1474,6 +1482,7 @@ static inline void _Scheduler_SMP_Set_affinity(
     ( *set_affinity )( context, node, arg );
     ( *enqueue )( context, node, insert_priority );
   } else {
+    _Assert( node_state == SCHEDULER_SMP_NODE_BLOCKED );
     ( *set_affinity )( context, node, arg );
   }
 }
diff --git a/cpukit/include/rtems/score/schedulerstrongapa.h b/cpukit/include/rtems/score/schedulerstrongapa.h
index d961f20c68..bc113197e7 100644
--- a/cpukit/include/rtems/score/schedulerstrongapa.h
+++ b/cpukit/include/rtems/score/schedulerstrongapa.h
@@ -7,7 +7,7 @@
  */
 
 /*
- * Copyright (c) 2013, 2016 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2013, 2018 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -89,6 +89,8 @@ typedef struct {
     _Scheduler_strong_APA_Ask_for_help, \
     _Scheduler_strong_APA_Reconsider_help_request, \
     _Scheduler_strong_APA_Withdraw_node, \
+    _Scheduler_default_Pin_or_unpin, \
+    _Scheduler_default_Pin_or_unpin, \
     _Scheduler_strong_APA_Add_processor, \
     _Scheduler_strong_APA_Remove_processor, \
     _Scheduler_strong_APA_Node_initialize, \
diff --git a/cpukit/include/rtems/score/smpimpl.h b/cpukit/include/rtems/score/smpimpl.h
index 75d36ac865..532e5a6b4e 100644
--- a/cpukit/include/rtems/score/smpimpl.h
+++ b/cpukit/include/rtems/score/smpimpl.h
@@ -79,7 +79,8 @@ typedef enum {
   SMP_FATAL_MULTITASKING_START_ON_UNASSIGNED_PROCESSOR,
   SMP_FATAL_SHUTDOWN,
   SMP_FATAL_SHUTDOWN_RESPONSE,
-  SMP_FATAL_START_OF_MANDATORY_PROCESSOR_FAILED
+  SMP_FATAL_START_OF_MANDATORY_PROCESSOR_FAILED,
+  SMP_FATAL_SCHEDULER_PIN_OR_UNPIN_NOT_SUPPORTED
 } SMP_Fatal_code;
 
 static inline void _SMP_Fatal( SMP_Fatal_code code )
diff --git a/cpukit/include/rtems/score/thread.h b/cpukit/include/rtems/score/thread.h
index 7e0e2722dd..4dd1023045 100644
--- a/cpukit/include/rtems/score/thread.h
+++ b/cpukit/include/rtems/score/thread.h
@@ -259,9 +259,14 @@ typedef struct {
   Thread_Scheduler_state state;
 
   /**
-   * @brief The home scheduler control of this thread.
+   * @brief The home scheduler of this thread.
    */
-  const struct _Scheduler_Control *home;
+  const struct _Scheduler_Control *home_scheduler;
+
+  /**
+   * @brief The pinned scheduler of this thread.
+   */
+  const struct _Scheduler_Control *pinned_scheduler;
 
   /**
    * @brief The processor assigned by the current scheduler.
@@ -270,12 +275,12 @@ typedef struct {
 
   /**
    * @brief Scheduler nodes immediately available to the thread by its home
-   * scheduler instance and due to thread queue ownerships.
+   * scheduler and due to thread queue ownerships.
    *
    * This chain is protected by the thread wait lock.
    *
    * This chain is never empty.  The first scheduler node on the chain is the
-   * scheduler node of the home scheduler instance.
+   * scheduler node of the home scheduler.
    */
   Chain_Control Wait_nodes;
 
@@ -285,8 +290,12 @@ typedef struct {
    *
    * This chain is protected by the thread state lock.
    *
-   * This chain is never empty.  The first scheduler node on the chain is the
-   * scheduler node of the home scheduler instance.
+   * This chain is never empty for normal threads (the only exception are idle
+   * threads associated with an online processor which is not used by a
+   * scheduler).  In case a pinned scheduler is set for this thread, then the
+   * first scheduler node of this chain belongs to the pinned scheduler,
+   * otherwise the first scheduler node of this chain belongs to the home
+   * scheduler.
    */
   Chain_Control Scheduler_nodes;
 
@@ -313,6 +322,28 @@ typedef struct {
   Scheduler_Node *requests;
 
   /**
+   * @brief The thread pinning to current processor level.
+   *
+   * Must be touched only by the executing thread with thread dispatching
+   * disabled.  If non-zero, then the thread is pinned to its current
+   * processor.  The pin level is incremented and decremented by two.  The
+   * least-significant bit indicates that the thread was pre-empted and must
+   * undo the pinning with respect to the scheduler once the level changes from
+   * three to one.
+   *
+   * The thread pinning may be used to access per-processor data structures in
+   * critical sections with enabled thread dispatching, e.g. a pinned thread is
+   * allowed to block.
+   *
+   * Thread pinning should be used only for short critical sections and not all
+   * the time.  Thread pinning is a very low overhead operation in case the
+   * thread is not preempted during the pinning.
+   *
+   * @see _Thread_Pin() and _Thread_Unpin().
+   */
+  int pin_level;
+
+  /**
    * @brief The thread processor affinity set.
    */
   Processor_mask Affinity;
diff --git a/cpukit/include/rtems/score/threadimpl.h b/cpukit/include/rtems/score/threadimpl.h
index 4ab855d3ff..530035ff67 100644
--- a/cpukit/include/rtems/score/threadimpl.h
+++ b/cpukit/include/rtems/score/threadimpl.h
@@ -1025,7 +1025,7 @@ RTEMS_INLINE_ROUTINE const Scheduler_Control *_Thread_Scheduler_get_home(
 )
 {
 #if defined(RTEMS_SMP)
-  return the_thread->Scheduler.home;
+  return the_thread->Scheduler.home_scheduler;
 #else
   (void) the_thread;
   return &_Scheduler_Table[ 0 ];
@@ -1953,6 +1953,56 @@ size_t _Thread_Get_name(
   size_t                buffer_size
 );
 
+#if defined(RTEMS_SMP)
+#define THREAD_PIN_STEP 2
+
+#define THREAD_PIN_PREEMPTION 1
+
+void _Thread_Do_unpin(
+  Thread_Control  *executing,
+  Per_CPU_Control *cpu_self
+);
+#endif
+
+RTEMS_INLINE_ROUTINE void _Thread_Pin( Thread_Control *executing )
+{
+#if defined(RTEMS_SMP)
+  _Assert( executing == _Thread_Executing );
+
+  executing->Scheduler.pin_level += THREAD_PIN_STEP;
+#else
+  (void) executing;
+#endif
+}
+
+RTEMS_INLINE_ROUTINE void _Thread_Unpin(
+  Thread_Control  *executing,
+  Per_CPU_Control *cpu_self
+)
+{
+#if defined(RTEMS_SMP)
+  unsigned int pin_level;
+
+  _Assert( executing == _Thread_Executing );
+
+  pin_level = executing->Scheduler.pin_level;
+  _Assert( pin_level > 0 );
+
+  if (
+    RTEMS_PREDICT_TRUE(
+      pin_level != ( THREAD_PIN_STEP | THREAD_PIN_PREEMPTION )
+    )
+  ) {
+    executing->Scheduler.pin_level = pin_level - THREAD_PIN_STEP;
+  } else {
+    _Thread_Do_unpin( executing, cpu_self );
+  }
+#else
+  (void) executing;
+  (void) cpu_self;
+#endif
+}
+
 /** @}*/
 
 #ifdef __cplusplus