Chapter 209: KNX IP Gateway Implementation

#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "freertos/queue.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "driver/uart.h"

#include "lwip/err.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"
#include <lwip/netdb.h>

// Wi-Fi Configuration
#define WIFI_SSID      "YOUR_WIFI_SSID"
#define WIFI_PASS      "YOUR_WIFI_PASSWORD"
#define MAX_WIFI_RETRY 10

// TPUART Configuration (Conceptual - matches Chapter 208)
#define TPUART_NUM UART_NUM_2
#define TPUART_TXD_PIN 17
#define TPUART_RXD_PIN 16
#define BUF_SIZE 1024

// KNX IP Gateway Configuration
#define KNX_IP_PORT 3671
#define MAX_CLIENTS 1 // For simplicity, this example supports only one client

static const char *TAG = "KNX_GW";

// Event group for Wi-Fi
static EventGroupHandle_t s_wifi_event_group;
#define WIFI_CONNECTED_BIT BIT0
#define WIFI_FAIL_BIT      BIT1
static int s_retry_num = 0;

// Queues for message passing between TPUART task and KNX IP task
static QueueHandle_t xQueueKnxTpToIp; // cEMI frames from TP bus to IP client
static QueueHandle_t xQueueKnxIpToTp; // cEMI frames from IP client to TP bus

// KNXnet/IP Service Identifiers (from Chapter 208, abbreviated)
#define KNXNETIP_CORE_CONNECT_REQUEST       0x0205
#define KNXNETIP_CORE_CONNECT_RESPONSE      0x0206
#define KNXNETIP_CORE_CONNECTIONSTATE_REQUEST 0x0207
#define KNXNETIP_CORE_CONNECTIONSTATE_RESPONSE 0x0208
#define KNXNETIP_CORE_DISCONNECT_REQUEST    0x0209
#define KNXNETIP_CORE_DISCONNECT_RESPONSE   0x020A
#define KNXNETIP_TUNNEL_TUNNELLING_REQUEST  0x0420
#define KNXNETIP_TUNNEL_TUNNELLING_ACK      0x0421

// Basic KNXnet/IP Header
typedef struct __attribute__((packed)) {
    uint8_t header_len;
    uint8_t protocol_version;
    uint16_t service_type_id;
    uint16_t total_len;
} knx_ip_header_t;

// Simplified cEMI Frame structure (L_Data.ind / L_Data.req)
// Max cEMI frame size for tunneling is typically around 22 bytes for TP1
#define CEMI_FRAME_MAX_LEN 32 
typedef struct {
    uint8_t len; // Actual length of cEMI data
    uint8_t data[CEMI_FRAME_MAX_LEN];
} cemi_frame_t;

// Client State Structure
typedef struct {
    bool active;
    uint8_t channel_id;
    uint8_t seq_counter_rx; // Expected sequence counter from this client
    uint8_t seq_counter_tx; // Next sequence counter to send to this client
    struct sockaddr_in client_addr;
    TickType_t last_seen;
} knx_client_state_t;

static knx_client_state_t clients[MAX_CLIENTS];
static int gateway_socket = -1;


