feat(gateway): enhance handling of local routing and tunnel frames in KNX integration

Signed-off-by: Tony <tonylu@tony-cloud.com>

components/gateway_knx/include/gateway_knx.hpp

@@ -338,7 +338,9 @@ class GatewayKnxTpIpRouter {
   uint16_t effectiveTunnelAddressForSlot(size_t slot) const;
   void pruneStaleTunnelClients();
   bool handleOpenKnxTunnelFrame(const uint8_t* data, size_t len,
-                                TunnelClient* response_client, uint16_t response_service);
+                                TunnelClient* response_client, uint16_t response_service,
+                                const uint8_t* suppress_routing_echo = nullptr,
+                                size_t suppress_routing_echo_len = 0);
   bool handleOpenKnxBusFrame(const uint8_t* data, size_t len);
   bool transmitOpenKnxTpFrame(const uint8_t* data, size_t len);
   void selectOpenKnxNetworkInterface(const ::sockaddr_in& remote);

components/gateway_knx/src/gateway_knx.cpp

@@ -740,6 +740,79 @@ uint16_t KnxIpServiceForCemi(const uint8_t* data, size_t len, uint16_t fallback_
   }
 }
 
+bool MatchesOpenKnxLocalIndividualAddress(const CemiFrame& frame,
+                                         const openknx::EtsDeviceRuntime& ets_device) {
+  if (frame.addressType() != IndividualAddress) {
+    return false;
+  }
+  const uint16_t dest = frame.destinationAddress();
+  const uint16_t own_address = ets_device.individualAddress();
+  const uint16_t client_address = ets_device.tunnelClientAddress();
+  const bool commissioning = !ets_device.configured() || ets_device.programmingMode();
+  return dest == own_address || dest == client_address || (commissioning && dest == 0xffff);
+}
+
+bool BuildLocalRoutingTunnelFrame(const uint8_t* data, size_t len,
+                                  std::vector<uint8_t>* local_frame) {
+  if (data == nullptr || local_frame == nullptr || len < 2) {
+    return false;
+  }
+  std::vector<uint8_t> frame_data(data, data + len);
+  CemiFrame frame(frame_data.data(), static_cast<uint16_t>(frame_data.size()));
+  if (!frame.valid()) {
+    return false;
+  }
+  switch (frame.messageCode()) {
+    case L_data_req:
+      *local_frame = std::move(frame_data);
+      return true;
+    case L_data_ind:
+      frame.messageCode(L_data_req);
+      *local_frame = std::move(frame_data);
+      return true;
+    default:
+      return false;
+  }
+}
+
+bool IsLocalRoutingEchoIndication(const uint8_t* response, size_t response_len,
+                                  const uint8_t* original_request,
+                                  size_t original_request_len) {
+  if (response == nullptr || original_request == nullptr || response_len < 2 ||
+      original_request_len < 2) {
+    return false;
+  }
+  std::vector<uint8_t> response_data(response, response + response_len);
+  std::vector<uint8_t> original_data(original_request,
+                                     original_request + original_request_len);
+  CemiFrame response_frame(response_data.data(), static_cast<uint16_t>(response_data.size()));
+  CemiFrame original_frame(original_data.data(), static_cast<uint16_t>(original_data.size()));
+  if (!response_frame.valid() || !original_frame.valid() ||
+      response_frame.messageCode() != L_data_ind ||
+      original_frame.addressType() != IndividualAddress ||
+      response_frame.addressType() != original_frame.addressType() ||
+      response_frame.sourceAddress() != original_frame.sourceAddress() ||
+      response_frame.destinationAddress() != original_frame.destinationAddress() ||
+      response_frame.tpdu().type() != original_frame.tpdu().type() ||
+      response_frame.apdu().type() != original_frame.apdu().type()) {
+    return false;
+  }
+  const uint8_t response_apdu_length = response_frame.apdu().length();
+  const uint8_t original_apdu_length = original_frame.apdu().length();
+  if (response_apdu_length != original_apdu_length) {
+    return false;
+  }
+  const uint8_t* response_apdu = response_frame.apdu().data();
+  const uint8_t* original_apdu = original_frame.apdu().data();
+  if (response_apdu_length == 0) {
+    return true;
+  }
+  if (response_apdu == nullptr || original_apdu == nullptr) {
+    return false;
+  }
+  return std::memcmp(response_apdu, original_apdu, response_apdu_length) == 0;
+}
+
 bool BuildTunnelConfirmationFrame(const uint8_t* data, size_t len,
                                   std::vector<uint8_t>* confirmation) {
   if (data == nullptr || confirmation == nullptr || len < 2) {
@@ -3170,16 +3243,14 @@ void GatewayKnxTpIpRouter::handleRoutingIndication(const uint8_t* packet_data, s
   const uint8_t* cemi = frame.data();
   const size_t cemi_len = frame.dataLength();
   bool consumed_by_local_application = false;
-  if (frame.messageCode() == L_data_req && frame.addressType() == IndividualAddress &&
+  if (ets_device_ != nullptr && MatchesOpenKnxLocalIndividualAddress(frame, *ets_device_)) {
-      ets_device_ != nullptr) {
+    std::vector<uint8_t> local_tunnel_frame;
-    const uint16_t dest = frame.destinationAddress();
+    if (BuildLocalRoutingTunnelFrame(cemi, cemi_len, &local_tunnel_frame)) {
-    const uint16_t own_address = ets_device_->individualAddress();
+        consumed_by_local_application = handleOpenKnxTunnelFrame(
-    const uint16_t client_address = ets_device_->tunnelClientAddress();
+          local_tunnel_frame.data(), local_tunnel_frame.size(), nullptr,
-    const bool commissioning = !ets_device_->configured() || ets_device_->programmingMode();
+          kServiceRoutingIndication,
-    if (dest == own_address || dest == client_address ||
+          frame.messageCode() == L_data_ind ? cemi : nullptr,
-        (commissioning && dest == 0xffff)) {
+          frame.messageCode() == L_data_ind ? cemi_len : 0);
-      consumed_by_local_application =
-          handleOpenKnxTunnelFrame(cemi, cemi_len, nullptr, kServiceRoutingIndication);
     }
   }
   if (consumed_by_local_application) {
@@ -4020,7 +4091,9 @@ void GatewayKnxTpIpRouter::selectOpenKnxNetworkInterface(const sockaddr_in& remo
 
 bool GatewayKnxTpIpRouter::handleOpenKnxTunnelFrame(const uint8_t* data, size_t len,
                                                     TunnelClient* response_client,
-                                                    uint16_t response_service) {
+                                                    uint16_t response_service,
+                                                    const uint8_t* suppress_routing_echo,
+                                                    size_t suppress_routing_echo_len) {
   SemaphoreGuard guard(openknx_lock_);
   if (ets_device_ == nullptr) {
     return false;
@@ -4034,14 +4107,20 @@ bool GatewayKnxTpIpRouter::handleOpenKnxTunnelFrame(const uint8_t* data, size_t
   const bool consumed = ets_device_->handleTunnelFrame(
       data, len,
       [this, response_client, response_service, needs_tunnel_confirmation,
-       &tunnel_confirmation, &sent_tunnel_confirmation](const uint8_t* response,
+       &tunnel_confirmation, &sent_tunnel_confirmation,
-                                                        size_t response_len) {
+       suppress_routing_echo, suppress_routing_echo_len](const uint8_t* response,
+                                                         size_t response_len) {
         if (response == nullptr || response_len == 0) {
           return;
         }
         const bool routing_context =
             response_client == nullptr && response_service == kServiceRoutingIndication;
         const auto message_code = CemiMessageCode(response, response_len);
+        if (routing_context && suppress_routing_echo != nullptr &&
+            IsLocalRoutingEchoIndication(response, response_len, suppress_routing_echo,
+                                         suppress_routing_echo_len)) {
+          return;
+        }
         if (needs_tunnel_confirmation && !sent_tunnel_confirmation &&
             message_code.has_value() && message_code.value() != L_data_con) {
           sent_tunnel_confirmation = sendCemiFrameToClient(
@@ -4059,6 +4138,9 @@ bool GatewayKnxTpIpRouter::handleOpenKnxTunnelFrame(const uint8_t* data, size_t
           return;
         }
         if (message_code.has_value() && message_code.value() == L_data_con) {
+          if (routing_context) {
+            return;
+          }
           if (response_client != nullptr && response_client->connected) {
             sent_tunnel_confirmation =
                 sendCemiFrameToClient(*response_client, service, response, response_len) ||