[Raspberry Pi Pico W] MODBUS Ep 4. MODBUS/TCP Security || LWIP || MbedTLS || Node-RED

 本文章介紹MODBUS/TCP Security, 網路library使用LWIP altcp(application layer TCP)，此library將SSL/TLS(MbedTLS)結合原來的TCP library。

Modbus server使用Raspberry Pi Pico W，Clinet端分使用Node-RED與Raspberry Pi Pico W來進行測試，在本專案中利用Wireshark擷取封包來比較未加密與透過Transport layer security加密的ADU封包。

MODBUS/TCP security在原來MODBUS TCP加上TLS Transport Layer Security, 如下圖比較：

LWIP網路API使用altcp，此組函式庫擴張原來TCP library，在原來使用tcp_*的呼叫流程中為呼叫altcp_*即可。另外須先初始或tls_config將Root CA, server primary key與server certification加入呼叫

server:  altcp_tls_create_config_server_privkey_cert(...)

與

client:  altcp_tls_create_config_client(...)

中。

若須使用self-signed certification可使用下列步驟建立：

1. openssl genrsa -des3 -out ca.key 2048:
   建立Root CA key。
2. openssl req -new -x509 -days 3650 -key ca.key -out ca.cert:
   建立self-signed root Certificate。
3. openssl genrsa -out server.key 2048:
   建立server side private key。
   
4. openssl req -new -out server.csr -key server.key
   使用server key製作server憑證需求(certificate request)
5. openssl x509 -req -in server.csr -CA ca.cert -CAkey ca.key -CAcreateserial -out server.cert -days 365
   使用root CA ca.cert簽署server side certificate。

完整程式碼附於文章末尾。

成果影片：

程式碼：

Server:

• CMakeLists.txt

# Generated Cmake Pico project file

cmake_minimum_required(VERSION 3.13)

set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# Initialise pico_sdk from installed location
# (note this can come from environment, CMake cache etc)

# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
if(WIN32)
    set(USERHOME $ENV{USERPROFILE})
else()
    set(USERHOME $ENV{HOME})
endif()
set(sdkVersion 2.1.0)
set(toolchainVersion 13_3_Rel1)
set(picotoolVersion 2.1.0)
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
if (EXISTS ${picoVscode})
    include(${picoVscode})
endif()
# ====================================================================================
set(PICO_BOARD pico_w CACHE STRING "Board type")

# Pull in Raspberry Pi Pico SDK (must be before project)
include(pico_sdk_import.cmake)

project(picow_modbus_tcp_security_server C CXX ASM)

# Initialise the Raspberry Pi Pico SDK
pico_sdk_init()

# Add executable. Default name is the project name, version 0.1

add_executable(picow_modbus_tcp_security_server 
                picow_modbus_tcp_security_server.c 
                nanomodbus/nanomodbus.c
                picow_tls_server/picow_tls_server.c 
                modbus_tcp_server/modbus_tcp_server.c)

pico_set_program_name(picow_modbus_tcp_security_server "picow_modbus_tcp_security_server")
pico_set_program_version(picow_modbus_tcp_security_server "0.1")

# Modify the below lines to enable/disable output over UART/USB
pico_enable_stdio_uart(picow_modbus_tcp_security_server 1)
pico_enable_stdio_usb(picow_modbus_tcp_security_server 0)

# Add the standard library to the build
target_link_libraries(picow_modbus_tcp_security_server
        pico_stdlib)

# Add the standard include files to the build
target_include_directories(picow_modbus_tcp_security_server PRIVATE
        ${CMAKE_CURRENT_LIST_DIR}
)

add_subdirectory(GPIO_StepperMotor)
# Add any user requested libraries
target_link_libraries(picow_modbus_tcp_security_server 
        pico_cyw43_arch_lwip_threadsafe_background
        pico_lwip_mbedtls
        pico_mbedtls
        pico_lwip_mdns
        gpio_stepper_motor
        )

pico_add_extra_outputs(picow_modbus_tcp_security_server)

# Ignore warnings from lwip code
set_source_files_properties(
        ${PICO_LWIP_PATH}/src/apps/altcp_tls/altcp_tls_mbedtls.c
        PROPERTIES
        COMPILE_OPTIONS "-Wno-unused-result"
        )

• picow_modbus_tcp_security_server.c

#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "lwip/apps/mdns.h"

#include "nanomodbus/nanomodbus.h"
#include "picow_tls_server/picow_tls_server.h"
#include "modbus_tcp_server/modbus_tcp_server.h"

#include "lwip/pbuf.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"
#include "lwip/dns.h"

#include "GPIO_StepperMotor/steppermotor.h"

#define MODBUS_SERVER_HOST_NAME "modbus_tls_tcp"


int main()
{
    stdio_init_all();
    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN,true);

    stepperMotor_init(MOTOR_PIN1, MOTOR_PIN2, MOTOR_PIN3, MOTOR_PIN4);
    stepperMotor_set_speed(5);

