From b092321f542d6ad596a15a9fc9492e848715b32a Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 18:00:00 +0200
Subject: [PATCH 01/25] fix: broaden deserialize() catch to std::exception so
 oversize-length input cannot cross extern "C"

---
 rmw_unix_socket_cpp/src/serialization.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/serialization.cpp b/rmw_unix_socket_cpp/src/serialization.cpp
index 449b7ff..9883f79 100644
--- a/rmw_unix_socket_cpp/src/serialization.cpp
+++ b/rmw_unix_socket_cpp/src/serialization.cpp
@@ -152,6 +152,11 @@ bool deserialize(
     }
   } catch (const eprosima::fastcdr::exception::Exception &) {
     return false;
+  } catch (const std::exception &) {
+    // cdr_deserialize() resizes message strings/sequences from attacker-controlled
+    // wire lengths and can throw std::bad_alloc/std::length_error. Callers are
+    // extern "C", so nothing may escape across the C ABI.
+    return false;
   }
   return true;
 }

From 8f51257d43f5ee47532cf5efded91c8036ead3ab Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 18:12:00 +0200
Subject: [PATCH 02/25] fix: deep-copy discovery_options in
 rmw_init/options_copy to stop pointer aliasing and leaks

---
 rmw_unix_socket_cpp/src/rmw_init.cpp | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp
index 927f112..f3f6d28 100644
--- a/rmw_unix_socket_cpp/src/rmw_init.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_init.cpp
@@ -7,6 +7,7 @@
 #include "rcutils/strdup.h"
 #include "rmw/check_type_identifiers_match.h"
 #include "rmw/error_handling.h"
+#include "rmw/discovery_options.h"
 #include "rmw/init.h"
 #include "rmw/init_options.h"
 #include "rmw/rmw.h"
@@ -57,6 +58,15 @@ rmw_ret_t rmw_init_options_copy(
   if (ret != RMW_RET_OK) {
     return ret;
   }
+  // discovery_options.static_peers is heap-owned; deep-copy it so src and dst
+  // don't alias the same array and double-free in rmw_init_options_fini.
+  dst->discovery_options = rmw_get_zero_initialized_discovery_options();
+  ret = rmw_discovery_options_copy(
+    &src->discovery_options, &src->allocator, &dst->discovery_options);
+  if (ret != RMW_RET_OK) {
+    rmw_security_options_fini(&dst->security_options, &dst->allocator);
+    return ret;
+  }
   if (src->enclave) {
     dst->enclave = rcutils_strdup(src->enclave, src->allocator);
   }
@@ -71,6 +81,7 @@ rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options)
     init_options->enclave = nullptr;
   }
   rmw_security_options_fini(&init_options->security_options, &init_options->allocator);
+  rmw_discovery_options_fini(&init_options->discovery_options);
   *init_options = rmw_get_zero_initialized_init_options();
   return RMW_RET_OK;
 }
@@ -145,6 +156,12 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
   if (options->enclave) {
     context->options.enclave = rcutils_strdup(options->enclave, options->allocator);
   }
+  // discovery_options.static_peers is heap-owned; deep-copy it so context->options
+  // owns its own array rather than aliasing the caller's options (mirrors enclave).
+  context->options.discovery_options = rmw_get_zero_initialized_discovery_options();
+  rmw_discovery_options_copy(
+    &options->discovery_options, &options->allocator,
+    &context->options.discovery_options);
   context->actual_domain_id = domain_id;
 
   return RMW_RET_OK;
@@ -186,6 +203,7 @@ rmw_ret_t rmw_context_fini(rmw_context_t * context)
       context->options.enclave, context->options.allocator.state);
     context->options.enclave = nullptr;
   }
+  rmw_discovery_options_fini(&context->options.discovery_options);
 
   *context = rmw_get_zero_initialized_context();
   return RMW_RET_OK;

From 260e5fa99bd32f72783576aa4c267487b708124b Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 18:24:00 +0200
Subject: [PATCH 03/25] fix: enforce RMW_CHECK_TYPE_IDENTIFIERS_MATCH on all
 service/client entry points

---
 rmw_unix_socket_cpp/src/rmw_client.cpp  | 24 ++++++++++++++++++++++++
 rmw_unix_socket_cpp/src/rmw_service.cpp | 18 ++++++++++++++++++
 2 files changed, 42 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/rmw_client.cpp b/rmw_unix_socket_cpp/src/rmw_client.cpp