// --- TPUART Task (Conceptual) ---
// This task would interface with a real TPUART module
// For this example, it simulates receiving a cEMI frame and allows sending one
void tpuart_init_conceptual() {
    uart_config_t uart_config = {
        .baud_rate = 19200, .data_bits = UART_DATA_8_BITS, .parity = UART_PARITY_EVEN,
        .stop_bits = UART_STOP_BITS_1, .flow_ctrl = UART_HW_FLOWCTRL_DISABLE, .source_clk = UART_SCLK_DEFAULT,
    };
    ESP_ERROR_CHECK(uart_driver_install(TPUART_NUM, BUF_SIZE * 2, 0, 0, NULL, 0));
    ESP_ERROR_CHECK(uart_param_config(TPUART_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(TPUART_NUM, TPUART_TXD_PIN, TPUART_RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
    ESP_LOGI(TAG, "Conceptual TPUART initialized.");
}

// Simulate sending a cEMI frame to KNX TP bus via TPUART
void tpuart_send_cemi_to_knx_bus(const cemi_frame_t *cemi) {
    ESP_LOGI(TAG, "TPUART: Sending cEMI frame to KNX TP bus (Length: %d)", cemi->len);
    // Actual implementation:
    // 1. Convert cEMI frame to TPUART specific serial protocol
    // 2. uart_write_bytes(...)
    // This is highly dependent on the TPUART module.
    // For now, just log it.
    esp_log_buffer_hex(TAG, cemi->data, cemi->len);

    // Simulate sending to actual bus for loopback testing without real hardware
    // If you had a real TPUART, you would not do this loopback.
    // This part is for simplified testing of the gateway logic.
    if (clients[0].active) { // If a client is connected, "loop back" this message as if it came from bus
        cemi_frame_t received_cemi_for_ip_client;
        memcpy(&received_cemi_for_ip_client, cemi, sizeof(cemi_frame_t));
        // Modify if needed, e.g., change message code from L_Data.req to L_Data.ind
        // For simplicity, we'll just forward it as is.
        if (xQueueSend(xQueueKnxTpToIp, &received_cemi_for_ip_client, pdMS_TO_TICKS(100)) != pdPASS) {
            ESP_LOGW(TAG, "TPUART: Failed to queue cEMI to IP task (simulated loopback)");
        } else {
            ESP_LOGI(TAG, "TPUART: Queued cEMI for IP client (simulated loopback)");
        }
    }
}

// Task to handle communication with TPUART (receiving from KNX TP, sending from IP)
static void tpuart_task(void *pvParameters) {
    tpuart_init_conceptual();
    cemi_frame_t cemi_from_ip;

    // Simulate receiving a KNX TP telegram periodically for testing
    // In a real system, this would come from uart_read_bytes and parsing TPUART protocol
    cemi_frame_t simulated_tp_telegram;
    simulated_tp_telegram.len = 11; // Example L_Data.ind, GA 1/1/1, DPT 1.001 ON
    // L_Data.ind: MC=0x29, AddInfoLen=0x00
    // Ctrl1=0xBC, Ctrl2=0xE0 (standard frame, group addr, hop 6)
    // SrcPA=0x1101 (1.1.1), DestGA=0x0901 (1/1/1 in 3-level), Len=1
    // APCI+Data = 0x81 (GroupValueWrite, ON)
    uint8_t sample_cemi_data[] = {0x29, 0x00, 0xBC, 0xE0, 0x11, 0x01, 0x09, 0x01, 0x01, 0x00, 0x81}; // Note: APCI for L_Data.ind is just data_len (idx 8), then TPCI/APCI (idx 9), then data (idx 10)
                                                                                             // For L_Data.ind: APCI is 2bits (upper two of byte 9), Data is in byte 10
                                                                                             // Example: Byte 9 = 0b00000000 (TPCI, APCI=00 for GroupValueResponse/Read)
                                                                                             // Byte 10 = 0x01 for value
                                                                                             // For GroupValueWrite Indication it should be APCI=GroupValueWrite (0b10)
    // Corrected sample cEMI for L_Data.ind, group write indication, value 1 (ON) to GA 1/1/1 from 1.1.1
    // Message Code: 0x29 (L_Data.ind)
    // Additional Info Length: 0x00
    // Control Field 1: 0xBC (Standard Frame, Priority System, Repeat OK, System Broadcast)
    // Control Field 2: 0xE0 (Group Address, Hop Count = 6)
    // Source Address: 0x1101 (e.g. 1.1.1)
    // Destination Address: 0x0901 (e.g. 1/1/1 for 3-level)
    // Data Length: 0x01 (1 byte of payload: APCI_DATA)
    // TPCI/APCI: (APCI = 0b10xx for GroupValue_Write in cEMI payload) -> 0x00 here in this position, APCI is part of data for L_DATA.req
    // For L_Data.ind, the last 2 bytes are Data Service (1byte) + Data (1 byte for DPT 1.001)
    // The actual encoding can be complex. For simplicity, let's assume the TPUART provides a cEMI-like structure.
    // Payload for DPT 1.001 "ON", as a GroupValueWrite indication: [APCI (2bit) + Data (6bit)]
    // APCI 0b10 (GroupValueWrite) + data 0b000001 (ON). So data byte = 0x81.
    // Let's use a simpler cEMI for this simulation:
    // This is what the IP client might send, and we echo it as if it came from bus after a bit
    uint8_t sim_tp_payload[] = {
        0x11, 0x00,             // Message Code (L_Data.req), AddInfoLen=0
        0xBC, 0xF0,             // Control1, Control2 (dest group, hop count from 1.1.F0)
        0x11, 0xF0,             // Source Physical Address (e.g., 1.1.240, a simulated sensor)
        0x08, 0x01,             // Destination Group Address (e.g., 1/0/1)
        0x01,                   // Data Length (1 byte for APCI+Data)
        0x00,                   // TPCI=0, APCI=0 (GroupValueRead) - for a sensor value, this might be GroupValueResponse
                                // Or for a switch sending its state: APCI=GroupValueWrite
        0x81                    // Data: 1 for "ON" if GroupValueWrite (APCI needs to reflect write)
                                // if it's a GroupValueRead on GA 1/0/1, then this byte isn't used, Data length would be 0.
                                // For a GroupValueWrite from a sensor: APCI (e.g. 0b10 for write in upper 2 bits of data) + data (e.g. 01 for ON)
                                // Let's make it a GroupValueWrite from 1.1.240 to 1/0/1 with value ON (0x81)
    };
    memcpy(simulated_tp_telegram.data, sim_tp_payload, sizeof(sim_tp_payload));
    simulated_tp_telegram.len = sizeof(sim_tp_payload);
    // To make it an L_Data.ind (from bus to IP client), change message code
    simulated_tp_telegram.data[0] = 0x29; // L_Data.ind


    TickType_t last_simulated_send = xTaskGetTickCount();

    while (1) {
        // 1. Check for cEMI frames from KNX IP task to send to TP bus
        if (xQueueReceive(xQueueKnxIpToTp, &cemi_from_ip, pdMS_TO_TICKS(10)) == pdPASS) {
            tpuart_send_cemi_to_knx_bus(&cemi_from_ip);
        }

        // 2. Simulate receiving from TPUART (replace with actual uart_read_bytes and parsing)
        if ((xTaskGetTickCount() - last_simulated_send) > pdMS_TO_TICKS(15000)) { // Every 15s
             if (clients[0].active) { // Only if a client is connected
                ESP_LOGI(TAG, "TPUART: Simulating incoming KNX TP telegram for IP client.");
                // Modify the simulated telegram slightly if needed (e.g. source PA)
                if (xQueueSend(xQueueKnxTpToIp, &simulated_tp_telegram, pdMS_TO_TICKS(100)) != pdPASS) {
                    ESP_LOGW(TAG, "TPUART: Failed to queue simulated cEMI to IP task");
                }
            }
            last_simulated_send = xTaskGetTickCount();
        }
        // In a real system:
        // int len = uart_read_bytes(TPUART_NUM, uart_rx_buffer, BUF_SIZE - 1, pdMS_TO_TICKS(20));
        // if (len > 0) { parse_tpuart_data_to_cemi(uart_rx_buffer, len, &received_cemi);
        // xQueueSend(xQueueKnxTpToIp, &received_cemi, ...); }
    }
}


// --- Wi-Fi Functions (from Chapter 208) ---
static void wifi_event_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data) {
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
        if (s_retry_num < MAX_WIFI_RETRY) { esp_wifi_connect(); s_retry_num++; ESP_LOGI(TAG, "Retrying Wi-Fi"); }
        else { xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT); }
    } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
        ESP_LOGI(TAG, "Got IP: " IPSTR, IP2STR(&event->ip_info.ip));
        s_retry_num = 0; xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
    }
}

