Fix lock order inversion in rmw_wait

rmw_zenoh_cpp/src/detail/event.cpp

@@ -252,13 +252,8 @@ void EventsManager::notify_event(rmw_zenoh_event_type_t event_id)
     return;
   }
 
-  /* Make sure to not lock both event_mutex_ and event_condition_mutex_ at the same time to avoid
-   * deadlocks with rmw_wait */
-  rmw_wait_set_data_t * wait_set_data = nullptr;
-  {
-    std::lock_guard<std::mutex> lock(event_condition_mutex_);
-    wait_set_data = wait_set_data_[event_id];
-  }
+  std::lock_guard<std::mutex> lock(event_condition_mutex_);
+  auto *wait_set_data = wait_set_data_[event_id];
   if (wait_set_data != nullptr) {
     std::lock_guard<std::mutex> wait_set_lock(wait_set_data->condition_mutex);
     wait_set_data->triggered = true;

rmw_zenoh_cpp/src/detail/guard_condition.cpp

@@ -30,21 +30,17 @@ GuardCondition::GuardCondition()
 ///=============================================================================
 void GuardCondition::trigger()
 {
-  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
-  {
-    std::lock_guard<std::mutex> lock(internal_mutex_);
+  std::lock_guard<std::mutex> lock(internal_mutex_);
 
-    // the change to hasTriggered_ needs to be mutually exclusive with
-    // rmw_wait() which checks hasTriggered() and decides if wait() needs to
-    // be called
-    has_triggered_ = true;
-    wait_set_data_to_trigger = wait_set_data_;
-  }
+  // the change to hasTriggered_ needs to be mutually exclusive with
+  // rmw_wait() which checks hasTriggered() and decides if wait() needs to
+  // be called
+  has_triggered_ = true;
 
-  if (wait_set_data_to_trigger != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
-    wait_set_data_to_trigger->triggered = true;
-    wait_set_data_to_trigger->condition_variable.notify_one();
+  if (wait_set_data_ != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
+    wait_set_data_->triggered = true;
+    wait_set_data_->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/detail/rmw_client_data.cpp

@@ -231,34 +231,29 @@ std::array<uint8_t, RMW_GID_STORAGE_SIZE> ClientData::copy_gid() const
 ///=============================================================================
 void ClientData::add_new_reply(std::unique_ptr<ZenohReply> reply)
 {
-  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
+  std::lock_guard<std::mutex> lock(mutex_);
+  const rmw_qos_profile_t adapted_qos_profile =
+    entity_->topic_info().value().qos_;
+  if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
+    reply_queue_.size() >= adapted_qos_profile.depth)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
-    const rmw_qos_profile_t adapted_qos_profile =
-      entity_->topic_info().value().qos_;
-    if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
-      reply_queue_.size() >= adapted_qos_profile.depth)
-    {
-      // Log warning if message is discarded due to hitting the queue depth
-      RMW_ZENOH_LOG_ERROR_NAMED(
-        "rmw_zenoh_cpp",
-        "Query queue depth of %ld reached, discarding oldest Query "
-        "for client for %s",
-        adapted_qos_profile.depth,
-        this->entity_->topic_info().value().topic_keyexpr_.c_str());
-      reply_queue_.pop_front();
-    }
-    reply_queue_.emplace_back(std::move(reply));
-
-    // Since we added new data, trigger user callback and guard condition if they are available
-    data_callback_mgr_.trigger_callback();
-    wait_set_data_to_trigger = wait_set_data_;
+    // Log warning if message is discarded due to hitting the queue depth
+    RMW_ZENOH_LOG_ERROR_NAMED(
+      "rmw_zenoh_cpp",
+      "Query queue depth of %ld reached, discarding oldest Query "
+      "for client for %s",
+      adapted_qos_profile.depth,
+      this->entity_->topic_info().value().topic_keyexpr_.c_str());
+    reply_queue_.pop_front();
   }
-
-  if (wait_set_data_to_trigger != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
-    wait_set_data_to_trigger->triggered = true;
-    wait_set_data_to_trigger->condition_variable.notify_one();
+  reply_queue_.emplace_back(std::move(reply));
+
+  // Since we added new data, trigger user callback and guard condition if they are available
+  data_callback_mgr_.trigger_callback();
+  if (wait_set_data_ != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
+    wait_set_data_->triggered = true;
+    wait_set_data_->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/detail/rmw_service_data.cpp

@@ -237,41 +237,36 @@ bool ServiceData::liveliness_is_valid() const
 ///=============================================================================
 void ServiceData::add_new_query(std::unique_ptr<ZenohQuery> query)
 {
-  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (is_shutdown_.load(std::memory_order_acquire)) {
+    RMW_ZENOH_LOG_DEBUG_NAMED(
+      "rmw_zenoh_cpp",
+      "Request from client will be ignored since the service is shutdown."
+    );
+    return;
+  }
+  const rmw_qos_profile_t adapted_qos_profile =
+    entity_->topic_info().value().qos_;
+  if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
+    query_queue_.size() >= adapted_qos_profile.depth)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
-    if (is_shutdown_.load(std::memory_order_acquire)) {
-      RMW_ZENOH_LOG_DEBUG_NAMED(
-        "rmw_zenoh_cpp",
-        "Request from client will be ignored since the service is shutdown."
-      );
-      return;
-    }
-    const rmw_qos_profile_t adapted_qos_profile =
-      entity_->topic_info().value().qos_;
-    if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
-      query_queue_.size() >= adapted_qos_profile.depth)
-    {
-      // Log warning if message is discarded due to hitting the queue depth
-      RMW_ZENOH_LOG_ERROR_NAMED(
-        "rmw_zenoh_cpp",
-        "Query queue depth of %ld reached, discarding oldest Query "
-        "for service '%s'",
-        adapted_qos_profile.depth,
-        entity_->topic_info().value().name_.c_str());
-      query_queue_.pop_front();
-    }
-    query_queue_.emplace_back(std::move(query));
-
-    // Since we added new data, trigger user callback and guard condition if they are available
-    data_callback_mgr_.trigger_callback();
-    wait_set_data_to_trigger = wait_set_data_;
+    // Log warning if message is discarded due to hitting the queue depth
+    RMW_ZENOH_LOG_ERROR_NAMED(
+      "rmw_zenoh_cpp",
+      "Query queue depth of %ld reached, discarding oldest Query "
+      "for service '%s'",
+      adapted_qos_profile.depth,
+      entity_->topic_info().value().name_.c_str());
+    query_queue_.pop_front();
   }
-
-  if (wait_set_data_to_trigger != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
-    wait_set_data_to_trigger->triggered = true;
-    wait_set_data_to_trigger->condition_variable.notify_one();
+  query_queue_.emplace_back(std::move(query));
+
+  // Since we added new data, trigger user callback and guard condition if they are available
+  data_callback_mgr_.trigger_callback();
+  if (wait_set_data_ != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
+    wait_set_data_->triggered = true;
+    wait_set_data_->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/detail/rmw_subscription_data.cpp

@@ -1111,80 +1111,67 @@ void SubscriptionData::add_new_message(
   std::unique_ptr<SubscriptionData::Message> msg,
   const std::string & topic_name)
 {
-  rmw_wait_set_data_t * wait_set_data_to_trigger = nullptr;
-  bool message_lost = false;
-  int32_t num_msg_lost = 0;
-
+  std::lock_guard<std::mutex> lock(mutex_);
+  if (is_shutdown_) {
+    return;
+  }
+  RMW_ZENOH_ROSIDL_BUFFER_LOG_DEBUG_NAMED(
+    "rmw_zenoh_cpp",
+    "[Subscription] add_new_message topic='%s' is_buffer_aware=%d payload_size=%zu",
+    topic_name.c_str(),
+    is_buffer_aware_,
+    msg->payload.size());
+  const rmw_qos_profile_t adapted_qos_profile = entity_->topic_info().value().qos_;
+  if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
+    message_queue_.size() >= adapted_qos_profile.depth)
   {
-    std::lock_guard<std::mutex> lock(mutex_);
-    if (is_shutdown_) {
-      return;
-    }
-    RMW_ZENOH_ROSIDL_BUFFER_LOG_DEBUG_NAMED(
+    // Log warning if message is discarded due to hitting the queue depth
+    RMW_ZENOH_LOG_DEBUG_NAMED(
       "rmw_zenoh_cpp",
-      "[Subscription] add_new_message topic='%s' is_buffer_aware=%d payload_size=%zu",
-      topic_name.c_str(),
-      is_buffer_aware_,
-      msg->payload.size());
-    const rmw_qos_profile_t adapted_qos_profile = entity_->topic_info().value().qos_;
-    if (adapted_qos_profile.history != RMW_QOS_POLICY_HISTORY_KEEP_ALL &&
-      message_queue_.size() >= adapted_qos_profile.depth)
-    {
-      // Log warning if message is discarded due to hitting the queue depth
-      RMW_ZENOH_LOG_DEBUG_NAMED(
-        "rmw_zenoh_cpp",
-        "Message queue depth of %ld reached, discarding oldest message "
-        "for subscription for %s",
-        adapted_qos_profile.depth,
-        topic_name.c_str());
-
-      // If the adapted_qos_profile.depth is 0, the std::move command below will result
-      // in UB and the z_drop will segfault. We explicitly set the depth to a minimum of 1
-      // in rmw_create_subscription() but to be safe, we only attempt to discard from the
-      // queue if it is non-empty.
-      if (!message_queue_.empty()) {
-        std::unique_ptr<Message> old = std::move(message_queue_.front());
-        message_queue_.pop_front();
-      }
-    }
+      "Message queue depth of %ld reached, discarding oldest message "
+      "for subscription for %s",
+      adapted_qos_profile.depth,
+      topic_name.c_str());
 
-    // Check for messages lost if the new sequence number is not monotonically increasing.
-    const size_t gid_hash = hash_gid(msg->attachment.copy_gid());
-    auto last_known_pub_it = last_known_published_msg_.find(gid_hash);
-    if (last_known_pub_it != last_known_published_msg_.end()) {
-      const int64_t seq_increment = std::abs(
-        msg->attachment.sequence_number() -
-        last_known_pub_it->second);
-      if (seq_increment > 1) {
-        num_msg_lost =
-          static_cast<int32_t>(std::clamp(
-            seq_increment - 1,
-            static_cast<int64_t>(std::numeric_limits<int32_t>::min()),
-            static_cast<int64_t>(std::numeric_limits<int32_t>::max())));
-        message_lost = true;
-      }
+    // If the adapted_qos_profile.depth is 0, the std::move command below will result
+    // in UB and the z_drop will segfault. We explicitly set the depth to a minimum of 1
+    // in rmw_create_subscription() but to be safe, we only attempt to discard from the
+    // queue if it is non-empty.
+    if (!message_queue_.empty()) {
+      std::unique_ptr<Message> old = std::move(message_queue_.front());
+      message_queue_.pop_front();
     }
-    // Always update the last known sequence number for the publisher.
-    last_known_published_msg_[gid_hash] = msg->attachment.sequence_number();
-
-    message_queue_.emplace_back(std::move(msg));
-
-    // Since we added new data, trigger user callback and guard condition if they are available
-    data_callback_mgr_.trigger_callback();
-    wait_set_data_to_trigger = wait_set_data_;
   }
 
-  // Trigger lost message event outside the subscription mutex to avoid deadlocks.
-  if (message_lost) {
-    events_mgr_->update_event_status(
-      ZENOH_EVENT_MESSAGE_LOST,
-      std::move(num_msg_lost));
+  // Check for messages lost if the new sequence number is not monotonically increasing.
+  const size_t gid_hash = hash_gid(msg->attachment.copy_gid());
+  auto last_known_pub_it = last_known_published_msg_.find(gid_hash);
+  if (last_known_pub_it != last_known_published_msg_.end()) {
+    const int64_t seq_increment = std::abs(
+      msg->attachment.sequence_number() -
+      last_known_pub_it->second);
+    if (seq_increment > 1) {
+      int32_t num_msg_lost =
+        static_cast<int32_t>(std::clamp(
+          seq_increment - 1,
+          static_cast<int64_t>(std::numeric_limits<int32_t>::min()),
+          static_cast<int64_t>(std::numeric_limits<int32_t>::max())));
+      events_mgr_->update_event_status(
+        ZENOH_EVENT_MESSAGE_LOST,
+        std::move(num_msg_lost));
+    }
   }
+  // Always update the last known sequence number for the publisher.
+  last_known_published_msg_[gid_hash] = msg->attachment.sequence_number();
+
+  message_queue_.emplace_back(std::move(msg));
 
-  if (wait_set_data_to_trigger != nullptr) {
-    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_to_trigger->condition_mutex);
-    wait_set_data_to_trigger->triggered = true;
-    wait_set_data_to_trigger->condition_variable.notify_one();
+  // Since we added new data, trigger user callback and guard condition if they are available
+  data_callback_mgr_.trigger_callback();
+  if (wait_set_data_ != nullptr) {
+    std::lock_guard<std::mutex> wait_set_lock(wait_set_data_->condition_mutex);
+    wait_set_data_->triggered = true;
+    wait_set_data_->condition_variable.notify_one();
   }
 }
 

rmw_zenoh_cpp/src/rmw_zenoh.cpp

@@ -2235,13 +2235,27 @@ rmw_wait(
   // a valid pointer.
 
   {
-    // Take the lock before the check_and_attach_condition to ensure conditions and flags
-    // are not modified while being checked by concurrent calls.
+    // reset the trigger prior to attaching any entities
     std::unique_lock<std::mutex> lock(wait_set_data->condition_mutex);
+    wait_set_data->triggered = false;
+  }
 
+  {
+    // We explicitly do not lock the condition_mutex here
+    // This is fine, as the attachment returns atomically is a signal was ready
+    // If anything triggers after that point, wait_set_data->triggered will be set
+    // to true under mutex.
+    // Note taking the mutex here leads to a deadlock.
     bool skip_wait = check_and_attach_condition(
       subscriptions, guard_conditions, services, clients, events, wait_set_data);
+
+
     if (!skip_wait) {
+      // now it is safe to take the lock
+      // if wait_set_data->triggered was set to true in between,
+      // the wait on the conditional will instantly return.
+      std::unique_lock<std::mutex> lock(wait_set_data->condition_mutex);
+
       // According to the RMW documentation, if wait_timeout is NULL that means
       // "wait forever", if it specified as 0 it means "never wait", and if it is anything else wait
       // for that amount of time.
@@ -2258,12 +2272,6 @@ rmw_wait(
             [wait_set_data]() {return wait_set_data->triggered;});
         }
       }
-
-      // It is important to reset this here while still holding the lock, otherwise every subsequent
-      // call to rmw_wait() will be immediately ready.  We could handle this another way by making
-      // "triggered" a stack variable in this function and "attaching" it during
-      // "check_and_attach_condition", but that isn't clearly better so leaving this.
-      wait_set_data->triggered = false;
     }
   }
 

test_rmw_zenoh_cpp/CMakeLists.txt

@@ -16,7 +16,6 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   find_package(ament_lint_common REQUIRED)
   find_package(rclcpp REQUIRED)
-  find_package(std_msgs REQUIRED)
   find_package(rmw_zenoh_cpp REQUIRED)
   find_package(zenoh_cpp_vendor REQUIRED)
 
@@ -31,16 +30,6 @@ if(BUILD_TESTING)
     zenohcxx::zenohc
     ament_cmake_ros_core::ament_ros_defaults
   )
-
-  ament_add_ros_isolated_gtest(test_issue_921
-    test/test_issue_921.cpp
-    ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp)
-  target_link_libraries(test_issue_921
-    rclcpp::rclcpp
-    std_msgs::std_msgs
-    rmw_zenoh_cpp::rmw_zenoh_cpp
-    ament_cmake_ros_core::ament_ros_defaults
-  )
 endif()
 
 ament_package()

test_rmw_zenoh_cpp/package.xml

@@ -15,7 +15,6 @@
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>rclcpp</test_depend>
-  <test_depend>std_msgs</test_depend>
   <test_depend>rmw_zenoh_cpp</test_depend>
   <test_depend>zenoh_cpp_vendor</test_depend>