score: Task get/set affinity

Make rtems_task_get_affinity() and rtems_task_set_affinity() available
on non-SMP configurations.  Allow larger CPU sets.

7 files changed, 215 insertions, 59 deletions

diff --git a/cpukit/score/Makefile.am b/cpukit/score/Makefile.am
index 4cd89d792a..ae925507d1 100644
--- a/cpukit/score/Makefile.am
+++ b/cpukit/score/Makefile.am
@@ -195,6 +195,8 @@ libscore_a_SOURCES += src/objectallocate.c src/objectclose.c \
 ## SCHEDULER_C_FILES
 libscore_a_SOURCES += src/log2table.c
 libscore_a_SOURCES += src/scheduler.c
+libscore_a_SOURCES += src/schedulergetaffinity.c
+libscore_a_SOURCES += src/schedulersetaffinity.c
 libscore_a_SOURCES += src/schedulerdefaultallocatefree.c
 libscore_a_SOURCES += src/schedulerdefaultreleasejob.c
 libscore_a_SOURCES += src/schedulerdefaultstartidle.c
diff --git a/cpukit/score/include/rtems/score/cpusetimpl.h b/cpukit/score/include/rtems/score/cpusetimpl.h
index 06fe3f5a8e..8ae240847b 100644
--- a/cpukit/score/include/rtems/score/cpusetimpl.h
+++ b/cpukit/score/include/rtems/score/cpusetimpl.h
@@ -20,6 +20,9 @@
 #define _RTEMS_SCORE_CPUSETIMPL_H
 
 #include <rtems/score/cpuset.h>
+#include <rtems/score/smp.h>
+
+#include <limits.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -58,6 +61,21 @@ void _CPU_set_Show_default( const char *description );
  */
 const CPU_set_Control *_CPU_set_Default(void);
 
