smp: Fix timeout for MrsP semaphores

The previous timeout handling was flawed.  In case a waiting thread
helped out the owner could use the scheduler node indefinitely long.
Update the resource tree in _MRSP_Timeout() to avoid this issue.

Bug reported by Luca Bonato.

4 files changed, 184 insertions, 92 deletions

diff --git a/cpukit/score/include/rtems/score/mrsp.h b/cpukit/score/include/rtems/score/mrsp.h
index c31d5f6f19..9eb2887766 100644
--- a/cpukit/score/include/rtems/score/mrsp.h
+++ b/cpukit/score/include/rtems/score/mrsp.h
@@ -19,8 +19,8 @@
 
 #if defined(RTEMS_SMP)
 
-#include <rtems/score/atomic.h>
 #include <rtems/score/chain.h>
+#include <rtems/score/scheduler.h>
 #include <rtems/score/thread.h>
 
 #ifdef __cplusplus
@@ -66,7 +66,13 @@ typedef enum {
   MRSP_INCORRECT_STATE = 14,
   MRSP_INVALID_PRIORITY = 19,
   MRSP_NOT_OWNER_OF_RESOURCE = 23,
-  MRSP_NO_MEMORY = 26
+  MRSP_NO_MEMORY = 26,
+
+  /**
+   * @brief Internal state used for MRSP_Rival::status to indicate that this
+   * rival waits for resource ownership.
+   */
+  MRSP_WAIT_FOR_OWNERSHIP = 255
 } MRSP_Status;
 
 /**
@@ -79,6 +85,9 @@ typedef struct {
   /**
    * @brief The node for registration in the MRSP rival chain.
    *
+   * The chain operations are protected by the Giant lock and disabled
+   * interrupts.
+   *
    * @see MRSP_Control::Rivals.
    */
   Chain_Node Node;
@@ -89,13 +98,30 @@ typedef struct {
   Thread_Control *thread;
 
   /**
-   * @brief The rival state.
+   * @brief The initial priority of the thread at the begin of the resource
+   * obtain sequence.
+   *
+   * Used to restore the priority after a release of this resource or timeout.
+   */
+  Priority_Control initial_priority;
+
+  /**
+   * @brief The initial help state of the thread at the begin of the resource
+   * obtain sequence.
+   *
+   * Used to restore this state after a timeout.
+   */
+  Scheduler_Help_state initial_help_state;
+
+  /**
+   * @brief The rival status.
    *
-   * Initially no state bits are set (MRSP_RIVAL_STATE_WAITING).  The rival
-   * will busy wait until a state change happens.  This can be
-   * MRSP_RIVAL_STATE_NEW_OWNER or MRSP_RIVAL_STATE_TIMEOUT.
+   * Initially the status is set to MRSP_WAIT_FOR_OWNERSHIP.  The rival will
+   * busy wait until a status change happens.  This can be MRSP_SUCCESSFUL or
+   * MRSP_TIMEOUT.  State changes are protected by the Giant lock and disabled
+   * interrupts.
    */
-  Atomic_Uint state;
+  volatile MRSP_Status status;
 } MRSP_Rival;
 
 /**
diff --git a/cpukit/score/include/rtems/score/mrspimpl.h b/cpukit/score/include/rtems/score/mrspimpl.h
index 1571594b8d..dc89b69b39 100644
--- a/cpukit/score/include/rtems/score/mrspimpl.h
+++ b/cpukit/score/include/rtems/score/mrspimpl.h
@@ -36,12 +36,6 @@ extern "C" {
  * @{
  */
 
