From a70f6b13c0158098a059ed67e9291f9441f04ecc Mon Sep 17 00:00:00 2001
From: Miguel Company <MiguelCompany@eprosima.com>
Date: Tue, 4 Jan 2022 15:54:35 +0100
Subject: [PATCH] Refs 13364. Fix #2379.

Signed-off-by: Miguel Company <MiguelCompany@eprosima.com>
---
 src/cpp/rtps/messages/RTPSMessageGroup.cpp    | 27 +++++++++++++------
 .../rtps/participant/RTPSParticipantImpl.h    | 11 ++++++++
 2 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/src/cpp/rtps/messages/RTPSMessageGroup.cpp b/src/cpp/rtps/messages/RTPSMessageGroup.cpp
index 5ca45d27cac..f4999e6f46e 100644
--- a/src/cpp/rtps/messages/RTPSMessageGroup.cpp
+++ b/src/cpp/rtps/messages/RTPSMessageGroup.cpp
@@ -74,18 +74,29 @@ static bool data_exceeds_limitation(
 }
 
 static bool append_message(
+        RTPSParticipantImpl* participant,
         CDRMessage_t* full_msg,
         CDRMessage_t* submsg)
 {
-#ifndef FASTDDS_STATISTICS
-    return CDRMessage::appendMsg(full_msg, submsg);
-#else
+    static_cast<void>(participant);
+
+    uint32_t extra_size = 0;
+
+#if HAVE_SECURITY
+    // Avoid full message growing over estimated extra size for RTPS encryption
+    extra_size += participant->calculate_extra_size_for_rtps_message();
+#endif  // HAVE_SECURITY
+
+#ifdef FASTDDS_STATISTICS
     // Keep room for the statistics submessage by reducing max_size while appending submessage
-    full_msg->max_size -= eprosima::fastdds::statistics::rtps::statistics_submessage_length;
+    extra_size += eprosima::fastdds::statistics::rtps::statistics_submessage_length;
+#endif  // FASTDDS_STATISTICS
+
+    full_msg->max_size -= extra_size;
     bool ret_val = CDRMessage::appendMsg(full_msg, submsg);
-    full_msg->max_size += eprosima::fastdds::statistics::rtps::statistics_submessage_length;
+    full_msg->max_size += extra_size;
+
     return ret_val;
-#endif // FASTDDS_STATISTICS
 }
 
 bool sort_changes_group (
@@ -336,13 +347,13 @@ bool RTPSMessageGroup::insert_submessage(
         const GuidPrefix_t& destination_guid_prefix,
         bool is_big_submessage)
 {
-    if (!append_message(full_msg_, submessage_msg_))
+    if (!append_message(participant_, full_msg_, submessage_msg_))
     {
         // Retry
         flush_and_reset();
         add_info_dst_in_buffer(full_msg_, destination_guid_prefix);
 
-        if (!append_message(full_msg_, submessage_msg_))
+        if (!append_message(participant_, full_msg_, submessage_msg_))
         {
             EPROSIMA_LOG_ERROR(RTPS_WRITER, "Cannot add RTPS submesage to the CDRMessage. Buffer too small");
             return false;
diff --git a/src/cpp/rtps/participant/RTPSParticipantImpl.h b/src/cpp/rtps/participant/RTPSParticipantImpl.h
index eff26d891a1..d5e40d1a1ec 100644
--- a/src/cpp/rtps/participant/RTPSParticipantImpl.h
+++ b/src/cpp/rtps/participant/RTPSParticipantImpl.h
@@ -360,6 +360,17 @@ class RTPSParticipantImpl
             uint32_t length);
 
 #if HAVE_SECURITY
+    uint32_t calculate_extra_size_for_rtps_message()
+    {
+        uint32_t ret_val = 0u;
+        if (security_attributes_.is_rtps_protected)
+        {
+            ret_val = m_security_manager.calculate_extra_size_for_rtps_message();
+        }
+
+        return ret_val;
+    }
+
     security::SecurityManager& security_manager()
     {
         return m_security_manager;