本帖最后由 龙鳞铁碎牙 于 2025-5-19 18:25 编辑
很高兴收到了21世纪发放的STM32C092开发板，想着借此机会来移植使用一下Modbus从机，测试一下modbus的性能和各种功能！
由于帖子上面发图有很大限制，modbus原理我就不讲解了，大家自己去下载modbus标准去理解！
我这里只讲解移植使用过程。。
1. 打开ST的cubumx。
使能两个串口uart2和uart3
时钟选择48MHZ，两个串口都是232,119200波特率
2.下面是我实现的modbus源代码
/*
nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers
MIT License
Copyright (c) 2024 Valerio De Benedetto (@debevv)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
/** @file */
/*! \mainpage nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers
* nanoMODBUS is a small C library that implements the Modbus protocol. It is especially useful in resource-constrained
* system like microcontrollers.
*
* GtiHub: <a >https://github.com/debevv/nanoMODBUS</a>
*
* API reference: \link nanomodbus.h \endlink
*
*/
#ifndef NANOMODBUS_H
#define NANOMODBUS_H
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#ifdef __cplusplus
extern "C" {
#endif
/**
* nanoMODBUS errors.
* Values <= 0 are library errors, > 0 are modbus exceptions.
*/
typedef enum nmbs_error {
// Library errors
NMBS_ERROR_INVALID_REQUEST = -8, /**< Received invalid request from client */
NMBS_ERROR_INVALID_UNIT_ID = -7, /**< Received invalid unit ID in response from server */
NMBS_ERROR_INVALID_TCP_MBAP = -6, /**< Received invalid TCP MBAP */
NMBS_ERROR_CRC = -5, /**< Received invalid CRC */
NMBS_ERROR_TRANSPORT = -4, /**< Transport error */
NMBS_ERROR_TIMEOUT = -3, /**< Read/write timeout occurred */
NMBS_ERROR_INVALID_RESPONSE = -2, /**< Received invalid response from server */
NMBS_ERROR_INVALID_ARGUMENT = -1, /**< Invalid argument provided */
NMBS_ERROR_NONE = 0, /**< No error */
// Modbus exceptions
NMBS_EXCEPTION_ILLEGAL_FUNCTION = 1, /**< Modbus exception 1 */
NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2, /**< Modbus exception 2 */
NMBS_EXCEPTION_ILLEGAL_DATA_VALUE = 3, /**< Modbus exception 3 */
NMBS_EXCEPTION_SERVER_DEVICE_FAILURE = 4, /**< Modbus exception 4 */
} nmbs_error;
/**
* Return whether the nmbs_error is a modbus exception
* @e nmbs_error to check
*/
#define nmbs_error_is_exception(e) ((e) > 0 && (e) < 5)
/**
* Bitfield consisting of 2000 coils/discrete inputs
*/
typedef uint8_t nmbs_bitfield[250];
/**
* Bitfield consisting of 256 values
*/
typedef uint8_t nmbs_bitfield_256[32];
/**
* Read a bit from the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_read(bf, b) ((bool) ((bf)[(b) / 8] & (0x1 << ((b) % 8))))
/**
* Set a bit of the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_set(bf, b) (((bf)[(b) / 8]) = (((bf)[(b) / 8]) | (0x1 << ((b) % 8))))
/**
* Reset a bit of the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_unset(bf, b) (((bf)[(b) / 8]) = (((bf)[(b) / 8]) & ~(0x1 << ((b) % 8))))
/**
* Write value v to the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_write(bf, b, v) \
(((bf)[(b) / 8]) = ((v) ? (((bf)[(b) / 8]) | (0x1 << ((b) % 8))) : (((bf)[(b) / 8]) & ~(0x1 << ((b) % 8)))))
/**
* Reset (zero) the whole bitfield
*/
#define nmbs_bitfield_reset(bf) memset(bf, 0, sizeof(bf))
/**
* Modbus transport type.
*/
typedef enum nmbs_transport {
NMBS_TRANSPORT_RTU = 1,
NMBS_TRANSPORT_TCP = 2,
} nmbs_transport;
/**
* nanoMODBUS platform configuration struct.
* Passed to nmbs_server_create() and nmbs_client_create().
*
* read() and write() are the platform-specific methods that read/write data to/from a serial port or a TCP connection.
*
* Both methods should block until either:
* - `count` bytes of data are read/written
* - the byte timeout, with `byte_timeout_ms >= 0`, expires
*
* A value `< 0` for `byte_timeout_ms` means infinite timeout.
* With a value `== 0` for `byte_timeout_ms`, the method should read/write once in a non-blocking fashion and return immediately.
*
*
* Their return value should be the number of bytes actually read/written, or `< 0` in case of error.
* A return value between `0` and `count - 1` will be treated as if a timeout occurred on the transport side. All other
* values will be treated as transport errors.
*
* Additionally, an optional crc_calc() function can be defined to override the default nanoMODBUS CRC calculation function.
*
* These methods accept a pointer to arbitrary user-data, which is the arg member of this struct.
* After the creation of an instance it can be changed with nmbs_set_platform_arg().
*/
typedef struct nmbs_platform_conf {
nmbs_transport transport; /*!< Transport type */
int32_t (*read)(uint8_t* buf, uint16_t count, int32_t byte_timeout_ms,
void* arg); /*!< Bytes read transport function pointer */
int32_t (*write)(const uint8_t* buf, uint16_t count, int32_t byte_timeout_ms,
void* arg); /*!< Bytes write transport function pointer */
uint16_t (*crc_calc)(const uint8_t* data, uint32_t length,
void* arg); /*!< CRC calculation function pointer. Optional */
void* arg; /*!< User data, will be passed to functions above */
uint32_t initialized; /*!< Reserved, workaround for older user code not calling nmbs_platform_conf_create() */
} nmbs_platform_conf;
/**
* Modbus server request callbacks. Passed to nmbs_server_create().
*
* These methods accept a pointer to arbitrary user data, which is the arg member of the nmbs_platform_conf that was passed
* to nmbs_server_create together with this struct.
*
* `unit_id` is the RTU unit ID of the request sender. It is always 0 on TCP.
*/
typedef struct nmbs_callbacks {
nmbs_error (*read_coils)(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg);
nmbs_error (*read_discrete_inputs)(uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out, uint8_t unit_id,
void* arg);
nmbs_error (*read_holding_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id,
void* arg);
nmbs_error (*read_input_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id,
void* arg);
nmbs_error (*write_single_coil)(uint16_t address, bool value, uint8_t unit_id, void* arg);
nmbs_error (*write_single_register)(uint16_t address, uint16_t value, uint8_t unit_id, void* arg);
nmbs_error (*write_multiple_coils)(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id,
void* arg);
nmbs_error (*write_multiple_registers)(uint16_t address, uint16_t quantity, const uint16_t* registers,
uint8_t unit_id, void* arg);
nmbs_error (*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 (*write_file_record)(uint16_t file_number, uint16_t record_number, const uint16_t* registers,
uint16_t count, uint8_t unit_id, void* arg);
#define NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH 128
nmbs_error (*read_device_identification)(uint8_t object_id, char buffer[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH]);
nmbs_error (*read_device_identification_map)(nmbs_bitfield_256 map);
void* arg; // User data, will be passed to functions above
uint32_t initialized; // Reserved, workaround for older user code not calling nmbs_callbacks_create()
} nmbs_callbacks;
/**
* nanoMODBUS client/server instance type. All struct members are to be considered private,
* it is not advisable to read/write them directly.
*/
typedef struct nmbs_t {
struct {
uint8_t buf[260];
uint16_t buf_idx;
uint8_t unit_id;
uint8_t fc;
uint16_t transaction_id;
bool broadcast;
bool ignored;
bool complete;
} msg;
nmbs_callbacks callbacks;
int32_t byte_timeout_ms;
int32_t read_timeout_ms;
nmbs_platform_conf platform;
uint8_t address_rtu;
uint8_t dest_address_rtu;
uint16_t current_tid;
} nmbs_t;
/**
* Modbus broadcast address. Can be passed to nmbs_set_destination_rtu_address().
*/
static const uint8_t NMBS_BROADCAST_ADDRESS = 0;
/** Set the request/response timeout.
* If the target instance is a server, sets the timeout of the nmbs_server_poll() function.
* If the target instance is a client, sets the response timeout after sending a request. In case of timeout,
* the called method will return NMBS_ERROR_TIMEOUT.
* @param nmbs pointer to the nmbs_t instance
* @param timeout_ms timeout in milliseconds. If < 0, the timeout is disabled.
*/
void nmbs_set_read_timeout(nmbs_t* nmbs, int32_t timeout_ms);
/** Set the timeout between the reception/transmission of two consecutive bytes.
* @param nmbs pointer to the nmbs_t instance
* @param timeout_ms timeout in milliseconds. If < 0, the timeout is disabled.
*/
void nmbs_set_byte_timeout(nmbs_t* nmbs, int32_t timeout_ms);
/** Create a new nmbs_platform_conf struct.
* @param platform_conf pointer to the nmbs_platform_conf instance
*/
void nmbs_platform_conf_create(nmbs_platform_conf* platform_conf);
/** Set the pointer to user data argument passed to platform functions.
* @param nmbs pointer to the nmbs_t instance
* @param arg user data argument
*/
void nmbs_set_platform_arg(nmbs_t* nmbs, void* arg);
/** Create a new nmbs_callbacks struct.
* @param callbacks pointer to the nmbs_callbacks instance
*/
void nmbs_callbacks_create(nmbs_callbacks* callbacks);
/** Create a new Modbus server.
* @param nmbs pointer to the nmbs_t instance where the client will be created.
* @param address_rtu RTU address of this server. Can be 0 if transport is not RTU.
* @param platform_conf nmbs_platform_conf struct with platform configuration. It may be discarded after calling this method.
* @param callbacks nmbs_callbacks struct with server request callbacks. It may be discarded after calling this method.
*
* @return NMBS_ERROR_NONE if successful, NMBS_ERROR_INVALID_ARGUMENT otherwise.
*/
nmbs_error nmbs_server_create(nmbs_t* nmbs, uint8_t address_rtu, const nmbs_platform_conf* platform_conf,
const nmbs_callbacks* callbacks);
/** Handle incoming requests to the server.
* This function should be called in a loop in order to serve any incoming request. Its maximum duration, in case of no
* received request, is the value set with nmbs_set_read_timeout() (unless set to < 0).
* @param nmbs pointer to the nmbs_t instance
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_server_poll(nmbs_t* nmbs);
/** Set the pointer to user data argument passed to server request callbacks.
* @param nmbs pointer to the nmbs_t instance
* @param arg user data argument
*/
void nmbs_set_callbacks_arg(nmbs_t* nmbs, void* arg);
/** Create a new Modbus client.
* @param nmbs pointer to the nmbs_t instance where the client will be created.
* @param platform_conf nmbs_platform_conf struct with platform configuration. It may be discarded after calling this method.
*
* @return NMBS_ERROR_NONE if successful, NMBS_ERROR_INVALID_ARGUMENT otherwise.
*/
nmbs_error nmbs_client_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf);
/** Set the recipient server address of the next request on RTU transport.
* @param nmbs pointer to the nmbs_t instance
* @param address server address
*/
void nmbs_set_destination_rtu_address(nmbs_t* nmbs, uint8_t address);
/** Send a FC 01 (0x01) Read Coils request
* @param nmbs pointer to the nmbs_t instance
* @param address starting address
* @param quantity quantity of coils
* @param coils_out nmbs_bitfield where the coils will be stored
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield coils_out);
/** Send a FC 02 (0x02) Read Discrete Inputs request
* @param nmbs pointer to the nmbs_t instance
* @param address starting address
* @param quantity quantity of inputs
* @param inputs_out nmbs_bitfield where the discrete inputs will be stored
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out);
/** Send a FC 03 (0x03) Read Holding Registers request
* @param nmbs pointer to the nmbs_t instance
* @param address starting address
* @param quantity quantity of registers
* @param registers_out array where the registers will be stored
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_holding_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out);
/** Send a FC 04 (0x04) Read Input Registers request
* @param nmbs pointer to the nmbs_t instance
* @param address starting address
* @param quantity quantity of registers
* @param registers_out array where the registers will be stored
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_input_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out);
/** Send a FC 05 (0x05) Write Single Coil request
* @param nmbs pointer to the nmbs_t instance
* @param address coil address
* @param value coil value
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, bool value);
/** Send a FC 06 (0x06) Write Single Register request
* @param nmbs pointer to the nmbs_t instance
* @param address register address
* @param value register value
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t value);
/** Send a FC 15 (0x0F) Write Multiple Coils
* @param nmbs pointer to the nmbs_t instance
* @param address starting address
* @param quantity quantity of coils
* @param coils bitfield of coils values
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const nmbs_bitfield coils);
/** Send a FC 16 (0x10) Write Multiple Registers
* @param nmbs pointer to the nmbs_t instance
* @param address starting address
* @param quantity quantity of registers
* @param registers array of registers values
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const uint16_t* registers);
/** Send a FC 20 (0x14) Read File Record
* @param nmbs pointer to the nmbs_t instance
* @param file_number file number (1 to 65535)
* @param record_number record number from file (0000 to 9999)
* @param registers array of registers to read
* @param count count of registers
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, uint16_t* registers,
uint16_t count);
/** Send a FC 21 (0x15) Write File Record
* @param nmbs pointer to the nmbs_t instance
* @param file_number file number (1 to 65535)
* @param record_number record number from file (0000 to 9999)
* @param registers array of registers to write
* @param count count of registers
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, const uint16_t* registers,
uint16_t count);
/** Send a FC 23 (0x17) Read Write Multiple registers
* @param nmbs pointer to the nmbs_t instance
* @param read_address starting read address
* @param read_quantity quantity of registers to read
* @param registers_out array where the read registers will be stored
* @param write_address starting write address
* @param write_quantity quantity of registers to write
* @param registers array of registers values to write
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16_t read_quantity,
uint16_t* registers_out, uint16_t write_address, uint16_t write_quantity,
const uint16_t* registers);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Basic Object Id values (Read Device ID code 1)
* @param nmbs pointer to the nmbs_t instance
* @param vendor_name char array where the read VendorName value will be stored
* @param product_code char array where the read ProductCode value will be stored
* @param major_minor_revision char array where the read MajorMinorRevision value will be stored
* @param buffers_length length of every char array
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_device_identification_basic(nmbs_t* nmbs, char* vendor_name, char* product_code,
char* major_minor_revision, uint8_t buffers_length);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Regular Object Id values (Read Device ID code 2)
* @param nmbs pointer to the nmbs_t instance
* @param vendor_url char array where the read VendorUrl value will be stored
* @param product_name char array where the read ProductName value will be stored
* @param model_name char array where the read ModelName value will be stored
* @param user_application_name char array where the read UserApplicationName value will be stored
* @param buffers_length length of every char array
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_device_identification_regular(nmbs_t* nmbs, char* vendor_url, char* product_name, char* model_name,
char* user_application_name, uint8_t buffers_length);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Extended Object Id values (Read Device ID code 3)
* @param nmbs pointer to the nmbs_t instance
* @param object_id_start Object Id to start reading from
* @param ids array where the read Object Ids will be stored
* @param buffers array of char arrays where the read values will be stored
* @param ids_length length of the ids array and buffers array
* @param buffer_length length of each char array
* @param objects_count_out retrieved Object Ids count
*
* @return NMBS_ERROR_NONE if successful, NMBS_INVALID_ARGUMENT if buffers_count is less than retrieved Object Ids count,
* other errors otherwise.
*/
nmbs_error nmbs_read_device_identification_extended(nmbs_t* nmbs, uint8_t object_id_start, uint8_t* ids, char** buffers,
uint8_t ids_length, uint8_t buffer_length,
uint8_t* objects_count_out);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to retrieve a single Object Id value (Read Device ID code 4)
* @param nmbs pointer to the nmbs_t instance
* @param object_id requested Object Id
* @param buffer char array where the resulting value will be stored
* @param buffer_length length of the char array
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, uint8_t object_id, char* buffer, uint8_t buffer_length);
/** Send a raw Modbus PDU.
* CRC on RTU will be calculated and sent by this function.
* @param nmbs pointer to the nmbs_t instance
* @param fc request function code
* @param data request data. It's up to the caller to convert this data to network byte order
* @param data_len length of the data parameter
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint16_t data_len);
/** Receive a raw response Modbus PDU.
* @param nmbs pointer to the nmbs_t instance
* @param data_out response data. It's up to the caller to convert this data to host byte order. Can be NULL.
* @param data_out_len number of bytes to receive
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint8_t data_out_len);
/** Calculate the Modbus CRC of some data.
* @param data Data
* @param length Length of the data
*/
uint16_t nmbs_crc_calc(const uint8_t* data, uint32_t length, void* arg);
/** Convert a nmbs_error to string
* @param error error to be converted
*
* @return string representation of the error
*/
const char* nmbs_strerror(nmbs_error error);
#ifdef __cplusplus
} // extern "C"
#endif
#endif //NANOMODBUS_H
/*
nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers
MIT License
Copyright (c) 2024 Valerio De Benedetto (@debevv)
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#include "nanomodbus.h"
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#define NMBS_UNUSED_PARAM(x) ((x) = (x))
#include <stdio.h>
#define NMBS_DEBUG_PRINT(...) printf(__VA_ARGS__)
static uint8_t get_1(nmbs_t* nmbs) {
uint8_t result = nmbs->msg.buf[nmbs->msg.buf_idx];
nmbs->msg.buf_idx++;
return result;
}
static void put_1(nmbs_t* nmbs, uint8_t data) {
nmbs->msg.buf[nmbs->msg.buf_idx] = data;
nmbs->msg.buf_idx++;
}
static void discard_1(nmbs_t* nmbs) {
nmbs->msg.buf_idx++;
}
static void discard_n(nmbs_t* nmbs, uint16_t n) {
nmbs->msg.buf_idx += n;
}
static uint16_t get_2(nmbs_t* nmbs) {
const uint16_t result =
((uint16_t) nmbs->msg.buf[nmbs->msg.buf_idx]) << 8 | (uint16_t) nmbs->msg.buf[nmbs->msg.buf_idx + 1];
nmbs->msg.buf_idx += 2;
return result;
}
static void put_2(nmbs_t* nmbs, uint16_t data) {
nmbs->msg.buf[nmbs->msg.buf_idx] = (uint8_t) ((data >> 8) & 0xFFU);
nmbs->msg.buf[nmbs->msg.buf_idx + 1] = (uint8_t) data;
nmbs->msg.buf_idx += 2;
}
static void set_1(nmbs_t* nmbs, uint8_t data, uint8_t index) {
nmbs->msg.buf[index] = data;
}
static void set_2(nmbs_t* nmbs, uint16_t data, uint8_t index) {
nmbs->msg.buf[index] = (uint8_t) ((data >> 8) & 0xFFU);
nmbs->msg.buf[index + 1] = (uint8_t) data;
}
static uint8_t* get_n(nmbs_t* nmbs, uint16_t n) {
uint8_t* msg_buf_ptr = nmbs->msg.buf + nmbs->msg.buf_idx;
nmbs->msg.buf_idx += n;
return msg_buf_ptr;
}
static void put_n(nmbs_t* nmbs, const uint8_t* data, uint8_t size) {
memcpy(&nmbs->msg.buf[nmbs->msg.buf_idx], data, size);
nmbs->msg.buf_idx += size;
}
static uint16_t* get_regs(nmbs_t* nmbs, uint16_t n) {
uint16_t* msg_buf_ptr = (uint16_t*) (nmbs->msg.buf + nmbs->msg.buf_idx);
nmbs->msg.buf_idx += n * 2;
while (n--) {
msg_buf_ptr[n] = (msg_buf_ptr[n] << 8) | ((msg_buf_ptr[n] >> 8) & 0xFF);
}
return msg_buf_ptr;
}
static void put_regs(nmbs_t* nmbs, const uint16_t* data, uint16_t n) {
uint16_t* msg_buf_ptr = (uint16_t*) (nmbs->msg.buf + nmbs->msg.buf_idx);
nmbs->msg.buf_idx += n * 2;
while (n--) {
msg_buf_ptr[n] = (data[n] << 8) | ((data[n] >> 8) & 0xFF);
}
}
static void swap_regs(uint16_t* data, uint16_t n) {
while (n--) {
data[n] = (data[n] << 8) | ((data[n] >> 8) & 0xFF);
}
}
static nmbs_error recv(nmbs_t* nmbs, uint16_t count) {
if (nmbs->msg.complete) {
return NMBS_ERROR_NONE;
}
int32_t ret =
nmbs->platform.read(nmbs->msg.buf + nmbs->msg.buf_idx, count, nmbs->byte_timeout_ms, nmbs->platform.arg);
if (ret == count)
return NMBS_ERROR_NONE;
if (ret < count) {
if (ret < 0)
return NMBS_ERROR_TRANSPORT;
return NMBS_ERROR_TIMEOUT;
}
return NMBS_ERROR_TRANSPORT;
}
static nmbs_error send(const nmbs_t* nmbs, uint16_t count) {
int32_t ret = nmbs->platform.write(nmbs->msg.buf, count, nmbs->byte_timeout_ms, nmbs->platform.arg);
if (ret == count)
return NMBS_ERROR_NONE;
if (ret < count) {
if (ret < 0)
return NMBS_ERROR_TRANSPORT;
return NMBS_ERROR_TIMEOUT;
}
return NMBS_ERROR_TRANSPORT;
}
static void flush(nmbs_t* nmbs) {
nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg);
}
static void msg_buf_reset(nmbs_t* nmbs) {
nmbs->msg.buf_idx = 0;
}
static void msg_state_reset(nmbs_t* nmbs) {
msg_buf_reset(nmbs);
nmbs->msg.unit_id = 0;
nmbs->msg.fc = 0;
nmbs->msg.transaction_id = 0;
nmbs->msg.broadcast = false;
nmbs->msg.ignored = false;
nmbs->msg.complete = false;
}
static void msg_state_req(nmbs_t* nmbs, uint8_t fc) {
if (nmbs->current_tid == UINT16_MAX)
nmbs->current_tid = 1;
else
nmbs->current_tid++;
// Flush the remaining data on the line before sending the request
flush(nmbs);
msg_state_reset(nmbs);
nmbs->msg.unit_id = nmbs->dest_address_rtu;
nmbs->msg.fc = fc;
nmbs->msg.transaction_id = nmbs->current_tid;
if (nmbs->msg.unit_id == 0 && nmbs->platform.transport == NMBS_TRANSPORT_RTU)
nmbs->msg.broadcast = true;
}
nmbs_error nmbs_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf) {
if (!nmbs)
return NMBS_ERROR_INVALID_ARGUMENT;
memset(nmbs, 0, sizeof(nmbs_t));
nmbs->byte_timeout_ms = -1;
nmbs->read_timeout_ms = -1;
if (!platform_conf || platform_conf->initialized != 0xFFFFDEBE)
return NMBS_ERROR_INVALID_ARGUMENT;
if (platform_conf->transport != NMBS_TRANSPORT_RTU && platform_conf->transport != NMBS_TRANSPORT_TCP)
return NMBS_ERROR_INVALID_ARGUMENT;
if (!platform_conf->read || !platform_conf->write)
return NMBS_ERROR_INVALID_ARGUMENT;
nmbs->platform = *platform_conf;
return NMBS_ERROR_NONE;
}
void nmbs_set_read_timeout(nmbs_t* nmbs, int32_t timeout_ms) {
nmbs->read_timeout_ms = timeout_ms;
}
void nmbs_set_byte_timeout(nmbs_t* nmbs, int32_t timeout_ms) {
nmbs->byte_timeout_ms = timeout_ms;
}
void nmbs_platform_conf_create(nmbs_platform_conf* platform_conf) {
memset(platform_conf, 0, sizeof(nmbs_platform_conf));
platform_conf->crc_calc = nmbs_crc_calc;
// Workaround for older user code not calling nmbs_platform_conf_create()
platform_conf->initialized = 0xFFFFDEBE;
}
void nmbs_set_destination_rtu_address(nmbs_t* nmbs, uint8_t address) {
nmbs->dest_address_rtu = address;
}
void nmbs_set_platform_arg(nmbs_t* nmbs, void* arg) {
nmbs->platform.arg = arg;
}
uint16_t nmbs_crc_calc(const uint8_t* data, uint32_t length, void* arg) {
NMBS_UNUSED_PARAM(arg);
uint16_t crc = 0xFFFF;
for (uint32_t i = 0; i < length; i++) {
crc ^= (uint16_t) data;
for (int j = 8; j != 0; j--) {
if ((crc & 0x0001) != 0) {
crc >>= 1;
crc ^= 0xA001;
}
else
crc >>= 1;
}
}
return (uint16_t) (crc << 8) | (uint16_t) (crc >> 8);
}
static nmbs_error recv_msg_footer(nmbs_t* nmbs) {
NMBS_DEBUG_PRINT("\n");
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
uint16_t crc = nmbs->platform.crc_calc(nmbs->msg.buf, nmbs->msg.buf_idx, nmbs->platform.arg);
nmbs_error err = recv(nmbs, 2);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t recv_crc = get_2(nmbs);
if (recv_crc != crc)
return NMBS_ERROR_CRC;
}
return NMBS_ERROR_NONE;
}
static nmbs_error recv_msg_header(nmbs_t* nmbs, bool* first_byte_received) {
// We wait for the read timeout here, just for the first message byte
int32_t old_byte_timeout = nmbs->byte_timeout_ms;
nmbs->byte_timeout_ms = nmbs->read_timeout_ms;
msg_state_reset(nmbs);
*first_byte_received = false;
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
nmbs_error err = recv(nmbs, 1);
nmbs->byte_timeout_ms = old_byte_timeout;
if (err != NMBS_ERROR_NONE)
return err;
*first_byte_received = true;
nmbs->msg.unit_id = get_1(nmbs);
err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
nmbs->msg.fc = get_1(nmbs);
}
else if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) {
nmbs_error err = recv(nmbs, 1);
nmbs->byte_timeout_ms = old_byte_timeout;
if (err != NMBS_ERROR_NONE)
return err;
*first_byte_received = true;
// Advance buf_idx
discard_1(nmbs);
err = recv(nmbs, 7);
if (err != NMBS_ERROR_NONE)
return err;
// Starting over
msg_buf_reset(nmbs);
nmbs->msg.transaction_id = get_2(nmbs);
uint16_t protocol_id = get_2(nmbs);
uint16_t length = get_2(nmbs);
nmbs->msg.unit_id = get_1(nmbs);
nmbs->msg.fc = get_1(nmbs);
if (length < 2 || length > 255)
return NMBS_ERROR_INVALID_TCP_MBAP;
// Receive the rest of the message
err = recv(nmbs, length - 2);
if (err != NMBS_ERROR_NONE)
return err;
if (protocol_id != 0)
return NMBS_ERROR_INVALID_TCP_MBAP;
nmbs->msg.complete = true;
}
return NMBS_ERROR_NONE;
}
static void put_msg_header(nmbs_t* nmbs, uint16_t data_length) {
msg_buf_reset(nmbs);
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
put_1(nmbs, nmbs->msg.unit_id);
}
else if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) {
put_2(nmbs, nmbs->msg.transaction_id);
put_2(nmbs, 0);
put_2(nmbs, (uint16_t) (1 + 1 + data_length));
put_1(nmbs, nmbs->msg.unit_id);
}
put_1(nmbs, nmbs->msg.fc);
}
static void set_msg_header_size(nmbs_t* nmbs, uint16_t data_length) {
if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) {
data_length += 2;
set_2(nmbs, data_length, 4);
}
}
static nmbs_error send_msg(nmbs_t* nmbs) {
NMBS_DEBUG_PRINT("\n");
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
uint16_t crc = nmbs->platform.crc_calc(nmbs->msg.buf, nmbs->msg.buf_idx, nmbs->platform.arg);
put_2(nmbs, crc);
}
nmbs_error err = send(nmbs, nmbs->msg.buf_idx);
return err;
}
static nmbs_error recv_req_header(nmbs_t* nmbs, bool* first_byte_received) {
const nmbs_error err = recv_msg_header(nmbs, first_byte_received);
if (err != NMBS_ERROR_NONE)
return err;
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
// Check if request is for us
if (nmbs->msg.unit_id == NMBS_BROADCAST_ADDRESS)
nmbs->msg.broadcast = true;
else if (nmbs->msg.unit_id != nmbs->address_rtu)
nmbs->msg.ignored = true;
else
nmbs->msg.ignored = false;
}
return NMBS_ERROR_NONE;
}
static void put_res_header(nmbs_t* nmbs, uint16_t data_length) {
put_msg_header(nmbs, data_length);
NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->address_rtu, nmbs->msg.fc);
}
static nmbs_error send_exception_msg(nmbs_t* nmbs, uint8_t exception) {
nmbs->msg.fc += 0x80;
put_msg_header(nmbs, 1);
put_1(nmbs, exception);
NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\texception %d", nmbs->address_rtu, nmbs->address_rtu, exception);
return send_msg(nmbs);
}
static nmbs_error recv_res_header(nmbs_t* nmbs) {
uint16_t req_transaction_id = nmbs->msg.transaction_id;
uint8_t req_unit_id = nmbs->msg.unit_id;
uint8_t req_fc = nmbs->msg.fc;
bool first_byte_received = false;
nmbs_error err = recv_msg_header(nmbs, &first_byte_received);
if (err != NMBS_ERROR_NONE)
return err;
if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) {
if (nmbs->msg.transaction_id != req_transaction_id)
return NMBS_ERROR_INVALID_TCP_MBAP;
}
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU && nmbs->msg.unit_id != req_unit_id)
return NMBS_ERROR_INVALID_UNIT_ID;
if (nmbs->msg.fc != req_fc) {
if (nmbs->msg.fc - 0x80 == req_fc) {
err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t exception = get_1(nmbs);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (exception < 1 || exception > 4)
return NMBS_ERROR_INVALID_RESPONSE;
NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\texception %d\n", nmbs->address_rtu, nmbs->msg.unit_id,
exception);
return (nmbs_error) exception;
}
return NMBS_ERROR_INVALID_RESPONSE;
}
NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->msg.unit_id, nmbs->msg.fc);
return NMBS_ERROR_NONE;
}
static void put_req_header(nmbs_t* nmbs, uint16_t data_length) {
put_msg_header(nmbs, data_length);
printf("%d ", nmbs->address_rtu);
printf("NMBS req -> ");
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
if (nmbs->msg.broadcast)
printf("broadcast\t");
else
printf("address_rtu %d\t", nmbs->dest_address_rtu);
}
printf("fc %d\t", nmbs->msg.fc);
}
static nmbs_error recv_read_discrete_res(nmbs_t* nmbs, nmbs_bitfield values) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t coils_bytes = get_1(nmbs);
NMBS_DEBUG_PRINT("b %d\t", coils_bytes);
if (coils_bytes > 250) {
return NMBS_ERROR_INVALID_RESPONSE;
}
err = recv(nmbs, coils_bytes);
if (err != NMBS_ERROR_NONE)
return err;
NMBS_DEBUG_PRINT("coils ");
for (int i = 0; i < coils_bytes; i++) {
uint8_t coil = get_1(nmbs);
if (values)
values = coil;
NMBS_DEBUG_PRINT("%d ", coil);
}
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
return NMBS_ERROR_NONE;
}
static nmbs_error recv_read_registers_res(nmbs_t* nmbs, uint16_t quantity, uint16_t* registers) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t registers_bytes = get_1(nmbs);
NMBS_DEBUG_PRINT("b %d\t", registers_bytes);
if (registers_bytes > 250)
return NMBS_ERROR_INVALID_RESPONSE;
err = recv(nmbs, registers_bytes);
if (err != NMBS_ERROR_NONE)
return err;
NMBS_DEBUG_PRINT("regs ");
for (int i = 0; i < registers_bytes / 2; i++) {
uint16_t reg = get_2(nmbs);
if (registers)
registers = reg;
NMBS_DEBUG_PRINT("%d ", reg);
}
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (registers_bytes != quantity * 2)
return NMBS_ERROR_INVALID_RESPONSE;
return NMBS_ERROR_NONE;
}
nmbs_error recv_write_single_coil_res(nmbs_t* nmbs, uint16_t address, uint16_t value_req) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address_res = get_2(nmbs);
uint16_t value_res = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value_res);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (address_res != address)
return NMBS_ERROR_INVALID_RESPONSE;
if (value_res != value_req)
return NMBS_ERROR_INVALID_RESPONSE;
return NMBS_ERROR_NONE;
}
nmbs_error recv_write_single_register_res(nmbs_t* nmbs, uint16_t address, uint16_t value_req) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address_res = get_2(nmbs);
uint16_t value_res = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_res);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (address_res != address)
return NMBS_ERROR_INVALID_RESPONSE;
if (value_res != value_req)
return NMBS_ERROR_INVALID_RESPONSE;
return NMBS_ERROR_NONE;
}
nmbs_error recv_write_multiple_coils_res(nmbs_t* nmbs, uint16_t address, uint16_t quantity) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address_res = get_2(nmbs);
uint16_t quantity_res = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (address_res != address)
return NMBS_ERROR_INVALID_RESPONSE;
if (quantity_res != quantity)
return NMBS_ERROR_INVALID_RESPONSE;
return NMBS_ERROR_NONE;
}
nmbs_error recv_write_multiple_registers_res(nmbs_t* nmbs, uint16_t address, uint16_t quantity) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address_res = get_2(nmbs);
uint16_t quantity_res = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (address_res != address)
return NMBS_ERROR_INVALID_RESPONSE;
if (quantity_res != quantity)
return NMBS_ERROR_INVALID_RESPONSE;
return NMBS_ERROR_NONE;
}
nmbs_error recv_read_file_record_res(nmbs_t* nmbs, uint16_t* registers, uint16_t count) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t response_size = get_1(nmbs);
if (response_size > 250) {
return NMBS_ERROR_INVALID_RESPONSE;
}
err = recv(nmbs, response_size);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t subreq_data_size = get_1(nmbs) - 1;
uint8_t subreq_reference_type = get_1(nmbs);
uint16_t* subreq_record_data = (uint16_t*) get_n(nmbs, subreq_data_size);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (registers) {
if (subreq_reference_type != 6)
return NMBS_ERROR_INVALID_RESPONSE;
if (count != (subreq_data_size / 2))
return NMBS_ERROR_INVALID_RESPONSE;
swap_regs(subreq_record_data, subreq_data_size / 2);
memcpy(registers, subreq_record_data, subreq_data_size);
}
return NMBS_ERROR_NONE;
}
nmbs_error recv_write_file_record_res(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number,
const uint16_t* registers, uint16_t count) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t response_size = get_1(nmbs);
if (response_size > 251)
return NMBS_ERROR_INVALID_RESPONSE;
err = recv(nmbs, response_size);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t subreq_reference_type = get_1(nmbs);
uint16_t subreq_file_number = get_2(nmbs);
uint16_t subreq_record_number = get_2(nmbs);
uint16_t subreq_record_length = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", subreq_file_number, subreq_record_number, subreq_record_length);
uint16_t subreq_data_size = subreq_record_length * 2;
uint16_t* subreq_record_data = (uint16_t*) get_n(nmbs, subreq_data_size);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (registers) {
if (subreq_reference_type != 6)
return NMBS_ERROR_INVALID_RESPONSE;
if (subreq_file_number != file_number)
return NMBS_ERROR_INVALID_RESPONSE;
if (subreq_record_number != record_number)
return NMBS_ERROR_INVALID_RESPONSE;
if (subreq_record_length != count)
return NMBS_ERROR_INVALID_RESPONSE;
swap_regs(subreq_record_data, subreq_record_length);
if (memcmp(registers, subreq_record_data, subreq_data_size) != 0)
return NMBS_ERROR_INVALID_RESPONSE;
}
return NMBS_ERROR_NONE;
}
nmbs_error recv_read_device_identification_res(nmbs_t* nmbs, uint8_t buffers_count, char** buffers_out,
uint8_t buffers_length, const uint8_t* order, uint8_t* ids_out,
uint8_t* next_object_id_out, uint8_t* objects_count_out) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 6);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t mei_type = get_1(nmbs);
if (mei_type != 0x0E)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t read_device_id_code = get_1(nmbs);
if (read_device_id_code < 1 || read_device_id_code > 4)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t conformity_level = get_1(nmbs);
if (conformity_level < 1 || (conformity_level > 3 && conformity_level < 0x81) || conformity_level > 0x83)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t more_follows = get_1(nmbs);
if (more_follows != 0 && more_follows != 0xFF)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t next_object_id = get_1(nmbs);
uint8_t objects_count = get_1(nmbs);
if (objects_count_out)
*objects_count_out = objects_count;
if (buffers_count == 0) {
buffers_out = NULL;
}
else if (objects_count > buffers_count)
return NMBS_ERROR_INVALID_ARGUMENT;
if (more_follows == 0)
next_object_id = 0x7F; // This value is reserved in the spec, we use it to signal stream is finished
if (next_object_id_out)
*next_object_id_out = next_object_id;
uint8_t res_size_left = 253 - 7;
for (int i = 0; i < objects_count; i++) {
err = recv(nmbs, 2);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t object_id = get_1(nmbs);
uint8_t object_length = get_1(nmbs);
res_size_left -= 2;
if (object_length > res_size_left)
return NMBS_ERROR_INVALID_RESPONSE;
err = recv(nmbs, object_length);
if (err != NMBS_ERROR_NONE)
return err;
const char* str = (const char*) get_n(nmbs, object_length);
if (ids_out)
ids_out = object_id;
uint8_t buf_index = i;
if (order)
buf_index = order[object_id];
if (buffers_out) {
strncpy(buffers_out[buf_index], str, buffers_length);
buffers_out[buf_index][object_length] = 0;
}
}
return recv_msg_footer(nmbs);
}
static nmbs_error handle_read_discrete(nmbs_t* nmbs,
nmbs_error (*callback)(uint16_t, uint16_t, nmbs_bitfield, uint8_t, void*)) {
nmbs_error err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address = get_2(nmbs);
uint16_t quantity = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (quantity < 1 || quantity > 2000)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (callback) {
nmbs_bitfield bitfield = {0};
err = callback(address, quantity, bitfield, nmbs->msg.unit_id, nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
if (!nmbs->msg.broadcast) {
uint8_t discrete_bytes = (quantity + 7) / 8;
put_res_header(nmbs, 1 + discrete_bytes);
put_1(nmbs, discrete_bytes);
NMBS_DEBUG_PRINT("b %d\t", discrete_bytes);
NMBS_DEBUG_PRINT("coils ");
for (int i = 0; i < discrete_bytes; i++) {
put_1(nmbs, bitfield);
NMBS_DEBUG_PRINT("%d ", bitfield);
}
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
}
else {
return recv_read_discrete_res(nmbs, NULL);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_read_registers(nmbs_t* nmbs,
nmbs_error (*callback)(uint16_t, uint16_t, uint16_t*, uint8_t, void*)) {
nmbs_error err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address = get_2(nmbs);
uint16_t quantity = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (quantity < 1 || quantity > 125)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (callback) {
uint16_t regs[125] = {0};
err = callback(address, quantity, regs, nmbs->msg.unit_id, nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
// TODO check all these read request broadcast use cases
if (!nmbs->msg.broadcast) {
const uint8_t regs_bytes = quantity * 2;
put_res_header(nmbs, 1 + regs_bytes);
put_1(nmbs, regs_bytes);
NMBS_DEBUG_PRINT("b %d\t", regs_bytes);
NMBS_DEBUG_PRINT("regs ");
for (int i = 0; i < quantity; i++) {
put_2(nmbs, regs);
NMBS_DEBUG_PRINT("%d ", regs);
}
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
}
else {
return recv_read_registers_res(nmbs, quantity, NULL);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_read_coils(nmbs_t* nmbs) {
return handle_read_discrete(nmbs, nmbs->callbacks.read_coils);
}
static nmbs_error handle_read_discrete_inputs(nmbs_t* nmbs) {
return handle_read_discrete(nmbs, nmbs->callbacks.read_discrete_inputs);
}
static nmbs_error handle_read_holding_registers(nmbs_t* nmbs) {
return handle_read_registers(nmbs, nmbs->callbacks.read_holding_registers);
}
static nmbs_error handle_read_input_registers(nmbs_t* nmbs) {
return handle_read_registers(nmbs, nmbs->callbacks.read_input_registers);
}
static nmbs_error handle_write_single_coil(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address = get_2(nmbs);
uint16_t value = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (nmbs->callbacks.write_single_coil) {
if (value != 0 && value != 0xFF00)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
err = nmbs->callbacks.write_single_coil(address, value == 0 ? false : true, nmbs->msg.unit_id,
nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
if (!nmbs->msg.broadcast) {
put_res_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, value);
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value);
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
}
else {
return recv_write_single_coil_res(nmbs, address, value);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_write_single_register(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 4);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address = get_2(nmbs);
uint16_t value = get_2(nmbs);
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (nmbs->callbacks.write_single_register) {
err = nmbs->callbacks.write_single_register(address, value, nmbs->msg.unit_id, nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
if (!nmbs->msg.broadcast) {
put_res_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, value);
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value);
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
}
else {
return recv_write_single_register_res(nmbs, address, value);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_write_multiple_coils(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 5);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address = get_2(nmbs);
uint16_t quantity = get_2(nmbs);
uint8_t coils_bytes = get_1(nmbs);
NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\tcoils ", address, quantity, coils_bytes);
if (coils_bytes > 246)
return NMBS_ERROR_INVALID_REQUEST;
err = recv(nmbs, coils_bytes);
if (err != NMBS_ERROR_NONE)
return err;
nmbs_bitfield coils = {0};
for (int i = 0; i < coils_bytes; i++) {
coils = get_1(nmbs);
NMBS_DEBUG_PRINT("%d ", coils);
}
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (quantity < 1 || quantity > 0x07B0)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (coils_bytes == 0)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if ((quantity + 7) / 8 != coils_bytes)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (nmbs->callbacks.write_multiple_coils) {
err = nmbs->callbacks.write_multiple_coils(address, quantity, coils, nmbs->msg.unit_id,
nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
if (!nmbs->msg.broadcast) {
put_res_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, quantity);
NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity);
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
}
else {
return recv_write_multiple_coils_res(nmbs, address, quantity);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_write_multiple_registers(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 5);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t address = get_2(nmbs);
uint16_t quantity = get_2(nmbs);
uint8_t registers_bytes = get_1(nmbs);
NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\tregs ", address, quantity, registers_bytes);
err = recv(nmbs, registers_bytes);
if (err != NMBS_ERROR_NONE)
return err;
if (registers_bytes > 246)
return NMBS_ERROR_INVALID_REQUEST;
uint16_t registers[0x007B];
for (int i = 0; i < registers_bytes / 2; i++) {
registers = get_2(nmbs);
NMBS_DEBUG_PRINT("%d ", registers);
}
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (quantity < 1 || quantity > 0x007B)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (registers_bytes == 0)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (registers_bytes != quantity * 2)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (nmbs->callbacks.write_multiple_registers) {
err = nmbs->callbacks.write_multiple_registers(address, quantity, registers, nmbs->msg.unit_id,
nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
if (!nmbs->msg.broadcast) {
put_res_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, quantity);
NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity);
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
}
else {
return recv_write_multiple_registers_res(nmbs, address, quantity);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_read_file_record(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t request_size = get_1(nmbs);
if (request_size > 245)
return NMBS_ERROR_INVALID_REQUEST;
err = recv(nmbs, request_size);
if (err != NMBS_ERROR_NONE)
return err;
const uint8_t subreq_header_size = 7;
const uint8_t subreq_count = request_size / subreq_header_size;
struct {
uint8_t reference_type;
uint16_t file_number;
uint16_t record_number;
uint16_t record_length;
}
#if defined(__STDC_NO_VLA__) || defined(_MSC_VER)
subreq[35]; // 245 / subreq_header_size
#else
subreq[subreq_count];
#endif
uint8_t response_data_size = 0;
for (uint8_t i = 0; i < subreq_count; i++) {
subreq.reference_type = get_1(nmbs);
subreq.file_number = get_2(nmbs);
subreq.record_number = get_2(nmbs);
subreq.record_length = get_2(nmbs);
response_data_size += 2 + subreq.record_length * 2;
}
discard_n(nmbs, request_size % subreq_header_size);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (request_size % subreq_header_size)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (request_size < 0x07 || request_size > 0xF5)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
for (uint8_t i = 0; i < subreq_count; i++) {
if (subreq.reference_type != 0x06)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (subreq.file_number == 0x0000)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (subreq.record_number > 0x270F)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (subreq.record_length > 124)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fread ", subreq.file_number, subreq.record_number,
subreq.record_length);
}
put_res_header(nmbs, 1 + response_data_size);
put_1(nmbs, response_data_size);
if (nmbs->callbacks.read_file_record) {
for (uint8_t i = 0; i < subreq_count; i++) {
uint16_t subreq_data_size = subreq.record_length * 2;
put_1(nmbs, subreq_data_size + 1);
put_1(nmbs, 0x06); // add Reference Type const
uint16_t* subreq_data = (uint16_t*) get_n(nmbs, subreq_data_size);
err = nmbs->callbacks.read_file_record(subreq.file_number, subreq.record_number, subreq_data,
subreq.record_length, nmbs->msg.unit_id, nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
swap_regs(subreq_data, subreq.record_length);
}
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
if (!nmbs->msg.broadcast) {
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return recv_read_file_record_res(nmbs, NULL, 0);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_write_file_record(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 1);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t request_size = get_1(nmbs);
if (request_size > 251) {
return NMBS_ERROR_INVALID_REQUEST;
}
err = recv(nmbs, request_size);
if (err != NMBS_ERROR_NONE)
return err;
// We can save msg.buf index and use it later for context recovery.
uint16_t msg_buf_idx = nmbs->msg.buf_idx;
discard_n(nmbs, request_size);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
const uint8_t subreq_header_size = 7;
uint16_t size = request_size;
nmbs->msg.buf_idx = msg_buf_idx; // restore context
if (request_size < 7)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
do {
uint8_t subreq_reference_type = get_1(nmbs);
uint16_t subreq_file_number_c = get_2(nmbs);
uint16_t subreq_record_number_c = get_2(nmbs);
uint16_t subreq_record_length_c = get_2(nmbs);
discard_n(nmbs, subreq_record_length_c * 2);
if (subreq_reference_type != 0x06)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (subreq_file_number_c == 0x0000)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (subreq_record_number_c > 0x270F)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (subreq_record_length_c > 122)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", subreq_file_number_c, subreq_record_number_c,
subreq_record_length_c);
size -= (subreq_header_size + subreq_record_length_c * 2);
} while (size >= subreq_header_size);
if (size)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
// checks completed
size = request_size;
nmbs->msg.buf_idx = msg_buf_idx; // restore context
do {
discard_1(nmbs);
uint16_t subreq_file_number = get_2(nmbs);
uint16_t subreq_record_number = get_2(nmbs);
uint16_t subreq_record_length = get_2(nmbs);
uint16_t* subreq_data = get_regs(nmbs, subreq_record_length);
if (nmbs->callbacks.write_file_record) {
err = nmbs->callbacks.write_file_record(subreq_file_number, subreq_record_number, subreq_data,
subreq_record_length, nmbs->msg.unit_id, nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
swap_regs(subreq_data, subreq_record_length); // restore swapping
}
else {
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
size -= (subreq_header_size + subreq_record_length * 2);
} while (size >= subreq_header_size);
if (!nmbs->msg.broadcast) {
// The normal response to 'Write File' is an echo of the request.
// We can restore buffer index and response msg.
nmbs->msg.buf_idx = msg_buf_idx;
discard_n(nmbs, request_size);
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return recv_write_file_record_res(nmbs, 0, 0, NULL, 0);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_read_write_registers(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 9);
if (err != NMBS_ERROR_NONE)
return err;
uint16_t read_address = get_2(nmbs);
uint16_t read_quantity = get_2(nmbs);
uint16_t write_address = get_2(nmbs);
uint16_t write_quantity = get_2(nmbs);
uint8_t byte_count_write = get_1(nmbs);
NMBS_DEBUG_PRINT("ra %d\trq %d\t wa %d\t wq %d\t b %d\tregs ", read_address, read_quantity, write_address,
write_quantity, byte_count_write);
if (byte_count_write > 242)
return NMBS_ERROR_INVALID_REQUEST;
err = recv(nmbs, byte_count_write);
if (err != NMBS_ERROR_NONE)
return err;
#if defined(__STDC_NO_VLA__) || defined(_MSC_VER)
uint16_t registers[0x007B];
#else
uint16_t registers[byte_count_write / 2];
#endif
for (int i = 0; i < byte_count_write / 2; i++) {
registers = get_2(nmbs);
NMBS_DEBUG_PRINT("%d ", registers);
}
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (read_quantity < 1 || read_quantity > 0x007D)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (write_quantity < 1 || write_quantity > 0x007B)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (byte_count_write != write_quantity * 2)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if ((uint32_t) read_address + (uint32_t) read_quantity > ((uint32_t) 0xFFFF) + 1)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if ((uint32_t) write_address + (uint32_t) write_quantity > ((uint32_t) 0xFFFF) + 1)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (!nmbs->callbacks.write_multiple_registers || !nmbs->callbacks.read_holding_registers)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
err = nmbs->callbacks.write_multiple_registers(write_address, write_quantity, registers, nmbs->msg.unit_id,
nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
if (!nmbs->msg.broadcast) {
#if defined(__STDC_NO_VLA__) || defined(_MSC_VER)
uint16_t regs[125];
#else
uint16_t regs[read_quantity];
#endif
err = nmbs->callbacks.read_holding_registers(read_address, read_quantity, regs, nmbs->msg.unit_id,
nmbs->callbacks.arg);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
uint8_t regs_bytes = read_quantity * 2;
put_res_header(nmbs, 1 + regs_bytes);
put_1(nmbs, regs_bytes);
NMBS_DEBUG_PRINT("b %d\t", regs_bytes);
NMBS_DEBUG_PRINT("regs ");
for (int i = 0; i < read_quantity; i++) {
put_2(nmbs, regs);
NMBS_DEBUG_PRINT("%d ", regs);
}
err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
}
}
else {
return recv_write_multiple_registers_res(nmbs, write_address, write_quantity);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_read_device_identification(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 3);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t mei_type = get_1(nmbs);
uint8_t read_device_id_code = get_1(nmbs);
uint8_t object_id = get_1(nmbs);
NMBS_DEBUG_PRINT("c %d\to %d", read_device_id_code, object_id);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (!nmbs->callbacks.read_device_identification_map || !nmbs->callbacks.read_device_identification)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
if (mei_type != 0x0E)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
if (read_device_id_code < 1 || read_device_id_code > 4)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (object_id > 6 && object_id < 0x80)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (!nmbs->msg.broadcast) {
char str[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH];
nmbs_bitfield_256 map;
nmbs_bitfield_reset(map);
err = nmbs->callbacks.read_device_identification_map(map);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
put_res_header(nmbs, 0); // Length will be set later
put_1(nmbs, 0x0E);
put_1(nmbs, read_device_id_code);
put_1(nmbs, 0x83);
if (read_device_id_code == 4) {
if (!nmbs_bitfield_read(map, object_id))
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
put_1(nmbs, 0); // More follows
put_1(nmbs, 0); // Next Object Id
put_1(nmbs, 1); // Number of objects
str[0] = 0;
err = nmbs->callbacks.read_device_identification(object_id, str);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
size_t str_len = strlen(str);
put_1(nmbs, object_id); // Object id
put_1(nmbs, str_len); // Object length
put_n(nmbs, (uint8_t*) str, str_len);
set_msg_header_size(nmbs, 6 + 2 + str_len);
return send_msg(nmbs);
}
uint8_t more_follows_idx = nmbs->msg.buf_idx;
put_1(nmbs, 0);
uint8_t next_object_id_idx = nmbs->msg.buf_idx;
put_1(nmbs, 0);
uint8_t number_of_objects_idx = nmbs->msg.buf_idx;
put_1(nmbs, 0);
int16_t res_size_left = 253 - 7;
uint8_t last_id = 0;
uint8_t msg_size = 6;
uint8_t res_more_follows = 0;
uint8_t res_next_object_id = 0;
uint8_t res_number_of_objects = 0;
switch (read_device_id_code) {
case 1:
if (object_id > 0x02)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
last_id = 0x02;
break;
case 2:
if (object_id < 0x03 || object_id > 0x07)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
last_id = 0x07;
break;
case 3:
if (object_id < 0x80)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
last_id = 0xFF;
break;
default:
// Unreachable
break;
}
for (uint16_t id = object_id; id <= last_id; id++) {
if (!nmbs_bitfield_read(map, id)) {
if (id < 0x03)
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
continue;
}
str[0] = 0;
err = nmbs->callbacks.read_device_identification((uint8_t) id, str);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
int16_t str_len = (int16_t) strlen(str);
res_size_left = (int16_t) (res_size_left - 2 - str_len);
if (res_size_left < 0) {
res_more_follows = 0xFF;
res_next_object_id = id;
break;
}
put_1(nmbs, (uint8_t) id); // Object id
put_1(nmbs, str_len); // Object length
put_n(nmbs, (uint8_t*) str, str_len);
msg_size += (2 + str_len);
res_number_of_objects++;
}
set_1(nmbs, res_more_follows, more_follows_idx);
set_1(nmbs, res_next_object_id, next_object_id_idx);
set_1(nmbs, res_number_of_objects, number_of_objects_idx);
set_msg_header_size(nmbs, msg_size);
return send_msg(nmbs);
}
}
else {
return recv_read_device_identification_res(nmbs, 0, NULL, 0, NULL, NULL, NULL, NULL);
}
return NMBS_ERROR_NONE;
}
static nmbs_error handle_req_fc(nmbs_t* nmbs) {
NMBS_DEBUG_PRINT("fc %d\t", nmbs->msg.fc);
nmbs_error err = NMBS_ERROR_NONE;
switch (nmbs->msg.fc) {
case 1:
err = handle_read_coils(nmbs);
break;
case 2:
err = handle_read_discrete_inputs(nmbs);
break;
case 3:
err = handle_read_holding_registers(nmbs);
break;
case 4:
err = handle_read_input_registers(nmbs);
break;
case 5:
err = handle_write_single_coil(nmbs);
break;
case 6:
err = handle_write_single_register(nmbs);
break;
case 15:
err = handle_write_multiple_coils(nmbs);
break;
case 16:
err = handle_write_multiple_registers(nmbs);
break;
case 20:
err = handle_read_file_record(nmbs);
break;
case 21:
err = handle_write_file_record(nmbs);
break;
case 23:
err = handle_read_write_registers(nmbs);
break;
case 43:
err = handle_read_device_identification(nmbs);
break;
default:
flush(nmbs);
if (!nmbs->msg.ignored && !nmbs->msg.broadcast)
err = send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
}
return err;
}
void nmbs_callbacks_create(nmbs_callbacks* callbacks) {
memset(callbacks, 0, sizeof(nmbs_callbacks));
callbacks->initialized = 0xFFFFDEBE;
}
nmbs_error nmbs_server_create(nmbs_t* nmbs, uint8_t address_rtu, const nmbs_platform_conf* platform_conf,
const nmbs_callbacks* callbacks) {
if (platform_conf->transport == NMBS_TRANSPORT_RTU && address_rtu == 0)
return NMBS_ERROR_INVALID_ARGUMENT;
if (!callbacks || callbacks->initialized != 0xFFFFDEBE)
return NMBS_ERROR_INVALID_ARGUMENT;
nmbs_error ret = nmbs_create(nmbs, platform_conf);
if (ret != NMBS_ERROR_NONE)
return ret;
nmbs->address_rtu = address_rtu;
nmbs->callbacks = *callbacks;
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_server_poll(nmbs_t* nmbs) {
msg_state_reset(nmbs);
bool first_byte_received = false;
nmbs_error err = recv_req_header(nmbs, &first_byte_received);
if (err != NMBS_ERROR_NONE) {
if (!first_byte_received && err == NMBS_ERROR_TIMEOUT)
return NMBS_ERROR_NONE;
return err;
}
printf("%d ", nmbs->address_rtu);
printf("NMBS req <- ");
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
if (nmbs->msg.broadcast)
printf("broadcast\t");
else
printf("address_rtu %d\t", nmbs->msg.unit_id);
}
err = handle_req_fc(nmbs);
if (err != NMBS_ERROR_NONE) {
if (err != NMBS_ERROR_TIMEOUT)
flush(nmbs);
return err;
}
return NMBS_ERROR_NONE;
}
void nmbs_set_callbacks_arg(nmbs_t* nmbs, void* arg) {
nmbs->callbacks.arg = arg;
}
nmbs_error nmbs_client_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf) {
return nmbs_create(nmbs, platform_conf);
}
static nmbs_error read_discrete(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint16_t quantity, nmbs_bitfield values) {
if (quantity < 1 || quantity > 2000)
return NMBS_ERROR_INVALID_ARGUMENT;
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return NMBS_ERROR_INVALID_ARGUMENT;
msg_state_req(nmbs, fc);
put_req_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, quantity);
NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
return recv_read_discrete_res(nmbs, values);
}
nmbs_error nmbs_read_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield coils_out) {
return read_discrete(nmbs, 1, address, quantity, coils_out);
}
nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out) {
return read_discrete(nmbs, 2, address, quantity, inputs_out);
}
static nmbs_error read_registers(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint16_t quantity, uint16_t* registers) {
if (quantity < 1 || quantity > 125)
return NMBS_ERROR_INVALID_ARGUMENT;
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return NMBS_ERROR_INVALID_ARGUMENT;
msg_state_req(nmbs, fc);
put_req_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, quantity);
NMBS_DEBUG_PRINT("a %d\tq %d ", address, quantity);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
return recv_read_registers_res(nmbs, quantity, registers);
}
nmbs_error nmbs_read_holding_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out) {
return read_registers(nmbs, 3, address, quantity, registers_out);
}
nmbs_error nmbs_read_input_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out) {
return read_registers(nmbs, 4, address, quantity, registers_out);
}
nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, bool value) {
msg_state_req(nmbs, 5);
put_req_header(nmbs, 4);
uint16_t value_req = value ? 0xFF00 : 0;
put_2(nmbs, address);
put_2(nmbs, value_req);
NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_req);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast)
return recv_write_single_coil_res(nmbs, address, value_req);
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t value) {
msg_state_req(nmbs, 6);
put_req_header(nmbs, 4);
put_2(nmbs, address);
put_2(nmbs, value);
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast)
return recv_write_single_register_res(nmbs, address, value);
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const nmbs_bitfield coils) {
if (quantity < 1 || quantity > 0x07B0)
return NMBS_ERROR_INVALID_ARGUMENT;
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return NMBS_ERROR_INVALID_ARGUMENT;
uint8_t coils_bytes = (quantity + 7) / 8;
msg_state_req(nmbs, 15);
put_req_header(nmbs, 5 + coils_bytes);
put_2(nmbs, address);
put_2(nmbs, quantity);
put_1(nmbs, coils_bytes);
NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\t", address, quantity, coils_bytes);
NMBS_DEBUG_PRINT("coils ");
for (int i = 0; i < coils_bytes; i++) {
put_1(nmbs, coils);
NMBS_DEBUG_PRINT("%d ", coils);
}
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast)
return recv_write_multiple_coils_res(nmbs, address, quantity);
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const uint16_t* registers) {
if (quantity < 1 || quantity > 0x007B)
return NMBS_ERROR_INVALID_ARGUMENT;
if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1)
return NMBS_ERROR_INVALID_ARGUMENT;
uint8_t registers_bytes = quantity * 2;
msg_state_req(nmbs, 16);
put_req_header(nmbs, 5 + registers_bytes);
put_2(nmbs, address);
put_2(nmbs, quantity);
put_1(nmbs, registers_bytes);
NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\t", address, quantity, registers_bytes);
NMBS_DEBUG_PRINT("regs ");
for (int i = 0; i < quantity; i++) {
put_2(nmbs, registers);
NMBS_DEBUG_PRINT("%d ", registers);
}
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast)
return recv_write_multiple_registers_res(nmbs, address, quantity);
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, uint16_t* registers,
uint16_t count) {
if (file_number == 0x0000)
return NMBS_ERROR_INVALID_ARGUMENT;
if (record_number > 0x270F)
return NMBS_ERROR_INVALID_ARGUMENT;
// In expected response: max PDU length = 253, assuming a single file request, (253 - 1 - 1 - 1 - 1) / 2 = 124
if (count > 124)
return NMBS_ERROR_INVALID_ARGUMENT;
msg_state_req(nmbs, 20);
put_req_header(nmbs, 8);
put_1(nmbs, 7); // add Byte Count
put_1(nmbs, 6); // add Reference Type const
put_2(nmbs, file_number);
put_2(nmbs, record_number);
put_2(nmbs, count);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fread ", file_number, record_number, count);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
return recv_read_file_record_res(nmbs, registers, count);
}
nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, const uint16_t* registers,
uint16_t count) {
if (file_number == 0x0000)
return NMBS_ERROR_INVALID_ARGUMENT;
if (record_number > 0x270F)
return NMBS_ERROR_INVALID_ARGUMENT;
if (count > 122)
return NMBS_ERROR_INVALID_ARGUMENT;
uint16_t data_size = count * 2;
msg_state_req(nmbs, 21);
put_req_header(nmbs, 8 + data_size);
put_1(nmbs, 7 + data_size); // add Byte Count
put_1(nmbs, 6); // add Reference Type const
put_2(nmbs, file_number);
put_2(nmbs, record_number);
put_2(nmbs, count);
put_regs(nmbs, registers, count);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", file_number, record_number, count);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast)
return recv_write_file_record_res(nmbs, file_number, record_number, registers, count);
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16_t read_quantity,
uint16_t* registers_out, uint16_t write_address, uint16_t write_quantity,
const uint16_t* registers) {
if (read_quantity < 1 || read_quantity > 0x007D)
return NMBS_ERROR_INVALID_ARGUMENT;
if ((uint32_t) read_address + (uint32_t) read_quantity > ((uint32_t) 0xFFFF) + 1)
return NMBS_ERROR_INVALID_ARGUMENT;
if (write_quantity < 1 || write_quantity > 0x0079)
return NMBS_ERROR_INVALID_ARGUMENT;
if ((uint32_t) write_address + (uint32_t) write_quantity > ((uint32_t) 0xFFFF) + 1)
return NMBS_ERROR_INVALID_ARGUMENT;
uint8_t registers_bytes = write_quantity * 2;
msg_state_req(nmbs, 23);
put_req_header(nmbs, 9 + registers_bytes);
put_2(nmbs, read_address);
put_2(nmbs, read_quantity);
put_2(nmbs, write_address);
put_2(nmbs, write_quantity);
put_1(nmbs, registers_bytes);
NMBS_DEBUG_PRINT("read a %d\tq %d ", read_address, read_quantity);
NMBS_DEBUG_PRINT("write a %d\tq %d\tb %d\t", write_address, write_quantity, registers_bytes);
NMBS_DEBUG_PRINT("regs ");
for (int i = 0; i < write_quantity; i++) {
put_2(nmbs, registers);
NMBS_DEBUG_PRINT("%d ", registers);
}
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast) {
return recv_read_registers_res(nmbs, read_quantity, registers_out);
}
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification_basic(nmbs_t* nmbs, char* vendor_name, char* product_code,
char* major_minor_revision, uint8_t buffers_length) {
const uint8_t order[3] = {0, 1, 2};
char* buffers[3] = {vendor_name, product_code, major_minor_revision};
uint8_t total_received = 0;
uint8_t next_object_id = 0x00;
while (next_object_id != 0x7F) {
msg_state_req(nmbs, 43);
put_msg_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 1);
put_1(nmbs, next_object_id);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t objects_received = 0;
err = recv_read_device_identification_res(nmbs, 3, buffers, buffers_length, order, NULL, &next_object_id,
&objects_received);
if (err != NMBS_ERROR_NONE)
return err;
total_received += objects_received;
if (total_received > 3)
return NMBS_ERROR_INVALID_RESPONSE;
if (objects_received == 0)
return NMBS_ERROR_INVALID_RESPONSE;
}
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification_regular(nmbs_t* nmbs, char* vendor_url, char* product_name, char* model_name,
char* user_application_name, uint8_t buffers_length) {
const uint8_t order[7] = {0, 0, 0, 0, 1, 2, 3};
char* buffers[4] = {vendor_url, product_name, model_name, user_application_name};
uint8_t total_received = 0;
uint8_t next_object_id = 0x03;
while (next_object_id != 0x7F) {
msg_state_req(nmbs, 43);
put_req_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 2);
put_1(nmbs, next_object_id);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t objects_received = 0;
err = recv_read_device_identification_res(nmbs, 4, buffers, buffers_length, order, NULL, &next_object_id,
&objects_received);
if (err != NMBS_ERROR_NONE)
return err;
total_received += objects_received;
if (total_received > 4)
return NMBS_ERROR_INVALID_RESPONSE;
if (objects_received == 0)
return NMBS_ERROR_INVALID_RESPONSE;
}
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification_extended(nmbs_t* nmbs, uint8_t object_id_start, uint8_t* ids, char** buffers,
uint8_t ids_length, uint8_t buffer_length,
uint8_t* objects_count_out) {
if (object_id_start < 0x80)
return NMBS_ERROR_INVALID_ARGUMENT;
uint8_t total_received = 0;
uint8_t next_object_id = object_id_start;
while (next_object_id != 0x7F) {
msg_state_req(nmbs, 43);
put_req_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 3);
put_1(nmbs, next_object_id);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t objects_received = 0;
err = recv_read_device_identification_res(nmbs, ids_length - total_received, &buffers[total_received],
buffer_length, NULL, &ids[total_received], &next_object_id,
&objects_received);
if (err != NMBS_ERROR_NONE)
return err;
total_received += objects_received;
}
*objects_count_out = total_received;
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, uint8_t object_id, char* buffer, uint8_t buffer_length) {
if (object_id > 0x06 && object_id < 0x80)
return NMBS_ERROR_INVALID_ARGUMENT;
msg_state_req(nmbs, 43);
put_req_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 4);
put_1(nmbs, object_id);
nmbs_error err = send_msg(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
char* buf[1] = {buffer};
return recv_read_device_identification_res(nmbs, 1, buf, buffer_length, NULL, NULL, NULL, NULL);
}
nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint16_t data_len) {
msg_state_req(nmbs, fc);
put_msg_header(nmbs, data_len);
NMBS_DEBUG_PRINT("raw ");
for (uint16_t i = 0; i < data_len; i++) {
put_1(nmbs, data);
NMBS_DEBUG_PRINT("%d ", data);
}
return send_msg(nmbs);
}
nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint8_t data_out_len) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, data_out_len);
if (err != NMBS_ERROR_NONE)
return err;
if (data_out) {
for (uint16_t i = 0; i < data_out_len; i++)
data_out = get_1(nmbs);
}
else {
for (uint16_t i = 0; i < data_out_len; i++)
get_1(nmbs);
}
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
return NMBS_ERROR_NONE;
}
const char* nmbs_strerror(nmbs_error error) {
switch (error) {
case NMBS_ERROR_INVALID_REQUEST:
return "invalid request received";
case NMBS_ERROR_INVALID_UNIT_ID:
return "invalid unit ID received";
case NMBS_ERROR_INVALID_TCP_MBAP:
return "invalid TCP MBAP received";
case NMBS_ERROR_CRC:
return "invalid CRC received";
case NMBS_ERROR_TRANSPORT:
return "transport error";
case NMBS_ERROR_TIMEOUT:
return "timeout";
case NMBS_ERROR_INVALID_RESPONSE:
return "invalid response received";
case NMBS_ERROR_INVALID_ARGUMENT:
return "invalid argument provided";
case NMBS_ERROR_NONE:
return "no error";
case NMBS_EXCEPTION_ILLEGAL_FUNCTION:
return "modbus exception 1: illegal function";
case NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS:
return "modbus exception 2: illegal data address";
case NMBS_EXCEPTION_ILLEGAL_DATA_VALUE:
return "modbus exception 3: data value";
case NMBS_EXCEPTION_SERVER_DEVICE_FAILURE:
return "modbus exception 4: server device failure";
default:
return "unknown error";
}
}
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2025 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "nanomodbus.h"
#define RTU_SERVER_ADDRESS 1
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
nmbs_t nmbs;
int32_t read_serial(uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, void* arg) {
HAL_StatusTypeDef status = HAL_UART_Receive(&huart2, buf, count, byte_timeout_ms);
if (status == HAL_OK) {
return count;
} else {
return 0;
}
}
int32_t write_serial(const uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, void* arg) {
HAL_StatusTypeDef status = HAL_UART_Transmit(&huart2, (uint8_t *)buf, count, byte_timeout_ms);
if (status == HAL_OK) {
return count;
} else {
return 0;
}
}
static void modbus_init(nmbs_t *nmbs) {
nmbs_platform_conf platform_conf;
platform_conf.transport = NMBS_TRANSPORT_RTU;
platform_conf.read = read_serial;
platform_conf.write = write_serial;
nmbs_error err = nmbs_client_create(nmbs, &platform_conf);
if (err != NMBS_ERROR_NONE)
Error_Handler();
nmbs_set_read_timeout(nmbs, 1000);
nmbs_set_byte_timeout(nmbs, 100);
nmbs_set_destination_rtu_address(nmbs, RTU_SERVER_ADDRESS);
}
uint16_t get_moisture(nmbs_t *nmbs) {
uint16_t r_reg = 0;
nmbs_error err = nmbs_read_holding_registers(nmbs, 1, 1, &r_reg);
if (err != NMBS_ERROR_NONE)
Error_Handler();
return r_reg;
}
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_USART2_UART_Init();
MX_USART3_UART_Init();
MX_TIM3_Init();
/* USER CODE BEGIN 2 */
modbus_init(&nmbs);
/* USER CODE END 2 */
printf("dadada\r\n");
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
uint16_t moisture = get_moisture(&nmbs);
HAL_Delay(2000);
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
__HAL_FLASH_SET_LATENCY(FLASH_LATENCY_1);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
编译后，烧录到板子
打开串口助手
修改地址0，发送12
接着发13
接着发14
全部发完
可以看到modbus从站功能正常
附上源代码
|
|
楼主
标题置顶
标题高亮