-#define MRSP_RIVAL_STATE_WAITING 0x0U
-
-#define MRSP_RIVAL_STATE_NEW_OWNER 0x1U
-
-#define MRSP_RIVAL_STATE_TIMEOUT 0x2U
-
 RTEMS_INLINE_ROUTINE void _MRSP_Elevate_priority(
   MRSP_Control     *mrsp,
   Thread_Control   *new_owner,
@@ -52,9 +46,8 @@ RTEMS_INLINE_ROUTINE void _MRSP_Elevate_priority(
 }
 
 RTEMS_INLINE_ROUTINE void _MRSP_Restore_priority(
-  const MRSP_Control *mrsp,
-  Thread_Control     *thread,
-  Priority_Control    initial_priority
+  Thread_Control   *thread,
+  Priority_Control  initial_priority
 )
 {
   /*
@@ -134,24 +127,34 @@ RTEMS_INLINE_ROUTINE void _MRSP_Set_ceiling_priority(
   mrsp->ceiling_priorities[ scheduler_index ] = ceiling_priority;
 }
 
-RTEMS_INLINE_ROUTINE void _MRSP_Add_state(
-  MRSP_Rival   *rival,
-  unsigned int  state
-)
-{
-  _Atomic_Fetch_or_uint( &rival->state, state, ATOMIC_ORDER_RELEASE );
-}
-
 RTEMS_INLINE_ROUTINE void _MRSP_Timeout(
   Objects_Id  id,
   void       *arg
 )
 {
   MRSP_Rival *rival = arg;
+  Thread_Control *thread = rival->thread;
+  ISR_Level level;
 
   (void) id;
 
-  _MRSP_Add_state( rival, MRSP_RIVAL_STATE_TIMEOUT );
+  _ISR_Disable( level );
+
+  if ( rival->status == MRSP_WAIT_FOR_OWNERSHIP ) {
+    _Chain_Extract_unprotected( &rival->Node );
+
+    _ISR_Enable( level );
+
+    rival->status = MRSP_TIMEOUT;
+
+    _Resource_Node_extract( &thread->Resource_node );
+    _Resource_Node_set_dependency( &thread->Resource_node, NULL );
+    _Scheduler_Thread_change_help_state( thread, rival->initial_help_state );
+    _Scheduler_Thread_change_resource_root( thread, thread );
+    _MRSP_Restore_priority( thread, rival->initial_priority );
+  } else {
+    _ISR_Enable( level );
+  }
 }
 
 RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
@@ -165,19 +168,23 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
 {
   MRSP_Status status;
   MRSP_Rival rival;
-  bool previous_life_protection;
-  unsigned int state;
-  Scheduler_Help_state previous_help_state;
+  bool initial_life_protection;
+  ISR_Level level;
+
+  rival.thread = executing;
+  rival.initial_priority = initial_priority;
+  rival.initial_help_state =
+    _Scheduler_Thread_change_help_state( executing, SCHEDULER_HELP_ACTIVE_RIVAL );
+  rival.status = MRSP_WAIT_FOR_OWNERSHIP;
 
   _MRSP_Elevate_priority( mrsp, executing, ceiling_priority );
 
-  rival.thread = executing;
-  _Atomic_Init_uint( &rival.state, MRSP_RIVAL_STATE_WAITING );
+  _ISR_Disable( level );
   _Chain_Append_unprotected( &mrsp->Rivals, &rival.Node );
+  _ISR_Enable( level );
+
   _Resource_Add_rival( &mrsp->Resource, &executing->Resource_node );
   _Resource_Node_set_dependency( &executing->Resource_node, &mrsp->Resource );
-  previous_help_state =
-    _Scheduler_Thread_change_help_state( executing, SCHEDULER_HELP_ACTIVE_RIVAL );
 
   _Scheduler_Thread_change_resource_root(
     executing,
@@ -194,41 +201,23 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Wait_for_ownership(
     _Watchdog_Insert_ticks( &executing->Timer, timeout );
   }
 
-  previous_life_protection = _Thread_Set_life_protection( true );
+  initial_life_protection = _Thread_Set_life_protection( true );
   _Thread_Enable_dispatch();
 
   _Assert( _Debug_Is_thread_dispatching_allowed() );
 
-  while (
-    _Atomic_Load_uint( &rival.state, ATOMIC_ORDER_ACQUIRE )
-      == MRSP_RIVAL_STATE_WAITING
-  ) {
-    /* Wait for state change */
-  }
+  /* Wait for state change */
+  do {
+    status = rival.status;
+  } while ( status == MRSP_WAIT_FOR_OWNERSHIP );
 
   _Thread_Disable_dispatch();
-  _Thread_Set_life_protection( previous_life_protection );
+  _Thread_Set_life_protection( initial_life_protection );
 
   if ( timeout > 0 ) {
     _Watchdog_Remove( &executing->Timer );
   }
 
-  _Chain_Extract_unprotected( &rival.Node );
-  state = _Atomic_Load_uint( &rival.state, ATOMIC_ORDER_RELAXED );
-
-  if ( ( state & MRSP_RIVAL_STATE_NEW_OWNER ) != 0 ) {
-    mrsp->initial_priority_of_owner = initial_priority;
-    status = MRSP_SUCCESSFUL;
-  } else {
-    _Resource_Node_extract( &executing->Resource_node );
-    _Resource_Node_set_dependency( &executing->Resource_node, NULL );
-    _Scheduler_Thread_change_help_state( executing, previous_help_state );
-    _Scheduler_Thread_change_resource_root( executing, executing );
-    _MRSP_Restore_priority( mrsp, executing, initial_priority );
-
-    status = MRSP_TIMEOUT;
-  }
-
   return status;
 }
 