index 92ee324..336e7ba 100644
--- a/rmw_unix_socket_cpp/src/rmw_client.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_client.cpp
@@ -132,6 +132,9 @@ rmw_ret_t rmw_destroy_client(rmw_node_t * node, rmw_client_t * client)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
   if (cli_data) {
@@ -159,6 +162,9 @@ rmw_ret_t rmw_send_request(
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
 
@@ -222,6 +228,9 @@ rmw_ret_t rmw_take_response(
   RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   *taken = false;
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
@@ -272,6 +281,9 @@ rmw_ret_t rmw_client_request_publisher_get_actual_qos(
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
   *qos = cli_data->qos;
   return RMW_RET_OK;
@@ -283,6 +295,9 @@ rmw_ret_t rmw_client_response_subscription_get_actual_qos(
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
   *qos = cli_data->qos;
   return RMW_RET_OK;
@@ -292,6 +307,9 @@ rmw_ret_t rmw_get_gid_for_client(const rmw_client_t * client, rmw_gid_t * gid)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
   gid->implementation_identifier = rmw_uds::identifier;
   std::memcpy(gid->data, cli_data->gid.data, RMW_GID_STORAGE_SIZE);
@@ -306,6 +324,9 @@ rmw_ret_t rmw_service_server_is_available(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(is_available, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
   auto * header = rmw_uds::registry_header(cli_data->context->registry_ptr);
@@ -323,6 +344,9 @@ rmw_ret_t rmw_client_set_on_new_response_callback(
   const void * user_data)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    client, client->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * cli_data = static_cast<rmw_uds::UdsClient *>(client->data);
   std::lock_guard<std::mutex> lock(cli_data->callback_mutex);
   cli_data->on_new_response_cb = callback;
diff --git a/rmw_unix_socket_cpp/src/rmw_service.cpp b/rmw_unix_socket_cpp/src/rmw_service.cpp
index 63a041a..2b4db89 100644
--- a/rmw_unix_socket_cpp/src/rmw_service.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_service.cpp
@@ -132,6 +132,9 @@ rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service, service->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * srv_data = static_cast<rmw_uds::UdsService *>(service->data);
   if (srv_data) {
@@ -161,6 +164,9 @@ rmw_ret_t rmw_take_request(
   RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service, service->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   *taken = false;
   auto * srv_data = static_cast<rmw_uds::UdsService *>(service->data);
@@ -214,6 +220,9 @@ rmw_ret_t rmw_send_response(
   RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service, service->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * srv_data = static_cast<rmw_uds::UdsService *>(service->data);
 
@@ -275,6 +284,9 @@ rmw_ret_t rmw_service_request_subscription_get_actual_qos(
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service, service->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * srv_data = static_cast<rmw_uds::UdsService *>(service->data);
   *qos = srv_data->qos;
   return RMW_RET_OK;
@@ -286,6 +298,9 @@ rmw_ret_t rmw_service_response_publisher_get_actual_qos(
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service, service->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * srv_data = static_cast<rmw_uds::UdsService *>(service->data);
   *qos = srv_data->qos;
   return RMW_RET_OK;
@@ -297,6 +312,9 @@ rmw_ret_t rmw_service_set_on_new_request_callback(
   const void * user_data)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    service, service->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   auto * srv_data = static_cast<rmw_uds::UdsService *>(service->data);
   std::lock_guard<std::mutex> lock(srv_data->callback_mutex);
   srv_data->on_new_request_cb = callback;

From 8906cc34d220d3ec30096d367e473082010522dc Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 18:36:00 +0200
Subject: [PATCH 04/25] fix: return an error instead of silently dropping when
 rmw_send_response cannot find the client

---
 rmw_unix_socket_cpp/src/rmw_service.cpp | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_service.cpp b/rmw_unix_socket_cpp/src/rmw_service.cpp
index 2b4db89..a8cdfca 100644
--- a/rmw_unix_socket_cpp/src/rmw_service.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_service.cpp
@@ -274,8 +274,23 @@ rmw_ret_t rmw_send_response(
       return RMW_RET_OK;
     }
   }
-  // Client not found — might have disconnected
-  return RMW_RET_OK;
+
+  // Cache miss — the cache may be stale. Do an authoritative direct query by
+  // client GID before giving up so a response is never silently discarded.
+  auto clients = rmw_uds::registry_query(
+    header, rmw_uds::ENTRY_CLIENT, srv_data->service_name.c_str(),
+    nullptr, nullptr);
+  for (const auto & c : clients) {
+    if (c.socket_path.empty()) {continue;}
+    if (std::memcmp(c.gid, request_header->writer_guid, sizeof(c.gid)) == 0) {
+      rmw_uds::send_to(
+        srv_data->context->send_socket_fd,
+        c.socket_path, hdr, payload.data(), payload.size());
+      return RMW_RET_OK;
+    }
+  }
+  RMW_SET_ERROR_MSG("no client matching request id; response discarded");
+  return RMW_RET_ERROR;
 }
 
 rmw_ret_t rmw_service_request_subscription_get_actual_qos(

From 473740ac36c5725bae7fecbb27ef5444232dca79 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 18:48:00 +0200
Subject: [PATCH 05/25] fix: roll back rmw_create_node on strdup failure
 instead of returning a NULL-named node

---
 rmw_unix_socket_cpp/src/rmw_node.cpp | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/rmw_node.cpp b/rmw_unix_socket_cpp/src/rmw_node.cpp
index e2e09e8..d3b900c 100644
--- a/rmw_unix_socket_cpp/src/rmw_node.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_node.cpp
@@ -78,6 +78,22 @@ rmw_node_t * rmw_create_node(
   node->namespace_ = rcutils_strdup(namespace_, context->options.allocator);
   node->context = context;
 
+  if (!node->name || !node->namespace_) {
+    rcutils_allocator_t alloc = context->options.allocator;
+    if (node->name) {
+      alloc.deallocate(const_cast<char *>(node->name), alloc.state);
+    }
+    if (node->namespace_) {
+      alloc.deallocate(const_cast<char *>(node->namespace_), alloc.state);
+    }
+    rmw_node_free(node);
+    rmw_uds::registry_remove(header, node_data->registry_index);
+    auto _r [[maybe_unused]] = rmw_destroy_guard_condition(graph_gc);
+    delete node_data;
+    RMW_SET_ERROR_MSG("failed to copy node name/namespace");
+    return nullptr;
+  }
+
   return node;
 }
 

From 766082d7934b80703e608ccc3a8fc2dadf7362e2 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 19:00:00 +0200
Subject: [PATCH 06/25] fix: re-arm epoll on every wait so a reused fd number
 is never left unregistered

---
 rmw_unix_socket_cpp/src/rmw_wait.cpp | 98 +++++++++++++++-------------
 rmw_unix_socket_cpp/src/types.hpp    |  4 --
 2 files changed, 51 insertions(+), 51 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp
index 41fcc1b..c057936 100644
--- a/rmw_unix_socket_cpp/src/rmw_wait.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp
@@ -3,6 +3,7 @@
 #include "transport.hpp"
 #include "types.hpp"
 
+#include <cerrno>
 #include <chrono>
 #include <cstring>
 #include <limits>
@@ -223,6 +224,56 @@ rmw_ret_t rmw_wait(
     }
   }
 
+  // Arm every entity fd with epoll on every wait. EPOLL_CTL_ADD is idempotent
+  // here: a still-live fd returns EEXIST (already armed), while a fd number
+  // reused after its previous owner was closed gets freshly armed. The kernel
+  // auto-removes closed fds, so no EPOLL_CTL_DEL is needed.
+  {
+    struct epoll_event ev;
+    std::memset(&ev, 0, sizeof(ev));
+    auto register_fd = [&](int fd) {
+        if (fd < 0) {return;}
+        ev.events = EPOLLIN;
+        ev.data.fd = fd;
+        // EEXIST means the fd is already armed -> treat as success.
+        if (epoll_ctl(ws_data->epoll_fd, EPOLL_CTL_ADD, fd, &ev) != 0 &&
+          errno != EEXIST)
+        {
+          // Other errors are ignored, as before.
+        }
+      };
+
+    if (subscriptions) {
+      for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
+        if (!subscriptions->subscribers[i]) {continue;}
+        auto * sub = static_cast<rmw_uds::UdsSubscription *>(subscriptions->subscribers[i]);
+        register_fd(sub->socket_fd);
+      }
+    }
+    if (services) {
+      for (size_t i = 0; i < services->service_count; ++i) {
+        if (!services->services[i]) {continue;}
+        auto * srv = static_cast<rmw_uds::UdsService *>(services->services[i]);
+        register_fd(srv->socket_fd);
+      }
+    }
+    if (clients) {
+      for (size_t i = 0; i < clients->client_count; ++i) {
+        if (!clients->clients[i]) {continue;}
+        auto * cli = static_cast<rmw_uds::UdsClient *>(clients->clients[i]);
+        register_fd(cli->socket_fd);
+      }
+    }
+    if (guard_conditions) {
+      for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
+        if (!guard_conditions->guard_conditions[i]) {continue;}
+        auto * gc = static_cast<rmw_uds::UdsGuardCondition *>(
+          guard_conditions->guard_conditions[i]);
+        register_fd(gc->eventfd_fd);
+      }
+    }
+  }
+
   // 3. Check if anything is already ready
   bool something_ready = false;
 
@@ -276,53 +327,6 @@ rmw_ret_t rmw_wait(
 
   // 4. If nothing ready, block with epoll
   if (!something_ready) {
-    // PERFORMANCE: only EPOLL_CTL_ADD fds we haven't registered before.
-    // Closed fds are auto-removed by the kernel, so we never need DEL.
-    struct epoll_event ev;
-    std::memset(&ev, 0, sizeof(ev));
-
-    auto register_fd = [&](int fd) {
-        if (fd < 0) {return;}
-        if (ws_data->registered_fds.find(fd) != ws_data->registered_fds.end()) {
-          return;
-        }
-        ev.events = EPOLLIN;
-        ev.data.fd = fd;
-        if (epoll_ctl(ws_data->epoll_fd, EPOLL_CTL_ADD, fd, &ev) == 0) {
-          ws_data->registered_fds.insert(fd);
-        }
-      };
-
-    if (subscriptions) {
-      for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
-        if (!subscriptions->subscribers[i]) {continue;}
-        auto * sub = static_cast<rmw_uds::UdsSubscription *>(subscriptions->subscribers[i]);
-        register_fd(sub->socket_fd);
-      }
-    }
-    if (services) {
-      for (size_t i = 0; i < services->service_count; ++i) {
-        if (!services->services[i]) {continue;}
-        auto * srv = static_cast<rmw_uds::UdsService *>(services->services[i]);
-        register_fd(srv->socket_fd);
-      }
-    }
-    if (clients) {
-      for (size_t i = 0; i < clients->client_count; ++i) {
-        if (!clients->clients[i]) {continue;}
-        auto * cli = static_cast<rmw_uds::UdsClient *>(clients->clients[i]);
-        register_fd(cli->socket_fd);
-      }
-    }
-    if (guard_conditions) {
-      for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
-        if (!guard_conditions->guard_conditions[i]) {continue;}
-        auto * gc = static_cast<rmw_uds::UdsGuardCondition *>(
-          guard_conditions->guard_conditions[i]);
-        register_fd(gc->eventfd_fd);
-      }
-    }
-
     // Compute timeout. -1 means block forever (epoll_wait sentinel).
     int timeout_ms = -1;
     if (wait_timeout) {
diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp
index 6d4e4e4..1437ad7 100644
--- a/rmw_unix_socket_cpp/src/types.hpp
+++ b/rmw_unix_socket_cpp/src/types.hpp
@@ -239,10 +239,6 @@ struct UdsGuardCondition
 struct UdsWaitSet
 {
   int epoll_fd = -1;
-  // PERFORMANCE: track which fds are already registered with epoll so we
-  // don't issue EPOLL_CTL_ADD/DEL on every rmw_wait call. The kernel
-  // automatically removes fds from epoll when they are closed.
-  std::set<int> registered_fds;
 };
 
 }  // namespace rmw_uds

From 95c30c3e08a24538e4a85b593f39ca1099ee65a7 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 19:12:00 +0200
Subject: [PATCH 07/25] fix: free node-name arrays when enclave init fails in
 rmw_get_node_names_with_enclaves

---
 rmw_unix_socket_cpp/src/rmw_graph.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_graph.cpp b/rmw_unix_socket_cpp/src/rmw_graph.cpp
index d42a444..54a14f4 100644
--- a/rmw_unix_socket_cpp/src/rmw_graph.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_graph.cpp
@@ -119,7 +119,11 @@ rmw_ret_t rmw_get_node_names_with_enclaves(
 
   rcutils_allocator_t alloc = rcutils_get_default_allocator();
   auto ret2 = rcutils_string_array_init(enclaves, node_names->size, &alloc);
-  if (ret2 != RCUTILS_RET_OK) {return RMW_RET_ERROR;}
+  if (ret2 != RCUTILS_RET_OK) {
+    auto _rn [[maybe_unused]] = rcutils_string_array_fini(node_names);
+    auto _rns [[maybe_unused]] = rcutils_string_array_fini(node_namespaces);
+    return RMW_RET_ERROR;
+  }
 
   for (size_t i = 0; i < node_names->size; ++i) {
     enclaves->data[i] = rcutils_strdup("/", alloc);

From f238a3c7a5cc2161b414f8891297c36eadd6f315 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 19:24:00 +0200
Subject: [PATCH 08/25] fix: static_assert the on-wire WireHeader layout to
 guard against silent ABI breaks

---
 rmw_unix_socket_cpp/src/registry.hpp | 32 ++++++++++++++++++++++++++++
 rmw_unix_socket_cpp/src/types.hpp    | 12 +++++++++++
 2 files changed, 44 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/registry.hpp b/rmw_unix_socket_cpp/src/registry.hpp
index 672683a..b515444 100644
--- a/rmw_unix_socket_cpp/src/registry.hpp
+++ b/rmw_unix_socket_cpp/src/registry.hpp
@@ -2,6 +2,7 @@
 #define RMW_UNIX_SOCKET_CPP__REGISTRY_HPP_
 
 #include <atomic>
+#include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <string>
@@ -53,6 +54,17 @@ struct RegistryEntry
   uint32_t qos_depth;
 };
 
+// Shm-mapped layout guard (see RegistryEntrySlot below). These structs are
+// copied byte-for-byte through shared memory; the offsets below depend only on
+// the Linux ABI (pid_t == 4) and the fixed char-array sizes. A change here must
+// bump REGISTRY_VERSION.
+static_assert(sizeof(RegistryEntry) == 1164, "RegistryEntry shm layout changed");
+static_assert(offsetof(RegistryEntry, type) == 0, "RegistryEntry layout changed");
+static_assert(offsetof(RegistryEntry, pid) == 4, "RegistryEntry layout changed");
+static_assert(offsetof(RegistryEntry, gid) == 8, "RegistryEntry layout changed");
+static_assert(offsetof(RegistryEntry, node_name) == 24, "RegistryEntry layout changed");
+static_assert(offsetof(RegistryEntry, qos_depth) == 1160, "RegistryEntry layout changed");
+
 // Internal slot layout as stored in shared memory. Implements a per-slot
 // seqlock + atomic state machine so readers never need a global mutex.
 //
@@ -99,6 +111,18 @@ struct RegistryEntrySlot
   uint32_t qos_depth;
 };
 
+// Shm-mapped layout guard: RegistryEntrySlot is mapped across processes; DESIGN
+// pins it at 1168 bytes. The seqlock head (seq + state + pad) makes it 4 bytes
+// larger than RegistryEntry. sizeof(pthread_mutex_t) is NOT asserted (libc/arch
+// specific), so RegistryHeader's total size is intentionally left unguarded.
+static_assert(sizeof(RegistryEntrySlot) == 1168, "RegistryEntrySlot shm layout changed");
+static_assert(offsetof(RegistryEntrySlot, seq) == 0, "RegistryEntrySlot layout changed");
+static_assert(offsetof(RegistryEntrySlot, state) == 4, "RegistryEntrySlot layout changed");
+static_assert(offsetof(RegistryEntrySlot, pid) == 8, "RegistryEntrySlot layout changed");
+static_assert(offsetof(RegistryEntrySlot, gid) == 12, "RegistryEntrySlot layout changed");
+static_assert(offsetof(RegistryEntrySlot, node_name) == 28, "RegistryEntrySlot layout changed");
+static_assert(offsetof(RegistryEntrySlot, qos_depth) == 1164, "RegistryEntrySlot layout changed");
+
 struct RegistryHeader
 {
   std::atomic<uint32_t> version;
@@ -109,6 +133,14 @@ struct RegistryHeader
   pthread_mutex_t init_mutex;
 };
 
+// Header layout guard: only the integer prefix is asserted. The total size
+// depends on sizeof(pthread_mutex_t), which is libc/arch specific, so it is
+// deliberately not pinned here (DESIGN documents 56 bytes for glibc/x86_64).
+static_assert(offsetof(RegistryHeader, version) == 0, "RegistryHeader layout changed");
+static_assert(offsetof(RegistryHeader, max_entries) == 4, "RegistryHeader layout changed");
+static_assert(offsetof(RegistryHeader, generation) == 8, "RegistryHeader layout changed");
+static_assert(offsetof(RegistryHeader, init_mutex) == 16, "RegistryHeader layout changed");
+
 // Open or create the shared memory registry for the given domain_id.
 // Returns fd >= 0 on success, sets *out_ptr and *out_size.
 int registry_open(
diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp
index 1437ad7..c3a2488 100644
--- a/rmw_unix_socket_cpp/src/types.hpp
+++ b/rmw_unix_socket_cpp/src/types.hpp
@@ -2,6 +2,7 @@
 #define RMW_UNIX_SOCKET_CPP__TYPES_HPP_
 
 #include <atomic>
+#include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <deque>
@@ -67,6 +68,17 @@ struct __attribute__((packed)) WireHeader
   uint8_t msg_type;                     // 1 byte: 0=topic, 1=request, 2=response
 };
 
+// Wire-format guard: WireHeader is blitted onto the datagram, so its packed
+// layout is a cross-process / cross-build contract. DESIGN pins 16+8+8+4+1.
+// A field reorder or type change must be deliberate (and bump the protocol).
+static_assert(RMW_GID_STORAGE_SIZE == 16, "WireHeader assumes a 16-byte GID");
+static_assert(sizeof(WireHeader) == 37, "WireHeader wire layout changed");
+static_assert(offsetof(WireHeader, gid) == 0, "WireHeader layout changed");
+static_assert(offsetof(WireHeader, sequence_number) == 16, "WireHeader layout changed");
+static_assert(offsetof(WireHeader, source_timestamp_ns) == 24, "WireHeader layout changed");
+static_assert(offsetof(WireHeader, payload_size) == 32, "WireHeader layout changed");
+static_assert(offsetof(WireHeader, msg_type) == 36, "WireHeader layout changed");
+
 // Message stored in subscription/service/client queues
 struct ReceivedMessage
 {

From 5d872c4716aac59feeb8b85f8e1d839662511c74 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 19:36:00 +0200
Subject: [PATCH 09/25] fix: re-validate slot seq before cleanup_stale reclaims
 it to close an ABA window

---
 rmw_unix_socket_cpp/src/registry.cpp | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/registry.cpp b/rmw_unix_socket_cpp/src/registry.cpp
index ee79649..65d70fd 100644
--- a/rmw_unix_socket_cpp/src/registry.cpp
+++ b/rmw_unix_socket_cpp/src/registry.cpp
@@ -161,7 +161,9 @@ void registry_close(int fd, void * ptr, size_t size)
 
 // Snapshot one slot under the seqlock protocol. Returns true on a successful
 // snapshot of a non-empty slot, false if the slot is empty / we gave up.
-static bool snapshot_slot(RegistryEntrySlot * slot, RegistryEntrySlot * out)
+// On success, *out_seq (if non-null) receives the consistent even seq the
+// snapshot was taken at, so callers can detect a later remove+re-add (ABA).
+static bool snapshot_slot(RegistryEntrySlot * slot, RegistryEntrySlot * out, uint32_t * out_seq = nullptr)
 {
   for (int retry = 0; retry < MAX_READ_RETRIES; ++retry) {
     uint32_t s1 = slot->seq.load(std::memory_order_acquire);
@@ -193,6 +195,9 @@ static bool snapshot_slot(RegistryEntrySlot * slot, RegistryEntrySlot * out)
 
     uint32_t s2 = slot->seq.load(std::memory_order_acquire);
     if (s1 == s2) {
+      if (out_seq) {
+        *out_seq = s1;
+      }
       return true;
     }
     // Writer touched the slot — retry.
@@ -351,7 +356,8 @@ void registry_cleanup_stale(RegistryHeader * header)
 
   for (uint32_t i = 0; i < max; ++i) {
     RegistryEntrySlot snap;
-    if (!snapshot_slot(&slots[i], &snap)) {
+    uint32_t snap_seq;
+    if (!snapshot_slot(&slots[i], &snap, &snap_seq)) {
       continue;
     }
     if (snap.pid == 0) {
@@ -362,6 +368,13 @@ void registry_cleanup_stale(RegistryHeader * header)
     if (pid_is_alive_or_unreachable(snap.pid)) {
       continue;
     }
+    // ABA guard: a remove + live re-add of the same type leaves the state
+    // value unchanged but advances seq. Confirm the slot has not been touched
+    // since the snapshot, otherwise we would tear down a live entry. The
+    // acquire load is ordered before the acq_rel CAS by the control dependency.
+    if (slots[i].seq.load(std::memory_order_acquire) != snap_seq) {
+      continue;  // slot was rewritten since snapshot — not the entry we vetted
+    }
     // Try to claim the slot for removal. The CAS-expected value is the state
     // we observed in the snapshot.
     uint8_t expected = snap.state.load(std::memory_order_relaxed);

From abbd2a15329fd819521425ae5d928a9cd5745f35 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 19:48:00 +0200
Subject: [PATCH 10/25] fix: enforce identifier checks on
 subscription/graph/node entry points and guard graph node->data

---
 rmw_unix_socket_cpp/src/rmw_graph.cpp        | 30 ++++++++++++++++++++
 rmw_unix_socket_cpp/src/rmw_node.cpp         |  3 ++
 rmw_unix_socket_cpp/src/rmw_subscription.cpp |  9 ++++++
 3 files changed, 42 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/rmw_graph.cpp b/rmw_unix_socket_cpp/src/rmw_graph.cpp
index 54a14f4..7a34f62 100644
--- a/rmw_unix_socket_cpp/src/rmw_graph.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_graph.cpp
@@ -22,6 +22,9 @@
 
 static rmw_uds::UdsContext * get_context(const rmw_node_t * node)
 {
+  if (!node->data) {
+    return nullptr;
+  }
   auto * nd = static_cast<rmw_uds::UdsNode *>(node->data);
   return nd->context;
 }
@@ -139,6 +142,9 @@ rmw_ret_t rmw_count_publishers(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto pubs = query_all(ctx, rmw_uds::ENTRY_PUBLISHER, topic_name);
@@ -154,6 +160,9 @@ rmw_ret_t rmw_count_subscribers(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto subs = query_all(ctx, rmw_uds::ENTRY_SUBSCRIPTION, topic_name);
@@ -169,6 +178,9 @@ rmw_ret_t rmw_count_clients(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto clients = query_all(ctx, rmw_uds::ENTRY_CLIENT, service_name);
@@ -184,6 +196,9 @@ rmw_ret_t rmw_count_services(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(service_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto services = query_all(ctx, rmw_uds::ENTRY_SERVICE, service_name);
@@ -201,6 +216,9 @@ rmw_ret_t rmw_get_topic_names_and_types(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto pubs = query_all(ctx, rmw_uds::ENTRY_PUBLISHER);
@@ -225,6 +243,9 @@ rmw_ret_t rmw_get_service_names_and_types(
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto services = query_all(ctx, rmw_uds::ENTRY_SERVICE);
@@ -255,6 +276,9 @@ static rmw_ret_t get_names_types_by_node(
   RMW_CHECK_ARGUMENT_FOR_NULL(node_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(node_namespace, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(names_and_types, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto results = query_all(ctx, entry_type, nullptr, node_name, node_namespace);
@@ -331,6 +355,9 @@ rmw_ret_t rmw_get_publishers_info_by_topic(
   RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(publishers_info, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto pubs = query_all(ctx, rmw_uds::ENTRY_PUBLISHER, topic_name);
@@ -374,6 +401,9 @@ rmw_ret_t rmw_get_subscriptions_info_by_topic(
   RMW_CHECK_ARGUMENT_FOR_NULL(allocator, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(subscriptions_info, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * ctx = get_context(node);
   auto subs = query_all(ctx, rmw_uds::ENTRY_SUBSCRIPTION, topic_name);
diff --git a/rmw_unix_socket_cpp/src/rmw_node.cpp b/rmw_unix_socket_cpp/src/rmw_node.cpp
index d3b900c..b2256df 100644
--- a/rmw_unix_socket_cpp/src/rmw_node.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_node.cpp
@@ -134,6 +134,9 @@ rmw_ret_t rmw_destroy_node(rmw_node_t * node)
 rmw_ret_t rmw_node_assert_liveliness(const rmw_node_t * node)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node, node->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
   return RMW_RET_OK;
 }
 
diff --git a/rmw_unix_socket_cpp/src/rmw_subscription.cpp b/rmw_unix_socket_cpp/src/rmw_subscription.cpp
index 4758991..1fc0961 100644
--- a/rmw_unix_socket_cpp/src/rmw_subscription.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_subscription.cpp
@@ -353,6 +353,9 @@ rmw_ret_t rmw_take_sequence(
   RMW_CHECK_ARGUMENT_FOR_NULL(message_sequence, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(message_info_sequence, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription, subscription->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   *taken = 0;
 
@@ -414,6 +417,9 @@ rmw_ret_t rmw_take_serialized_message(
   RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(serialized_message, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription, subscription->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   *taken = false;
   auto * sub_data = static_cast<rmw_uds::UdsSubscription *>(subscription->data);
@@ -448,6 +454,9 @@ rmw_ret_t rmw_take_serialized_message_with_info(
   RMW_CHECK_ARGUMENT_FOR_NULL(serialized_message, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription, subscription->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   *taken = false;
   auto * sub_data = static_cast<rmw_uds::UdsSubscription *>(subscription->data);

From 24f4a7c7335f9594eb52df3fe491c78f4ab6afd2 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 20:00:00 +0200
Subject: [PATCH 11/25] fix: retry epoll_wait on EINTR instead of returning a
 spurious early timeout

---
 rmw_unix_socket_cpp/src/rmw_wait.cpp | 26 ++++++++++++++++++++++++--
 1 file changed, 24 insertions(+), 2 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp
index c057936..a57f0bb 100644
--- a/rmw_unix_socket_cpp/src/rmw_wait.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp
@@ -343,9 +343,31 @@ rmw_ret_t rmw_wait(
       }
     }
 
-    // Block
+    // Block, retrying on EINTR. A finite timeout uses a steady_clock deadline
+    // so a signal interruption neither returns TIMEOUT early nor busy-loops.
     struct epoll_event ready_events[64];
-    epoll_wait(ws_data->epoll_fd, ready_events, 64, timeout_ms);
+    const int64_t deadline_ns =
+      (timeout_ms >= 0) ? now_ns() + static_cast<int64_t>(timeout_ms) * 1000000 : 0;
+    int remaining_ms = timeout_ms;
+    while (true) {
+      int n = epoll_wait(ws_data->epoll_fd, ready_events, 64, remaining_ms);
+      if (n >= 0) {
+        break;
+      }
+      if (errno != EINTR) {
+        RMW_SET_ERROR_MSG("epoll_wait failed");
+        return RMW_RET_ERROR;
+      }
+      if (timeout_ms < 0) {
+        continue;  // Infinite wait: just re-block.
+      }
+      const int64_t rem_ns = deadline_ns - now_ns();
+      if (rem_ns <= 0) {
+        break;  // Deadline passed -> timeout; fall through to drain.
+      }
+      const int64_t rem_ms = rem_ns / 1000000;
+      remaining_ms = (rem_ms > 0) ? static_cast<int>(rem_ms) : 1;  // >=1ms while time remains
+    }
     // No EPOLL_CTL_DEL needed — fds stay registered across calls.
 
     // Drain again after epoll

From 03a607c1f549b8c6bacb53ad2110d7dcbf92acf3 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 20:12:00 +0200
Subject: [PATCH 12/25] fix: make UdsContext::last_registry_generation atomic
 to avoid a data race in rmw_wait

---
 rmw_unix_socket_cpp/src/rmw_init.cpp | 3 ++-
 rmw_unix_socket_cpp/src/rmw_wait.cpp | 4 ++--
 rmw_unix_socket_cpp/src/types.hpp    | 2 +-
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp
index f3f6d28..e351d9d 100644
--- a/rmw_unix_socket_cpp/src/rmw_init.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_init.cpp
@@ -147,7 +147,8 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
 
   // Read initial generation
   auto * header = rmw_uds::registry_header(ctx->registry_ptr);
-  ctx->last_registry_generation = rmw_uds::registry_generation(header);
+  ctx->last_registry_generation.store(
+    rmw_uds::registry_generation(header), std::memory_order_relaxed);
 
   context->implementation_identifier = rmw_uds::identifier;
   context->impl = reinterpret_cast<rmw_context_impl_t *>(ctx);
diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp
index a57f0bb..aab74cc 100644
--- a/rmw_unix_socket_cpp/src/rmw_wait.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp
@@ -169,8 +169,8 @@ rmw_ret_t rmw_wait(
   if (ctx && ctx->registry_ptr) {
     auto * header = rmw_uds::registry_header(ctx->registry_ptr);
     uint64_t gen = rmw_uds::registry_generation(header);
-    if (gen != ctx->last_registry_generation) {
-      ctx->last_registry_generation = gen;
+    if (gen != ctx->last_registry_generation.load(std::memory_order_relaxed)) {
+      ctx->last_registry_generation.store(gen, std::memory_order_relaxed);
 
       // TRANSIENT_LOCAL late-joiner replay. Lock held across the loop —
       // rmw_destroy_publisher takes the same mutex then deletes, so this
diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp
index c3a2488..c00ded3 100644
--- a/rmw_unix_socket_cpp/src/types.hpp
+++ b/rmw_unix_socket_cpp/src/types.hpp
@@ -99,7 +99,7 @@ struct UdsContext
   size_t registry_size = 0;
   int send_socket_fd = -1;
   std::atomic<bool> is_shutdown{false};
-  uint64_t last_registry_generation = 0;
+  std::atomic<uint64_t> last_registry_generation{0};
   rmw_guard_condition_t * graph_guard_condition = nullptr;
 
   // TRANSIENT_LOCAL publishers, for wait-side cache replay on graph change.

From 0b983e4c4a365e43e3dd73950d0ddad58f012a20 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 20:24:00 +0200
Subject: [PATCH 13/25] fix: null-check guard_condition->data before triggering

---
 rmw_unix_socket_cpp/src/rmw_guard_condition.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/rmw_guard_condition.cpp b/rmw_unix_socket_cpp/src/rmw_guard_condition.cpp
index cea3d34..862c23a 100644
--- a/rmw_unix_socket_cpp/src/rmw_guard_condition.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_guard_condition.cpp
@@ -74,6 +74,9 @@ rmw_ret_t rmw_trigger_guard_condition(
     rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   auto * gc_data = static_cast<rmw_uds::UdsGuardCondition *>(guard_condition->data);
+  if (!gc_data || gc_data->eventfd_fd < 0) {
+    return RMW_RET_INVALID_ARGUMENT;
+  }
   uint64_t val = 1;
   ssize_t ret = write(gc_data->eventfd_fd, &val, sizeof(val));
   (void)ret;

From 24d1fbef69d25e54bcfa5d2da1e922f0dd05c35b Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 20:36:00 +0200
Subject: [PATCH 14/25] fix: read each guard-condition eventfd once per wait
 instead of a racy read-then-write-back

---
 rmw_unix_socket_cpp/src/rmw_wait.cpp | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp
index aab74cc..12fea08 100644
--- a/rmw_unix_socket_cpp/src/rmw_wait.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp
@@ -277,6 +277,11 @@ rmw_ret_t rmw_wait(
   // 3. Check if anything is already ready
   bool something_ready = false;
 
+  // Per-GC readiness from the step-3 consuming read, carried to step 5. One
+  // read drains the whole eventfd counter, so we never write it back (a
+  // non-atomic read-back would race a concurrent trigger and inflate it).
+  std::vector<bool> gc_triggered;
+
   if (subscriptions) {
     for (size_t i = 0; i < subscriptions->subscriber_count; ++i) {
       if (!subscriptions->subscribers[i]) {continue;}
@@ -311,6 +316,7 @@ rmw_ret_t rmw_wait(
   }
 
   if (guard_conditions) {
+    gc_triggered.assign(guard_conditions->guard_condition_count, false);
     for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) {
       if (!guard_conditions->guard_conditions[i]) {continue;}
       auto * gc = static_cast<rmw_uds::UdsGuardCondition *>(
@@ -319,8 +325,7 @@ rmw_ret_t rmw_wait(
       ssize_t r = read(gc->eventfd_fd, &val, sizeof(val));
       if (r == static_cast<ssize_t>(sizeof(val))) {
         something_ready = true;
-        // Write it back so it stays triggered for the ready check below
-        write(gc->eventfd_fd, &val, sizeof(val));
+        gc_triggered[i] = true;
       }
     }
   }
@@ -417,8 +422,10 @@ rmw_ret_t rmw_wait(
       auto * gc = static_cast<rmw_uds::UdsGuardCondition *>(
         guard_conditions->guard_conditions[i]);
       uint64_t val;
+      // Consume a trigger that landed during the epoll block; combine with the
+      // step-3 read so a GC seen ready then stays reported without a read-back.
       ssize_t r = read(gc->eventfd_fd, &val, sizeof(val));
-      if (r == static_cast<ssize_t>(sizeof(val))) {
+      if (r == static_cast<ssize_t>(sizeof(val)) || gc_triggered[i]) {
         any_ready = true;
       } else {
         guard_conditions->guard_conditions[i] = nullptr;

From 5a955b9ca2ce3b8a998bd24222d2fe9a985be703 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 20:48:00 +0200
Subject: [PATCH 15/25] fix: skip a corrupt request/response datagram instead
 of losing the in-flight round-trip

---
 rmw_unix_socket_cpp/src/rmw_client.cpp  | 43 +++++++++++++----------
 rmw_unix_socket_cpp/src/rmw_service.cpp | 45 ++++++++++++++-----------
 2 files changed, 51 insertions(+), 37 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_client.cpp b/rmw_unix_socket_cpp/src/rmw_client.cpp
index 336e7ba..487cfff 100644
--- a/rmw_unix_socket_cpp/src/rmw_client.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_client.cpp
@@ -1,4 +1,5 @@
 #include "identifier.hpp"
+#include "logging.hpp"
 #include "registry.hpp"
 #include "serialization.hpp"
 #include "transport.hpp"
@@ -250,28 +251,34 @@ rmw_ret_t rmw_take_response(
   }
 
   std::lock_guard<std::mutex> lock(cli_data->queue_mutex);
-  if (cli_data->response_queue.empty()) {
-    std::memset(request_header, 0, sizeof(rmw_service_info_t));
-    return RMW_RET_OK;
-  }
+  // Drop-garbage-and-continue: one corrupt datagram must not destroy a
+  // legitimate in-flight response and wedge the call into a timeout.
+  while (!cli_data->response_queue.empty()) {
+    auto msg = std::move(cli_data->response_queue.front());
+    cli_data->response_queue.pop_front();
+
+    if (!rmw_uds::deserialize(msg.payload.data(), msg.payload.size(),
+      cli_data->response_callbacks, ros_response))
+    {
+      RMW_UDS_LOG_ERROR_THROTTLE(
+        1000,
+        "rmw_take_response: CDR deserialization failed on service '%s' "
+        "(payload=%zu bytes) — dropping datagram",
+        cli_data->service_name.c_str(), msg.payload.size());
+      continue;
+    }
 
-  auto msg = std::move(cli_data->response_queue.front());
-  cli_data->response_queue.pop_front();
+    std::memcpy(request_header->request_id.writer_guid, msg.header.gid,
+      RMW_GID_STORAGE_SIZE);
+    request_header->request_id.sequence_number = msg.header.sequence_number;
+    request_header->source_timestamp = msg.header.source_timestamp_ns;
+    request_header->received_timestamp = msg.received_timestamp_ns;
 
-  if (!rmw_uds::deserialize(msg.payload.data(), msg.payload.size(),
-    cli_data->response_callbacks, ros_response))
-  {
-    RMW_SET_ERROR_MSG("failed to deserialize response");
-    return RMW_RET_ERROR;
+    *taken = true;
+    return RMW_RET_OK;
   }
 
-  std::memcpy(request_header->request_id.writer_guid, msg.header.gid,
-    RMW_GID_STORAGE_SIZE);
-  request_header->request_id.sequence_number = msg.header.sequence_number;
-  request_header->source_timestamp = msg.header.source_timestamp_ns;
-  request_header->received_timestamp = msg.received_timestamp_ns;
-
-  *taken = true;
+  std::memset(request_header, 0, sizeof(rmw_service_info_t));
   return RMW_RET_OK;
 }
 
diff --git a/rmw_unix_socket_cpp/src/rmw_service.cpp b/rmw_unix_socket_cpp/src/rmw_service.cpp
index a8cdfca..1bf5a1b 100644
--- a/rmw_unix_socket_cpp/src/rmw_service.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_service.cpp
@@ -1,4 +1,5 @@
 #include "identifier.hpp"
+#include "logging.hpp"
 #include "registry.hpp"
 #include "serialization.hpp"
 #include "transport.hpp"
@@ -186,29 +187,35 @@ rmw_ret_t rmw_take_request(
   }
 
   std::lock_guard<std::mutex> lock(srv_data->queue_mutex);
-  if (srv_data->request_queue.empty()) {
-    std::memset(request_header, 0, sizeof(rmw_service_info_t));
-    return RMW_RET_OK;
-  }
+  // Drop-garbage-and-continue: one corrupt datagram must not destroy a
+  // legitimate in-flight request and wedge the client into a timeout.
+  while (!srv_data->request_queue.empty()) {
+    auto msg = std::move(srv_data->request_queue.front());
+    srv_data->request_queue.pop_front();
+
+    if (!rmw_uds::deserialize(msg.payload.data(), msg.payload.size(),
+      srv_data->request_callbacks, ros_request))
+    {
+      RMW_UDS_LOG_ERROR_THROTTLE(
+        1000,
+        "rmw_take_request: CDR deserialization failed on service '%s' "
+        "(payload=%zu bytes) — dropping datagram",
+        srv_data->service_name.c_str(), msg.payload.size());
+      continue;
+    }
 
-  auto msg = std::move(srv_data->request_queue.front());
-  srv_data->request_queue.pop_front();
+    // Fill request header
+    std::memcpy(request_header->request_id.writer_guid, msg.header.gid,
+      RMW_GID_STORAGE_SIZE);
+    request_header->request_id.sequence_number = msg.header.sequence_number;
+    request_header->source_timestamp = msg.header.source_timestamp_ns;
+    request_header->received_timestamp = msg.received_timestamp_ns;
 
-  if (!rmw_uds::deserialize(msg.payload.data(), msg.payload.size(),
-    srv_data->request_callbacks, ros_request))
-  {
-    RMW_SET_ERROR_MSG("failed to deserialize request");
-    return RMW_RET_ERROR;
+    *taken = true;
+    return RMW_RET_OK;
   }
 
-  // Fill request header
-  std::memcpy(request_header->request_id.writer_guid, msg.header.gid,
-    RMW_GID_STORAGE_SIZE);
-  request_header->request_id.sequence_number = msg.header.sequence_number;
-  request_header->source_timestamp = msg.header.source_timestamp_ns;
-  request_header->received_timestamp = msg.received_timestamp_ns;
-
-  *taken = true;
+  std::memset(request_header, 0, sizeof(rmw_service_info_t));
   return RMW_RET_OK;
 }
 

From dec184133a7ebc984dda367928cc3538f8db2d21 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 21:00:00 +0200
Subject: [PATCH 16/25] fix: cache the pre-scan registry generation so a
 concurrent endpoint is not dropped until the next change

---
 rmw_unix_socket_cpp/src/rmw_client.cpp    | 2 +-
 rmw_unix_socket_cpp/src/rmw_publisher.cpp | 6 ++----
 rmw_unix_socket_cpp/src/rmw_service.cpp   | 2 +-
 3 files changed, 4 insertions(+), 6 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_client.cpp b/rmw_unix_socket_cpp/src/rmw_client.cpp
index 487cfff..b6f9bb3 100644
--- a/rmw_unix_socket_cpp/src/rmw_client.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_client.cpp
@@ -198,7 +198,7 @@ rmw_ret_t rmw_send_request(
       auto services = rmw_uds::registry_query(
         header, rmw_uds::ENTRY_SERVICE, cli_data->service_name.c_str(),
         nullptr, nullptr);
-      cli_data->cached_generation = rmw_uds::registry_generation(header);
+      cli_data->cached_generation = current_gen;
       cli_data->cached_service_path.clear();
       for (const auto & srv : services) {
         if (!srv.socket_path.empty()) {
diff --git a/rmw_unix_socket_cpp/src/rmw_publisher.cpp b/rmw_unix_socket_cpp/src/rmw_publisher.cpp
index d75887c..0269a95 100644
--- a/rmw_unix_socket_cpp/src/rmw_publisher.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_publisher.cpp
@@ -216,12 +216,10 @@ rmw_ret_t rmw_publish(
   {
     std::lock_guard<std::mutex> lock(pub_data->sub_cache_mutex);
     if (current_gen != pub_data->cached_generation) {
-      // Re-read generation after the query so we don't miss updates that
-      // happened during the (lock-free) scan.
       auto subs = rmw_uds::registry_query(
         header, rmw_uds::ENTRY_SUBSCRIPTION, pub_data->topic_name.c_str(),
         nullptr, nullptr);
-      pub_data->cached_generation = rmw_uds::registry_generation(header);
+      pub_data->cached_generation = current_gen;
       pub_data->cached_subscriber_paths.clear();
       pub_data->cached_subscriber_paths.reserve(subs.size());
       for (const auto & s : subs) {
@@ -310,7 +308,7 @@ rmw_ret_t rmw_publish_serialized_message(
       auto subs = rmw_uds::registry_query(
         header, rmw_uds::ENTRY_SUBSCRIPTION, pub_data->topic_name.c_str(),
         nullptr, nullptr);
-      pub_data->cached_generation = rmw_uds::registry_generation(header);
+      pub_data->cached_generation = current_gen;
       pub_data->cached_subscriber_paths.clear();
       pub_data->cached_subscriber_paths.reserve(subs.size());
       for (const auto & s : subs) {
diff --git a/rmw_unix_socket_cpp/src/rmw_service.cpp b/rmw_unix_socket_cpp/src/rmw_service.cpp
index 1bf5a1b..84207ec 100644
--- a/rmw_unix_socket_cpp/src/rmw_service.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_service.cpp
@@ -259,7 +259,7 @@ rmw_ret_t rmw_send_response(
       auto clients = rmw_uds::registry_query(
         header, rmw_uds::ENTRY_CLIENT, srv_data->service_name.c_str(),
         nullptr, nullptr);
-      srv_data->cached_generation = rmw_uds::registry_generation(header);
+      srv_data->cached_generation = current_gen;
       srv_data->cached_clients.clear();
       srv_data->cached_clients.reserve(clients.size());
       for (const auto & c : clients) {

From 239d47c372ee29cfd21547defc27d36c90ead21c Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 21:12:00 +0200
Subject: [PATCH 17/25] fix: check rcutils_strdup in graph queries and return
 BAD_ALLOC instead of NULL array elements

---
 rmw_unix_socket_cpp/src/rmw_graph.cpp | 23 +++++++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/rmw_unix_socket_cpp/src/rmw_graph.cpp b/rmw_unix_socket_cpp/src/rmw_graph.cpp
index 7a34f62..3f392c9 100644
--- a/rmw_unix_socket_cpp/src/rmw_graph.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_graph.cpp
@@ -55,6 +55,11 @@ static rmw_ret_t fill_names_and_types(
   size_t idx = 0;
   for (const auto & [topic, types] : topic_types) {
     names_and_types->names.data[idx] = rcutils_strdup(topic.c_str(), *allocator);
+    if (!names_and_types->names.data[idx]) {
+      auto _r [[maybe_unused]] = rmw_names_and_types_fini(names_and_types);
+      RMW_SET_ERROR_MSG("failed to allocate topic name");
+      return RMW_RET_BAD_ALLOC;
+    }
 
     auto ret2 = rcutils_string_array_init(
       &names_and_types->types[idx], types.size(), allocator);
@@ -66,6 +71,11 @@ static rmw_ret_t fill_names_and_types(
     size_t tidx = 0;
     for (const auto & t : types) {
       names_and_types->types[idx].data[tidx] = rcutils_strdup(t.c_str(), *allocator);
+      if (!names_and_types->types[idx].data[tidx]) {
+        auto _r [[maybe_unused]] = rmw_names_and_types_fini(names_and_types);
+        RMW_SET_ERROR_MSG("failed to allocate type name");
+        return RMW_RET_BAD_ALLOC;
+      }
       tidx++;
     }
     idx++;
@@ -103,6 +113,12 @@ rmw_ret_t rmw_get_node_names(
   for (size_t i = 0; i < nodes.size(); ++i) {
     node_names->data[i] = rcutils_strdup(nodes[i].node_name.c_str(), alloc);
     node_namespaces->data[i] = rcutils_strdup(nodes[i].node_namespace.c_str(), alloc);
+    if (!node_names->data[i] || !node_namespaces->data[i]) {
+      auto _rn [[maybe_unused]] = rcutils_string_array_fini(node_names);
+      auto _rns [[maybe_unused]] = rcutils_string_array_fini(node_namespaces);
+      RMW_SET_ERROR_MSG("failed to allocate node name/namespace");
+      return RMW_RET_BAD_ALLOC;
+    }
   }
 
   return RMW_RET_OK;
@@ -130,6 +146,13 @@ rmw_ret_t rmw_get_node_names_with_enclaves(
 
   for (size_t i = 0; i < node_names->size; ++i) {
     enclaves->data[i] = rcutils_strdup("/", alloc);
+    if (!enclaves->data[i]) {
+      auto _rn [[maybe_unused]] = rcutils_string_array_fini(node_names);
+      auto _rns [[maybe_unused]] = rcutils_string_array_fini(node_namespaces);
+      auto _re [[maybe_unused]] = rcutils_string_array_fini(enclaves);
+      RMW_SET_ERROR_MSG("failed to allocate enclave");
+      return RMW_RET_BAD_ALLOC;
+    }
   }
   return RMW_RET_OK;
 }

From 655e7c357e1bf5d4ec016f1ffe57857daed02959 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 21:24:00 +0200
Subject: [PATCH 18/25] fix: add identifier/double-init guards and check
 enclave strdup + ensure_socket_dir in rmw_init

---
 rmw_unix_socket_cpp/src/rmw_init.cpp | 42 ++++++++++++++++++++++++----
 1 file changed, 37 insertions(+), 5 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp
index e351d9d..ff19052 100644
--- a/rmw_unix_socket_cpp/src/rmw_init.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_init.cpp
@@ -1,3 +1,5 @@
+#include <sys/stat.h>
+
 #include "identifier.hpp"
 #include "logging.hpp"
 #include "registry.hpp"
@@ -90,6 +92,13 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
 {
   RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    options, options->implementation_identifier,
+    rmw_uds::identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  if (context->impl != nullptr) {
+    RMW_SET_ERROR_MSG("expected a zero-initialized context");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
 
   auto * ctx = new (std::nothrow) rmw_uds::UdsContext();
   if (!ctx) {
@@ -103,8 +112,19 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
   }
   ctx->domain_id = domain_id;
 
-  // Ensure socket directory exists
-  rmw_uds::ensure_socket_dir(domain_id);
+  // Ensure socket directory exists. ensure_socket_dir swallows mkdir errors and
+  // only returns the path, so validate the directory here (in-scope) rather than
+  // changing its signature; downstream registry/socket setup would otherwise fail
+  // with a less specific error.
+  std::string socket_dir = rmw_uds::ensure_socket_dir(domain_id);
+  struct stat dir_st;
+  if (stat(socket_dir.c_str(), &dir_st) != 0 || !S_ISDIR(dir_st.st_mode)) {
+    RMW_UDS_LOG_ERROR(
+      "rmw_init: socket directory '%s' is not usable", socket_dir.c_str());
+    delete ctx;
+    RMW_SET_ERROR_MSG("failed to create socket directory");
+    return RMW_RET_ERROR;
+  }
 
   // Open shared memory registry
   ctx->registry_fd = rmw_uds::registry_open(
@@ -150,13 +170,25 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
   ctx->last_registry_generation.store(
     rmw_uds::registry_generation(header), std::memory_order_relaxed);
 
+  // Deep-copy the caller-owned enclave string before committing any field on
+  // context, so a copy failure can tear the context down cleanly.
+  char * enclave_copy = nullptr;
+  if (options->enclave) {
+    enclave_copy = rcutils_strdup(options->enclave, options->allocator);
+    if (!enclave_copy) {
+      close(ctx->send_socket_fd);
+      rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size);
+      delete ctx;
+      RMW_SET_ERROR_MSG("failed to copy enclave string");
+      return RMW_RET_BAD_ALLOC;
+    }
+  }
+
   context->implementation_identifier = rmw_uds::identifier;
   context->impl = reinterpret_cast<rmw_context_impl_t *>(ctx);
   context->instance_id = options->instance_id;
   context->options = *options;
-  if (options->enclave) {
-    context->options.enclave = rcutils_strdup(options->enclave, options->allocator);
-  }
+  context->options.enclave = enclave_copy;
   // discovery_options.static_peers is heap-owned; deep-copy it so context->options
   // owns its own array rather than aliasing the caller's options (mirrors enclave).
   context->options.discovery_options = rmw_get_zero_initialized_discovery_options();

From 177d0c2b26b109902afd5c2f3015d2a1954614b8 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 21:36:00 +0200
Subject: [PATCH 19/25] fix: guard node->context before dereferencing it in
 destroy paths

---
 rmw_unix_socket_cpp/src/rmw_node.cpp      | 3 ++-
 rmw_unix_socket_cpp/src/rmw_publisher.cpp | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_node.cpp b/rmw_unix_socket_cpp/src/rmw_node.cpp
index b2256df..f299e98 100644
--- a/rmw_unix_socket_cpp/src/rmw_node.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_node.cpp
@@ -119,7 +119,8 @@ rmw_ret_t rmw_destroy_node(rmw_node_t * node)
     delete node_data;
   }
 
-  rcutils_allocator_t alloc = node->context->options.allocator;
+  rcutils_allocator_t alloc = node->context ?
+    node->context->options.allocator : rcutils_get_default_allocator();
   if (node->name) {
     alloc.deallocate(const_cast<char *>(node->name), alloc.state);
   }
diff --git a/rmw_unix_socket_cpp/src/rmw_publisher.cpp b/rmw_unix_socket_cpp/src/rmw_publisher.cpp
index 0269a95..10c3dfc 100644
--- a/rmw_unix_socket_cpp/src/rmw_publisher.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_publisher.cpp
@@ -164,7 +164,8 @@ rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher)
     delete pub_data;
   }
 
-  rcutils_allocator_t alloc = node->context->options.allocator;
+  rcutils_allocator_t alloc = node->context ?
+    node->context->options.allocator : rcutils_get_default_allocator();
   if (publisher->topic_name) {
     alloc.deallocate(const_cast<char *>(publisher->topic_name), alloc.state);
   }

From 35a06975008a833b04983f465d02c0748b1a5a23 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 21:48:00 +0200
Subject: [PATCH 20/25] fix: compute serialize data_length in size_t to avoid a
 uint32 wrap

---
 rmw_unix_socket_cpp/src/serialization.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rmw_unix_socket_cpp/src/serialization.cpp b/rmw_unix_socket_cpp/src/serialization.cpp
index 9883f79..3391293 100644
--- a/rmw_unix_socket_cpp/src/serialization.cpp
+++ b/rmw_unix_socket_cpp/src/serialization.cpp
@@ -104,7 +104,7 @@ bool serialize(
 {
   try {
     // Get estimated serialized size (4 bytes encapsulation + message)
-    auto data_length = 4 + callbacks->get_serialized_size(ros_message);
+    size_t data_length = size_t{4} + callbacks->get_serialized_size(ros_message);
     buffer.resize(data_length);
 
     eprosima::fastcdr::FastBuffer fastbuffer(

From 3703fbf6cb8647b0271c70e1bf968460fb8141cf Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 22:00:00 +0200
Subject: [PATCH 21/25] fix: append QoS-incompatibility reasons instead of
 overwriting, and correct stale comments

---
 rmw_unix_socket_cpp/src/rmw_features.cpp | 20 ++++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_features.cpp b/rmw_unix_socket_cpp/src/rmw_features.cpp
index 399fe1d..82e1664 100644
--- a/rmw_unix_socket_cpp/src/rmw_features.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_features.cpp
@@ -52,13 +52,25 @@ rmw_ret_t rmw_qos_profile_check_compatible(
     reason[0] = '\0';
   }
 
+  // Appends msg to reason (with a "; " separator) so multiple mismatches are
+  // all reported instead of the last one overwriting the rest.
+  size_t reason_len = 0;
   auto set_reason = [&](const char * msg) {
-      if (reason && reason_size > 0) {
-        std::snprintf(reason, reason_size, "%s", msg);
+      if (!reason || reason_size == 0 || reason_len >= reason_size - 1) {
+        return;
+      }
+      const char * sep = (reason_len > 0) ? "; " : "";
+      int n = std::snprintf(
+        reason + reason_len, reason_size - reason_len, "%s%s", sep, msg);
+      // snprintf returns the length it would have written; clamp to the
+      // space actually consumed so reason_len never overruns the buffer.
+      if (n > 0) {
+        size_t avail = reason_size - reason_len - 1;
+        reason_len += (static_cast<size_t>(n) < avail) ? static_cast<size_t>(n) : avail;
       }
     };
 
-  // Reliability: BEST_EFFORT pub + RELIABLE sub = incompatible
+  // Reliability: BEST_EFFORT pub + RELIABLE sub = warning
   auto pub_rel = publisher_profile.reliability;
   auto sub_rel = subscription_profile.reliability;
   if (pub_rel == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT &&
@@ -68,7 +80,7 @@ rmw_ret_t rmw_qos_profile_check_compatible(
     set_reason("best effort publisher with reliable subscriber");
   }
 
-  // Durability: VOLATILE pub + TRANSIENT_LOCAL sub = incompatible
+  // Durability: VOLATILE pub + TRANSIENT_LOCAL sub = warning
   auto pub_dur = publisher_profile.durability;
   auto sub_dur = subscription_profile.durability;
   if (pub_dur == RMW_QOS_POLICY_DURABILITY_VOLATILE &&

From 193dfb0db78104d3ff1ef3d4cc4e2decdc63c5f6 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 22:12:00 +0200
Subject: [PATCH 22/25] perf: move the payload into the TRANSIENT_LOCAL cache
 instead of copying it

---
 rmw_unix_socket_cpp/src/rmw_publisher.cpp | 12 +++++++++++-
 1 file changed, 11 insertions(+), 1 deletion(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_publisher.cpp b/rmw_unix_socket_cpp/src/rmw_publisher.cpp
index 10c3dfc..eb1da62 100644
--- a/rmw_unix_socket_cpp/src/rmw_publisher.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_publisher.cpp
@@ -238,7 +238,7 @@ rmw_ret_t rmw_publish(
 
     rmw_uds::CachedMessage cached;
     cached.header = hdr;
-    cached.payload = payload;
+    cached.payload = std::move(payload);  // last use of payload
     pub_data->message_cache.push_back(std::move(cached));
     while (pub_data->message_cache.size() > pub_data->qos.depth) {
       pub_data->message_cache.pop_front();
@@ -255,6 +255,16 @@ rmw_ret_t rmw_publish(
         }
       }
     }
+
+    // Send the current message, read from the cached copy since payload was
+    // moved. Trimming never disturbs back(), so it is the message just pushed.
+    const auto & current = pub_data->message_cache.back();
+    for (const auto & path : sub_paths) {
+      rmw_uds::send_to(
+        pub_data->context->send_socket_fd,
+        path, current.header, current.payload.data(), current.payload.size());
+    }
+    return RMW_RET_OK;
   }
 
   // Surface EMSGSIZE; soft drops (EAGAIN/ENOENT) are logged in send_to.

From 250c347464efd3a55f060473828f76eb7da42b0c Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 22:24:00 +0200
Subject: [PATCH 23/25] refactor: remove verified-dead includes/statements and
 use RMW_GID_STORAGE_SIZE

---
 rmw_unix_socket_cpp/src/rmw_serialize.cpp    | 1 -
 rmw_unix_socket_cpp/src/rmw_subscription.cpp | 2 --
 rmw_unix_socket_cpp/src/rmw_wait.cpp         | 1 -
 rmw_unix_socket_cpp/src/serialization.cpp    | 4 ----
 rmw_unix_socket_cpp/src/types.hpp            | 2 +-
 5 files changed, 1 insertion(+), 9 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_serialize.cpp b/rmw_unix_socket_cpp/src/rmw_serialize.cpp
index b050355..e755f8e 100644
--- a/rmw_unix_socket_cpp/src/rmw_serialize.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_serialize.cpp
@@ -1,4 +1,3 @@
-#include "identifier.hpp"
 #include "serialization.hpp"
 
 #include <cstring>
diff --git a/rmw_unix_socket_cpp/src/rmw_subscription.cpp b/rmw_unix_socket_cpp/src/rmw_subscription.cpp
index 1fc0961..5f274a5 100644
--- a/rmw_unix_socket_cpp/src/rmw_subscription.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_subscription.cpp
@@ -83,8 +83,6 @@ static void drain_subscription(rmw_uds::UdsSubscription * sub)
         sub->on_new_message_cb(sub->on_new_message_user_data, 1);
       }
     }
-
-    payload.clear();
   }
 }
 
diff --git a/rmw_unix_socket_cpp/src/rmw_wait.cpp b/rmw_unix_socket_cpp/src/rmw_wait.cpp
index 12fea08..212f236 100644
--- a/rmw_unix_socket_cpp/src/rmw_wait.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_wait.cpp
@@ -52,7 +52,6 @@ static void drain_socket(
         queue.pop_front();
       }
     }
-    payload.clear();
   }
 }
 
diff --git a/rmw_unix_socket_cpp/src/serialization.cpp b/rmw_unix_socket_cpp/src/serialization.cpp
index 3391293..2c709b1 100644
--- a/rmw_unix_socket_cpp/src/serialization.cpp
+++ b/rmw_unix_socket_cpp/src/serialization.cpp
@@ -5,8 +5,6 @@
 
 #include "serialization.hpp"
 
-#include <cstring>
-
 #include "fastcdr/Cdr.h"
 #include "fastcdr/FastBuffer.h"
 
@@ -68,7 +66,6 @@ ServiceCallbacks get_service_callbacks(
       result.service_name = srv_cb->service_name_;
       return result;
     }
-    rcutils_reset_error();
   }
   rcutils_reset_error();
 
@@ -91,7 +88,6 @@ ServiceCallbacks get_service_callbacks(
       result.service_name = srv_cb->service_name_;
       return result;
     }
-    rcutils_reset_error();
   }
   rcutils_reset_error();
   return result;
diff --git a/rmw_unix_socket_cpp/src/types.hpp b/rmw_unix_socket_cpp/src/types.hpp
index c00ded3..03521a9 100644
--- a/rmw_unix_socket_cpp/src/types.hpp
+++ b/rmw_unix_socket_cpp/src/types.hpp
@@ -179,7 +179,7 @@ struct UdsSubscription
 struct CachedClient
 {
   std::string socket_path;
-  uint8_t gid[16];
+  uint8_t gid[RMW_GID_STORAGE_SIZE];
 };
 
 // Service server data

From dd400de7a585f1703032c21241155d36bddd5bfc Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Tue, 2 Jun 2026 22:36:00 +0200
Subject: [PATCH 24/25] fix: pass a non-const allocator to
 rmw_discovery_options_copy and handle its warn_unused_result (build fix for
 P2-4)

---
 rmw_unix_socket_cpp/src/rmw_init.cpp | 28 +++++++++++++++++++++++-----
 1 file changed, 23 insertions(+), 5 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_init.cpp b/rmw_unix_socket_cpp/src/rmw_init.cpp
index ff19052..73fd6a6 100644
--- a/rmw_unix_socket_cpp/src/rmw_init.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_init.cpp
@@ -63,8 +63,11 @@ rmw_ret_t rmw_init_options_copy(
   // discovery_options.static_peers is heap-owned; deep-copy it so src and dst
   // don't alias the same array and double-free in rmw_init_options_fini.
   dst->discovery_options = rmw_get_zero_initialized_discovery_options();
+  // rmw_discovery_options_copy takes a non-const allocator (unlike the security
+  // variant), so copy the caller's allocator into a local first.
+  rcutils_allocator_t disc_alloc = src->allocator;
   ret = rmw_discovery_options_copy(
-    &src->discovery_options, &src->allocator, &dst->discovery_options);
+    &src->discovery_options, &disc_alloc, &dst->discovery_options);
   if (ret != RMW_RET_OK) {
     rmw_security_options_fini(&dst->security_options, &dst->allocator);
     return ret;
@@ -83,7 +86,8 @@ rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options)
     init_options->enclave = nullptr;
   }
   rmw_security_options_fini(&init_options->security_options, &init_options->allocator);
-  rmw_discovery_options_fini(&init_options->discovery_options);
+  rmw_ret_t disc_ret = rmw_discovery_options_fini(&init_options->discovery_options);
+  (void)disc_ret;
   *init_options = rmw_get_zero_initialized_init_options();
   return RMW_RET_OK;
 }
@@ -192,9 +196,22 @@ rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
   // discovery_options.static_peers is heap-owned; deep-copy it so context->options
   // owns its own array rather than aliasing the caller's options (mirrors enclave).
   context->options.discovery_options = rmw_get_zero_initialized_discovery_options();
-  rmw_discovery_options_copy(
-    &options->discovery_options, &options->allocator,
+  // rmw_discovery_options_copy needs a non-const allocator; copy into a local.
+  rcutils_allocator_t disc_alloc = options->allocator;
+  rmw_ret_t disc_ret = rmw_discovery_options_copy(
+    &options->discovery_options, &disc_alloc,
     &context->options.discovery_options);
+  if (disc_ret != RMW_RET_OK) {
+    if (enclave_copy) {
+      options->allocator.deallocate(enclave_copy, options->allocator.state);
+    }
+    close(ctx->send_socket_fd);
+    rmw_uds::registry_close(ctx->registry_fd, ctx->registry_ptr, ctx->registry_size);
+    delete ctx;
+    *context = rmw_get_zero_initialized_context();
+    RMW_SET_ERROR_MSG("failed to copy discovery options");
+    return disc_ret;
+  }
   context->actual_domain_id = domain_id;
 
   return RMW_RET_OK;
@@ -236,7 +253,8 @@ rmw_ret_t rmw_context_fini(rmw_context_t * context)
       context->options.enclave, context->options.allocator.state);
     context->options.enclave = nullptr;
   }
-  rmw_discovery_options_fini(&context->options.discovery_options);
+  rmw_ret_t disc_ret = rmw_discovery_options_fini(&context->options.discovery_options);
+  (void)disc_ret;
 
   *context = rmw_get_zero_initialized_context();
   return RMW_RET_OK;

From 6964ae50e0c309326ed04525b11f34e714f8aed4 Mon Sep 17 00:00:00 2001
From: Abderahmane BENALI <benaliabderrahmen88@gmail.com>
Date: Fri, 5 Jun 2026 20:18:11 +0200
Subject: [PATCH 25/25] fix: return RMW_RET_OK when rmw_send_response finds no
 live client

A client that times out and shuts down before a slow handler responds leaves a
true client-gone miss. Returning RMW_RET_ERROR there makes rclcpp throw inside
the executor; rmw_cyclonedds returns OK on client GONE for exactly this reason.
Drop the response and return OK, with a debug log. Adds a regression test that
sends a response with no matching client and asserts RMW_RET_OK.
---
 rmw_unix_socket_cpp/src/rmw_service.cpp       | 12 +++++++++--
 .../test/test_rmw_service_client.cpp          | 20 +++++++++++++++++++
 2 files changed, 30 insertions(+), 2 deletions(-)

diff --git a/rmw_unix_socket_cpp/src/rmw_service.cpp b/rmw_unix_socket_cpp/src/rmw_service.cpp
index 84207ec..a2218a9 100644
--- a/rmw_unix_socket_cpp/src/rmw_service.cpp
+++ b/rmw_unix_socket_cpp/src/rmw_service.cpp
@@ -296,8 +296,16 @@ rmw_ret_t rmw_send_response(
       return RMW_RET_OK;
     }
   }
-  RMW_SET_ERROR_MSG("no client matching request id; response discarded");
-  return RMW_RET_ERROR;
+  // No live client matches this request id: the client timed out and shut down
+  // between issuing its request and our response, so its registry entry is gone.
+  // This is normal lifecycle, not an error — return OK and drop the response,
+  // matching rmw_cyclonedds (client_present_t::GONE -> RMW_RET_OK). Returning an
+  // error here would make rclcpp throw inside the executor callback.
+  RMW_UDS_LOG_DEBUG(
+    "rmw_send_response on '%s': no live client for the request id "
+    "(client gone) — response dropped",
+    srv_data->service_name.c_str());
+  return RMW_RET_OK;
 }
 
 rmw_ret_t rmw_service_request_subscription_get_actual_qos(
diff --git a/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp b/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp
index 8dd0ce0..da3cd93 100644
--- a/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp
+++ b/rmw_unix_socket_cpp/test/test_rmw_service_client.cpp
@@ -113,3 +113,23 @@ TEST_F(ServiceClientTest, RequestResponseRoundTrip)
   EXPECT_TRUE(recv_response.bool_value);
   EXPECT_EQ("success", recv_response.string_value);
 }
+
+TEST_F(ServiceClientTest, SendResponseToGoneClientReturnsOk)
+{
+  // A service that responds after its client has shut down must NOT return an
+  // error: rclcpp throws inside the executor on any non-OK/non-TIMEOUT ret.
+  // Matches rmw_cyclonedds (client GONE -> RMW_RET_OK); the response is dropped.
+  srv = rmw_create_service(node, ts, "/gone_client_srv", &qos);
+  ASSERT_NE(nullptr, srv);
+
+  // No client is registered for this service, so the GID lookup in
+  // rmw_send_response finds no match — a true client-gone miss.
+  rmw_request_id_t request_id;
+  std::memset(&request_id, 0, sizeof(request_id));
+  request_id.sequence_number = 1;  // writer_guid left all-zero: matches no client
+
+  test_msgs::srv::BasicTypes::Response response;
+  response.bool_value = true;
+  response.int32_value = 7;
+  EXPECT_EQ(RMW_RET_OK, rmw_send_response(srv, &request_id, &response));
+}