Audit fixes: 3 P1 + P2/P3 + perf + dead-code (full stack)

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);

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(

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"
@@ -132,6 +133,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 +163,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);
 
@@ -191,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()) {
@@ -222,6 +229,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);
@@ -241,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;
 }
 
@@ -272,6 +288,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 +302,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 +314,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 +331,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 +351,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;

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 &&