// ====== Initialise the Wi-Fi chip =====
    if (cyw43_arch_init()) {
        printf("Wi-Fi init failed\n");
        return -1;
    }
    cyw43_gpio_set(&cyw43_state, CYW43_WL_GPIO_LED_PIN,false);
    
    // Enable wifi station
    cyw43_arch_enable_sta_mode();

    DEBUG_print("Connecting to Wi-Fi...\n");
    if (cyw43_arch_wifi_connect_timeout_ms(SSID, PWD, CYW43_AUTH_WPA2_AES_PSK, 30000)) {
        DEBUG_print("failed to connect.\n");
        return 1;
    } else {
        DEBUG_print("Connected.\n");
        // Read the ip address in a human readable way
        uint8_t *ip_address = (uint8_t*)&(cyw43_state.netif[0].ip_addr.addr);
        DEBUG_print("IP address %d.%d.%d.%d\n", ip_address[0], ip_address[1], ip_address[2], ip_address[3]);
    }
#if LWIP_MDNS_RESPONDER
    mdns_resp_init();
    uint8_t host_name[] = MODBUS_SERVER_HOST_NAME;
    if (mdns_resp_add_netif(netif_default, host_name)== ERR_OK)
    {
        printf("mDNS add successfully\n");
    } else {
        printf("mDNS failure\n");
    }
#endif
    picow_tls_server_init();

    
//======= modbus tcp server(modbus server) init ======
    nmbs_platform_conf platform_conf;
    nmbs_platform_conf_create(&platform_conf);
    platform_conf.transport = NMBS_TRANSPORT_TCP;
    platform_conf.read = nmbs_transport_read;
    platform_conf.write = nmbs_transport_write;

    extern ALTCP_SERVER_T *altcp_tcp_server;
    platform_conf.arg = altcp_tcp_server;
    
    nmbs_callbacks callbacks;
    nmbs_callbacks_create(&callbacks);
    callbacks.read_coils = handle_altcp_read_coils;
    callbacks.read_discrete_inputs = handle_altcp_read_discrete_inputs;
    callbacks.write_single_coil = handle_altcp_write_single_coil;
    callbacks.write_multiple_coils = handle_altcp_write_multiple_coils;
    callbacks.read_holding_registers = handler_tcp_read_holding_registers;
    callbacks.write_multiple_registers = handle_altcp_write_multiple_registers;
    callbacks.write_single_register = handle_altcp_write_single_register;
    callbacks.read_file_record = handle_altcp_read_file_record;
    callbacks.write_file_record = handle_altcp_write_file_record;
    callbacks.arg = altcp_tcp_server;
    
    nmbs_error err = nmbs_server_create(&nmbs_tls, 0, &platform_conf, &callbacks);
    if (err != NMBS_ERROR_NONE) {
        printf("Error creating modbus server\n");
        return 1;
    }

    // Set only the polling timeout. Byte timeout will be handled by the TCP connection
    nmbs_set_read_timeout(&nmbs_tls, 2000);
//======= modbus tcp server(modbus server) init ======
    
    printf("Modbus TCP server started\n");
    cyw43_gpio_set(&cyw43_state, CYW43_WL_GPIO_LED_PIN,true);
//========================

    while (true) {
        nmbs_server_poll(&nmbs_tls);
        
#if PICO_CYW43_ARCH_POLL
        cyw43_arch_poll();
         cyw43_arch_wait_for_work_until(make_timeout_time_ms(1000));
#else
        tight_loop_contents();
#endif
    }
}

• picow_tls_server.c

#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"

#include "nanomodbus/nanomodbus.h"

#include "lwip/pbuf.h"
#include "lwip/altcp_tls.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp.h"
#include "lwip/def.h"

#include "picow_tls_server.h"
#include "modbus_tcp_server/modbus_tcp_server.h"
#include "modbus_server_cert.h"

static int8_t conn_count=0;

ALTCP_SERVER_T *altcp_tcp_server;
static struct altcp_tls_config* tls_config;

static err_t altcp_client_close(struct altcp_pcb *client_pcb) {
    err_t err = ERR_OK;
    if (client_pcb != NULL) {
        altcp_arg(client_pcb, NULL);
        altcp_poll(client_pcb, NULL, 0);
        altcp_sent(client_pcb, NULL);
        altcp_recv(client_pcb, NULL);
        altcp_err(client_pcb, NULL);
        err = altcp_close(client_pcb);
        if (err != ERR_OK) {
            DEBUG_print("close failed %d, calling abort\n", err);
            altcp_abort(client_pcb);
            err = ERR_ABRT;
        }
        client_pcb = NULL;
        altcp_tcp_server->connected = false;
    }
    DEBUG_print("connections after client_closed:%d\n", --conn_count);
    return err;
}

static err_t altcp_server_close(void *arg) {
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;
    err_t err = ERR_OK;

    if (state->client_pcb != NULL) {
        altcp_arg(state->client_pcb, NULL);
        altcp_poll(state->client_pcb, NULL, 0);
        altcp_sent(state->client_pcb, NULL);
        altcp_recv(state->client_pcb, NULL);
        altcp_err(state->client_pcb, NULL);
        err = altcp_close(state->client_pcb);
        if (err != ERR_OK) {
            DEBUG_print("close failed %d, calling abort\n", err);
            altcp_abort(state->client_pcb);
            err = ERR_ABRT;
        }
        state->client_pcb = NULL;
    }
    if (state->server_pcb) {
        altcp_arg(state->server_pcb, NULL);
        altcp_close(state->server_pcb);
        state->server_pcb = NULL;
    }
    DEBUG_print("altcp server closed\n");
    return err;
}

