[PATCH v2] score: PR2179: Fix initially locked PCP mutexes

[Sebastian Huber]

Elevate the priority of the creating task to the ceiling priority in
case a semaphore is created as initially locked.
---
 cpukit/score/src/coremutex.c     |   19 ++++++++++++++++---
 testsuites/sptests/sp65/init.c   |   35 +++++++++++++++++++++++++++++++++++
 testsuites/sptests/sp65/sp65.doc |    2 ++
 3 files changed, 53 insertions(+), 3 deletions(-)

diff --git a/cpukit/score/src/coremutex.c b/cpukit/score/src/coremutex.c
index 96b11c9..e13c7aa 100644
--- a/cpukit/score/src/coremutex.c
+++ b/cpukit/score/src/coremutex.c
@@ -43,10 +43,20 @@ CORE_mutex_Status _CORE_mutex_Initialize(
     the_mutex->holder     = executing;
     if ( _CORE_mutex_Is_inherit_priority( &the_mutex->Attributes ) ||
          _CORE_mutex_Is_priority_ceiling( &the_mutex->Attributes ) ) {
+      Priority_Control ceiling = the_mutex->Attributes.priority_ceiling;
+
+      /*
+       * The mutex initialization is only protected by the allocator lock in
+       * general.  Disable thread dispatching before the priority check to
+       * prevent interference with priority inheritance.
+       */
+      _Thread_Disable_dispatch();
+
+      if ( executing->current_priority < ceiling ) {
+        _Thread_Enable_dispatch();
+        return CORE_MUTEX_STATUS_CEILING_VIOLATED;
+      }
 
-      if ( executing->current_priority <
-             the_mutex->Attributes.priority_ceiling )
-       return CORE_MUTEX_STATUS_CEILING_VIOLATED;
 #ifdef __RTEMS_STRICT_ORDER_MUTEX__
        _Chain_Prepend_unprotected( &executing->lock_mutex,
                                    &the_mutex->queue.lock_queue );
@@ -54,6 +64,9 @@ CORE_mutex_Status _CORE_mutex_Initialize(
 #endif
 
       executing->resource_count++;
+
+      _Thread_Change_priority( executing, ceiling, false );
+      _Thread_Enable_dispatch();
     }
   } else {
     the_mutex->nest_count = 0;
diff --git a/testsuites/sptests/sp65/init.c b/testsuites/sptests/sp65/init.c
index cba836d..7c6d8d3 100644
--- a/testsuites/sptests/sp65/init.c
+++ b/testsuites/sptests/sp65/init.c
@@ -28,6 +28,17 @@ rtems_task Task_1(rtems_task_argument arg);
   #define TASK_PRIORITY            1
 #endif
 
+static void assert_priority(rtems_task_priority expected)
+{
+  rtems_status_code sc;
+  rtems_task_priority prio;
+
+  sc = rtems_task_set_priority(RTEMS_SELF, RTEMS_CURRENT_PRIORITY, &prio);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  rtems_test_assert(prio == expected);
+}
+
 rtems_task Init(
   rtems_task_argument ignored
 )
@@ -38,6 +49,30 @@ rtems_task Init(
   TEST_BEGIN();
 
   /*
+   * Verify that an initially locked priority ceiling mutex elevates the
+   * priority of the creating task.
+   */
+
+  status = rtems_semaphore_create(
+    rtems_build_name( 's','e','m','1' ),
+    0,
+    RTEMS_BINARY_SEMAPHORE | RTEMS_PRIORITY | RTEMS_PRIORITY_CEILING,
+    1,
+    &Mutex_id
+  );
+  rtems_test_assert(status == RTEMS_SUCCESSFUL);
+
+  assert_priority(1);
+
+  status = rtems_semaphore_release(Mutex_id);
+  rtems_test_assert(status == RTEMS_SUCCESSFUL);
+
+  assert_priority(TASK_PRIORITY);
+
+  status = rtems_semaphore_delete(Mutex_id);
+  rtems_test_assert(status == RTEMS_SUCCESSFUL);
+
+  /*
    *  Create binary semaphore (a.k.a. Mutex) with Priority Ceiling
    *  attribute.
    */
diff --git a/testsuites/sptests/sp65/sp65.doc b/testsuites/sptests/sp65/sp65.doc
index 73786ee..854dede 100644
--- a/testsuites/sptests/sp65/sp65.doc
+++ b/testsuites/sptests/sp65/sp65.doc
@@ -23,3 +23,5 @@ concepts:
 + Verify the priority ceiling mutex getting released which results in a thread
   being unblocked and the unblocked thread's priority being elevated.
 
++ Verify that an initially locked priority ceiling mutex elevates the priority
+  of the creating task.
-- 
1.7.7