From 6dbbccec33f220086215e676bf21eeba52c4c6c7 Mon Sep 17 00:00:00 2001 From: mj Date: Sun, 5 Dec 2021 16:36:41 +0100 Subject: [PATCH] Modbus refactor --- fw/Core/Inc/modbus.h | 46 +++++- fw/Core/Src/main.c | 12 ++ fw/Core/Src/modbus.c | 328 ++++++++++++++++++++++++++++--------------- 3 files changed, 267 insertions(+), 119 deletions(-) diff --git a/fw/Core/Inc/modbus.h b/fw/Core/Inc/modbus.h index a215328..9003d71 100644 --- a/fw/Core/Inc/modbus.h +++ b/fw/Core/Inc/modbus.h @@ -39,6 +39,7 @@ #define SRC_MODBUS_H_ #include "stdint.h" +#include "stddef.h" /* * Defines & macros @@ -48,10 +49,13 @@ #define MODBUS_DEFAULT_SLAVE_ADDRESS 254 /* 255 may be used for bridge device */ /* minimal frame length is 4 bytes: 1 B slave address, 1 B function code, 2 B CRC */ #define MODBUS_MINIMAL_FRAME_LEN 4 +#define MODBUS_READ_DEVICE_ID_REQUEST_LEN 4 #define MODBUS_MAX_RTU_FRAME_SIZE 256 #define MODBUS_BUFFER_SIZE MODBUS_MAX_RTU_FRAME_SIZE /* alias */ #define MODBUS_ERROR_FLAG 0x80 #define MODBUS_MAX_REGISTERS 125 +#define MODBUS_MEI 0x0E +#define MODBUS_DEVICE_ID_INDIVIDUAL_ACCESS_FLAG 0x80 /* * Return values @@ -64,6 +68,7 @@ #define MODBUS_ERROR_OUT_OF_BOUNDS -4 // requested register is out of bounds #define MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED -5 // function not implemented in callback #define MODBUS_ERROR_REGISTER_NOT_IMPLEMENTED -6 // register not implemented in callback +#define MODBUS_ERROR_DEVICE_ID_NOT_IMPLEMENTED -7 /* * Data types @@ -115,19 +120,18 @@ typedef enum { MODBUS_EXCEPTION_MEMORY_PARITY_ERROR = 8, MODBUS_EXCEPTION_GATEWAY_PATH_UNAVAILABLE = 10, MODBUS_EXCEPTION_GATEWAY_TARGET_DEVICE_FAILED_TO_RESPOND = 11, + MODBUS_EXCEPTION_ILLEGAL_DEVICE_ID_CODE = 3 } modbus_exception_code_t; -typedef struct { - uint8_t exception_code; -} exception_t; - typedef struct { modbus_function_code_t function_code : 8; uint16_t register_address; // e.g. first register of A0: 0 uint16_t register_number; // e.g. first register of A0: 40001 uint8_t register_count; // number of registers to be read/written - exception_t exception; + modbus_exception_code_t exception; + + uint8_t broadcast; // 1 if broadcast, 0 otherwise union { uint8_t buffer8b[MODBUS_MAX_RTU_FRAME_SIZE]; @@ -150,6 +154,34 @@ typedef enum { MODBUS_AO_END_NUMBER = 49999 } modbus_register_number_t; +typedef enum { + MODBUS_CONFORMITY_BASIC = 1, + MODBUS_CONFORMITY_REGULAR = 2, + MODBUS_CONFORMITY_EXTENDED = 3, + MODBUS_INDIVIDUAL_ACCESS = 4 +} modbus_conformity_level_t; + +/* Device ID datatypes */ +#define MODBUS_DEVICE_ID_OBJECT_NUM 7 +typedef struct { + union { + struct { + /* Basic category (mandatory part) */ + char *VendorName; + char *ProductCode; + char *MajorMinorRevision; + /* Regular category (optional part) */ + char *VendorUrl; + char *ProductName; + char *ModelName; + char *UserApplicationName; + /* Extended category (optional part) */ + // Nothing here yet! + } object_name; + char *object_id[MODBUS_DEVICE_ID_OBJECT_NUM]; + }; + uint8_t conformity_level; +} modbus_device_id_t; /* * Global variables @@ -161,6 +193,9 @@ extern uint8_t modbus_slave_address; /* shared modbus buffer; defined in modbus.c; may be used elsewhere in code */ extern uint8_t modbus_buffer[]; +/* modbus device id struct */ +extern modbus_device_id_t *modbus_device_id; + /* * Function prototypes */ @@ -172,6 +207,7 @@ extern uint8_t modbus_buffer[]; * Both functions have to be implemented by user. */ int8_t modbus_slave_process_msg(const uint8_t *buffer, int len); +int8_t modbus_slave_init_device_id(modbus_device_id_t *device_id); int8_t modbus_slave_set_address(uint8_t address); /* modbus callback function type - should be implemented by user (e.g. in main.c) */ int8_t modbus_slave_callback(modbus_transaction_t *transaction); diff --git a/fw/Core/Src/main.c b/fw/Core/Src/main.c index 0eaef87..05f3a25 100644 --- a/fw/Core/Src/main.c +++ b/fw/Core/Src/main.c @@ -108,6 +108,8 @@ uint16_t sps30_measured_data[10]; /* Struct to store the sensor config */ config_t sensor_config; +/* Device ID struct */ +modbus_device_id_t device_id; uint8_t sensor_config_pending_write = 0; uint8_t baudrate_changed = 0; uint8_t modbus_address_changed = 0; @@ -228,6 +230,16 @@ int main(void) rgbled_set_intensity(sensor_config.led_brightness); rgbled_set_color(RGBLED_WHITE); /* white color indicates init process */ + /* Fill device ID struct */ + device_id.object_name.VendorName = "NeoMokoshTron Corp."; + device_id.object_name.ProductCode = "124C41"; + device_id.object_name.MajorMinorRevision = "1.2"; + device_id.object_name.VendorUrl = "https://neomokoshtron.com"; + device_id.object_name.ProductName = "SensCO2"; + device_id.object_name.ModelName = "Hugo"; + modbus_slave_init_device_id(&device_id); + + LL_mDelay(2000); scd4x_start_periodic_measurement(); diff --git a/fw/Core/Src/modbus.c b/fw/Core/Src/modbus.c index 4ec6e09..648e865 100644 --- a/fw/Core/Src/modbus.c +++ b/fw/Core/Src/modbus.c @@ -16,9 +16,12 @@ * during execution of modbus_process_message() */ uint8_t modbus_buffer[MODBUS_MAX_RTU_FRAME_SIZE]; -/* device address: declared */ +/* MODBUS device address */ uint8_t modbus_slave_address = MODBUS_DEFAULT_SLAVE_ADDRESS; +/* Device ID struct */ +modbus_device_id_t *modbus_device_id = NULL; + /* * CRC16 functions * see https://modbus.org/docs/Modbus_over_serial_line_V1_02.pdf @@ -53,7 +56,7 @@ uint16_t modbus_CRC16(const uint8_t *buf, int len) /* here we assume buffer has minimal size of MODBUS_MAX_RTU_FRAME_SIZE; * this function is private, so hopefully it's going to be ok */ -int8_t modbus_copy_reply_to_buffer(uint8_t *buffer, uint8_t *msg_len, modbus_transaction_t *transaction) +static int8_t modbus_transaction_to_buffer(uint8_t *buffer, uint8_t *msg_len, modbus_transaction_t *transaction) { uint16_t crc16; uint8_t byte_count; @@ -65,7 +68,7 @@ int8_t modbus_copy_reply_to_buffer(uint8_t *buffer, uint8_t *msg_len, modbus_tra if (transaction->function_code & MODBUS_ERROR_FLAG) { /* sending error reply */ - buffer[2] = transaction->exception.exception_code; + buffer[2] = transaction->exception; } else { switch (transaction->function_code) { case MODBUS_READ_HOLDING_REGISTERS: @@ -101,6 +104,169 @@ int8_t modbus_copy_reply_to_buffer(uint8_t *buffer, uint8_t *msg_len, modbus_tra buffer[*msg_len - 1] = crc16 >> 8; } +static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, modbus_transaction_t *transaction) +{ + uint8_t MEI_type; + uint8_t read_device_id_code; + uint8_t object_id; + uint8_t conformity_masked; + uint8_t individual_object_access; + uint8_t buffer_pos = 0; + + if (transaction->broadcast == 1) { + /* Read device ID broadcast - invalid; ignore (master will get timeout) */ + return MODBUS_ERROR; + } + if (modbus_device_id == NULL) { + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DEVICE_ID_CODE; + return MODBUS_OK; + } + if (len < MODBUS_READ_DEVICE_ID_REQUEST_LEN) { + /* frame too short, ignore */ + return MODBUS_ERROR; + } + /* next byte should be MEI = 0x0E */ + MEI_type = buffer[buffer_pos++]; + if (MEI_type != MODBUS_MEI) { + /* invalid MEI, ignore. I have no idea what this MEI does, but it should always be 0x0E */ + return MODBUS_ERROR; + } + /* next byte is read device id code */ + read_device_id_code = buffer[buffer_pos++]; + /* read device id code can only have values 1,2,3,4 */ + if (read_device_id_code < 1 || read_device_id_code > 4) { + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DEVICE_ID_CODE; + return MODBUS_OK; + } + /* cross-check with conformity level (mask 0x80 bit, which only specified individual access) */ + if (read_device_id_code == MODBUS_INDIVIDUAL_ACCESS) { + individual_object_access = 1; + } else { + /* check conformity level */ + conformity_masked = modbus_device_id->conformity_level & ~0x80; + if (read_device_id_code > conformity_masked) { + /* requested conformity higher than allowed */ + read_device_id_code = conformity_masked; + } + } + /* next byte is object id */ + object_id = buffer[buffer_pos++]; + if (object_id > MODBUS_DEVICE_ID_OBJECT_NUM) { + /* illegal object ID */ + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS; + return MODBUS_OK; + } + /* Message processed; create reply message */ +} + +/* returns ERROR only when no response to master is needed */ +static int8_t modbus_process_read_write_request(const uint8_t *buffer, int len, modbus_transaction_t *transaction) +{ + uint8_t byte_count; + int8_t callback_result; + uint8_t buffer_pos = 0; + + /* set starting register number */ + switch (transaction->function_code) { + /* coils */ + case MODBUS_READ_DO: + case MODBUS_WRITE_SINGLE_DO: + case MODBUS_WRITE_MULTIPLE_DO: + transaction->register_number = MODBUS_DO_START_NUMBER; + break; + /* discrete inputs */ + case MODBUS_READ_DI: + transaction->register_number = MODBUS_DI_START_NUMBER; + break; + /* input registers */ + case MODBUS_READ_AI: + transaction->register_number = MODBUS_AI_START_NUMBER; + break; + /* holding registers */ + case MODBUS_READ_AO: + case MODBUS_WRITE_SINGLE_AO: + case MODBUS_WRITE_MULTIPLE_AO: + case MODBUS_READ_WRITE_MULTIPLE_REGISTERS: + transaction->register_number = MODBUS_AO_START_NUMBER; + break; + } + + #define MODBUS_FLAG_WRITE 0x01 + #define MODBUS_FLAG_SINGLE 0x02 + uint8_t flags = 0x00; + + /* process message */ + switch (transaction->function_code) { + case MODBUS_WRITE_SINGLE_COIL: + case MODBUS_WRITE_SINGLE_REGISTER: /* holding register */ + flags |= MODBUS_FLAG_SINGLE; + case MODBUS_WRITE_MULTIPLE_COILS: + case MODBUS_WRITE_MULTIPLE_REGISTERS: + flags |= MODBUS_FLAG_WRITE; + case MODBUS_READ_DISCRETE_INPUTS: + case MODBUS_READ_COILS: + case MODBUS_READ_INPUT_REGISTERS: + case MODBUS_READ_HOLDING_REGISTERS: + if (len < (MODBUS_MINIMAL_FRAME_LEN + 2)) { + /* buffer too short to contain everything we need */ + return MODBUS_ERROR; + } + transaction->register_address = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + // TODO check length! + if (flags & MODBUS_FLAG_WRITE) { + if (flags & MODBUS_FLAG_SINGLE) { + transaction->holding_registers[0] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + } else { + /* Write multiple registers */ + transaction->register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + byte_count = buffer[buffer_pos++]; + if (transaction->register_count > 123 || 2*transaction->register_count != byte_count) { + /* Max number of register is defined by Modbus_Application_Protocol_V1_1b, section 6.12 */ + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_REGISTER_QUANTITY; + } else { + for (uint8_t i = 0; i < transaction->register_count; i++) { + transaction->holding_registers[i] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + } + } + } + } else { + transaction->register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + if ( + transaction->register_count < 1 || + transaction->register_count > MODBUS_MAX_REGISTERS + ) { + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; + } + } + // add offset to register number + transaction->register_number += transaction->register_address; + break; + default: + /* function code not known / not implemented, reply with + * ExceptionCode 1 */ + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_FUNCTION; + break; + } + /* data in modbus_buffer have been processed and buffer can be re-used for TX */ + /* handle reply */ + if (transaction->exception != 0) { + /* indicate error */ + transaction->function_code |= MODBUS_ERROR_FLAG; + } else { + callback_result = modbus_slave_callback(transaction); + /* error handling */ + if (callback_result != MODBUS_OK) { + transaction->function_code |= MODBUS_ERROR_FLAG; + if (callback_result == MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED) { + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_FUNCTION; + } else if (callback_result == MODBUS_ERROR_REGISTER_NOT_IMPLEMENTED) { + transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS; + } + } + } + return MODBUS_OK; +} + /* * Public function definitions */ @@ -115,14 +281,19 @@ int8_t modbus_slave_set_address(uint8_t address) return MODBUS_OK; } + int8_t modbus_slave_process_msg(const uint8_t *buffer, int len) { + + /* * TODO list: * * 1) check that errors and exceptions are handled according to Modbus_Application_Protocol_V1_1b.pdf * 2) buffer overflow prevention: for each function code, check that buffer is long enough */ + + /* transaction holds message context and content: * it wraps all necessary buffers and variables */ modbus_transaction_t transaction; @@ -131,135 +302,64 @@ int8_t modbus_slave_process_msg(const uint8_t *buffer, int len) uint8_t byte_count; if (len < MODBUS_MINIMAL_FRAME_LEN) { - /* frame too short; return error */ + /* frame too short; return error (no reply needed) */ return MODBUS_ERROR_FRAME_INVALID; } /* check CRC first */ uint16_t crc_received = (buffer[len - 1] << 8) | buffer[len - 2]; uint16_t crc_calculated = modbus_CRC16(buffer, len - 2); if (crc_received != crc_calculated) { - /* CRC mismatch, return error */ - //printf("crc mismatch: received 0x%x, calculated 0x%x\n", crc_received, crc_calculated); + /* CRC mismatch, return error (no reply needed) */ return MODBUS_ERROR_CRC; } /* check if address matches ours */ uint8_t address = buffer[buffer_pos++]; - if (address != modbus_slave_address && address != MODBUS_BROADCAST_ADDR) { + transaction.broadcast = (address == MODBUS_BROADCAST_ADDR); + if (address != modbus_slave_address && transaction.broadcast != 1) { /* Message is not for us */ return MODBUS_OK; } /* get function code */ transaction.function_code = buffer[buffer_pos++]; - transaction.exception.exception_code = 0; - + transaction.exception = 0; + uint8_t request_processing_error; if (transaction.function_code == MODBUS_READ_DEVICE_IDENTIFICATION) { - // TODO - goto modbus_send; - } - - /* set starting register number */ - switch (transaction.function_code) { - /* coils */ - case MODBUS_READ_DO: - case MODBUS_WRITE_SINGLE_DO: - case MODBUS_WRITE_MULTIPLE_DO: - transaction.register_number = MODBUS_DO_START_NUMBER; - break; - /* discrete inputs */ - case MODBUS_READ_DI: - transaction.register_number = MODBUS_DI_START_NUMBER; - break; - /* input registers */ - case MODBUS_READ_AI: - transaction.register_number = MODBUS_AI_START_NUMBER; - break; - /* holding registers */ - case MODBUS_READ_AO: - case MODBUS_WRITE_SINGLE_AO: - case MODBUS_WRITE_MULTIPLE_AO: - case MODBUS_READ_WRITE_MULTIPLE_REGISTERS: - transaction.register_number = MODBUS_AO_START_NUMBER; - break; - } - - #define MODBUS_FLAG_WRITE 0x01 - #define MODBUS_FLAG_SINGLE 0x02 - uint8_t flags = 0x00; - - /* process message */ - switch (transaction.function_code) { - case MODBUS_WRITE_SINGLE_COIL: - case MODBUS_WRITE_SINGLE_REGISTER: /* holding register */ - flags |= MODBUS_FLAG_SINGLE; - case MODBUS_WRITE_MULTIPLE_COILS: - case MODBUS_WRITE_MULTIPLE_REGISTERS: - flags |= MODBUS_FLAG_WRITE; - case MODBUS_READ_DISCRETE_INPUTS: - case MODBUS_READ_COILS: - case MODBUS_READ_INPUT_REGISTERS: - case MODBUS_READ_HOLDING_REGISTERS: - if (len < (MODBUS_MINIMAL_FRAME_LEN + 4)) { - /* buffer too short to contain everything we need */ - return MODBUS_ERROR; - } - transaction.register_address = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; - // TODO check length! - if (flags & MODBUS_FLAG_WRITE) { - if (flags & MODBUS_FLAG_SINGLE) { - transaction.holding_registers[0] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; - } else { - /* Write multiple registers */ - transaction.register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; - byte_count = buffer[buffer_pos++]; - if (transaction.register_count > 123 || 2*transaction.register_count != byte_count) { - /* Max number of register is defined by Modbus_Application_Protocol_V1_1b, section 6.12 */ - transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_REGISTER_QUANTITY; - } else { - for (uint8_t i = 0; i < transaction.register_count; i++) { - transaction.holding_registers[i] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; - } - } - } - } else { - transaction.register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; - if ( - transaction.register_count < 1 || - transaction.register_count > MODBUS_MAX_REGISTERS - ) { - transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE; - } - } - // add offset to register number - transaction.register_number += transaction.register_address; - break; - default: - /* function code not known / not implemented, reply with - * ExceptionCode 1 */ - transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_FUNCTION; - break; - } - /* data in modbus_buffer have been processed and buffer can be re-used for TX */ - /* handle reply */ - if (transaction.exception.exception_code != 0) { - /* indicate error */ - transaction.function_code |= MODBUS_ERROR_FLAG; + request_processing_error = modbus_process_device_id_request(buffer + buffer_pos, len - buffer_pos, &transaction); } else { - callback_result = modbus_slave_callback(&transaction); - /* error handling */ - if (callback_result != MODBUS_OK) { - transaction.function_code |= MODBUS_ERROR_FLAG; - if (callback_result == MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED) { - transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_FUNCTION; - } else if (callback_result == MODBUS_ERROR_REGISTER_NOT_IMPLEMENTED) { - transaction.exception.exception_code = MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS; - } - } + request_processing_error = modbus_process_read_write_request(buffer + buffer_pos, len - buffer_pos, &transaction); } - uint8_t msg_len = 0; -modbus_send: - if (address != MODBUS_BROADCAST_ADDR) { - /* send only if master request was not broadcast */ - modbus_copy_reply_to_buffer(modbus_buffer, &msg_len, &transaction); + uint8_t msg_len; + /* reply only if request was processed successfully and message was not broadcast */ + if (request_processing_error == 0 && transaction.broadcast == 0) { + modbus_transaction_to_buffer(modbus_buffer, &msg_len, &transaction); modbus_transmit_function(modbus_buffer, msg_len); } } + +int8_t modbus_slave_init_device_id(modbus_device_id_t *device_id) +{ + if (device_id == NULL) { + return MODBUS_ERROR; + } + /* at least basic category objects have to be implemented */ + if ( device_id->object_name.VendorName == NULL || + device_id->object_name.ProductCode == NULL || + device_id->object_name.MajorMinorRevision == NULL + ) { + return MODBUS_ERROR; + } + /* set conformity level: currently only "basic" and "regular" is implemented */ + if ( device_id->object_id[3] != NULL && + device_id->object_id[4] != NULL && + device_id->object_id[5] != NULL + + ) { + /* strings are present in regular category (optional) */ + device_id->conformity_level = MODBUS_CONFORMITY_REGULAR; + } else { + device_id->conformity_level = MODBUS_CONFORMITY_BASIC; + } + /* we support both stream and individual access to objects */ + device_id->conformity_level |= MODBUS_DEVICE_ID_INDIVIDUAL_ACCESS_FLAG; + modbus_device_id = device_id; +}