void wifi_init_sta(void) {
    s_wifi_event_group = xEventGroupCreate();
    ESP_ERROR_CHECK(esp_netif_init()); ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_sta();
    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));
    esp_event_handler_instance_t instance_any_id, instance_got_ip;
    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, &instance_any_id));
    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL, &instance_got_ip));
    wifi_config_t wifi_config = { .sta = { .ssid = WIFI_SSID, .password = WIFI_PASS }};
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
    ESP_ERROR_CHECK(esp_wifi_start());
    xEventGroupWaitBits(s_wifi_event_group, WIFI_CONNECTED_BIT | WIFI_FAIL_BIT, pdFALSE, pdFALSE, portMAX_DELAY);
    if (xEventGroupGetBits(s_wifi_event_group) & WIFI_CONNECTED_BIT) ESP_LOGI(TAG, "Connected to AP"); else ESP_LOGE(TAG, "Failed to connect to AP");
}

// --- KNX IP Gateway Task ---
void send_knx_ip_response(int sock, const struct sockaddr_in *client_addr, const void *data, size_t len) {
    int err = sendto(sock, data, len, 0, (struct sockaddr *)client_addr, sizeof(*client_addr));
    if (err < 0) {
        ESP_LOGE(TAG, "Error sending KNX IP response: errno %d", errno);
    }
}

