From 4ebdbee81523bbf8ae08d0d8c1346ef472263c52 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Fri, 12 Mar 2021 14:37:10 +0100
Subject: rtems: Allow initially locked MrsP semaphores

Rejecting initially locked MrsP semaphores was due to a limitiation of
the early limitiation of the MrsP protocol.  This limitation no longer
exists.
---
 cpukit/include/rtems/rtems/sem.h      |  3 ---
 cpukit/include/rtems/score/mrspimpl.h | 28 ++++++++++++++++++++++------
 testsuites/smptests/smpmrsp01/init.c  | 14 ++++++++++----
 3 files changed, 32 insertions(+), 13 deletions(-)

diff --git a/cpukit/include/rtems/rtems/sem.h b/cpukit/include/rtems/rtems/sem.h
index 1f0c952294..e117f1c211 100644
--- a/cpukit/include/rtems/rtems/sem.h
+++ b/cpukit/include/rtems/rtems/sem.h
@@ -240,9 +240,6 @@ extern "C" {
  * * When the directive operates on a global object, the directive sends a
  *   message to remote nodes.  This may preempt the calling task.
  *
- * * When a semaphore using the MrsP locking protocol is created, the initial
- *   count shall be exactly one.
- *
  * * The number of semaphores available to the application is configured
  *   through the #CONFIGURE_MAXIMUM_SEMAPHORES application configuration
  *   option.
diff --git a/cpukit/include/rtems/score/mrspimpl.h b/cpukit/include/rtems/score/mrspimpl.h
index 3cd5bcf33c..89711d6b1b 100644
--- a/cpukit/include/rtems/score/mrspimpl.h
+++ b/cpukit/include/rtems/score/mrspimpl.h
@@ -295,12 +295,13 @@ RTEMS_INLINE_ROUTINE Status_Control _MRSP_Initialize(
   bool                     initially_locked
 )
 {
-  uint32_t scheduler_count = _Scheduler_Count;
-  uint32_t i;
+  Thread_queue_Context queue_context;
+  ISR_Level            level;
+  size_t               scheduler_count;
+  size_t               i;
+  Status_Control       status;
 
-  if ( initially_locked ) {
-    return STATUS_INVALID_NUMBER;
-  }
+  scheduler_count = _Scheduler_Count;
 
   for ( i = 0 ; i < scheduler_count ; ++i ) {
     const Scheduler_Control *scheduler_of_index;
@@ -316,7 +317,22 @@ RTEMS_INLINE_ROUTINE Status_Control _MRSP_Initialize(
   }
 
   _Thread_queue_Object_initialize( &mrsp->Wait_queue );
-  return STATUS_SUCCESSFUL;
+
+  if ( !initially_locked ) {
+    return STATUS_SUCCESSFUL;
+  }
+
+  _Thread_queue_Context_initialize( &queue_context );
+  _Thread_queue_Context_ISR_disable( &queue_context, level );
+  _Thread_queue_Context_set_ISR_level( &queue_context, level );
+  _MRSP_Acquire_critical( mrsp, &queue_context );
+  status = _MRSP_Claim_ownership( mrsp, executing, &queue_context );
+
+  if ( status != STATUS_SUCCESSFUL ) {
+    _Thread_queue_Destroy( &mrsp->Wait_queue );
+  }
+
+  return status;
 }
 
 /**
diff --git a/testsuites/smptests/smpmrsp01/init.c b/testsuites/smptests/smpmrsp01/init.c
index 76bb928982..147838957d 100644
--- a/testsuites/smptests/smpmrsp01/init.c
+++ b/testsuites/smptests/smpmrsp01/init.c
@@ -734,12 +734,12 @@ static void test_mrsp_flush_error(test_context *ctx)
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 }
 
-static void test_mrsp_initially_locked_error(void)
+static void test_mrsp_initially_locked(void)
 {
   rtems_status_code sc;
   rtems_id id;
 
-  puts("test MrsP initially locked error");
+  puts("test MrsP initially locked");
 
   sc = rtems_semaphore_create(
     rtems_build_name('M', 'R', 'S', 'P'),
@@ -749,7 +749,13 @@ static void test_mrsp_initially_locked_error(void)
     1,
     &id
   );
-  rtems_test_assert(sc == RTEMS_INVALID_NUMBER);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  sc = rtems_semaphore_release(id);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  sc = rtems_semaphore_delete(id);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 }
 
 static void test_mrsp_nested_obtain_error(test_context *ctx)
@@ -1750,7 +1756,7 @@ static void Init(rtems_task_argument arg)
   }
 
   test_mrsp_flush_error(ctx);
-  test_mrsp_initially_locked_error();
+  test_mrsp_initially_locked();
   test_mrsp_nested_obtain_error(ctx);
   test_mrsp_deadlock_error(ctx);
   test_mrsp_multiple_obtain(ctx);
-- 
cgit v1.2.3