static err_t altcp_server_sent(void *arg, struct altcp_pcb *tpcb, u16_t len) {
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;
    return ERR_OK;
}

static err_t altcp_server_send_data(void *arg, struct altcp_pcb *tpcb)
{
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;
    return ERR_OK;
}

err_t altcp_server_recv(void *arg, struct altcp_pcb *tpcb, struct pbuf *p, err_t err) {
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;
    //state->client_pcb = tpcb;
    static altcp_queue_t temp_recv;
    if (!p) {
        DEBUG_print("client connect close---------\n");
        return altcp_client_close(tpcb);
    }

    if (p->tot_len > 0) {
        // Receive the buffer 
        uint16_t recv_len = pbuf_copy_partial(p, temp_recv.buffer, p->tot_len, 0); 
        temp_recv.buffer_len = recv_len;
        //queue_add_blocking(&state->recv_queue, &temp_recv);
    if (!queue_try_add(&state->recv_queue, &temp_recv)) {
        DEBUG_print("\n=======\n\nbusy\n-------\n");
        return ERR_INPROGRESS;
    }

        altcp_recved(tpcb, recv_len);
#ifdef __DEBUG__
        DEBUG_print("---------recv---------\n");
        for (int i =0; i < recv_len; i++) {
            DEBUG_print("%02x ",temp_recv.buffer[i]);
        }
        DEBUG_print("\n-------------\n");
#endif
        
    }
    pbuf_free(p);
    return ERR_OK;
}

static err_t altcp_server_poll(void *arg, struct altcp_pcb *tpcb) {

    return ERR_OK;
}

static void altcp_server_err(void *arg, err_t err) {
    if (err != ERR_ABRT) {
        DEBUG_print("tcp_client_err_fn %d\n", err);
    }
}

static err_t altcp_server_accept(void *arg, struct altcp_pcb *client_pcb, err_t err) {
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;
    if (err != ERR_OK || client_pcb == NULL) {
        DEBUG_print("Failure in accept\n");
        return err;
    }
    
    //only allow one client(modbus master) to connect
    if (state->connected) {
        DEBUG_print("More than one connection!\n");
        return ERR_CONN;
    }
        
    state->client_pcb = client_pcb;
    
    altcp_arg(state->client_pcb, state);
    altcp_sent(state->client_pcb, altcp_server_sent);
    altcp_recv(state->client_pcb, altcp_server_recv);
    altcp_poll(state->client_pcb, altcp_server_poll, POLL_TIME_S * 2);
    altcp_err(state->client_pcb, altcp_server_err);
    state->connected = true;
    strcpy(state->client_ip_addr, ip4addr_ntoa(altcp_get_ip(client_pcb, false)));
    DEBUG_print("Client connected:%d:%s\n", ++conn_count, state->client_ip_addr);
    return ERR_OK;
}

static bool altcp_server_open(void *arg) {
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;
    DEBUG_print("Starting server at %s on port %u\n", ip4addr_ntoa(netif_ip4_addr(netif_list)), MODBUS_ALTCP_PORT);

    // set TLS connection
    tls_config = altcp_tls_create_config_server_privkey_cert(MODBUS_SERVER_KEY, sizeof(MODBUS_SERVER_KEY), NULL, 0, MODBUS_SERVER_CERT, sizeof(MODBUS_SERVER_CERT));
    //mbedtls_x509_crt_parse(tls_config->cert, CA_CERT, sizeof(CA_CERT));
    
    struct altcp_pcb *alpcb = altcp_tls_new(tls_config, IPADDR_TYPE_ANY);
    if (!alpcb) {
        DEBUG_print("failed to create pcb\n");
        return false;
    }
    
    err_t err = altcp_bind(alpcb, NULL, MODBUS_ALTCP_PORT);
    if (err) {
        DEBUG_print("failed to bind to port %u\n", MODBUS_ALTCP_PORT);
        return false;
    }

    
    state->server_pcb = altcp_listen_with_backlog(alpcb,1);
    if (!state->server_pcb) {
        DEBUG_print("failed to listen\n");
        if (alpcb) {
            altcp_close(alpcb);
        }
        return false;
    }

    altcp_arg(state->server_pcb, state);
    altcp_accept(state->server_pcb, altcp_server_accept);

    return true;
}

bool picow_tls_server_init(void) {
    /* modbus TCP server  init*/
    altcp_tcp_server = calloc(1, sizeof(ALTCP_SERVER_T));
    if (!altcp_tcp_server) {
        return false;
    }

    //altcp_tcp_server initial values;
    queue_init(&altcp_tcp_server->recv_queue, sizeof(altcp_queue_t), 4);
    altcp_tcp_server->connected = false;
    altcp_tcp_server->queue_left=0;
    altcp_tcp_server->recv_count=0;
    altcp_tcp_server->start_index=0;

    if (!altcp_server_open(altcp_tcp_server)) {
        DEBUG_print("tcp server open error\n");
        return false;
    }

    return true;
}