+RTEMS_INLINE_ROUTINE size_t _CPU_set_Maximum_CPU_count(
+  size_t cpusetsize
+)
+{
+  return cpusetsize * CHAR_BIT;
+}
+
+RTEMS_INLINE_ROUTINE bool _CPU_set_Is_large_enough(
+  size_t cpusetsize
+)
+{
+  return _CPU_set_Maximum_CPU_count( cpusetsize )
+    >= _SMP_Get_processor_count();
+}
+
 #endif
 
 /**
diff --git a/cpukit/score/include/rtems/score/schedulerimpl.h b/cpukit/score/include/rtems/score/schedulerimpl.h
index 5c787239d0..e088d26b88 100644
--- a/cpukit/score/include/rtems/score/schedulerimpl.h
+++ b/cpukit/score/include/rtems/score/schedulerimpl.h
@@ -20,6 +20,7 @@
 #define _RTEMS_SCORE_SCHEDULERIMPL_H
 
 #include <rtems/score/scheduler.h>
+#include <rtems/score/cpusetimpl.h>
 #include <rtems/score/threadimpl.h>
 
 #ifdef __cplusplus
@@ -257,49 +258,81 @@ RTEMS_INLINE_ROUTINE void _Scheduler_Start_idle(
   ( *scheduler->Operations.start_idle )( scheduler, the_thread, cpu );
 }
 
-#if defined(__RTEMS_HAVE_SYS_CPUSET_H__) && defined(RTEMS_SMP)
-  /**
-   * @brief Obtain the processor affinity for a thread.
-   *
-   * @param[in,out] thread The thread.
-   * @parma[out] cpuset The processor affinity for this thread
-   */
-  RTEMS_INLINE_ROUTINE int _Scheduler_Get_affinity(
-    const Scheduler_Control *scheduler,
-    Thread_Control          *the_thread,
-    size_t                   cpusetsize,
-    cpu_set_t               *cpuset
-  )
-  {
-    return ( *scheduler->Operations.get_affinity )(
-      scheduler,
-      the_thread,
-      cpusetsize,
-      cpuset
-    );
+#if defined(__RTEMS_HAVE_SYS_CPUSET_H__)
+
+RTEMS_INLINE_ROUTINE void _Scheduler_Get_processor_set(
+  const Scheduler_Control *scheduler,
+  size_t                   cpusetsize,
+  cpu_set_t               *cpuset
+)
+{
+  uint32_t cpu_count = _SMP_Get_processor_count();
+  uint32_t cpu_index;
+
+  (void) scheduler;
+
+  CPU_ZERO_S( cpusetsize, cpuset );
+
+  for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
+    CPU_SET_S( (int) cpu_index, cpusetsize, cpuset );
+  }
+}
+
+RTEMS_INLINE_ROUTINE bool _Scheduler_default_Get_affinity_body(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  size_t                   cpusetsize,
+  cpu_set_t               *cpuset
+)
+{
+  (void) the_thread;
+
+  _Scheduler_Get_processor_set( scheduler, cpusetsize, cpuset );
+
+  return true;
+}
+
+bool _Scheduler_Get_affinity(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  size_t                   cpusetsize,
+  cpu_set_t               *cpuset
+);
+
+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
+)
+{
+  size_t   cpu_max   = _CPU_set_Maximum_CPU_count( cpusetsize );
+  uint32_t cpu_count = _SMP_Get_processor_count();
+  uint32_t cpu_index;
+  bool     ok = true;
+
+  (void) scheduler;
+  (void) the_thread;
+
+  for ( cpu_index = 0 ; cpu_index < cpu_count ; ++cpu_index ) {
+    ok = ok && CPU_ISSET_S( (int) cpu_index, cpusetsize, cpuset );
   }
 
-  /**
-   * @brief Set the processor affinity for a thread.
-   *
-   * @param[in,out] thread The thread.
-   * @parma[in] cpuset The processor affinity for this thread
-   */
-  RTEMS_INLINE_ROUTINE int _Scheduler_Set_affinity(
-    const Scheduler_Control *scheduler,
-    Thread_Control          *the_thread,
-    size_t                   cpusetsize,
-    const cpu_set_t         *cpuset
-  )
-  {
-    return ( *scheduler->Operations.set_affinity )(
-      scheduler,
-      the_thread,
-      cpusetsize,
-      cpuset
-    );
+  for ( ; cpu_index < cpu_max ; ++cpu_index ) {
+    ok = ok && !CPU_ISSET_S( (int) cpu_index, cpusetsize, cpuset );
   }
-#endif
+
+  return ok;
+}
+
+bool _Scheduler_Set_affinity(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  size_t                   cpusetsize,
+  const cpu_set_t         *cpuset
+);
+
+#endif /* defined(__RTEMS_HAVE_SYS_CPUSET_H__) */
 
 RTEMS_INLINE_ROUTINE void _Scheduler_Update_heir(
   Thread_Control *heir,
diff --git a/cpukit/score/src/schedulerdefaultgetaffinity.c b/cpukit/score/src/schedulerdefaultgetaffinity.c
index 367fc25293..56a4ee4fad 100644
--- a/cpukit/score/src/schedulerdefaultgetaffinity.c
+++ b/cpukit/score/src/schedulerdefaultgetaffinity.c
@@ -20,7 +20,6 @@
 #endif
 
 #include <rtems/score/schedulerimpl.h>
-#include <rtems/score/cpusetimpl.h>
 
 bool _Scheduler_default_Get_affinity(
   const Scheduler_Control *scheduler,
@@ -29,17 +28,10 @@ bool _Scheduler_default_Get_affinity(
   cpu_set_t               *cpuset
 )
 {
-  const CPU_set_Control *ctl;
-
-  (void) scheduler;
-  (void) thread;
-
-  ctl = _CPU_set_Default();
-  if ( cpusetsize != ctl->setsize ) {
-    return false;
-  }
-
-  CPU_COPY( cpuset, ctl->set );
-
-  return true;
+  return _Scheduler_default_Get_affinity_body(
+    scheduler,
+    thread,
+    cpusetsize,
+    cpuset
+  );
 }
diff --git a/cpukit/score/src/schedulerdefaultsetaffinity.c b/cpukit/score/src/schedulerdefaultsetaffinity.c
index 33be12b286..e53f8b8349 100644
--- a/cpukit/score/src/schedulerdefaultsetaffinity.c
+++ b/cpukit/score/src/schedulerdefaultsetaffinity.c
@@ -20,7 +20,6 @@
 #endif
 
 #include <rtems/score/schedulerimpl.h>
-#include <rtems/score/cpusetimpl.h>
 
 bool _Scheduler_default_Set_affinity(
   const Scheduler_Control *scheduler,
@@ -29,8 +28,10 @@ bool _Scheduler_default_Set_affinity(
   const cpu_set_t         *cpuset
 )
 {
-  (void) scheduler;
-  (void) thread;
-
-  return _CPU_set_Is_valid( cpuset, cpusetsize );
+  return _Scheduler_default_Set_affinity_body(
+    scheduler,
+    thread,
+    cpusetsize,
+    cpuset
+  );
 }
diff --git a/cpukit/score/src/schedulergetaffinity.c b/cpukit/score/src/schedulergetaffinity.c
new file mode 100644
index 0000000000..d9e62b5650
--- /dev/null
+++ b/cpukit/score/src/schedulergetaffinity.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+  #include "config.h"
+#endif
+
+#include <rtems/score/schedulerimpl.h>
+
+#if defined(__RTEMS_HAVE_SYS_CPUSET_H__)
+
+bool _Scheduler_Get_affinity(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  size_t                   cpusetsize,
+  cpu_set_t               *cpuset
+)
+{
+  bool ok;
+
+  if ( _CPU_set_Is_large_enough( cpusetsize ) ) {
+#if defined(RTEMS_SMP)
+    ok = ( *scheduler->Operations.get_affinity )(
+      scheduler,
+      the_thread,
+      cpusetsize,
+      cpuset
+    );
+#else
+    ok = _Scheduler_default_Get_affinity_body(
+      scheduler,
+      the_thread,
+      cpusetsize,
+      cpuset
+    );
+#endif
+  } else {
+    ok = false;
+  }
+
+  return ok;
+}
+
+#endif /* defined(__RTEMS_HAVE_SYS_CPUSET_H__) */
diff --git a/cpukit/score/src/schedulersetaffinity.c b/cpukit/score/src/schedulersetaffinity.c
new file mode 100644
index 0000000000..2416a195c5
--- /dev/null
+++ b/cpukit/score/src/schedulersetaffinity.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.org/license/LICENSE.
+ */
+
+#if HAVE_CONFIG_H
+  #include "config.h"
+#endif
+
+#include <rtems/score/schedulerimpl.h>
+
+#if defined(__RTEMS_HAVE_SYS_CPUSET_H__)
+
+bool _Scheduler_Set_affinity(
+  const Scheduler_Control *scheduler,
+  Thread_Control          *the_thread,
+  size_t                   cpusetsize,
+  const cpu_set_t         *cpuset
+)
+{
+  bool ok;
+
+  if ( _CPU_set_Is_large_enough( cpusetsize ) ) {
+#if defined(RTEMS_SMP)
+    ok = ( *scheduler->Operations.set_affinity )(
+      scheduler,
+      the_thread,
+      cpusetsize,
+      cpuset
+    );
+#else
+    ok = _Scheduler_default_Set_affinity_body(
+      scheduler,
+      the_thread,
+      cpusetsize,
+      cpuset
+    );
+#endif
+  } else {
+    ok = false;
+  }
+
+  return ok;
+}
+
+#endif /* defined(__RTEMS_HAVE_SYS_CPUSET_H__) */