[19347] Fix encapsulation format in WLP

include/fastdds/rtps/builtin/liveliness/WLPListener.h

@@ -28,7 +28,7 @@
 #include <fastrtps/qos/QosPolicies.h>
 
 namespace eprosima {
-namespace fastrtps{
+namespace fastrtps {
 namespace rtps {
 
 class WLP;
@@ -39,14 +39,16 @@ struct CacheChange_t;
  * Class WLPListener that receives the liveliness messages asserting the liveliness of remote endpoints.
  * @ingroup LIVELINESS_MODULE
  */
-class WLPListener: public ReaderListener {
+class WLPListener : public ReaderListener
+{
 public:
 
     /**
      * @brief Constructor
      * @param pwlp Pointer to the writer liveliness protocol
      */
-    WLPListener(WLP* pwlp);
+    WLPListener(
+            WLP* pwlp);
 
     /**
      * @brief Destructor
@@ -60,27 +62,39 @@ class WLPListener: public ReaderListener {
      */
     void onNewCacheChangeAdded(
             RTPSReader* reader,
-            const CacheChange_t* const  change) override;
+            const CacheChange_t* const change) override;
 
 private:
 
     /**
-    * Separate the Key between the GuidPrefix_t and the liveliness Kind
-    * @param key InstanceHandle_t to separate.
-    * @param guidP GuidPrefix_t pointer to store the info.
-    * @param liveliness Liveliness Kind Pointer.
-    * @return True if correctly separated.
-    */
+     * Separate the Key between the GuidPrefix_t and the liveliness Kind
+     * @param key InstanceHandle_t to separate.
+     * @param guidP GuidPrefix_t pointer to store the info.
+     * @param liveliness Liveliness Kind Pointer.
+     * @return True if correctly separated.
+     */
     bool separateKey(
             InstanceHandle_t& key,
             GuidPrefix_t* guidP,
             LivelinessQosPolicyKind* liveliness);
 
     /**
-    * Compute the key from a CacheChange_t
-    * @param change
-    */
-    bool computeKey(CacheChange_t* change);
+     * Compute the key from a CacheChange_t
+     * @param change
+     */
+    bool computeKey(
+            CacheChange_t* change);
+
+    /**
+     * @brief Check that the ParticipantMessageData kind is a valid one for WLP
+     *
+     * @param kind A pointer to the first octet of the kind array. The function assumes 4 elements
+     *        in the array.
+     *
+     * @return True if the kind corresponds with one for WLP, false otherwise.
+     */
+    bool is_wlp_kind(
+            octet* kind);
 
     //! A pointer to the writer liveliness protocol
     WLP* mp_WLP;
@@ -89,6 +103,6 @@ class WLPListener: public ReaderListener {
 
 } /* namespace rtps */
 } /* namespace eprosima */
-}
-#endif
+} // namespace eprosima
+#endif // ifndef DOXYGEN_SHOULD_SKIP_THIS_PUBLIC
 #endif /* _FASTDDS_RTPS_WLPLISTENER_H_ */

include/fastdds/rtps/common/SerializedPayload.h

@@ -42,7 +42,7 @@ namespace rtps {
 #define PL_CDR_LE 0x0003
 
 #if FASTDDS_IS_BIG_ENDIAN_TARGET
-#define DEFAULT_ENCAPSULATION CDR_LE
+#define DEFAULT_ENCAPSULATION CDR_BE
 #define PL_DEFAULT_ENCAPSULATION PL_CDR_BE
 #else
 #define DEFAULT_ENCAPSULATION CDR_LE

src/cpp/rtps/builtin/liveliness/WLP.cpp

@@ -900,9 +900,9 @@ bool WLP::send_liveliness_message(
 
     if (change != nullptr)
     {
-        change->serializedPayload.encapsulation = (uint16_t)PL_DEFAULT_ENCAPSULATION;
+        change->serializedPayload.encapsulation = (uint16_t)DEFAULT_ENCAPSULATION;
         change->serializedPayload.data[0] = 0;
-        change->serializedPayload.data[1] = PL_DEFAULT_ENCAPSULATION;
+        change->serializedPayload.data[1] = DEFAULT_ENCAPSULATION;
         change->serializedPayload.data[2] = 0;
         change->serializedPayload.data[3] = 0;
 

src/cpp/rtps/builtin/liveliness/WLPListener.cpp

@@ -74,22 +74,28 @@ void WLPListener::onNewCacheChangeAdded(
             break;
         }
     }
-    if (change->serializedPayload.length > 0)
+
+    // Data should have at least 4 bytes of representation header, 12 of GuidPrefix, and 4 of kind.
+    if (change->serializedPayload.length >= 20)
     {
-        if (PL_CDR_BE == change->serializedPayload.data[1])
+        // Encapsulation in the second byte of the representation header.
+        change->serializedPayload.encapsulation = (uint16_t)change->serializedPayload.data[1];
+
+        // Extract GuidPrefix
+        memcpy(guidP.value, change->serializedPayload.data + 4, 12);
+
+        // Extract liveliness kind
+        if (is_wlp_kind(&change->serializedPayload.data[16]))
         {
-            change->serializedPayload.encapsulation = (uint16_t)PL_CDR_BE;
+            // Adjust and cast to LivelinessQosPolicyKind enum, where AUTOMATIC_LIVELINESS_QOS == 0
+            livelinessKind = (LivelinessQosPolicyKind)(change->serializedPayload.data[19] - 0x01);
         }
         else
         {
-            change->serializedPayload.encapsulation = (uint16_t)PL_CDR_LE;
-        }
-
-        for (size_t i = 0; i < 12; ++i)
-        {
-            guidP.value[i] = change->serializedPayload.data[i + 4];
+            logInfo(RTPS_LIVELINESS, "Ignoring not WLP ParticipantDataMessage");
+            history->remove_change(change);
+            return;
         }
-        livelinessKind = (LivelinessQosPolicyKind)(change->serializedPayload.data[19] - 0x01);
 
     }
     else
@@ -99,6 +105,8 @@ void WLPListener::onNewCacheChangeAdded(
                     &guidP,
                     &livelinessKind))
         {
+            logInfo(RTPS_LIVELINESS, "Ignoring not WLP ParticipantDataMessage");
+            history->remove_change(change);
             return;
         }
     }
@@ -130,12 +138,17 @@ bool WLPListener::separateKey(
         GuidPrefix_t* guidP,
         LivelinessQosPolicyKind* liveliness)
 {
-    for (uint8_t i = 0; i < 12; ++i)
+    bool ret = false;
+    if (is_wlp_kind(&key.value[12]))
     {
-        guidP->value[i] = key.value[i];
+        // Extract GuidPrefix
+        memcpy(guidP->value, key.value, 12);
+
+        // Extract liveliness kind
+        *liveliness = (LivelinessQosPolicyKind)key.value[15];
+        ret = true;
     }
-    *liveliness = (LivelinessQosPolicyKind)key.value[15];
-    return true;
+    return ret;
 }
 
 bool WLPListener::computeKey(
@@ -154,6 +167,22 @@ bool WLPListener::computeKey(
     return true;
 }
 
+bool WLPListener::is_wlp_kind(
+        octet* kind)
+{
+    /*
+     * From RTPS 2.5 9.6.3.1, the ParticipantMessageData kinds for WLP are:
+     *   - PARTICIPANT_MESSAGE_DATA_KIND_AUTOMATIC_LIVELINESS_UPDATE {0x00, 0x00, 0x00, 0x01}
+     *   - PARTICIPANT_MESSAGE_DATA_KIND_MANUAL_LIVELINESS_UPDATE {0x00, 0x00, 0x00, 0x02}
+     */
+    bool is_wlp = true;
+    is_wlp &= kind[0] == 0;
+    is_wlp &= kind[1] == 0;
+    is_wlp &= kind[2] == 0;
+    is_wlp &= kind[3] == 0x01 || kind[3] == 0x02;
+    return is_wlp;
+}
+
 } /* namespace rtps */
 } /* namespace eprosima */
 } // namespace eprosima