@@ -289,6 +278,8 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Release(
   Thread_Control *executing
 )
 {
+  ISR_Level level;
+
   if ( _Resource_Get_owner( &mrsp->Resource ) != &executing->Resource_node ) {
     return MRSP_NOT_OWNER_OF_RESOURCE;
   }
@@ -303,21 +294,35 @@ RTEMS_INLINE_ROUTINE MRSP_Status _MRSP_Release(
   }
 
   _Resource_Extract( &mrsp->Resource );
-  _MRSP_Restore_priority( mrsp, executing, mrsp->initial_priority_of_owner );
+  _MRSP_Restore_priority( executing, mrsp->initial_priority_of_owner );
+
+  _ISR_Disable( level );
 
   if ( _Chain_Is_empty( &mrsp->Rivals ) ) {
+    _ISR_Enable( level );
+
     _Resource_Set_owner( &mrsp->Resource, NULL );
   } else {
-    MRSP_Rival *rival = (MRSP_Rival *) _Chain_First( &mrsp->Rivals );
-    Thread_Control *new_owner = rival->thread;
+    MRSP_Rival *rival = (MRSP_Rival *)
+      _Chain_Get_first_unprotected( &mrsp->Rivals );
+    Thread_Control *new_owner;
+
+    /*
+     * This must be inside the critical section since the status prevents a
+     * potential double extraction in _MRSP_Timeout().
+     */
+    rival->status = MRSP_SUCCESSFUL;
+
+    _ISR_Enable( level );
 
+    new_owner = rival->thread;
+    mrsp->initial_priority_of_owner = rival->initial_priority;
     _Resource_Node_extract( &new_owner->Resource_node );
     _Resource_Node_set_dependency( &new_owner->Resource_node, NULL );
     _Resource_Node_add_resource( &new_owner->Resource_node, &mrsp->Resource );
     _Resource_Set_owner( &mrsp->Resource, &new_owner->Resource_node );
     _Scheduler_Thread_change_help_state( new_owner, SCHEDULER_HELP_ACTIVE_OWNER );
     _Scheduler_Thread_change_resource_root( new_owner, new_owner );
-    _MRSP_Add_state( rival, MRSP_RIVAL_STATE_NEW_OWNER );
   }
 
   if ( !_Resource_Node_owns_resources( &executing->Resource_node ) ) {
diff --git a/testsuites/smptests/smpmrsp01/init.c b/testsuites/smptests/smpmrsp01/init.c
index 71aa8442d3..224cea3041 100644
--- a/testsuites/smptests/smpmrsp01/init.c
+++ b/testsuites/smptests/smpmrsp01/init.c
@@ -208,6 +208,15 @@ static void print_switch_events(test_context *ctx)
   }
 }
 
