feat(gateway_knx): add TP-UART control and initialization functionality

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

components/gateway_knx/include/gateway_knx.hpp

@@ -133,6 +133,7 @@ class GatewayKnxTpIpRouter {
   void closeSockets();
   bool configureSocket();
   bool configureTpUart();
+  bool initializeTpUart();
   void handleUdpDatagram(const uint8_t* data, size_t len, const ::sockaddr_in& remote);
   void handleRoutingIndication(const uint8_t* body, size_t len);
   void handleTunnellingRequest(const uint8_t* body, size_t len, const ::sockaddr_in& remote);
@@ -151,6 +152,7 @@ class GatewayKnxTpIpRouter {
                            const ::sockaddr_in& remote);
   void sendRoutingIndication(const uint8_t* data, size_t len);
   void pollTpUart();
+  void handleTpUartControlByte(uint8_t byte);
   void handleTpTelegram(const uint8_t* data, size_t len);
   void forwardCemiToTp(const uint8_t* data, size_t len);
 
@@ -168,6 +170,10 @@ class GatewayKnxTpIpRouter {
   bool tunnel_connected_{false};
   ::sockaddr_in tunnel_remote_{};
   std::vector<uint8_t> tp_rx_frame_;
+  std::vector<uint8_t> tp_last_sent_telegram_;
+  TickType_t tp_uart_last_byte_tick_{0};
+  bool tp_uart_extended_frame_{false};
+  bool tp_uart_online_{false};
   std::string last_error_;
 };
 

components/gateway_knx/src/gateway_knx.cpp

@@ -38,6 +38,17 @@ constexpr uint8_t kKnxErrorNoMoreConnections = 0x24;
 constexpr uint8_t kKnxErrorSequenceNumber = 0x04;
 constexpr uint8_t kKnxConnectionTypeTunnel = 0x04;
 constexpr uint8_t kKnxTunnelLayerLink = 0x02;
+constexpr uint8_t kTpUartResetRequest = 0x01;
+constexpr uint8_t kTpUartResetIndication = 0x03;
+constexpr uint8_t kTpUartStateRequest = 0x02;
+constexpr uint8_t kTpUartStateIndicationMask = 0x07;
+constexpr uint8_t kTpUartSetAddressRequest = 0x28;
+constexpr uint8_t kTpUartAckInfo = 0x10;
+constexpr uint8_t kTpUartLDataConfirmPositive = 0x8b;
+constexpr uint8_t kTpUartLDataConfirmNegative = 0x0b;
+constexpr uint8_t kTpUartLDataStart = 0x80;
+constexpr uint8_t kTpUartLDataEnd = 0x40;
+constexpr uint8_t kTpUartBusy = 0xc0;
 
 struct DecodedGroupWrite {
   uint16_t group_address{0};
@@ -265,6 +276,46 @@ bool ValidateTpChecksum(const uint8_t* data, size_t len) {
   return data[len - 1] == crc;
 }
 
+bool IsTpUartControlByte(uint8_t byte) {
+  return byte == kTpUartResetIndication ||
+         byte == kTpUartLDataConfirmPositive ||
+         byte == kTpUartLDataConfirmNegative || byte == kTpUartBusy ||
+         (byte & kTpUartStateIndicationMask) == kTpUartStateIndicationMask;
+}
+
+bool IsTpUartFrameStart(uint8_t byte, bool* extended) {
+  if (extended == nullptr) {
+    return false;
+  }
+  *extended = (byte & 0x80) == 0;
+  return (byte & 0x50) == 0x10;
+}
+
+std::vector<uint8_t> WrapTpUartTelegram(const std::vector<uint8_t>& telegram) {
+  std::vector<uint8_t> wrapped;
+  wrapped.reserve(telegram.size() * 2U);
+  for (size_t index = 0; index < telegram.size(); ++index) {
+    const uint8_t control = static_cast<uint8_t>(
+        (index + 1U == telegram.size() ? kTpUartLDataEnd : kTpUartLDataStart) |
+        (index & 0x3fU));
+    wrapped.push_back(control);
+    wrapped.push_back(telegram[index]);
+  }
+  return wrapped;
+}
+
+bool TpTelegramEqualsIgnoringRepeatBit(const std::vector<uint8_t>& left,
+                                       const std::vector<uint8_t>& right) {
+  if (left.size() != right.size() || left.empty()) {
+    return false;
+  }
+  if ((left[0] & static_cast<uint8_t>(~0x20U)) !=
+      (right[0] & static_cast<uint8_t>(~0x20U))) {
+    return false;
+  }
+  return std::equal(left.begin() + 1, left.end(), right.begin() + 1);
+}
+
 std::optional<std::vector<uint8_t>> CemiToTpTelegram(const uint8_t* data, size_t len) {
   if (data == nullptr || len < 10 || data[1] != 0) {
     return std::nullopt;
@@ -633,7 +684,10 @@ void GatewayKnxTpIpRouter::taskLoop() {
     finishTask();
     return;
   }
-  configureTpUart();
+  if (!configureTpUart()) {
+    finishTask();
+    return;
+  }
 
   std::array<uint8_t, 768> buffer{};
   while (!stop_requested_) {
@@ -714,6 +768,7 @@ bool GatewayKnxTpIpRouter::configureSocket() {
 bool GatewayKnxTpIpRouter::configureTpUart() {
   const auto& serial = config_.tp_uart;
   if (serial.uart_port < 0 || serial.uart_port > 2) {
+    last_error_ = "invalid KNX TP-UART port";
     return false;
   }
   uart_config_t uart_config{};
@@ -725,18 +780,76 @@ bool GatewayKnxTpIpRouter::configureTpUart() {
   uart_config.source_clk = UART_SCLK_DEFAULT;
   const uart_port_t uart_port = static_cast<uart_port_t>(serial.uart_port);
   if (uart_param_config(uart_port, &uart_config) != ESP_OK) {
+    last_error_ = "failed to configure KNX TP-UART parameters";
     return false;
   }
   if (uart_set_pin(uart_port, serial.tx_pin, serial.rx_pin, UART_PIN_NO_CHANGE,
                    UART_PIN_NO_CHANGE) != ESP_OK) {
+    last_error_ = "failed to configure KNX TP-UART pins";
     return false;
   }
   if (uart_driver_install(uart_port, serial.rx_buffer_size, serial.tx_buffer_size, 0, nullptr,
                           0) != ESP_OK) {
+    last_error_ = "failed to install KNX TP-UART driver";
     return false;
   }
   tp_uart_port_ = serial.uart_port;
-  return true;
+  return initializeTpUart();
+}
+
+bool GatewayKnxTpIpRouter::initializeTpUart() {
+  if (tp_uart_port_ < 0) {
+    return false;
+  }
+  const uart_port_t uart_port = static_cast<uart_port_t>(tp_uart_port_);
+  tp_rx_frame_.clear();
+  tp_last_sent_telegram_.clear();
+  tp_uart_last_byte_tick_ = 0;
+  tp_uart_extended_frame_ = false;
+  tp_uart_online_ = false;
+  uart_flush_input(uart_port);
+
+  const uint8_t reset_request = kTpUartResetRequest;
+  if (uart_write_bytes(uart_port, &reset_request, 1) != 1) {
+    last_error_ = "failed to send KNX TP-UART reset request";
+    return false;
+  }
+
+  const TickType_t deadline = xTaskGetTickCount() + pdMS_TO_TICKS(1500);
+  bool saw_reset = false;
+  std::array<uint8_t, 32> buffer{};
+  while (xTaskGetTickCount() < deadline) {
+    const int read = uart_read_bytes(uart_port, buffer.data(), buffer.size(),
+                                     pdMS_TO_TICKS(config_.tp_uart.read_timeout_ms));
+    if (read <= 0) {
+      continue;
+    }
+    for (int index = 0; index < read; ++index) {
+      const uint8_t byte = buffer[static_cast<size_t>(index)];
+      if (!saw_reset) {
+        if (byte == kTpUartResetIndication) {
+          saw_reset = true;
+          const std::array<uint8_t, 3> set_address{
+              kTpUartSetAddressRequest,
+              static_cast<uint8_t>((config_.individual_address >> 8) & 0xff),
+              static_cast<uint8_t>(config_.individual_address & 0xff),
+          };
+          uart_write_bytes(uart_port, set_address.data(), set_address.size());
+          const uint8_t state_request = kTpUartStateRequest;
+          uart_write_bytes(uart_port, &state_request, 1);
+        }
+        continue;
+      }
+      if ((byte & kTpUartStateIndicationMask) == kTpUartStateIndicationMask) {
+        tp_uart_online_ = true;
+        return true;
+      }
+    }
+  }
+
+  last_error_ = saw_reset ? "timed out waiting for KNX TP-UART state indication"
+                          : "timed out waiting for KNX TP-UART reset indication";
+  return false;
 }
 
 void GatewayKnxTpIpRouter::handleUdpDatagram(const uint8_t* data, size_t len,
@@ -939,12 +1052,40 @@ void GatewayKnxTpIpRouter::pollTpUart() {
     return;
   }
   for (int index = 0; index < read; ++index) {
-    tp_rx_frame_.push_back(buffer[index]);
+    const uint8_t byte = buffer[static_cast<size_t>(index)];
+    if (tp_rx_frame_.empty()) {
+      if (IsTpUartControlByte(byte)) {
+        handleTpUartControlByte(byte);
+        continue;
+      }
+      if (byte == 0xcb || (byte & 0x17U) == 0x13U) {
+        continue;
+      }
+    }
+
+    const TickType_t now = xTaskGetTickCount();
+    if (!tp_rx_frame_.empty() && tp_uart_last_byte_tick_ != 0 &&
+        now - tp_uart_last_byte_tick_ > pdMS_TO_TICKS(1000)) {
+      tp_rx_frame_.clear();
+    }
+
+    if (tp_rx_frame_.empty()) {
+      if (IsTpUartFrameStart(byte, &tp_uart_extended_frame_)) {
+        tp_rx_frame_.push_back(byte);
+        tp_uart_last_byte_tick_ = now;
+      }
+      continue;
+    }
+
+    tp_rx_frame_.push_back(byte);
+    tp_uart_last_byte_tick_ = now;
     const size_t expected = ExpectedTpFrameSize(tp_rx_frame_.data(), tp_rx_frame_.size());
     if (expected == 0) {
       continue;
     }
     if (tp_rx_frame_.size() == expected) {
+      const uint8_t ack = kTpUartAckInfo;
+      uart_write_bytes(static_cast<uart_port_t>(tp_uart_port_), &ack, 1);
       handleTpTelegram(tp_rx_frame_.data(), tp_rx_frame_.size());
       tp_rx_frame_.clear();
     } else if (tp_rx_frame_.size() > expected || tp_rx_frame_.size() > 263U) {
@@ -953,7 +1094,40 @@ void GatewayKnxTpIpRouter::pollTpUart() {
   }
 }
 
+void GatewayKnxTpIpRouter::handleTpUartControlByte(uint8_t byte) {
+  if (byte == kTpUartResetIndication) {
+    ESP_LOGW(kTag, "KNX TP-UART reset indication received; marking link offline");
+    tp_uart_online_ = false;
+    return;
+  }
+  if (byte == kTpUartBusy) {
+    last_error_ = "KNX TP-UART bus busy";
+    ESP_LOGW(kTag, "%s", last_error_.c_str());
+    return;
+  }
+  if (byte == kTpUartLDataConfirmNegative) {
+    last_error_ = "KNX TP-UART negative confirmation";
+    ESP_LOGW(kTag, "%s", last_error_.c_str());
+    return;
+  }
+  if (byte == kTpUartLDataConfirmPositive) {
+    return;
+  }
+  if ((byte & kTpUartStateIndicationMask) == kTpUartStateIndicationMask) {
+    tp_uart_online_ = true;
+  }
+}
+
 void GatewayKnxTpIpRouter::handleTpTelegram(const uint8_t* data, size_t len) {
+  if (data == nullptr || len == 0) {
+    return;
+  }
+  const std::vector<uint8_t> telegram(data, data + len);
+  if (!tp_last_sent_telegram_.empty() &&
+      TpTelegramEqualsIgnoringRepeatBit(telegram, tp_last_sent_telegram_)) {
+    tp_last_sent_telegram_.clear();
+    return;
+  }
   const auto cemi = TpTelegramToCemi(data, len);
   if (!cemi.has_value()) {
     return;
@@ -967,14 +1141,16 @@ void GatewayKnxTpIpRouter::handleTpTelegram(const uint8_t* data, size_t len) {
 }
 
 void GatewayKnxTpIpRouter::forwardCemiToTp(const uint8_t* data, size_t len) {
-  if (tp_uart_port_ < 0 || data == nullptr || len == 0) {
+  if (tp_uart_port_ < 0 || data == nullptr || len == 0 || !tp_uart_online_) {
     return;
   }
   const auto telegram = CemiToTpTelegram(data, len);
   if (!telegram.has_value()) {
     return;
   }
-  uart_write_bytes(static_cast<uart_port_t>(tp_uart_port_), telegram->data(), telegram->size());
+  tp_last_sent_telegram_ = *telegram;
+  const auto wrapped = WrapTpUartTelegram(*telegram);
+  uart_write_bytes(static_cast<uart_port_t>(tp_uart_port_), wrapped.data(), wrapped.size());
 }
 
 }  // namespace gateway