[PATCH v1 2/3] Add pinning to priority SMP scheduler

[Ryan Long]

---
 cpukit/include/rtems/score/scheduler.h             | 36 ++++++++
 .../rtems/score/schedulerpriorityaffinitysmp.h     |  4 +-
 cpukit/score/src/schedulerpinusingaffinity.c       | 98 ++++++++++++++++++++++
 spec/build/cpukit/objsmp.yml                       |  1 +
 4 files changed, 137 insertions(+), 2 deletions(-)
 create mode 100644 cpukit/score/src/schedulerpinusingaffinity.c

diff --git a/cpukit/include/rtems/score/scheduler.h b/cpukit/include/rtems/score/scheduler.h
index df9477f..da71c61 100644
--- a/cpukit/include/rtems/score/scheduler.h
+++ b/cpukit/include/rtems/score/scheduler.h
@@ -436,6 +436,42 @@ Priority_Control _Scheduler_default_Unmap_priority(
     Scheduler_Node          *node,
     struct Per_CPU_Control  *cpu
   );
+
+  /**
+   * @brief Pins a thread to a core using affinity
+   *
+   * This default implementation for the thread pin or unpin operations should
+   * be used by uniprocessor schedulers if SMP support is enabled.
+   *
+   * @param scheduler This parameter is unused.
+   * @param the_thread This parameter is unused.
+   * @param node This parameter is unused.
+   * @param cpu This parameter is unused.
+   */
+  void _Scheduler_pin_using_affinity(
+    const Scheduler_Control *scheduler,
+    Thread_Control          *the_thread,
+    Scheduler_Node          *node,
+    struct Per_CPU_Control  *cpu
+  );
+
+  /**
+   * @brief Unpinds a thread to a core using affinity.
+   *
+   * This default implementation for the thread pin or unpin operations should
+   * be used by uniprocessor schedulers if SMP support is enabled.
+   *
+   * @param scheduler This parameter is unused.
+   * @param the_thread This parameter is unused.
+   * @param node This parameter is unused.
+   * @param cpu This parameter is unused.
+   */
+  void _Scheduler_unpin_using_affinity(
+    const Scheduler_Control *scheduler,
+    Thread_Control          *the_thread,
+    Scheduler_Node          *node,
+    struct Per_CPU_Control  *cpu
+  );
 #endif
 
 /**
diff --git a/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h b/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h
index 1b660fa..26a4562 100644
--- a/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h
+++ b/cpukit/include/rtems/score/schedulerpriorityaffinitysmp.h
@@ -65,8 +65,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_not_supported, \
-    _Scheduler_default_Pin_or_unpin_not_supported, \
+    _Scheduler_pin_using_affinity, \
+    _Scheduler_unpin_using_affinity, \
     _Scheduler_priority_affinity_SMP_Add_processor, \
     _Scheduler_priority_affinity_SMP_Remove_processor, \
     _Scheduler_priority_affinity_SMP_Node_initialize, \
diff --git a/cpukit/score/src/schedulerpinusingaffinity.c b/cpukit/score/src/schedulerpinusingaffinity.c
new file mode 100644
index 0000000..f9597d2
--- /dev/null
+++ b/cpukit/score/src/schedulerpinusingaffinity.c
@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+
+/**
+ *  @file
+ *
+ *  @ingroup RTEMSScoreScheduler
+ *
+ *  @brief Pins/unpins a thread to a core using affinity.
+ */
+
+/*
+ * Copyright (C) 2021 OAR Corporation
+ *
+ * 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.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <assert.h>
+#include <rtems/score/scheduler.h>
+#include <rtems/score/percpu.h>
+#include <rtems/score/schedulerimpl.h>
+
+void _Scheduler_pin_using_affinity(
+  const Scheduler_Control* scheduler,
+  Thread_Control*          the_thread,
+  Scheduler_Node*          node,
+  struct Per_CPU_Control*  cpu
+)
+{
+#if defined( RTEMS_SMP )
+  cpu_set_t                  cpuset;
+  int                        cpu_num;
+  Processor_mask             affinity;
+  Processor_mask_Copy_status status;
+  Status_Control             status2;
+
+  cpu_num = _Per_CPU_Get_index( cpu );
+
+  CPU_ZERO( &cpuset );
+  CPU_SET( cpu_num, &cpuset );
+
+  status = _Processor_mask_From_cpu_set_t( &affinity, sizeof( cpuset ), &cpuset );
+  assert( status == PROCESSOR_MASK_COPY_LOSSLESS );
+
+  status2 = ( *scheduler->Operations.set_affinity )(
+    scheduler,
+    the_thread,
+    node,
+    &affinity
+  );
+  assert( status2 == STATUS_SUCCESSFUL );
+#endif
+}
+
+void _Scheduler_unpin_using_affinity(
+  const Scheduler_Control* scheduler,
+  Thread_Control*          the_thread,
+  Scheduler_Node*          node,
+  struct Per_CPU_Control*  cpu
+)
+{
+#if defined( RTEMS_SMP )
+  const Processor_mask* processor_set;
+  Status_Control        status;
+
+  processor_set = _Scheduler_Get_processors( scheduler );
+
+  status = ( *scheduler->Operations.set_affinity )(
+    scheduler,
+    the_thread,
+    node,
+    processor_set
+  );
+  assert( status == STATUS_SUCCESSFUL );
+#endif
+}
diff --git a/spec/build/cpukit/objsmp.yml b/spec/build/cpukit/objsmp.yml
index 1a55708..3aca79b 100644
--- a/spec/build/cpukit/objsmp.yml
+++ b/spec/build/cpukit/objsmp.yml
@@ -15,6 +15,7 @@ source:
 - cpukit/score/src/profilingsmplock.c
 - cpukit/score/src/schedulerdefaultpinunpin.c
 - cpukit/score/src/schedulerdefaultpinunpindonothing.c
+- cpukit/score/src/schedulerpinusingaffinity.c
 - cpukit/score/src/schedulerdefaultsetaffinity.c
 - cpukit/score/src/scheduleredfsmp.c
 - cpukit/score/src/schedulerpriorityaffinitysmp.c
-- 
1.8.3.1