+static void run_task(rtems_task_argument arg)
+{
+  volatile bool *run = (volatile bool *) arg;
+
+  while (true) {
+    *run = true;
+  }
+}
+
 static void obtain_and_release_worker(rtems_task_argument arg)
 {
   test_context *ctx = &test_instance;
@@ -216,7 +225,7 @@ static void obtain_and_release_worker(rtems_task_argument arg)
 
   ctx->worker_task = _Thread_Get_executing();
 
-  assert_prio(RTEMS_SELF, 3);
+  assert_prio(RTEMS_SELF, 4);
 
   /* Obtain with timeout (A) */
   barrier(ctx, &barrier_state);
@@ -224,7 +233,7 @@ static void obtain_and_release_worker(rtems_task_argument arg)
   sc = rtems_semaphore_obtain(ctx->mrsp_ids[0], RTEMS_WAIT, 4);
   rtems_test_assert(sc == RTEMS_TIMEOUT);
 
-  assert_prio(RTEMS_SELF, 3);
+  assert_prio(RTEMS_SELF, 4);
 
   /* Obtain with priority change and timeout (B) */
   barrier(ctx, &barrier_state);
@@ -232,7 +241,7 @@ static void obtain_and_release_worker(rtems_task_argument arg)
   sc = rtems_semaphore_obtain(ctx->mrsp_ids[0], RTEMS_WAIT, 4);
   rtems_test_assert(sc == RTEMS_TIMEOUT);
 
-  assert_prio(RTEMS_SELF, 1);
+  assert_prio(RTEMS_SELF, 2);
 
   /* Restore priority (C) */
   barrier(ctx, &barrier_state);
@@ -243,14 +252,25 @@ static void obtain_and_release_worker(rtems_task_argument arg)
   sc = rtems_semaphore_obtain(ctx->mrsp_ids[0], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
-  assert_prio(RTEMS_SELF, 2);
+  assert_prio(RTEMS_SELF, 3);
 
   sc = rtems_semaphore_release(ctx->mrsp_ids[0]);
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
-  assert_prio(RTEMS_SELF, 3);
+  assert_prio(RTEMS_SELF, 4);
 
-  /* Worker done (E) */
+  /* Obtain and help with timeout (E) */
+  barrier(ctx, &barrier_state);
+
+  sc = rtems_semaphore_obtain(ctx->mrsp_ids[0], RTEMS_WAIT, 4);
+  rtems_test_assert(sc == RTEMS_TIMEOUT);
+
+  assert_prio(RTEMS_SELF, 4);
+
+  sc = rtems_task_suspend(ctx->high_task_id[0]);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  /* Worker done (H) */
   barrier(ctx, &barrier_state);
 
   rtems_task_suspend(RTEMS_SELF);
@@ -266,7 +286,21 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
 
   puts("test MrsP obtain and release");
 
-  change_prio(RTEMS_SELF, 2);
+  change_prio(RTEMS_SELF, 3);
+
+  reset_switch_events(ctx);
+
+  ctx->high_run[0] = false;
+
+  sc = rtems_task_create(
+    rtems_build_name('H', 'I', 'G', '0'),
+    1,
+    RTEMS_MINIMUM_STACK_SIZE,
+    RTEMS_DEFAULT_MODES,
+    RTEMS_DEFAULT_ATTRIBUTES,
+    &ctx->high_task_id[0]
+  );
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
   /* Check executing task parameters */
 
@@ -282,17 +316,17 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
     1,
     RTEMS_MULTIPROCESSOR_RESOURCE_SHARING
       | RTEMS_BINARY_SEMAPHORE,
-    1,
+    2,
     &ctx->mrsp_ids[0]
   );
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
-  assert_prio(RTEMS_SELF, 2);
+  assert_prio(RTEMS_SELF, 3);
 
   sc = rtems_semaphore_obtain(ctx->mrsp_ids[0], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
-  assert_prio(RTEMS_SELF, 1);
+  assert_prio(RTEMS_SELF, 2);
 
   /*
    * The ceiling priority values per scheduler are equal to the value specified
@@ -307,11 +341,11 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
     &prio
   );
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
-  rtems_test_assert(prio == 1);
+  rtems_test_assert(prio == 2);
 
   /* Check the old value and set a new ceiling priority for scheduler B */
 
-  prio = 2;
+  prio = 3;
   sc = rtems_semaphore_set_priority(
     ctx->mrsp_ids[0],
     ctx->scheduler_ids[1],
@@ -319,7 +353,7 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
     &prio
   );
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
-  rtems_test_assert(prio == 1);
+  rtems_test_assert(prio == 2);
 
   /* Check the ceiling priority values */
 
@@ -331,7 +365,7 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
     &prio
   );
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
-  rtems_test_assert(prio == 1);
+  rtems_test_assert(prio == 2);
 
   prio = RTEMS_CURRENT_PRIORITY;
   sc = rtems_semaphore_set_priority(
@@ -341,13 +375,13 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
     &prio
   );
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
-  rtems_test_assert(prio == 2);
+  rtems_test_assert(prio == 3);
 
   /* Check that a thread waiting to get ownership remains executing */
 
   sc = rtems_task_create(
     rtems_build_name('W', 'O', 'R', 'K'),
-    3,
+    4,
     RTEMS_MINIMUM_STACK_SIZE,
     RTEMS_DEFAULT_MODES,
     RTEMS_DEFAULT_ATTRIBUTES,
@@ -364,37 +398,68 @@ static void test_mrsp_obtain_and_release(test_context *ctx)
   /* Obtain with timeout (A) */
   barrier_and_delay(ctx, &barrier_state);
 
-  assert_prio(ctx->worker_ids[0], 2);
+  assert_prio(ctx->worker_ids[0], 3);
   assert_executing_worker(ctx);
 
   /* Obtain with priority change and timeout (B) */
   barrier_and_delay(ctx, &barrier_state);
 
-  assert_prio(ctx->worker_ids[0], 2);
-  change_prio(ctx->worker_ids[0], 1);
+  assert_prio(ctx->worker_ids[0], 3);
+  change_prio(ctx->worker_ids[0], 2);
   assert_executing_worker(ctx);
 
   /* Restore priority (C) */
   barrier(ctx, &barrier_state);
 
-  assert_prio(ctx->worker_ids[0], 1);
-  change_prio(ctx->worker_ids[0], 3);
+  assert_prio(ctx->worker_ids[0], 2);
+  change_prio(ctx->worker_ids[0], 4);
 
   /* Obtain without timeout (D) */
   barrier_and_delay(ctx, &barrier_state);
 
-  assert_prio(ctx->worker_ids[0], 2);
+  assert_prio(ctx->worker_ids[0], 3);
   assert_executing_worker(ctx);
 
   sc = rtems_semaphore_release(ctx->mrsp_ids[0]);
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
-  /* Worker done (E) */
+  /* Check that a timeout works in case the waiting thread actually helps */
+
+  sc = rtems_semaphore_obtain(ctx->mrsp_ids[0], RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  /* Obtain and help with timeout (E) */
+  barrier_and_delay(ctx, &barrier_state);
+
+  sc = rtems_task_start(
+    ctx->high_task_id[0],
+    run_task,
+    (rtems_task_argument) &ctx->high_run[0]
+  );
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  rtems_test_assert(rtems_get_current_processor() == 1);
+
+  while (rtems_get_current_processor() != 0) {
+    /* Wait */
+  }
+
+  rtems_test_assert(ctx->high_run[0]);
+
+  sc = rtems_semaphore_release(ctx->mrsp_ids[0]);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
+  print_switch_events(ctx);
+
+  /* Worker done (H) */
   barrier(ctx, &barrier_state);
 
   sc = rtems_task_delete(ctx->worker_ids[0]);
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 
+  sc = rtems_task_delete(ctx->high_task_id[0]);
+  rtems_test_assert(sc == RTEMS_SUCCESSFUL);
+
   sc = rtems_semaphore_delete(ctx->mrsp_ids[0]);
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 }
@@ -727,15 +792,6 @@ static void test_mrsp_multiple_obtain(void)
   rtems_test_assert(sc == RTEMS_SUCCESSFUL);
 }
 
-static void run_task(rtems_task_argument arg)
-{
-  volatile bool *run = (volatile bool *) arg;
-
-  while (true) {
-    *run = true;
-  }
-}
-
 static void ready_unlock_worker(rtems_task_argument arg)
 {
   test_context *ctx = &test_instance;
diff --git a/testsuites/smptests/smpmrsp01/smpmrsp01.scn b/testsuites/smptests/smpmrsp01/smpmrsp01.scn
index 1fc8b6b577..90184b571c 100644
--- a/testsuites/smptests/smpmrsp01/smpmrsp01.scn
+++ b/testsuites/smptests/smpmrsp01/smpmrsp01.scn
@@ -45,6 +45,11 @@ test MrsP obtain and release with help
 [0] IDLE -> MAIN (prio   3, node MAIN)
 [1] MAIN -> HELP (prio   2, node HELP)
 test MrsP obtain and release
+[1] IDLE -> WORK (prio   4, node WORK)
+[1] WORK -> MAIN (prio   3, node WORK)
+[0] MAIN -> HIG0 (prio   1, node HIG0)
+[1] MAIN -> WORK (prio   4, node WORK)
+[0] HIG0 -> MAIN (prio   2, node MAIN)
 test MrsP load
 worker[0]
   sleep = 53