• picow_tls_server.h

#define MODBUS_TCP_PORT 502            //MODBUS default port
#define MODBUS_ALTCP_PORT 802            //MODBUS default port

#define POLL_TIME_S 1

#define __DEBUG__
#ifdef __DEBUG__
#define DEBUG_print(...) printf(__VA_ARGS__)
#else
#define DEBUG_print(...) (void) (0)
#endif


#define SSID "your-SSID"
#define PWD "your-PASSWD"

#define LED_PIN 15
#define MOTOR_PIN1  21
#define MOTOR_PIN2  20
#define MOTOR_PIN3  19
#define MOTOR_PIN4  18

#include "pico/util/queue.h"
#include "lwip/altcp.h"

typedef struct __altcp_queue_t{
    uint8_t buffer[260];
    uint16_t buffer_len;
} altcp_queue_t;

typedef struct ALTCP_SERVER_T_ {
    struct altcp_pcb *server_pcb;
    struct altcp_pcb *client_pcb;
    unsigned char client_ip_addr[20];
    bool connected;
    queue_t recv_queue;
    uint8_t server_host_name[126];
    uint16_t recv_count;
    uint16_t start_index;
    uint16_t queue_left;
    altcp_queue_t temp_recv;
} ALTCP_SERVER_T;

bool picow_tls_server_init(void);

• modbus_tcp_server.c

#include "stdio.h"
#include "pico/stdlib.h"
#include "modbus_tcp_server.h"


#include "lwip/pbuf.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"
#include "picow_tls_server/picow_tls_server.h"
#include "GPIO_StepperMotor/steppermotor.h"


nmbs_t nmbs_tls;

#define COILS_ADDR_MAX      100
#define REGS_ADDR_MAX       32
#define FILE_SIZE_MAX       32

// A single nmbs_bitfield variable can keep 2000 coils
bool terminate = false;
nmbs_bitfield server_coils = {0};
uint16_t server_registers[REGS_ADDR_MAX] = {0};
uint16_t server_file[FILE_SIZE_MAX];


#define UNUSED_PARAM(x) ((x) = (x))

void sighandler(int s) {
    UNUSED_PARAM(s);
    terminate = true;
}

nmbs_error handle_altcp_read_coils(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);
    ALTCP_SERVER_T * mb = (ALTCP_SERVER_T*) arg;


    if (address + quantity > COILS_ADDR_MAX + 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

   
     
    for (int i = 0; i < quantity; i++) {
        nmbs_bitfield_write(server_coils, address + i, nmbs_bitfield_read(coils_out, i));
    }
  
    return NMBS_ERROR_NONE;
}

