微控制器應用實驗室: [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可使用下列步驟建立：

openssl genrsa -des3 -out ca.key 2048:
建立Root CA key。
openssl req -new -x509 -days 3650 -key ca.key -out ca.cert:
建立self-signed root Certificate。
openssl genrsa -out server.key 2048:
建立server side private key。
openssl req -new -out server.csr -key server.key
使用server key製作server憑證需求(certificate request)
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

微控制器應用實驗室

prettyprint

2025年3月19日星期三

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

沒有留言:

張貼留言

	# 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"
	)

	#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
	}
	}


	#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);

	#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;
	}

	#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

	#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

prettyprint

2025年3月19日 星期三

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

沒有留言:

張貼留言

2025年3月19日星期三