diff --git a/fw/Core/Inc/modbus.h b/fw/Core/Inc/modbus.h index 9003d71..c14302c 100644 --- a/fw/Core/Inc/modbus.h +++ b/fw/Core/Inc/modbus.h @@ -40,6 +40,7 @@ #include "stdint.h" #include "stddef.h" +#include "string.h" /* * Defines & macros @@ -49,13 +50,22 @@ #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_MINIMAL_READWRITE_LEN 4 +#define MODBUS_MINIMAL_WRITE_MULTIPLE_LEN 5 #define MODBUS_READ_DEVICE_ID_REQUEST_LEN 4 +#define MODBUS_READ_DEVICE_ID_RESPONSE_HEADER_LEN 4 +#define MODBUS_READ_DEVICE_ID_RESPONSE_OFFSET 3 #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 +/* read device id constants */ #define MODBUS_MEI 0x0E #define MODBUS_DEVICE_ID_INDIVIDUAL_ACCESS_FLAG 0x80 +#define MODBUS_MORE_FOLLOWS 0xFF +#define MODBUS_NO_MORE_FOLLOWS 0x00 +#define MODBUS_BASIC_OBJECT_COUNT 3 +#define MODBUS_REGULAR_OBJECT_COUNT 7 /* * Return values @@ -141,6 +151,10 @@ typedef struct { int16_t input_registers_signed[MODBUS_MAX_REGISTERS]; int16_t holding_registers_signed[MODBUS_MAX_REGISTERS]; }; + + /* process device id */ + uint8_t read_device_id_code; + uint8_t object_id; } modbus_transaction_t; typedef enum { @@ -158,7 +172,7 @@ typedef enum { MODBUS_CONFORMITY_BASIC = 1, MODBUS_CONFORMITY_REGULAR = 2, MODBUS_CONFORMITY_EXTENDED = 3, - MODBUS_INDIVIDUAL_ACCESS = 4 + MODBUS_INDIVIDUAL_ACCESS = 4 /* not actually part of conformity, but I'll keep it here anyway */ } modbus_conformity_level_t; /* Device ID datatypes */ diff --git a/fw/Core/Src/main.c b/fw/Core/Src/main.c index 05f3a25..f2e0ecc 100644 --- a/fw/Core/Src/main.c +++ b/fw/Core/Src/main.c @@ -231,10 +231,10 @@ int main(void) 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.VendorName = "Veles Labs"; 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.VendorUrl = "https://veles-labs.com"; device_id.object_name.ProductName = "SensCO2"; device_id.object_name.ModelName = "Hugo"; modbus_slave_init_device_id(&device_id); diff --git a/fw/Core/Src/modbus.c b/fw/Core/Src/modbus.c index 648e865..d19f1ea 100644 --- a/fw/Core/Src/modbus.c +++ b/fw/Core/Src/modbus.c @@ -54,54 +54,114 @@ uint16_t modbus_CRC16(const uint8_t *buf, int len) * Private functions */ +static uint8_t modbus_fill_device_id_objects(uint8_t *buffer, modbus_transaction_t *transaction) +{ + /* we assume buffer is 256 - MODBUS_READ_DEVICE_ID_RESPONSE_HEADER_LEN = 252 bytes long */ + /* find out how many objects we copy to buffer */ + int len; + uint8_t object_index = transaction->object_id; + uint8_t object_count; + uint8_t more_follows = MODBUS_NO_MORE_FOLLOWS; + uint8_t next_object_id; + uint8_t last_object; + const uint8_t max_len = 256 - MODBUS_READ_DEVICE_ID_RESPONSE_HEADER_LEN; + + /* last object index */ + if (transaction->read_device_id_code == MODBUS_CONFORMITY_BASIC) { + last_object = MODBUS_BASIC_OBJECT_COUNT; + } else if (transaction->read_device_id_code == MODBUS_CONFORMITY_REGULAR) { + last_object = MODBUS_REGULAR_OBJECT_COUNT; + /* extended not implemented */ +// } else if (transaction->read_device_id_code == MODBUS_CONFORMITY_EXTENDED){ +// last_object = MODBUS_EXTENDED_OBJECT_COUNT; + } else { + /* fallback: regular */ + last_object = MODBUS_REGULAR_OBJECT_COUNT; + } + last_object--; // we need index + /* copy as many objects as possible */ + do { + /* copy object */ + int object_len = strlen(modbus_device_id->object_id[object_index]); + if (len + object_len + 2 > max_len) { + more_follows = MODBUS_MORE_FOLLOWS; + next_object_id = object_index; + break; + } + /* offset is for "more follows", "next object id", "object count" */ + buffer[MODBUS_READ_DEVICE_ID_RESPONSE_OFFSET + len++] = object_index; + buffer[MODBUS_READ_DEVICE_ID_RESPONSE_OFFSET + len++] = object_len; + /* note that string copied to buffer is not null-terminated */ + strncpy((char*)(buffer + len), (char*)modbus_device_id->object_id[object_index++], object_len); + len += object_len; + object_count++; + } while (object_index < last_object); + buffer[0] = more_follows; + buffer[1] = next_object_id; + buffer[2] = object_count; + return MODBUS_READ_DEVICE_ID_RESPONSE_OFFSET + 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 */ 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; + uint8_t buffer_pos = 0; // TODO use relative indices (increments) instead of absolute - buffer[0] = modbus_slave_address; - buffer[1] = transaction->function_code; + buffer[buffer_pos++] = modbus_slave_address; + buffer[buffer_pos++] = transaction->function_code; *msg_len = 5; if (transaction->function_code & MODBUS_ERROR_FLAG) { /* sending error reply */ - buffer[2] = transaction->exception; + buffer[buffer_pos++] = transaction->exception; } else { switch (transaction->function_code) { case MODBUS_READ_HOLDING_REGISTERS: case MODBUS_READ_INPUT_REGISTERS: byte_count = transaction->register_count * 2; - buffer[2] = byte_count; + buffer[buffer_pos++] = byte_count; *msg_len = byte_count + 5; for (int i = 0; i < transaction->register_count; i++) { // TODO endianness handling /* buffer16b is alias for both holding and input register buffers */ - buffer[3 + 2*i] = transaction->buffer16b[i] >> 8; - buffer[4 + 2*i] = transaction->buffer16b[i] & 0xff; + buffer[buffer_pos++] = transaction->buffer16b[i] >> 8; + buffer[buffer_pos++] = transaction->buffer16b[i] & 0xff; } break; case MODBUS_WRITE_SINGLE_REGISTER: - buffer[2] = (uint8_t) (transaction->register_address >> 8); - buffer[3] = (uint8_t) transaction->register_address; - buffer[4] = (uint8_t) (transaction->holding_registers[0] >> 8); - buffer[5] = (uint8_t) transaction->holding_registers[0]; - *msg_len = 8; + buffer[buffer_pos++] = (uint8_t) (transaction->register_address >> 8); + buffer[buffer_pos++] = (uint8_t) transaction->register_address; + buffer[buffer_pos++] = (uint8_t) (transaction->holding_registers[0] >> 8); + buffer[buffer_pos++] = (uint8_t) transaction->holding_registers[0]; + *msg_len = 8; /* includes 2 bytes for CRC */ break; case MODBUS_WRITE_MULTIPLE_REGISTERS: - buffer[2] = (uint8_t) (transaction->register_address >> 8); - buffer[3] = (uint8_t) transaction->register_address; - buffer[4] = (uint8_t) (transaction->register_count >> 8); - buffer[5] = (uint8_t) transaction->register_count; - *msg_len = 8; + buffer[buffer_pos++] = (uint8_t) (transaction->register_address >> 8); + buffer[buffer_pos++] = (uint8_t) transaction->register_address; + buffer[buffer_pos++] = (uint8_t) (transaction->register_count >> 8); + buffer[buffer_pos++] = (uint8_t) transaction->register_count; + *msg_len = 8; /* includes 2 bytes for CRC */ + break; + case MODBUS_READ_DEVICE_IDENTIFICATION: + /* MEI type */ + buffer[buffer_pos++] = MODBUS_MEI; + /* read device id */ + buffer[buffer_pos++] = transaction->read_device_id_code; + /* conformity level */ + buffer[buffer_pos++] = modbus_device_id->conformity_level; + /* fill buffer with as many objects as possible */ + *msg_len = modbus_fill_device_id_objects(buffer+buffer_pos, transaction); + *msg_len += 7; /* includes 2 bytes for CRC */ break; } } - crc16 = modbus_CRC16(buffer, *msg_len - 2); /* last two bytes is the checksum itself */ - buffer[*msg_len - 2] = crc16 & 0xff; - buffer[*msg_len - 1] = crc16 >> 8; + crc16 = modbus_CRC16(buffer, buffer_pos); /* last two bytes is the checksum itself */ + buffer[buffer_pos++] = crc16 & 0xff; + buffer[buffer_pos++] = crc16 >> 8; } static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, modbus_transaction_t *transaction) @@ -118,6 +178,7 @@ static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, m return MODBUS_ERROR; } if (modbus_device_id == NULL) { + /* modbus_device_id not initialized; user should use modbus_slave_init_device_id() first */ transaction->exception = MODBUS_EXCEPTION_ILLEGAL_DEVICE_ID_CODE; return MODBUS_OK; } @@ -128,7 +189,7 @@ static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, m /* 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 */ + /* invalid MEI, ignore. I have no idea what MEI does, but it should always be 0x0E */ return MODBUS_ERROR; } /* next byte is read device id code */ @@ -138,25 +199,20 @@ static int8_t modbus_process_device_id_request(const uint8_t *buffer, int len, m 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; - } } + transaction->read_device_id_code = read_device_id_code; /* next byte is object id */ object_id = buffer[buffer_pos++]; + transaction->object_id = object_id; 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 */ + /* Message processed */ + return MODBUS_OK; } /* returns ERROR only when no response to master is needed */ @@ -207,7 +263,7 @@ static int8_t modbus_process_read_write_request(const uint8_t *buffer, int len, case MODBUS_READ_COILS: case MODBUS_READ_INPUT_REGISTERS: case MODBUS_READ_HOLDING_REGISTERS: - if (len < (MODBUS_MINIMAL_FRAME_LEN + 2)) { + if (len < MODBUS_MINIMAL_READWRITE_LEN) { /* buffer too short to contain everything we need */ return MODBUS_ERROR; } @@ -219,11 +275,17 @@ static int8_t modbus_process_read_write_request(const uint8_t *buffer, int len, } else { /* Write multiple registers */ transaction->register_count = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; + if (len < MODBUS_MINIMAL_WRITE_MULTIPLE_LEN) { + return MODBUS_ERROR; + } 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 { + if (len < MODBUS_MINIMAL_WRITE_MULTIPLE_LEN + byte_count) { + return MODBUS_ERROR; + } for (uint8_t i = 0; i < transaction->register_count; i++) { transaction->holding_registers[i] = (buffer[buffer_pos++] << 8) | buffer[buffer_pos++]; } @@ -297,9 +359,7 @@ int8_t modbus_slave_process_msg(const uint8_t *buffer, int len) /* transaction holds message context and content: * it wraps all necessary buffers and variables */ modbus_transaction_t transaction; - int8_t callback_result; uint8_t buffer_pos = 0; - uint8_t byte_count; if (len < MODBUS_MINIMAL_FRAME_LEN) { /* frame too short; return error (no reply needed) */ @@ -316,24 +376,28 @@ int8_t modbus_slave_process_msg(const uint8_t *buffer, int len) uint8_t address = buffer[buffer_pos++]; transaction.broadcast = (address == MODBUS_BROADCAST_ADDR); if (address != modbus_slave_address && transaction.broadcast != 1) { - /* Message is not for us */ + /* Message is not for us (no reply needed) */ return MODBUS_OK; } /* get function code */ transaction.function_code = buffer[buffer_pos++]; transaction.exception = 0; - uint8_t request_processing_error; + uint8_t request_processing_result; if (transaction.function_code == MODBUS_READ_DEVICE_IDENTIFICATION) { - request_processing_error = modbus_process_device_id_request(buffer + buffer_pos, len - buffer_pos, &transaction); + /* Read device ID request is quite complicated, therefore it has its own processing function */ + request_processing_result = modbus_process_device_id_request(buffer + buffer_pos, len - buffer_pos, &transaction); } else { - request_processing_error = modbus_process_read_write_request(buffer + buffer_pos, len - buffer_pos, &transaction); + /* process other requests: input register read, holding register read/write */ + request_processing_result = modbus_process_read_write_request(buffer + buffer_pos, len - buffer_pos, &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) { + if (request_processing_result == MODBUS_OK && transaction.broadcast == 0) { modbus_transaction_to_buffer(modbus_buffer, &msg_len, &transaction); + /* send reply */ modbus_transmit_function(modbus_buffer, msg_len); } + return MODBUS_OK; } int8_t modbus_slave_init_device_id(modbus_device_id_t *device_id) @@ -362,4 +426,5 @@ int8_t modbus_slave_init_device_id(modbus_device_id_t *device_id) /* we support both stream and individual access to objects */ device_id->conformity_level |= MODBUS_DEVICE_ID_INDIVIDUAL_ACCESS_FLAG; modbus_device_id = device_id; + return MODBUS_OK; }