nmbs_error handle_altcp_read_discrete_inputs(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (address + quantity > COILS_ADDR_MAX + 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

     
    for (int i = 0; i < quantity; i++) {
        nmbs_bitfield_write(server_coils, address + i, nmbs_bitfield_read(coils_out, i));
    }

    return NMBS_ERROR_NONE;
}

nmbs_error handle_altcp_write_single_coil(uint16_t address, bool value, uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    //if (address > COILS_ADDR_MAX )
    //    return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    
    if (address > 2 )
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

    
    // Write coils values to our server_coils
    nmbs_bitfield_write(server_coils, address, value);
    if (address == 1) {
        gpio_put(LED_PIN, value);
    }
    if (address == 0) {
        stepperMotor_rotate_angle(90, value);
    }

    return NMBS_ERROR_NONE;
}

nmbs_error handle_altcp_write_multiple_coils(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id,
                                       void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (address + quantity > COILS_ADDR_MAX + 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

    

    // Write coils values to our server_coils
    for (int i = 0; i < quantity; i++) {
        nmbs_bitfield_write(server_coils, address + i, nmbs_bitfield_read(coils, i));
    }

    return NMBS_ERROR_NONE;
}


nmbs_error handler_tcp_read_holding_registers(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id,
                                          void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (address + quantity > REGS_ADDR_MAX + 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

    
    for (int i = 0; i < quantity; i++)
        server_registers[address + i] = registers_out[i];

    return NMBS_ERROR_NONE;
}

nmbs_error handle_altcp_write_single_register(uint16_t address, uint16_t value, uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (address > REGS_ADDR_MAX)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    
    
    // Write registers values to our server_registers

        server_registers[address] = value;

    return NMBS_ERROR_NONE;
}

nmbs_error handle_altcp_write_multiple_registers(uint16_t address, uint16_t quantity, const uint16_t* registers,
                                           uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (address + quantity > REGS_ADDR_MAX + 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    
    

    // Write registers values to our server_registers
    for (int i = 0; i < quantity; i++)
        server_registers[address + i] = registers[i];

    return NMBS_ERROR_NONE;
}

nmbs_error handle_altcp_read_file_record(uint16_t file_number, uint16_t record_number, uint16_t* registers, uint16_t count,
                                   uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (file_number != 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

    if ((record_number + count) > FILE_SIZE_MAX)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    


    memcpy(registers, server_file + record_number, count * sizeof(uint16_t));

    return NMBS_ERROR_NONE;
}


nmbs_error handle_altcp_write_file_record(uint16_t file_number, uint16_t record_number, const uint16_t* registers,
                                    uint16_t count, uint8_t unit_id, void* arg) {
    UNUSED_PARAM(arg);
    UNUSED_PARAM(unit_id);

    if (file_number != 1)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

    if ((record_number + count) > FILE_SIZE_MAX)
        return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;

    
    memcpy(server_file + record_number, registers, count * sizeof(uint16_t));

    return NMBS_ERROR_NONE;
}

int32_t nmbs_transport_read(uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
    ALTCP_SERVER_T *state=(ALTCP_SERVER_T*) arg;
 
    uint16_t loop_count = 0;

    if (state->queue_left == 0) {
        while (queue_is_empty(&state->recv_queue) && loop_count < 20) {           
            loop_count++;
            busy_wait_ms(50);
        }

        if (!queue_try_remove(&(state->recv_queue), &state->temp_recv))  {
                return 0;
        } else {
            DEBUG_print("\t\t\t\t---loop count:%d\n", loop_count);
        }
        state->queue_left = state->temp_recv.buffer_len;
    }
  
    state->recv_count = state->queue_left <= count ? state->queue_left : count;
    state->start_index = state->temp_recv.buffer_len-state->queue_left;
    for (int i=0; i < state->recv_count; i++) buf[i] = state->temp_recv.buffer[state->start_index+i];

    state->queue_left = state->queue_left-state->recv_count;

    //Debug
#ifdef __DEBUG__
    DEBUG_print("\n=====transport read:%d:%d\n", count, state->temp_recv.buffer_len);
    for (int i=0; i < state->recv_count; i++) {
        DEBUG_print("%02x ", buf[i]);
    }
    DEBUG_print("\n==\n");
#endif
    return state->recv_count;
}

int32_t nmbs_transport_write(const uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
    ALTCP_SERVER_T *state = (ALTCP_SERVER_T*)arg;

    // Debug
#ifdef __DEBUG__
    DEBUG_print("====\nnmbs_transport_write count:%d\n", count);
    for (int i=0; i < count;i++) {
        DEBUG_print("%02x ", buf[i]);
    }
    DEBUG_print("\n====\n");
#endif
    err_t err = altcp_write(state->client_pcb, buf, count, TCP_WRITE_FLAG_COPY);
    if (err != ERR_OK) 
    {
        //tcp_abort(state->tcp_pcb);
        DEBUG_print("tcp_write error:%d:%d\n", err, altcp_sndbuf(state->client_pcb)); 
        return 0;
    }

    err = altcp_output(state->client_pcb);
    if (err != ERR_OK) 
    {
        DEBUG_print("%s:tcp_output:%d\n", state->server_host_name, err); 
        return 0;
    }

    return count;
}

• modbus_tcp_server.h

#ifndef __MODBUS_GATEWAY_TCP_SERVER__
#define __MODBUS_GATEWAY_TCP_SERVER__

#include "nanomodbus/nanomodbus.h"

extern nmbs_t nmbs_tls;

nmbs_error handle_altcp_read_coils(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg);
nmbs_error handle_altcp_read_discrete_inputs(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg);
nmbs_error handle_altcp_write_single_coil(uint16_t address, bool value, uint8_t unit_id, void* arg);
nmbs_error handle_altcp_write_multiple_coils(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id, void* arg);
nmbs_error handler_tcp_read_holding_registers(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, void* arg);
nmbs_error handle_altcp_write_single_register(uint16_t address, uint16_t value, uint8_t unit_id, void* arg);
nmbs_error handle_altcp_write_multiple_registers(uint16_t address, uint16_t quantity, const uint16_t* registers,
                                           uint8_t unit_id, void* arg);
nmbs_error handle_altcp_read_file_record(uint16_t file_number, uint16_t record_number, uint16_t* registers, uint16_t count,
                                   uint8_t unit_id, void* arg);
nmbs_error handle_altcp_write_file_record(uint16_t file_number, uint16_t record_number, const uint16_t* registers,
                                    uint16_t count, uint8_t unit_id, void* arg);                                  
int32_t nmbs_transport_read(uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, void* arg);
int32_t nmbs_transport_write(const uint8_t* buf, uint16_t count, int32_t byte_timeout_ms,void* arg);


#endif 

client:

• CMakeLists.txt

# Generated Cmake Pico project file

cmake_minimum_required(VERSION 3.13)

set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

# Initialise pico_sdk from installed location
# (note this can come from environment, CMake cache etc)

# == DO NOT EDIT THE FOLLOWING LINES for the Raspberry Pi Pico VS Code Extension to work ==
if(WIN32)
    set(USERHOME $ENV{USERPROFILE})
else()
    set(USERHOME $ENV{HOME})
endif()
set(sdkVersion 2.1.0)
set(toolchainVersion 13_3_Rel1)
set(picotoolVersion 2.1.0)
set(picoVscode ${USERHOME}/.pico-sdk/cmake/pico-vscode.cmake)
if (EXISTS ${picoVscode})
    include(${picoVscode})
endif()
# ====================================================================================
set(PICO_BOARD pico_w CACHE STRING "Board type")

# Pull in Raspberry Pi Pico SDK (must be before project)
include(pico_sdk_import.cmake)

project(picow_modbus_tcp_security_client C CXX ASM)

# Initialise the Raspberry Pi Pico SDK
pico_sdk_init()

# Add executable. Default name is the project name, version 0.1

add_executable(picow_modbus_tcp_security_client 
            picow_modbus_tcp_security_client.c 
            picow_altcp_client/picow_altcp_client.c
            nanomodbus/nanomodbus.c)

pico_set_program_name(picow_modbus_tcp_security_client "picow_modbus_tcp_security_client")
pico_set_program_version(picow_modbus_tcp_security_client "0.1")

# Modify the below lines to enable/disable output over UART/USB
pico_enable_stdio_uart(picow_modbus_tcp_security_client 1)
pico_enable_stdio_usb(picow_modbus_tcp_security_client 0)

# Add the standard library to the build
target_link_libraries(picow_modbus_tcp_security_client
        pico_stdlib)

# Add the standard include files to the build
target_include_directories(picow_modbus_tcp_security_client PRIVATE
        ${CMAKE_CURRENT_LIST_DIR}
)

# Add any user requested libraries
target_link_libraries(picow_modbus_tcp_security_client 
        pico_cyw43_arch_lwip_threadsafe_background
        pico_lwip_mbedtls
        pico_mbedtls
        pico_lwip_mdns
        )

pico_add_extra_outputs(picow_modbus_tcp_security_client)

# Ignore warnings from lwip code
set_source_files_properties(
        ${PICO_LWIP_PATH}/src/apps/altcp_tls/altcp_tls_mbedtls.c
        PROPERTIES
        COMPILE_OPTIONS "-Wno-unused-result"
        )

• picow_modbus_tcp_security_client.c

#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "lwip/altcp.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"
#include "lwip/pbuf.h"

#include "picow_altcp_client/picow_altcp_client.h"
#include "nanomodbus/nanomodbus.h"
#include "modbus_server_cert.h"

#define MODBUS_SERVER_HOST_NAME "modbus_server.local"
#define GATE_BUTTON_PIN 16  
#define LIGHT_BUTTON_PIN 17

nmbs_t nmbs;
ALTCP_CLIENT_T *modbus_client_addr1;

int32_t nmbs_transport_read(uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
    ALTCP_CLIENT_T *state=(ALTCP_CLIENT_T*) arg;
 
    uint16_t loop_count = 0;

    if (state->queue_left == 0) {
        while (queue_is_empty(&state->recv_queue) && loop_count < 20) {           
            loop_count++;
            busy_wait_ms(50);
        }
        if (!queue_try_remove(&(state->recv_queue), &state->temp_recv))  {
                return 0;
        } else {
            DEBUG_print("\t\t\t\t---loop count:%d\n", loop_count);
        }
        state->queue_left = state->temp_recv.buffer_len;
    }
   
    state->recv_count = state->queue_left <= count ? state->queue_left : count;
    state->start_index = state->temp_recv.buffer_len-state->queue_left;
    for (int i=0; i < state->recv_count; i++) buf[i] = state->temp_recv.buffer[state->start_index+i];

    state->queue_left = state->queue_left-state->recv_count;

    //Debug
    DEBUG_print("\n=====transport read:%d:%d\n", count, state->temp_recv.buffer_len);
    for (int i=0; i < state->recv_count; i++) {
        DEBUG_print("%02x ", buf[i]);
    }
    DEBUG_print("\n==\n");
 
    return state->recv_count;
}

int32_t nmbs_transport_write(const uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
    ALTCP_CLIENT_T *state = (ALTCP_CLIENT_T*)arg;

    // Debug
    DEBUG_print("====\nnmbs_transport_write count:%d\n", count);
    for (int i=0; i < count;i++) {
        DEBUG_print("%02x ", buf[i]);
    }
    DEBUG_print("\n====\n");

    err_t err = altcp_write(state->tcp_pcb, buf, count, TCP_WRITE_FLAG_COPY);
    if (err != ERR_OK) 
    {
        //tcp_abort(state->tcp_pcb);
        DEBUG_print("tcp_write error:%d:%d\n", err, altcp_sndbuf(state->tcp_pcb)); 
        return 0;
    }
    err = altcp_output(state->tcp_pcb);
    if (err != ERR_OK) 
    {
        DEBUG_print("%s:tcp_output:%d\n", state->server_host_name, err); 
        return 0;
    }
    return count;
}

void gpio_button_irq_cb(uint gpio, uint32_t event_mask){
    static bool addr0_value=false, addr1_value=false;
    if ((gpio == GATE_BUTTON_PIN || gpio == LIGHT_BUTTON_PIN)&&event_mask == GPIO_IRQ_EDGE_FALL)
    {
        gpio_acknowledge_irq(gpio, GPIO_IRQ_EDGE_FALL);
        if (modbus_client_addr1->conn_state == TCP_CONNECTED) {
            if (gpio == GATE_BUTTON_PIN) {
                addr0_value = !addr0_value;
                nmbs_write_single_coil(&nmbs,0, addr0_value);
                
            }
            if (gpio == LIGHT_BUTTON_PIN) {
                addr1_value = !addr1_value;
                nmbs_write_single_coil(&nmbs,1, addr1_value);
                
            }
        }
        
    }

}
int main()
{
    stdio_init_all();
    gpio_init(GATE_BUTTON_PIN);
    gpio_init(LIGHT_BUTTON_PIN);

    gpio_set_irq_enabled_with_callback(GATE_BUTTON_PIN, GPIO_IRQ_EDGE_FALL, true, gpio_button_irq_cb);
    gpio_set_irq_enabled_with_callback(LIGHT_BUTTON_PIN, GPIO_IRQ_EDGE_FALL, true, gpio_button_irq_cb);
    // Initialise the Wi-Fi chip
    if (cyw43_arch_init()) {
        DEBUG_print("Wi-Fi init failed\n");
        return -1;
    }

    if (!picow_sta_connect(WIFI_SSID, WIFI_PASSWD)) {
        return 0;
    }
    if (!picow_altcp_client_init(&modbus_client_addr1, MODBUS_SERVER_HOST_NAME, MODBUS_ALTCP_PORT, MODBUS_SERVER_CERT)) return 0; 
      

    nmbs_platform_conf platform_conf;
    nmbs_platform_conf_create(&platform_conf);
    platform_conf.transport = NMBS_TRANSPORT_TCP;
    platform_conf.read = nmbs_transport_read;
    platform_conf.write = nmbs_transport_write;
    
    
    // Create the modbus client
    nmbs_error err = nmbs_client_create(&nmbs, &platform_conf);
    if (err != NMBS_ERROR_NONE) {
        fprintf(stderr, "Error creating modbus client\n");
        if (!nmbs_error_is_exception(err))
            return 1;
    }

    // Set only the response timeout. Byte timeout will be handled by the TCP connection
    nmbs_set_read_timeout(&nmbs, 1000);
    nmbs_set_platform_arg(&nmbs, modbus_client_addr1);
    bool value = true;
    uint count=0;
    while (true) {
    
        tight_loop_contents();
    }
}

• picow_altcp_client.c

#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/cyw43_arch.h"
#include "lwip/altcp.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"
#include "lwip/pbuf.h"

#include "picow_altcp_client/picow_altcp_client.h"
#include "nanomodbus/nanomodbus.h"
#include "modbus_server_cert.h"

#define MODBUS_SERVER_HOST_NAME "modbus_server.local"
#define GATE_BUTTON_PIN 16  
#define LIGHT_BUTTON_PIN 17

nmbs_t nmbs;
ALTCP_CLIENT_T *modbus_client_addr1;

int32_t nmbs_transport_read(uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
    ALTCP_CLIENT_T *state=(ALTCP_CLIENT_T*) arg;
 
    uint16_t loop_count = 0;

    if (state->queue_left == 0) {
        while (queue_is_empty(&state->recv_queue) && loop_count < 20) {           
            loop_count++;
            busy_wait_ms(50);
        }
        if (!queue_try_remove(&(state->recv_queue), &state->temp_recv))  {
                return 0;
        } else {
            DEBUG_print("\t\t\t\t---loop count:%d\n", loop_count);
        }
        state->queue_left = state->temp_recv.buffer_len;
    }
   
    state->recv_count = state->queue_left <= count ? state->queue_left : count;
    state->start_index = state->temp_recv.buffer_len-state->queue_left;
    for (int i=0; i < state->recv_count; i++) buf[i] = state->temp_recv.buffer[state->start_index+i];

    state->queue_left = state->queue_left-state->recv_count;

    //Debug
    DEBUG_print("\n=====transport read:%d:%d\n", count, state->temp_recv.buffer_len);
    for (int i=0; i < state->recv_count; i++) {
        DEBUG_print("%02x ", buf[i]);
    }
    DEBUG_print("\n==\n");
 
    return state->recv_count;
}

int32_t nmbs_transport_write(const uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
    ALTCP_CLIENT_T *state = (ALTCP_CLIENT_T*)arg;

    // Debug
    DEBUG_print("====\nnmbs_transport_write count:%d\n", count);
    for (int i=0; i < count;i++) {
        DEBUG_print("%02x ", buf[i]);
    }
    DEBUG_print("\n====\n");

    err_t err = altcp_write(state->tcp_pcb, buf, count, TCP_WRITE_FLAG_COPY);
    if (err != ERR_OK) 
    {
        //tcp_abort(state->tcp_pcb);
        DEBUG_print("tcp_write error:%d:%d\n", err, altcp_sndbuf(state->tcp_pcb)); 
        return 0;
    }
    err = altcp_output(state->tcp_pcb);
    if (err != ERR_OK) 
    {
        DEBUG_print("%s:tcp_output:%d\n", state->server_host_name, err); 
        return 0;
    }
    return count;
}

void gpio_button_irq_cb(uint gpio, uint32_t event_mask){
    static bool addr0_value=false, addr1_value=false;
    if ((gpio == GATE_BUTTON_PIN || gpio == LIGHT_BUTTON_PIN)&&event_mask == GPIO_IRQ_EDGE_FALL)
    {
        gpio_acknowledge_irq(gpio, GPIO_IRQ_EDGE_FALL);
        if (modbus_client_addr1->conn_state == TCP_CONNECTED) {
            if (gpio == GATE_BUTTON_PIN) {
                addr0_value = !addr0_value;
                nmbs_write_single_coil(&nmbs,0, addr0_value);
                
            }
            if (gpio == LIGHT_BUTTON_PIN) {
                addr1_value = !addr1_value;
                nmbs_write_single_coil(&nmbs,1, addr1_value);
                
            }
        }
        
    }

}
int main()
{
    stdio_init_all();
    gpio_init(GATE_BUTTON_PIN);
    gpio_init(LIGHT_BUTTON_PIN);

    gpio_set_irq_enabled_with_callback(GATE_BUTTON_PIN, GPIO_IRQ_EDGE_FALL, true, gpio_button_irq_cb);
    gpio_set_irq_enabled_with_callback(LIGHT_BUTTON_PIN, GPIO_IRQ_EDGE_FALL, true, gpio_button_irq_cb);
    // Initialise the Wi-Fi chip
    if (cyw43_arch_init()) {
        DEBUG_print("Wi-Fi init failed\n");
        return -1;
    }

    if (!picow_sta_connect(WIFI_SSID, WIFI_PASSWD)) {
        return 0;
    }
    if (!picow_altcp_client_init(&modbus_client_addr1, MODBUS_SERVER_HOST_NAME, MODBUS_ALTCP_PORT, MODBUS_SERVER_CERT)) return 0; 
      

    nmbs_platform_conf platform_conf;
    nmbs_platform_conf_create(&platform_conf);
    platform_conf.transport = NMBS_TRANSPORT_TCP;
    platform_conf.read = nmbs_transport_read;
    platform_conf.write = nmbs_transport_write;
    
    
    // Create the modbus client
    nmbs_error err = nmbs_client_create(&nmbs, &platform_conf);
    if (err != NMBS_ERROR_NONE) {
        fprintf(stderr, "Error creating modbus client\n");
        if (!nmbs_error_is_exception(err))
            return 1;
    }

    // Set only the response timeout. Byte timeout will be handled by the TCP connection
    nmbs_set_read_timeout(&nmbs, 1000);
    nmbs_set_platform_arg(&nmbs, modbus_client_addr1);
    bool value = true;
    uint count=0;
    while (true) {
    
        tight_loop_contents();
    }
}

• picow_altcp_client.h

#ifndef __PICOW_ALTCP_CLIENT__
#define __PCIOW_ALTCP_CLIENT__

#include "pico/util/queue.h"

#include "lwip/altcp.h"
#include "lwip/altcp_tcp.h"
#include "lwip/altcp_tls.h"


#define WIFI_SSID "your-SSID"
#define WIFI_PASSWD "your-PASSWD"

#define MODBUS_TCP_PORT 502
#define MODBUS_ALTCP_PORT 802
#define POLL_TIME_S 1

#define __DEBUG__
#ifdef __DEBUG__
#define DEBUG_print(...) printf(__VA_ARGS__)
#else
#define DEBUG_print(...) (void) (0)
#endif

enum _TCP_STATE{
    TCP_CONNECTING  = 1,
    TCP_CONNECTED   = 2,
    TCP_CLOSING     = 3,
    TCP_CLOSED      = 4,
};

typedef struct __recv_queue_t{
    uint8_t buffer[260];
    uint16_t buffer_len;
} recv_queue_t;

typedef struct ALTCP_CLIENT_T_ {
    struct altcp_pcb *tcp_pcb;
    struct altcp_tls_config *tls_config;
    ip_addr_t remote_addr;
    uint      remote_port;
    uint8_t conn_state;
    bool dns_found;
    queue_t recv_queue;
    uint8_t server_host_name[256];
    uint16_t recv_count;
    uint16_t start_index;
    uint16_t queue_left;
    recv_queue_t temp_recv;
} ALTCP_CLIENT_T;

bool picow_altcp_client_init(ALTCP_CLIENT_T* *modbus_altcp_client, const char* hostname, uint port, const u8_t *cert);
err_t picow_altcp_client_close(void *arg);
err_t picow_altcp_client_conn(void *arg);
err_t picow_altcp_client_disconn(void *arg);
bool picow_altcp_client_open(ALTCP_CLIENT_T* state, const u8_t *cert);
bool picow_sta_connect(uint8_t* ssid, uint8_t* pwd);

#endif

• nanoMODBUS library:

https://github.com/debevv/nanoMODBUS