score: Use processor mask for set affinity

Update #3059.

6 files changed, 40 insertions, 62 deletions

diff --git a/cpukit/score/include/rtems/score/scheduler.h b/cpukit/score/include/rtems/score/scheduler.h
index 7a3ae6b466..5f3fb01206 100644
--- a/cpukit/score/include/rtems/score/scheduler.h
+++ b/cpukit/score/include/rtems/score/scheduler.h
@@ -207,8 +207,7 @@ typedef struct {
   bool ( *set_affinity )(
     const Scheduler_Control *,
     Thread_Control *,
-    size_t,
-    const cpu_set_t *
+    const Processor_mask *
   );
 #endif
 } Scheduler_Operations;
@@ -507,23 +506,19 @@ void _Scheduler_default_Start_idle(
 
 #if defined(RTEMS_SMP)
   /** 
-   * @brief Set affinity for the default scheduler.
+   * @brief Default implementation of the set affinity scheduler operation.
    *
    * @param[in] scheduler The scheduler instance.
    * @param[in] thread The associated thread.
-   * @param[in] cpusetsize The size of the cpuset.
-   * @param[in] cpuset Affinity new affinity set.
+   * @param[in] affinity The new processor affinity set for the thread.
    *
-   * @retval 0 Successful
-   *
-   *  This method always returns successful and does not save
-   *  the cpuset.
+   * @retval true The processor set of the scheduler is a subset of the affinity set.
+   * @retval false Otherwise.
    */
   bool _Scheduler_default_Set_affinity(
     const Scheduler_Control *scheduler,
     Thread_Control          *thread,
-    size_t                   cpusetsize,
-    const cpu_set_t         *cpuset
+    const Processor_mask    *affinity
   );
 
   #define SCHEDULER_OPERATION_DEFAULT_GET_SET_AFFINITY \
diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index 895f116066..6f220acf57 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -609,34 +609,13 @@ bool _Scheduler_Get_affinity(
 RTEMS_INLINE_ROUTINE bool _Scheduler_default_Set_affinity_body(
   const Scheduler_Control *scheduler,
   Thread_Control          *the_thread,
-  size_t                   cpusetsize,
-  const cpu_set_t         *cpuset
+  const Processor_mask    *affinity
 )
 {
-  uint32_t cpu_count = _SMP_Get_processor_count();
-  uint32_t cpu_index;
-  bool     ok = true;
-
-  for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
-#if defined(RTEMS_SMP)
-    const Per_CPU_Control   *cpu;
-    const Scheduler_Control *scheduler_of_cpu;
-
-    cpu = _Per_CPU_Get_by_index( cpu_index );
-    scheduler_of_cpu = _Scheduler_Get_by_CPU( cpu );
-
-    ok = ok
-      && ( CPU_ISSET_S( (int) cpu_index, cpusetsize, cpuset )
-        || ( !CPU_ISSET_S( (int) cpu_index, cpusetsize, cpuset )
-          && scheduler != scheduler_of_cpu ) );
-#else
-    (void) scheduler;
-
-    ok = ok && CPU_ISSET_S( (int) cpu_index, cpusetsize, cpuset );
-#endif
-  }
-
-  return ok;
+  return _Processor_mask_Is_subset(
+    affinity,
+    _Scheduler_Get_processors( scheduler )
+  );
 }
 
 bool _Scheduler_Set_affinity(
diff --git a/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h b/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h
index fd4a33670f..d53f76b090 100644
--- a/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h
+++ b/cpukit/score/include/rtems/score/schedulerpriorityaffinitysmp.h
@@ -141,8 +141,7 @@ Thread_Control *_Scheduler_priority_affinity_SMP_Remove_processor(
  *
  * @param[in] scheduler The scheduler of the thread.
  * @param[in] thread The associated thread.
- * @param[in] cpusetsize The size of the cpuset.
- * @param[in] cpuset Affinity new affinity set.
+ * @param[in] affinity The new affinity set.
  *
  * @retval true if successful
  * @retval false if unsuccessful
@@ -150,8 +149,7 @@ Thread_Control *_Scheduler_priority_affinity_SMP_Remove_processor(
 bool _Scheduler_priority_affinity_SMP_Set_affinity(
   const Scheduler_Control *scheduler,
   Thread_Control          *thread,
-  size_t                   cpusetsize,
-  const cpu_set_t         *cpuset
+  const Processor_mask    *affinity
 );
 
 /**
diff --git a/cpukit/score/src/schedulerdefaultsetaffinity.c b/cpukit/score/src/schedulerdefaultsetaffinity.c
index e53f8b8349..940d0939dc 100644
--- a/cpukit/score/src/schedulerdefaultsetaffinity.c
+++ b/cpukit/score/src/schedulerdefaultsetaffinity.c
@@ -24,14 +24,12 @@
 bool _Scheduler_default_Set_affinity(
   const Scheduler_Control *scheduler,
   Thread_Control          *thread,
-  size_t                   cpusetsize,
-  const cpu_set_t         *cpuset
+  const Processor_mask    *affinity
 )
 {
   return _Scheduler_default_Set_affinity_body(
     scheduler,
     thread,
-    cpusetsize,
-    cpuset
+    affinity
   );
 }
diff --git a/cpukit/score/src/schedulerpriorityaffinitysmp.c b/cpukit/score/src/schedulerpriorityaffinitysmp.c
index 4533dec9d7..6cc480d5c4 100644
--- a/cpukit/score/src/schedulerpriorityaffinitysmp.c
+++ b/cpukit/score/src/schedulerpriorityaffinitysmp.c
@@ -614,17 +614,20 @@ Thread_Control *_Scheduler_priority_affinity_SMP_Remove_processor(
 bool _Scheduler_priority_affinity_SMP_Set_affinity(
   const Scheduler_Control *scheduler,
   Thread_Control          *thread,
-  size_t                   cpusetsize,
-  const cpu_set_t         *cpuset
+  const Processor_mask    *affinity
 )
 {
   Scheduler_priority_affinity_SMP_Node *node;
   States_Control                        current_state;
+  cpu_set_t                             cpuset;
+  size_t                                cpusetsize;
 
+  cpusetsize = sizeof( cpuset );
+  _Processor_mask_To_cpu_set_t( affinity, cpusetsize, &cpuset );
   /*
    * Validate that the cpset meets basic requirements.
    */
-  if ( !_CPU_set_Is_valid( cpuset, cpusetsize ) ) {
+  if ( !_CPU_set_Is_valid( &cpuset, cpusetsize ) ) {
     return false;
   }
 
@@ -634,7 +637,7 @@ bool _Scheduler_priority_affinity_SMP_Set_affinity(
    * The old and new set are the same, there is no point in
    * doing anything.
    */
-  if ( CPU_EQUAL_S( cpusetsize, cpuset, node->Affinity.set ) )
+  if ( CPU_EQUAL_S( cpusetsize, &cpuset, node->Affinity.set ) )
     return true;
 
   current_state = thread->current_state;
@@ -643,7 +646,7 @@ bool _Scheduler_priority_affinity_SMP_Set_affinity(
     _Scheduler_priority_affinity_SMP_Block( scheduler, thread, &node->Base.Base.Base );
   }
 
-  CPU_COPY( cpuset, node->Affinity.set );
+  CPU_COPY( &cpuset, node->Affinity.set );
 
   if ( _States_Is_ready( current_state ) ) {
     /*
diff --git a/cpukit/score/src/schedulersetaffinity.c b/cpukit/score/src/schedulersetaffinity.c
index 67073dffd1..93ed851b9e 100644
--- a/cpukit/score/src/schedulersetaffinity.c
+++ b/cpukit/score/src/schedulersetaffinity.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2014, 2017 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
  *  Dornierstr. 4
@@ -19,16 +19,19 @@
 #include <rtems/score/schedulerimpl.h>
 
 bool _Scheduler_Set_affinity(
-  Thread_Control  *the_thread,
-  size_t           cpusetsize,
-  const cpu_set_t *cpuset
+  Thread_Control       *the_thread,
+  size_t                cpusetsize,
+  const cpu_set_t      *cpuset
 )
 {
-  const Scheduler_Control *scheduler;
-  ISR_lock_Context         lock_context;
-  bool                     ok;
+  Processor_mask             affinity;
+  Processor_mask_Copy_status status;
+  const Scheduler_Control   *scheduler;
+  ISR_lock_Context           lock_context;
+  bool                       ok;
 
-  if ( !_CPU_set_Is_large_enough( cpusetsize ) ) {
+  status = _Processor_mask_From_cpu_set_t( &affinity, cpusetsize, cpuset );
+  if ( !_Processor_mask_Is_at_most_partial_loss( status ) ) {
     return false;
   }
 
@@ -39,15 +42,17 @@ bool _Scheduler_Set_affinity(
   ok = ( *scheduler->Operations.set_affinity )(
     scheduler,
     the_thread,
-    cpusetsize,
-    cpuset
+    &affinity
   );
+
+  if ( ok ) {
+    _Processor_mask_Assign( &the_thread->Scheduler.Affinity, &affinity );
+  }
 #else
   ok = _Scheduler_default_Set_affinity_body(
     scheduler,
     the_thread,
-    cpusetsize,
-    cpuset
+    &affinity
   );
 #endif