Refs #8212. FIFO strategy in condition notify.

Signed-off-by: AdolfoMartinez <[email protected]>

Author: adolfomarver

src/cpp/rtps/transport/shared_mem/RobustInterprocessCondition.hpp

@@ -31,8 +31,8 @@ class RobustInterprocessCondition
 public:
 
     RobustInterprocessCondition()
-        : list_listening_(SemaphoreList::LIST_NULL)
-        , list_free_(MAX_LISTENERS-1)
+        : list_listening_(SemaphoreList::LIST_NULL, SemaphoreList::LIST_NULL)
+        , list_free_(0, MAX_LISTENERS-1)
     {
         init_sem_list();
     }
@@ -50,7 +50,7 @@ class RobustInterprocessCondition
     {
         bi::scoped_lock<bi::interprocess_mutex> lock(semaphore_lists_mutex_);
 
-        auto sem_index = list_listening_.tail();
+        auto sem_index = list_listening_.head();
 
         if (sem_index != SemaphoreList::LIST_NULL)
         {
@@ -67,12 +67,12 @@ class RobustInterprocessCondition
     {
         bi::scoped_lock<bi::interprocess_mutex> lock(semaphore_lists_mutex_);
 
-        auto sem_index = list_listening_.tail();
+        auto sem_index = list_listening_.head();
 
         while (sem_index != SemaphoreList::LIST_NULL)
         {
             semaphores_pool_[sem_index].sem.post();
-            sem_index = semaphores_pool_[sem_index].prev;
+            sem_index = semaphores_pool_[sem_index].next;
         }
     }
 
@@ -100,7 +100,9 @@ class RobustInterprocessCondition
             Pr pred)
     {
         while (!pred())
+        {
             do_wait(*lock.mutex());
+        }
     }
 
     /**
@@ -145,7 +147,8 @@ class RobustInterprocessCondition
             wait(lock, pred);
             return true;
         }
-        while (!pred()){
+        while (!pred())
+        {
             if (!do_timed_wait(abs_time, *lock.mutex()))
             {
                 return pred();
@@ -170,15 +173,18 @@ class RobustInterprocessCondition
     {
     private:
 
+        uint32_t head_;
         uint32_t tail_;
 
     public:
 
         static constexpr uint32_t LIST_NULL = static_cast<uint32_t>(-1);
 
         SemaphoreList(
+                uint32_t head,
                 uint32_t tail)
-            : tail_(tail)
+            : head_(head)
+            , tail_(tail)
         {
         }
 
@@ -190,11 +196,16 @@ class RobustInterprocessCondition
             {
                 sem_pool[tail_].next = sem_index;
             }
-
+            
             sem_pool[sem_index].prev = tail_;
             sem_pool[sem_index].next = LIST_NULL;
 
             tail_ = sem_index;
+
+            if (head_ == LIST_NULL)
+            {
+                head_ = sem_index;
+            }
         }
 
         inline uint32_t pop(
@@ -212,6 +223,11 @@ class RobustInterprocessCondition
             {
                 sem_pool[tail_].next = LIST_NULL;
             }
+            else
+            {
+                head_ = LIST_NULL;
+            }
+            
             return sem_index;
         }
 
@@ -220,6 +236,11 @@ class RobustInterprocessCondition
             return tail_;
         }
 
+        inline uint32_t head() const
+        {
+            return head_;
+        }
+
         inline void remove(
                 uint32_t sem_index,
                 SemaphoreNode* sem_pool)
@@ -239,6 +260,11 @@ class RobustInterprocessCondition
                 sem_pool[next].prev = prev;
             }
 
+            if (head_ == sem_index)
+            {
+                head_ = next;
+            }
+
             if (tail_ == sem_index)
             {
                 tail_ = prev;