Flowcontroller update

include/fastdds/rtps/common/CacheChange.hpp

@@ -45,15 +45,17 @@ struct CacheChange_t;
 struct CacheChangeWriterInfo_t
 {
     //!Number of DATA / DATA_FRAG submessages sent to the transport (only used in Writers)
-    size_t num_sent_submessages = 0;
+    size_t num_sent_submessages {0};
     //! Used to link with previous node in a list. Used by FlowControllerImpl.
     //! Cannot be cached because there are several comparisons without locking.
-    CacheChange_t* volatile previous = nullptr;
+    CacheChange_t* volatile previous {nullptr};
     //! Used to link with next node in a list. Used by FlowControllerImpl.
     //! Cannot be cached because there are several comparisons without locking.
-    CacheChange_t* volatile next = nullptr;
+    CacheChange_t* volatile next {nullptr};
     //! Used to know if the object is already in a list.
     std::atomic_bool is_linked {false};
+    //! Last fragment number sent.
+    FragmentNumber_t last_fragment_sent {0};
 };
 
 /*!
@@ -348,7 +350,7 @@ struct FASTDDS_EXPORTED_API CacheChange_t
         }
 
         // In order to avoid overflow on the calculations, we limit the maximum payload size
-        constexpr uint32_t MAX_PAYLOAD_SIZE = std::numeric_limits<uint32_t>::max() - 4u - 3u;
+        constexpr uint32_t MAX_PAYLOAD_SIZE = (std::numeric_limits<uint32_t>::max)() - 4u - 3u;
         if (payload_size > MAX_PAYLOAD_SIZE)
         {
             return false;

include/fastdds/rtps/common/LocatorSelector.hpp

@@ -86,6 +86,7 @@ class LocatorSelector
     bool add_entry(
             LocatorSelectorEntry* entry)
     {
+        force_reset_ = true;
         return entries_.push_back(entry) != nullptr;
     }
 
@@ -96,6 +97,7 @@ class LocatorSelector
     bool remove_entry(
             const GUID_t& guid)
     {
+        force_reset_ = true;
         return entries_.remove_if(
             [&guid](LocatorSelectorEntry* entry)
             {
@@ -111,11 +113,29 @@ class LocatorSelector
     void reset(
             bool enable_all)
     {
-        last_state_.clear();
-        for (LocatorSelectorEntry* entry : entries_)
+        if (last_state_.size() == entries_.size() && initial_allow_to_send_ && !force_reset_)
+        {
+            // Resuse the last state instead of recreating it.
+            for (size_t count {0}; count < entries_.size(); ++count)
+            {
+                if (last_state_.at(count).second != (entries_.at(count)->allowed_to_send ? 1 : 0))
+                {
+                    force_reset_ = true;
+                }
+                last_state_.at(count).first = entries_.at(count)->enabled ? 1 : 0;
+                last_state_.at(count).second = entries_.at(count)->allowed_to_send ? 1 : 0;
+                entries_.at(count)->enable(enable_all);
+            }
+        }
+        else
         {
-            last_state_.push_back(entry->enabled ? 1 : 0);
-            entry->enable(enable_all);
+            force_reset_ = last_state_.size() != entries_.size();
+            last_state_.clear();
+            for (LocatorSelectorEntry* entry : entries_)
+            {
+                last_state_.emplace_back(entry->enabled ? 1 : 0, entry->allowed_to_send ? 1 : 0);
+                entry->enable(enable_all);
+            }
         }
     }
 
@@ -144,14 +164,15 @@ class LocatorSelector
      */
     bool state_has_changed() const
     {
-        if (entries_.size() != last_state_.size())
+        if (entries_.size() != last_state_.size() || force_reset_)
         {
             return true;
         }
 
         for (size_t i = 0; i < entries_.size(); ++i)
         {
-            if (last_state_.at(i) != (entries_.at(i)->enabled ? 1 : 0))
+            if (last_state_.at(i).first != (entries_.at(i)->enabled ? 1 : 0) ||
+                    last_state_.at(i).second != (entries_.at(i)->allowed_to_send ? 1 : 0))
             {
                 return true;
             }
@@ -181,6 +202,7 @@ class LocatorSelector
      */
     ResourceLimitedVector<LocatorSelectorEntry*>& transport_starts()
     {
+        force_reset_ = false;
         for (LocatorSelectorEntry* entry : entries_)
         {
             entry->transport_should_process = entry->enabled;
@@ -204,6 +226,19 @@ class LocatorSelector
         }
     }
 
+    void unselect(
+            size_t index)
+    {
+        if (index < entries_.size())
+        {
+            auto it = std::find(selections_.begin(), selections_.end(), index);
+            if (it != selections_.end())
+            {
+                selections_.erase(it);
+            }
+        }
+    }
+
     /**
      * Count the number of selected locators.
      *
@@ -289,6 +324,36 @@ class LocatorSelector
         }
     }
 
+    template<class UnaryPredicate>
+    bool for_every_entry(
+            UnaryPredicate action) const
+    {
+        bool ret_value {true};
+        for (size_t count {0}; count < entries_.size(); ++count)
+        {
+            if (!action(entries_.at(count), count))
+            {
+                ret_value = false;
+                break;
+            }
+        }
+        return ret_value;
+    }
+
+    LocatorSelectorEntry* get_entry_by_guid(
+            const GUID_t& guid) const
+    {
+        for (LocatorSelectorEntry* entry : entries_)
+        {
+            if (guid == entry->remote_guid)
+            {
+                return entry;
+            }
+        }
+
+        return nullptr;
+    }
+
     struct IteratorIndex
     {
         size_t selections_index;
@@ -462,14 +527,31 @@ class LocatorSelector
         return iterator(*this, iterator::Position::End);
     }
 
+    void initial_allow_to_send(
+            bool value)
+    {
+        initial_allow_to_send_ = value;
+    }
+
+    bool initial_allow_to_send() const
+    {
+        return initial_allow_to_send_;
+    }
+
 private:
 
     //! Entries collection.
     ResourceLimitedVector<LocatorSelectorEntry*> entries_;
     //! List of selected indexes.
     ResourceLimitedVector<size_t> selections_;
     //! Enabling state when reset was called.
-    ResourceLimitedVector<int> last_state_;
+    ResourceLimitedVector<std::pair<int, int>> last_state_;
+
+    //! Whether it is the initial state of all allow_to_send flags.
+    bool initial_allow_to_send_ {true};
+
+    //! Whether a reset is forced due to changes in the entries.
+    bool force_reset_ {false};
 };
 
 } // namespace rtps

include/fastdds/rtps/common/LocatorSelectorEntry.hpp

@@ -75,8 +75,6 @@ struct LocatorSelectorEntry
         , unicast(ResourceLimitedContainerConfig::fixed_size_configuration(max_unicast_locators))
         , multicast(ResourceLimitedContainerConfig::fixed_size_configuration(max_multicast_locators))
         , state(max_unicast_locators, max_multicast_locators)
-        , enabled(false)
-        , transport_should_process(false)
     {
     }
 
@@ -139,9 +137,11 @@ struct LocatorSelectorEntry
     //! State of the entry
     EntryState state;
     //! Indicates whether this entry should be taken into consideration.
-    bool enabled;
+    bool enabled {false};
+    //! Indicates whether this entry is allowed to send data.
+    bool allowed_to_send {true};
     //! A temporary value for each transport to help optimizing some use cases.
-    bool transport_should_process;
+    bool transport_should_process {false};
 };
 
 } // namespace rtps

include/fastdds/rtps/common/SequenceNumber.hpp

@@ -84,7 +84,7 @@ struct FASTDDS_EXPORTED_API SequenceNumber_t
         ++low;
         if (low == 0)
         {
-            assert(std::numeric_limits<decltype(high)>::max() > high);
+            assert((std::numeric_limits<decltype(high)>::max)() > high);
             ++high;
         }
 
@@ -113,7 +113,7 @@ struct FASTDDS_EXPORTED_API SequenceNumber_t
         if (low < aux_low)
         {
             // Being the type of the parameter an 'int', the increment of 'high' will be as much as 1.
-            assert(std::numeric_limits<decltype(high)>::max() > high);
+            assert((std::numeric_limits<decltype(high)>::max)() > high);
             ++high;
         }
 
@@ -264,7 +264,7 @@ inline SequenceNumber_t operator +(
     if (res.low < seq.low)
     {
         // Being the type of the parameter an 'uint32_t', the increment of 'high' will be as much as 1.
-        assert(std::numeric_limits<decltype(res.high)>::max() > res.high);
+        assert((std::numeric_limits<decltype(res.high)>::max)() > res.high);
         ++res.high;
     }
 

include/fastdds/rtps/messages/RTPSMessageSenderInterface.hpp

@@ -83,7 +83,7 @@ class RTPSMessageSenderInterface
     virtual bool send(
             const std::vector<eprosima::fastdds::rtps::NetworkBuffer>& buffers,
             const uint32_t& total_bytes,
-            std::chrono::steady_clock::time_point max_blocking_time_point) const = 0;
+            std::chrono::steady_clock::time_point max_blocking_time_point) = 0;
 
     /*!
      * Lock the object.

include/fastdds/utils/fixed_size_bitmap.hpp

@@ -257,6 +257,87 @@ class BitmapRange
         return base_;
     }
 
+    /**
+     * Returns the lowest value set strictly after `after_item`.
+     *
+     * @param after_item  Item after which to search (exclusive).
+     * @return The lowest value set > after_item. If none is set, the result is undetermined.
+     */
+    T min_after(
+            const T after_item) const noexcept
+    {
+        const uint32_t n_longs = (num_bits_ + 31u) / 32u;
+        if (n_longs == 0u)
+        {
+            return base_;
+        }
+
+        // If the requested item is before the range, behave like min()
+        if (after_item < base_)
+        {
+            return min();
+        }
+
+        // Start searching from the next item (exclusive)
+        const T first = after_item + 1;
+
+        // If we're already past the range, nothing to find
+        const T end_exclusive = base_ + static_cast<T>(num_bits_);
+        if (first >= end_exclusive)
+        {
+            return base_;
+        }
+
+        const uint32_t rel = static_cast<uint32_t>(first - base_); // 0..num_bits_-1
+        const uint32_t start_long = rel / 32u;
+        const uint32_t start_off  = rel % 32u;// item offset inside that 32-bit word (0..31)
+
+        T item = base_ + static_cast<T>(start_long * 32u);
+
+        for (uint32_t i = start_long; i < n_longs; ++i)
+        {
+            uint32_t bits = bitmap_[i];
+
+            // In the first word, discard bits for items < first
+            // With your encoding: offset 0 => bit31, offset 31 => bit0.
+            // So we clear the top `start_off` bits (those represent earlier items).
+            if (i == start_long && start_off != 0u)
+            {
+                const uint64_t mask64 = (start_off >= 32u) ? 0ull
+                : ((1ull << (32u - start_off)) - 1ull); // keeps low (32-start_off) bits
+                bits &= static_cast<uint32_t>(mask64);
+            }
+
+            // In the last word, discard bits beyond num_bits_ (safety, if not already zeroed)
+            if (i == (n_longs - 1u))
+            {
+                const uint32_t valid = (num_bits_ & 31u); // 0 means full 32 valid
+                if (valid != 0u)
+                {
+                    // Keep only the top `valid` bits (offsets 0..valid-1 => bits 31..(32-valid))
+                    const uint64_t last_mask64 = ~((1ull << (32u - valid)) - 1ull);
+                    bits &= static_cast<uint32_t>(last_mask64);
+                }
+            }
+
+            if (bits)
+            {
+#if defined(_MSC_VER)
+                unsigned long bit;
+                _BitScanReverse(&bit, bits);          // index of highest set bit (0..31)
+                const uint32_t offset = 31u ^ static_cast<uint32_t>(bit);
+#else
+                const uint32_t offset = static_cast<uint32_t>(__builtin_clz(static_cast<unsigned>(bits)));
+#endif // if defined(_MSC_VER)
+                return item + static_cast<T>(offset);
+            }
+
+            item = item + 32u;
+        }
+
+        return base_;
+    }
+
     /**
      * Checks if an element is present in the bitmap.
      *

src/cpp/fastdds/core/policy/ParameterList.hpp

@@ -122,7 +122,7 @@ class ParameterList
             uint64_t new_qos_size = static_cast<uint64_t>(qos_size) + 4 + static_cast<uint64_t>(plength);
             new_qos_size = (new_qos_size + 3) & ~3; // Align to 4 byte boundary
             uint64_t new_pos = static_cast<uint64_t>(original_pos) + new_qos_size;
-            constexpr uint64_t max_uint32 = static_cast<uint64_t>(std::numeric_limits<uint32_t>::max());
+            constexpr uint64_t max_uint32 = static_cast<uint64_t>((std::numeric_limits<uint32_t>::max)());
             if ((new_qos_size > max_uint32) || (new_pos > max_uint32))
             {
                 return false;