1 files changed, 26 insertions, 30 deletions

diff --git a/cpukit/score/src/schedulercbsunblock.c b/cpukit/score/src/schedulercbsunblock.c
index 05e155a4c3..3f5bd33623 100644
--- a/cpukit/score/src/schedulercbsunblock.c
+++ b/cpukit/score/src/schedulercbsunblock.c
@@ -30,12 +30,15 @@ Scheduler_Void_or_thread _Scheduler_CBS_Unblock(
   Thread_Control          *the_thread
 )
 {
-  Scheduler_CBS_Node   *node = _Scheduler_CBS_Thread_get_node( the_thread );
-  Scheduler_CBS_Server *serv_info = node->cbs_server;
-  Priority_Control      new_priority;
+  Scheduler_EDF_Context *context;
+  Scheduler_CBS_Node    *node;
+  Scheduler_CBS_Server  *serv_info;
+  Priority_Control       priority;
 
-  _Scheduler_EDF_Enqueue( scheduler, the_thread );
-  /* TODO: flash critical section? */
+  context = _Scheduler_EDF_Get_context( scheduler );
+  node = _Scheduler_CBS_Thread_get_node( the_thread );
+  serv_info = node->cbs_server;
+  priority = node->Base.current_priority;
 
   /*
    * Late unblock rule for deadline-driven tasks. The remaining time to
@@ -43,29 +46,30 @@ Scheduler_Void_or_thread _Scheduler_CBS_Unblock(
    * without increased utilization of this task. It might cause a deadline
    * miss of another task.
    */
-  if (serv_info) {
+  if ( serv_info != NULL && ( priority & SCHEDULER_EDF_PRIO_MSB ) == 0 ) {
     time_t deadline = serv_info->parameters.deadline;
     time_t budget = serv_info->parameters.budget;
-    time_t deadline_left = the_thread->cpu_time_budget;
-    time_t budget_left = the_thread->real_priority -
-                           _Watchdog_Ticks_since_boot;
+    uint32_t deadline_left = the_thread->cpu_time_budget;
+    Priority_Control budget_left = priority - _Watchdog_Ticks_since_boot;
 
-    if ( deadline*budget_left > budget*deadline_left ) {
+    if ( deadline * budget_left > budget * deadline_left ) {
       /* Put late unblocked task to background until the end of period. */
-      new_priority = the_thread->Start.initial_priority;
-      the_thread->real_priority = new_priority;
-      if ( the_thread->current_priority != new_priority ) {
-        the_thread->current_priority = new_priority;
-        _Scheduler_EDF_Change_priority(
-          scheduler,
-          the_thread,
-          new_priority,
-          true
-        );
+
+      priority = node->Base.background_priority;
+      the_thread->real_priority = priority;
+
+      if (
+        _Thread_Priority_less_than( the_thread->current_priority, priority )
+          || !_Thread_Owns_resources( the_thread )
+      ) {
+        the_thread->current_priority = priority;
       }
     }
   }
 
+  node->Base.current_priority = priority;
+  _Scheduler_EDF_Enqueue( context, &node->Base, priority );
+
   /*
    *  If the thread that was unblocked is more important than the heir,
    *  then we have a new heir.  This may or may not result in a
@@ -78,16 +82,8 @@ Scheduler_Void_or_thread _Scheduler_CBS_Unblock(
    *    Even if the thread isn't preemptible, if the new heir is
    *    a pseudo-ISR system task, we need to do a context switch.
    */
-  if (
-    _Scheduler_EDF_Priority_compare(
-      the_thread->current_priority,
-      _Thread_Heir->current_priority
-    ) == 1 
-  ) {
-    _Scheduler_Update_heir(
-      the_thread,
-      the_thread->current_priority == PRIORITY_PSEUDO_ISR
-    );
+  if ( priority < _Thread_Heir->current_priority ) {
+    _Scheduler_Update_heir( the_thread, priority == PRIORITY_PSEUDO_ISR );
   }
 
   SCHEDULER_RETURN_VOID_OR_NULL;