rtems: Rework RTEMS API to SuperCore priority

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

1 files changed, 79 insertions, 22 deletions

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;
 }