// Helper to build and send a TUNNELLING_REQUEST to the active client
void forward_cemi_to_ip_client(const cemi_frame_t *cemi) {
    if (!clients[0].active || gateway_socket < 0) return;

    uint8_t tx_buffer[sizeof(knx_ip_header_t) + 4 /*TunnelHdr*/ + CEMI_FRAME_MAX_LEN];
    knx_ip_header_t *hdr = (knx_ip_header_t *)tx_buffer;
    hdr->header_len = 0x06;
    hdr->protocol_version = 0x10;
    hdr->service_type_id = htons(KNXNETIP_TUNNEL_TUNNELLING_REQUEST);

    // Tunneling Header (4 bytes: struct_len, channel_id, seq_counter, reserved)
    tx_buffer[sizeof(knx_ip_header_t) + 0] = 0x04; // Structure length
    tx_buffer[sizeof(knx_ip_header_t) + 1] = clients[0].channel_id;
    tx_buffer[sizeof(knx_ip_header_t) + 2] = clients[0].seq_counter_tx++; // Use and increment
    tx_buffer[sizeof(knx_ip_header_t) + 3] = 0x00; // Reserved

    memcpy(tx_buffer + sizeof(knx_ip_header_t) + 4, cemi->data, cemi->len);
    uint16_t total_len = sizeof(knx_ip_header_t) + 4 + cemi->len;
    hdr->total_len = htons(total_len);

    ESP_LOGI(TAG, "KNX_IP: Forwarding cEMI to client. ChID: %d, SeqTX: %d, Len: %d",
             clients[0].channel_id, clients[0].seq_counter_tx -1, cemi->len);
    send_knx_ip_response(gateway_socket, &clients[0].client_addr, tx_buffer, total_len);
}


