rtems: Rework RTEMS API to SuperCore priority

Use same structure as POSIX API for thread priority conversion to/from
SuperCore.

5 files changed, 164 insertions, 87 deletions

diff --git a/cpukit/rtems/src/semcreate.c b/cpukit/rtems/src/semcreate.c
index bb1c6b4e61..f03475ee33 100644
--- a/cpukit/rtems/src/semcreate.c
+++ b/cpukit/rtems/src/semcreate.c
@@ -22,6 +22,7 @@
 #include <rtems/rtems/attrimpl.h>
 #include <rtems/rtems/statusimpl.h>
 #include <rtems/rtems/tasksimpl.h>
+#include <rtems/score/schedulerimpl.h>
 #include <rtems/score/sysstate.h>
 
 #define SEMAPHORE_KIND_MASK ( RTEMS_SEMAPHORE_CLASS | RTEMS_INHERIT_PRIORITY \
@@ -35,12 +36,15 @@ rtems_status_code rtems_semaphore_create(
   rtems_id            *id
 )
 {
-  Semaphore_Control *the_semaphore;
-  Thread_Control    *executing;
-  Status_Control     status;
-  rtems_attribute    maybe_global;
-  rtems_attribute    mutex_with_protocol;
-  Semaphore_Variant  variant;
+  Semaphore_Control       *the_semaphore;
+  Thread_Control          *executing;
+  Status_Control           status;
+  rtems_attribute          maybe_global;
+  rtems_attribute          mutex_with_protocol;
+  Semaphore_Variant        variant;
+  const Scheduler_Control *scheduler;
+  bool                     valid;
+  Priority_Control         priority;
 
   if ( !rtems_is_name_valid( name ) )
     return RTEMS_INVALID_NAME;
@@ -122,7 +126,6 @@ rtems_status_code rtems_semaphore_create(
   }
 #endif
 
-  priority_ceiling = _RTEMS_tasks_Priority_to_Core( priority_ceiling );
   executing = _Thread_Get_executing();
 
   the_semaphore->variant = variant;
@@ -154,42 +157,57 @@ rtems_status_code rtems_semaphore_create(
       status = STATUS_SUCCESSFUL;
       break;
     case SEMAPHORE_VARIANT_MUTEX_PRIORITY_CEILING:
-      _CORE_ceiling_mutex_Initialize(
-        &the_semaphore->Core_control.Mutex,
-        priority_ceiling
-      );
-
-      if ( count == 0 ) {
-        Thread_queue_Context queue_context;
+      scheduler = _Scheduler_Get_own( executing );
+      priority = _RTEMS_Priority_To_core( scheduler, priority_ceiling, &valid );
 
-        _Thread_queue_Context_initialize( &queue_context );
-        _ISR_lock_ISR_disable( &queue_context.Lock_context );
-        _CORE_mutex_Acquire_critical(
-          &the_semaphore->Core_control.Mutex.Recursive.Mutex,
-          &queue_context
-        );
-        status = _CORE_ceiling_mutex_Set_owner(
+      if ( valid ) {
+        _CORE_ceiling_mutex_Initialize(
           &the_semaphore->Core_control.Mutex,
-          executing,
-          &queue_context
+          priority
         );
 
-        if ( status != STATUS_SUCCESSFUL ) {
-          _Thread_queue_Destroy( &the_semaphore->Core_control.Wait_queue );
+        if ( count == 0 ) {
+          Thread_queue_Context queue_context;
+
+          _Thread_queue_Context_initialize( &queue_context );
+          _ISR_lock_ISR_disable( &queue_context.Lock_context );
+          _CORE_mutex_Acquire_critical(
+            &the_semaphore->Core_control.Mutex.Recursive.Mutex,
+            &queue_context
+          );
+          status = _CORE_ceiling_mutex_Set_owner(
+            &the_semaphore->Core_control.Mutex,
+            executing,
+            &queue_context
+          );
+
+          if ( status != STATUS_SUCCESSFUL ) {
+            _Thread_queue_Destroy( &the_semaphore->Core_control.Wait_queue );
+          }
+        } else {
+          status = STATUS_SUCCESSFUL;
         }
       } else {
-        status = STATUS_SUCCESSFUL;
+        status = STATUS_INVALID_PRIORITY;
       }
 
       break;
 #if defined(RTEMS_SMP)
     case SEMAPHORE_VARIANT_MRSP:
-      status = _MRSP_Initialize(
-        &the_semaphore->Core_control.MRSP,
-        priority_ceiling,
-        executing,
-        count == 0
-      );
+      scheduler = _Scheduler_Get_own( executing );
+      priority = _RTEMS_Priority_To_core( scheduler, priority_ceiling, &valid );
+
+      if ( valid ) {
+        status = _MRSP_Initialize(
+          &the_semaphore->Core_control.MRSP,
+          priority,
+          executing,
+          count == 0
+        );
+      } else {
+        status = STATUS_INVALID_PRIORITY;
+      }
+
       break;
 #endif
     default:
diff --git a/cpukit/rtems/src/semsetpriority.c b/cpukit/rtems/src/semsetpriority.c
index 455cc15717..37d7a207ca 100644
--- a/cpukit/rtems/src/semsetpriority.c
+++ b/cpukit/rtems/src/semsetpriority.c
@@ -21,21 +21,26 @@
 #include <rtems/score/schedulerimpl.h>
 
 static rtems_status_code _Semaphore_Set_priority(
-  Semaphore_Control    *the_semaphore,
-  rtems_id              scheduler_id,
-  rtems_task_priority   new_priority,
-  rtems_task_priority  *old_priority_p,
-  Thread_queue_Context *queue_context
+  Semaphore_Control       *the_semaphore,
+  const Scheduler_Control *scheduler,
+  rtems_task_priority      new_priority,
+  rtems_task_priority     *old_priority_p,
+  Thread_queue_Context    *queue_context
 )
 {
   rtems_status_code    sc;
-  rtems_task_priority  old_priority;
+  bool                 valid;
+  Priority_Control     core_priority;
+  Priority_Control     old_priority;
 #if defined(RTEMS_SMP)
   MRSP_Control        *mrsp;
   uint32_t             scheduler_index;
 #endif
 
-  new_priority = _RTEMS_tasks_Priority_to_Core( new_priority );
+  core_priority = _RTEMS_Priority_To_core( scheduler, new_priority, &valid );
+  if ( new_priority != RTEMS_CURRENT_PRIORITY && !valid ) {
+    return RTEMS_INVALID_PRIORITY;
+  }
 
   switch ( the_semaphore->variant ) {
     case SEMAPHORE_VARIANT_MUTEX_PRIORITY_CEILING:
@@ -47,7 +52,7 @@ static rtems_status_code _Semaphore_Set_priority(
       old_priority = the_semaphore->Core_control.Mutex.priority_ceiling;
 
       if ( new_priority != RTEMS_CURRENT_PRIORITY ) {
-        the_semaphore->Core_control.Mutex.priority_ceiling = new_priority;
+        the_semaphore->Core_control.Mutex.priority_ceiling = core_priority;
       }
 
       _CORE_mutex_Release(
@@ -59,14 +64,14 @@ static rtems_status_code _Semaphore_Set_priority(
 #if defined(RTEMS_SMP)
     case SEMAPHORE_VARIANT_MRSP:
       mrsp = &the_semaphore->Core_control.MRSP;
-      scheduler_index = _Scheduler_Get_index_by_id( scheduler_id );
+      scheduler_index = _Scheduler_Get_index( scheduler );
 
       _MRSP_Acquire_critical( mrsp, queue_context );
 
       old_priority = _MRSP_Get_ceiling_priority( mrsp, scheduler_index );
 
       if ( new_priority != RTEMS_CURRENT_PRIORITY ) {
-        _MRSP_Set_ceiling_priority( mrsp, scheduler_index, new_priority );
+        _MRSP_Set_ceiling_priority( mrsp, scheduler_index, core_priority );
       }
 
       _MRSP_Release( mrsp, queue_context );
@@ -86,7 +91,7 @@ static rtems_status_code _Semaphore_Set_priority(
       break;
   }
 
-  *old_priority_p = _RTEMS_tasks_Priority_from_Core( old_priority );
+  *old_priority_p = _RTEMS_Priority_From_core( scheduler, old_priority );
 
   return sc;
 }
@@ -98,19 +103,15 @@ rtems_status_code rtems_semaphore_set_priority(
   rtems_task_priority *old_priority
 )
 {
-  Semaphore_Control    *the_semaphore;
-  Thread_queue_Context  queue_context;
-
-  if ( new_priority != RTEMS_CURRENT_PRIORITY &&
-       !_RTEMS_tasks_Priority_is_valid( new_priority ) ) {
-    return RTEMS_INVALID_PRIORITY;
-  }
+  const Scheduler_Control *scheduler;
+  Semaphore_Control       *the_semaphore;
+  Thread_queue_Context     queue_context;
 
   if ( old_priority == NULL ) {
     return RTEMS_INVALID_ADDRESS;
   }
 
-  if ( !_Scheduler_Is_id_valid( scheduler_id ) ) {
+  if ( !_Scheduler_Get_by_id( scheduler_id, &scheduler ) ) {
     return RTEMS_INVALID_ID;
   }
 
@@ -128,7 +129,7 @@ rtems_status_code rtems_semaphore_set_priority(
 
   return _Semaphore_Set_priority(
     the_semaphore,
-    scheduler_id,
+    scheduler,
     new_priority,
     old_priority,
     &queue_context
diff --git a/cpukit/rtems/src/taskcreate.c b/cpukit/rtems/src/taskcreate.c
index 5d9f9dda91..e231d4a0c3 100644
--- a/cpukit/rtems/src/taskcreate.c
+++ b/cpukit/rtems/src/taskcreate.c
@@ -37,6 +37,7 @@ rtems_status_code rtems_task_create(
 )
 {
   Thread_Control          *the_thread;
+  const Scheduler_Control *scheduler;
   bool                     is_fp;
 #if defined(RTEMS_MULTIPROCESSING)
   Objects_MP_Control      *the_global_object = NULL;
@@ -44,7 +45,8 @@ rtems_status_code rtems_task_create(
 #endif
   bool                     status;
   rtems_attribute          the_attribute_set;
-  Priority_Control         core_priority;
+  bool                     valid;
+  Priority_Control         priority;
   RTEMS_API_Control       *api;
   ASR_Information         *asr;
 
@@ -83,11 +85,17 @@ rtems_status_code rtems_task_create(
    */
 
   if ( !_Attributes_Is_system_task( the_attribute_set ) ) {
-    if ( !_RTEMS_tasks_Priority_is_valid( initial_priority ) )
+    if ( initial_priority == PRIORITY_MINIMUM ) {
       return RTEMS_INVALID_PRIORITY;
+    }
   }
 
-  core_priority = _RTEMS_tasks_Priority_to_Core( initial_priority );
+  scheduler = _Scheduler_Get_by_CPU_index( _SMP_Get_current_processor() );
+
+  priority = _RTEMS_Priority_To_core( scheduler, initial_priority, &valid );
+  if ( !valid ) {
+    return RTEMS_INVALID_PRIORITY;
+  }
 
 #if defined(RTEMS_MULTIPROCESSING)
   if ( _Attributes_Is_global( the_attribute_set ) ) {
@@ -136,11 +144,11 @@ rtems_status_code rtems_task_create(
   status = _Thread_Initialize(
     &_RTEMS_tasks_Information,
     the_thread,
-    _Scheduler_Get_by_CPU_index( _SMP_Get_current_processor() ),
+    scheduler,
     NULL,
     stack_size,
     is_fp,
-    core_priority,
+    priority,
     _Modes_Is_preempt(initial_modes)   ? true : false,
     _Modes_Is_timeslice(initial_modes) ?
       THREAD_CPU_BUDGET_ALGORITHM_RESET_TIMESLICE :
diff --git a/cpukit/rtems/src/tasksetpriority.c b/cpukit/rtems/src/tasksetpriority.c
index 32a77f77ba..0a2df6e1ca 100644
--- a/cpukit/rtems/src/tasksetpriority.c
+++ b/cpukit/rtems/src/tasksetpriority.c
@@ -19,52 +19,109 @@
 #endif
 
 #include <rtems/rtems/tasksimpl.h>
+#include <rtems/score/schedulerimpl.h>
 #include <rtems/score/threadimpl.h>
 
+typedef struct {
+  const Scheduler_Control *scheduler;
+  rtems_task_priority      new_priority;
+  Priority_Control         old_priority;
+  rtems_status_code        status;
+} RTEMS_tasks_Set_priority_context;
+
+static bool _RTEMS_tasks_Set_priority_filter(
+  Thread_Control   *the_thread,
+  Priority_Control *new_priority_p,
+  void             *arg
+)
+{
+  RTEMS_tasks_Set_priority_context *context;
+  const Scheduler_Control          *scheduler;
+  bool                              valid;
+  Priority_Control                  current_priority;
+  Priority_Control                  new_priority;
+
+  context = arg;
+  scheduler = _Scheduler_Get_own( the_thread );
+  current_priority = the_thread->current_priority;
+
+  context->scheduler = scheduler;
+  context->old_priority = current_priority;
+
+  new_priority = _RTEMS_Priority_To_core(
+    scheduler,
+    context->new_priority,
+    &valid
+  );
+
+  *new_priority_p = new_priority;
+
+  if ( !valid ) {
+    context->status = RTEMS_INVALID_PRIORITY;
+    return false;
+  }
+
+  the_thread->real_priority = new_priority;
+  context->status = STATUS_SUCCESSFUL;
+
+  return _Thread_Priority_less_than( current_priority, new_priority )
+    || !_Thread_Owns_resources( the_thread );
+}
+
 rtems_status_code rtems_task_set_priority(
   rtems_id             id,
   rtems_task_priority  new_priority,
-  rtems_task_priority *old_priority
+  rtems_task_priority *old_priority_p
 )
 {
-  Thread_Control   *the_thread;
-  ISR_lock_Context  lock_context;
-  Per_CPU_Control  *cpu_self;
+  Thread_Control          *the_thread;
+  ISR_lock_Context         lock_context;
+  const Scheduler_Control *scheduler;
+  Priority_Control         old_priority;
+  rtems_status_code        status;
 
-  if ( new_priority != RTEMS_CURRENT_PRIORITY &&
-       !_RTEMS_tasks_Priority_is_valid( new_priority ) )
-    return RTEMS_INVALID_PRIORITY;
-
-  if ( !old_priority )
+  if ( old_priority_p == NULL ) {
     return RTEMS_INVALID_ADDRESS;
+  }
 
   the_thread = _Thread_Get( id, &lock_context );
 
   if ( the_thread == NULL ) {
 #if defined(RTEMS_MULTIPROCESSING)
-    return _RTEMS_tasks_MP_Set_priority( id, new_priority, old_priority );
+    return _RTEMS_tasks_MP_Set_priority( id, new_priority, old_priority_p );
 #else
     return RTEMS_INVALID_ID;
 #endif
   }
 
-  cpu_self = _Thread_Dispatch_disable_critical( &lock_context );
-  _ISR_lock_ISR_enable( &lock_context );
-
   if ( new_priority != RTEMS_CURRENT_PRIORITY ) {
-    _Thread_Set_priority(
+    RTEMS_tasks_Set_priority_context  context;
+    Per_CPU_Control                  *cpu_self;
+
+    cpu_self = _Thread_Dispatch_disable_critical( &lock_context );
+    _ISR_lock_ISR_enable( &lock_context );
+
+    context.new_priority = new_priority;
+    _Thread_Change_priority(
       the_thread,
-      _RTEMS_tasks_Priority_to_Core( new_priority ),
-      old_priority,
+      0,
+      &context,
+      _RTEMS_tasks_Set_priority_filter,
       false
     );
-    *old_priority = _RTEMS_tasks_Priority_from_Core( *old_priority );
+
+    _Thread_Dispatch_enable( cpu_self );
+    scheduler = context.scheduler;
+    old_priority = context.old_priority;
+    status = context.status;
   } else {
-    *old_priority = _RTEMS_tasks_Priority_from_Core(
-      the_thread->current_priority
-    );
+    _Thread_State_acquire_critical( the_thread, &lock_context );
+    scheduler = _Scheduler_Get_own( the_thread );
+    old_priority = the_thread->current_priority;
+    _Thread_State_release( the_thread, &lock_context );
+    status = RTEMS_SUCCESSFUL;
   }
 
-  _Thread_Dispatch_enable( cpu_self );
-  return RTEMS_SUCCESSFUL;
+  *old_priority_p = _RTEMS_Priority_From_core( scheduler, old_priority );
+  return status;
 }
diff --git a/cpukit/rtems/src/timerserver.c b/cpukit/rtems/src/timerserver.c
index cf06319434..dfdba8c2e2 100644
--- a/cpukit/rtems/src/timerserver.c
+++ b/cpukit/rtems/src/timerserver.c
@@ -153,14 +153,7 @@ static rtems_status_code _Timer_server_Initiate(
     return RTEMS_INCORRECT_STATE;
   }
 
-  /*
-   *  Make sure the requested priority is valid.  The if is
-   *  structured so we check it is invalid before looking for
-   *  a specific invalid value as the default.
-   */
-  if ( !_RTEMS_tasks_Priority_is_valid( priority ) ) {
-    if ( priority != RTEMS_TIMER_SERVER_DEFAULT_PRIORITY )
-      return RTEMS_INVALID_PRIORITY;
+  if ( priority == RTEMS_TIMER_SERVER_DEFAULT_PRIORITY ) {
     priority = PRIORITY_PSEUDO_ISR;
   }