From 4edf348c3395560bcaaf3c020968e427f5683d28 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Thu, 22 May 2014 09:31:13 +0200
Subject: score: PR2179: Fix initially locked PCP mutexes

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 96b11c9cd1..e13c7aafac 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 cba836d422..7c6d8d3bef 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
 )
@@ -37,6 +48,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 73786eefc9..854dedeb07 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.
-- 
cgit v1.2.3