static void knx_ip_gateway_task(void *pvParameters) {
    char rx_buffer[256]; // Buffer for incoming UDP packets
    struct sockaddr_in local_addr;

    // Wait for Wi-Fi
    xEventGroupWaitBits(s_wifi_event_group, WIFI_CONNECTED_BIT, pdFALSE, pdTRUE, portMAX_DELAY);
    if (!(xEventGroupGetBits(s_wifi_event_group) & WIFI_CONNECTED_BIT)) {
        ESP_LOGE(TAG, "Wi-Fi not connected. Gateway task exiting.");
        vTaskDelete(NULL);
        return;
    }

    // Create UDP socket
    gateway_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
    if (gateway_socket < 0) {
        ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
        vTaskDelete(NULL);
        return;
    }

    // Bind to local address and KNX_IP_PORT
    local_addr.sin_family = AF_INET;
    local_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    local_addr.sin_port = htons(KNX_IP_PORT);
    if (bind(gateway_socket, (struct sockaddr *)&local_addr, sizeof(local_addr)) < 0) {
        ESP_LOGE(TAG, "Socket bind failed: errno %d", errno);
        close(gateway_socket); gateway_socket = -1;
        vTaskDelete(NULL);
        return;
    }
    ESP_LOGI(TAG, "KNX IP Gateway socket bound, listening on port %d", KNX_IP_PORT);

    clients[0].active = false; // Initialize client state

    while (1) {
        struct sockaddr_in source_addr_client; // To store client's address
        socklen_t socklen = sizeof(source_addr_client);
        cemi_frame_t cemi_from_tp;

        // 1. Check for cEMI frames from TPUART task to forward to IP client
        if (clients[0].active && xQueueReceive(xQueueKnxTpToIp, &cemi_from_tp, 0) == pdPASS) {
             ESP_LOGI(TAG, "KNX_IP: Received cEMI from TP queue to forward.");
             forward_cemi_to_ip_client(&cemi_from_tp);
        }

        // 2. Poll for incoming UDP packets (non-blocking with timeout)
        struct timeval tv;
        tv.tv_sec = 0;
        tv.tv_usec = 10000; // 10ms timeout for select
        fd_set rfds;
        FD_ZERO(&rfds);
        FD_SET(gateway_socket, &rfds);

        int s = select(gateway_socket + 1, &rfds, NULL, NULL, &tv);
        if (s > 0 && FD_ISSET(gateway_socket, &rfds)) {
            int len = recvfrom(gateway_socket, rx_buffer, sizeof(rx_buffer) - 1, 0, (struct sockaddr *)&source_addr_client, &socklen);
            if (len < 0) {
                ESP_LOGE(TAG, "recvfrom failed: errno %d", errno);
                continue;
            }
            rx_buffer[len] = 0; // Null-terminate (though not strictly needed for binary)
            // ESP_LOGI(TAG, "Received %d bytes from %s:%d", len, inet_ntoa(source_addr_client.sin_addr), ntohs(source_addr_client.sin_port));

            knx_ip_header_t *req_hdr = (knx_ip_header_t *)rx_buffer;
            uint16_t service_type = ntohs(req_hdr->service_type_id);

            if (service_type == KNXNETIP_CORE_CONNECT_REQUEST) {
                ESP_LOGI(TAG, "KNX_IP: CONNECT_REQUEST received.");
                if (!clients[0].active) {
                    clients[0].active = true;
                    clients[0].channel_id = 1; // Assign a channel ID
                    clients[0].seq_counter_rx = 0;
                    clients[0].seq_counter_tx = 0;
                    memcpy(&clients[0].client_addr, &source_addr_client, sizeof(struct sockaddr_in));
                    clients[0].last_seen = xTaskGetTickCount();

                    // Send CONNECT_RESPONSE
                    uint8_t resp_buffer[sizeof(knx_ip_header_t) + 8 + 8]; // Header + HPAI_Status + CRI
                    knx_ip_header_t *resp_hdr = (knx_ip_header_t *)resp_buffer;
                    resp_hdr->header_len = 0x06;
                    resp_hdr->protocol_version = 0x10;
                    resp_hdr->service_type_id = htons(KNXNETIP_CORE_CONNECT_RESPONSE);
                    
                    // Body: ChannelID (1), Status (1), HPAI (8), CRI Data (variable)
                    resp_buffer[sizeof(knx_ip_header_t) + 0] = clients[0].channel_id; // Channel ID
                    resp_buffer[sizeof(knx_ip_header_t) + 1] = 0x00; // Status E_NO_ERROR
                    // HPAI Data Endpoint (where client should send tunneling requests to)
                    // For simplicity, we assume client sends to same IP/Port of gateway. Real gateway might specify.
                    resp_buffer[sizeof(knx_ip_header_t) + 2] = 0x08; // HPAI Structure len
                    resp_buffer[sizeof(knx_ip_header_t) + 3] = 0x01; // Host Proto Code UDP
                    // IP Addr and Port can be gateway's IP/Port or specific data endpoint.
                    // Let's put 0.0.0.0:0 indicating client should use control endpoint for data.
                    memset(&resp_buffer[sizeof(knx_ip_header_t) + 4], 0, 4); // IP Address
                    memset(&resp_buffer[sizeof(knx_ip_header_t) + 8], 0, 2); // Port
                    
                    // CRI (Connection Response Information) - includes assigned PA, etc.
                    // For simplicity, just provide minimal CRI: type = tunneling
                    // Actual CRI would have more info like max APDU length.
                    // This example CRI is very basic.
                    uint8_t cri_data[] = {0x04, 0x04, 0x02, 0x00}; // Len 4, Type Tunneling, KnxLayer Link, Reserved
                    memcpy(&resp_buffer[sizeof(knx_ip_header_t) + 10], cri_data, sizeof(cri_data));
                    
                    resp_hdr->total_len = htons(sizeof(knx_ip_header_t) + 2 + 8 + sizeof(cri_data)); // Minimal length
                    send_knx_ip_response(gateway_socket, &source_addr_client, resp_buffer, ntohs(resp_hdr->total_len));
                    ESP_LOGI(TAG, "KNX_IP: Client connected. Channel ID: %d", clients[0].channel_id);
                } else {
                    ESP_LOGW(TAG, "KNX_IP: CONNECT_REQUEST received, but gateway busy (max clients reached).");
                    // Send CONNECT_RESPONSE with error E_NO_MORE_CONNECTIONS (0x24)
                    // ... (implementation omitted for brevity)
                }

            } else if (clients[0].active && memcmp(&clients[0].client_addr, &source_addr_client, sizeof(struct sockaddr_in)) == 0) {
                // Process only if client is the one we know and is active
                clients[0].last_seen = xTaskGetTickCount();
                uint8_t client_channel_id = rx_buffer[sizeof(knx_ip_header_t) + 1];
                uint8_t client_seq_counter = rx_buffer[sizeof(knx_ip_header_t) + 2];

                if (client_channel_id != clients[0].channel_id) {
                    ESP_LOGW(TAG, "KNX_IP: Received msg with invalid Channel ID %d, expected %d", client_channel_id, clients[0].channel_id);
                    continue;
                }

                // Basic sequence counter check (simple wrap-around, no out-of-order handling)
                // KNX Spec: "If the received sequence number is less than the stored one (considering 8-bit wrap-around)
                // then the frame is an old one and shall be acknowledged and discarded."
                // This check is simplified:
                // if (client_seq_counter != clients[0].seq_counter_rx) {
                //    ESP_LOGW(TAG, "KNX_IP: Seq counter mismatch. Got %d, Exp %d. Sending ACK for %d, ignoring data.",
                //             client_seq_counter, clients[0].seq_counter_rx, client_seq_counter);
                //     // Send ACK for the received sequence counter anyway
                // }


                if (service_type == KNXNETIP_TUNNEL_TUNNELLING_REQUEST) {
                    ESP_LOGI(TAG, "KNX_IP: TUNNELLING_REQUEST from client. SeqRX: %d", client_seq_counter);
                    // Extract cEMI
                    cemi_frame_t cemi_to_tp;
                    int cemi_offset = sizeof(knx_ip_header_t) + 4; // Skip KNX IP Hdr + Tunnel Hdr
                    cemi_to_tp.len = ntohs(req_hdr->total_len) - cemi_offset;
                    if (cemi_to_tp.len > 0 && cemi_to_tp.len <= CEMI_FRAME_MAX_LEN) {
                        memcpy(cemi_to_tp.data, rx_buffer + cemi_offset, cemi_to_tp.len);
                        // Queue cEMI for TPUART task
                        if (xQueueSend(xQueueKnxIpToTp, &cemi_to_tp, pdMS_TO_TICKS(100)) != pdPASS) {
                            ESP_LOGE(TAG, "KNX_IP: Failed to queue cEMI for TP bus.");
                        } else {
                            ESP_LOGI(TAG, "KNX_IP: Queued cEMI for TP bus. Len: %d", cemi_to_tp.len);
                        }
                    } else {
                        ESP_LOGE(TAG, "KNX_IP: Invalid cEMI length in Tunnelling_Request: %d", cemi_to_tp.len);
                    }
                    clients[0].seq_counter_rx = client_seq_counter + 1; // Update expected next

                    // Send TUNNELLING_ACK
                    uint8_t ack_buffer[sizeof(knx_ip_header_t) + 4 + 1]; // Header + TunnelHdr + Status
                    knx_ip_header_t *ack_hdr = (knx_ip_header_t *)ack_buffer;
                    ack_hdr->header_len = 0x06;
                    ack_hdr->protocol_version = 0x10;
                    ack_hdr->service_type_id = htons(KNXNETIP_TUNNEL_TUNNELLING_ACK);
                    ack_hdr->total_len = htons(sizeof(knx_ip_header_t) + 4 + 1);
                    ack_buffer[sizeof(knx_ip_header_t) + 0] = 0x04; // Tunnel Header len
                    ack_buffer[sizeof(knx_ip_header_t) + 1] = clients[0].channel_id;
                    ack_buffer[sizeof(knx_ip_header_t) + 2] = client_seq_counter; // ACK the received sequence
                    ack_buffer[sizeof(knx_ip_header_t) + 3] = 0x00; // Reserved
                    ack_buffer[sizeof(knx_ip_header_t) + 4] = 0x00; // Status E_NO_ERROR
                    send_knx_ip_response(gateway_socket, &clients[0].client_addr, ack_buffer, sizeof(ack_buffer));
                    ESP_LOGI(TAG, "KNX_IP: Sent TUNNELLING_ACK for seq %d", client_seq_counter);

                } else if (service_type == KNXNETIP_CORE_DISCONNECT_REQUEST) {
                    ESP_LOGI(TAG, "KNX_IP: DISCONNECT_REQUEST received. ChID: %d", client_channel_id);
                    clients[0].active = false;
                    // Send DISCONNECT_RESPONSE
                    uint8_t dresp_buffer[sizeof(knx_ip_header_t) + 2]; // Header + ChID + Status
                    knx_ip_header_t *dresp_hdr = (knx_ip_header_t *)dresp_buffer;
                    dresp_hdr->header_len = 0x06;
                    dresp_hdr->protocol_version = 0x10;
                    dresp_hdr->service_type_id = htons(KNXNETIP_CORE_DISCONNECT_RESPONSE);
                    dresp_hdr->total_len = htons(sizeof(knx_ip_header_t) + 2);
                    dresp_buffer[sizeof(knx_ip_header_t) + 0] = client_channel_id;
                    dresp_buffer[sizeof(knx_ip_header_t) + 1] = 0x00; // Status E_NO_ERROR
                    send_knx_ip_response(gateway_socket, &clients[0].client_addr, dresp_buffer, sizeof(dresp_buffer));
                    ESP_LOGI(TAG, "KNX_IP: Client disconnected.");

                } else if (service_type == KNXNETIP_CORE_CONNECTIONSTATE_REQUEST) {
                     ESP_LOGI(TAG, "KNX_IP: CONNECTIONSTATE_REQUEST received. ChID: %d", client_channel_id);
                    // Send CONNECTIONSTATE_RESPONSE
                    uint8_t cs_resp_buffer[sizeof(knx_ip_header_t) + 2]; // Header + ChID + Status
                    knx_ip_header_t *cs_resp_hdr = (knx_ip_header_t *)cs_resp_buffer;
                    cs_resp_hdr->header_len = 0x06;
                    cs_resp_hdr->protocol_version = 0x10;
                    cs_resp_hdr->service_type_id = htons(KNXNETIP_CORE_CONNECTIONSTATE_RESPONSE);
                    cs_resp_hdr->total_len = htons(sizeof(knx_ip_header_t) + 2);
                    cs_resp_buffer[sizeof(knx_ip_header_t) + 0] = client_channel_id;
                    cs_resp_buffer[sizeof(knx_ip_header_t) + 1] = 0x00; // Status E_NO_ERROR (tunnel is active)
                    send_knx_ip_response(gateway_socket, &clients[0].client_addr, cs_resp_buffer, sizeof(cs_resp_buffer));
                } else {
                     ESP_LOGW(TAG, "KNX_IP: Received unhandled service type 0x%04X from client.", service_type);
                }
            } else if (len > 0) { // Packet from an unknown/inactive client
                 ESP_LOGW(TAG, "KNX_IP: Received %d bytes from unknown or inactive client %s:%d", len,
                         inet_ntoa(source_addr_client.sin_addr), ntohs(source_addr_client.sin_port));
            }
        } // end if(select > 0)

        // Simple client timeout check (e.g., 60 seconds)
        if (clients[0].active && (xTaskGetTickCount() - clients[0].last_seen) > pdMS_TO_TICKS(60000)) {
            ESP_LOGW(TAG, "KNX_IP: Client timeout. Channel ID: %d. Disconnecting.", clients[0].channel_id);
            clients[0].active = false;
            // Optionally send DISCONNECT_REQUEST to client if it has a known structure
        }
    } // end while(1)

    ESP_LOGI(TAG, "KNX IP Gateway task_terminating.");
    if (gateway_socket != -1) { close(gateway_socket); gateway_socket = -1; }
    vTaskDelete(NULL);
}

void app_main(void) {
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
      ESP_ERROR_CHECK(nvs_flash_erase()); ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);
    wifi_init_sta();

    // Create queues for inter-task communication
    // Max 10 cEMI frames in each queue, each frame can be up to CEMI_FRAME_MAX_LEN
    xQueueKnxTpToIp = xQueueCreate(10, sizeof(cemi_frame_t));
    xQueueKnxIpToTp = xQueueCreate(10, sizeof(cemi_frame_t));

    if (!xQueueKnxTpToIp || !xQueueKnxIpToTp) {
        ESP_LOGE(TAG, "Failed to create queues. Halting.");
        return;
    }

    xTaskCreate(tpuart_task, "tpuart_task", 4096, NULL, 5, NULL);
    xTaskCreate(knx_ip_gateway_task, "knx_ip_gw_task", 4096 * 2, NULL, 5, NULL);
}

CONFIG_ESP_WIFI_SSID="YOUR_WIFI_SSID"
CONFIG_ESP_WIFI_PASSWORD="YOUR_WIFI_PASSWORD"
CONFIG_ESP_MAXIMUM_RETRY=10
CONFIG_LWIP_UDP_RECVMBOX_SIZE=12 # Increase if many small UDP packets are expected
CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32