142 lines
5.2 KiB
C
142 lines
5.2 KiB
C
/*******************************************************
|
|
*
|
|
* Implementation reference:
|
|
*
|
|
* http://www.mayor.de/lian98/doc.en/html/u_mbusser-rtu_struct.htm
|
|
* https://www.modbustools.com/modbus.html
|
|
*
|
|
* Example telegrams:
|
|
* Read holding registers: 010300000002c40b (read 2 registers starting from 40001)
|
|
* Read holding registers: 0103000000044409 (read 4 registers starting from 40001)
|
|
*
|
|
* Write single register: 01060000007bc9e9 (write "123" to register 40001)
|
|
*
|
|
* Write multiple registers: 01100000000204007b0237c300
|
|
*
|
|
*******************************************************/
|
|
|
|
#include "modbus_lib_types.h"
|
|
#include "modbus_lib.h"
|
|
#include "port.h"
|
|
#include "modbus_crc.h"
|
|
|
|
uint8_t g_modbus_lib_received_telegram[MODBUS_LIB_MAX_BUFFER];
|
|
uint16_t g_modbus_lib_received_length = 0;
|
|
uint8_t g_modbus_lib_exception_occurred = 0;
|
|
ModbusConfig_t* config;
|
|
|
|
int modbus_lib_init(ModbusConfig_t* cfg){
|
|
config = cfg;
|
|
return 0;
|
|
}
|
|
|
|
void modbus_lib_append_data(uint8_t byte){
|
|
if (g_modbus_lib_received_length < MODBUS_LIB_MAX_BUFFER){
|
|
g_modbus_lib_received_telegram[g_modbus_lib_received_length++] = byte;
|
|
}
|
|
}
|
|
|
|
void modbus_lib_end_of_telegram(){
|
|
(void) 0; // debugger: p/x *g_modbus_lib_received_telegram@g_modbus_lib_received_length
|
|
|
|
// Check length
|
|
if (g_modbus_lib_received_length < MODBUS_LIB_MIN_TELEGRAM_SIZE){
|
|
modbus_lib_send_error(MBUS_RESPONSE_NONE);
|
|
return;
|
|
}
|
|
|
|
// Check CRC
|
|
CRC_t expected = usMBCRC16(g_modbus_lib_received_telegram, g_modbus_lib_received_length-2);
|
|
UCHAR got_low = g_modbus_lib_received_telegram[g_modbus_lib_received_length-2];
|
|
UCHAR got_high = g_modbus_lib_received_telegram[g_modbus_lib_received_length-1];
|
|
if ((expected.bytes.low != got_low) || (expected.bytes.high != got_high)){
|
|
modbus_lib_send_error(MBUS_RESPONSE_NONE);
|
|
return;
|
|
}
|
|
|
|
// Check address
|
|
if (config->address != g_modbus_lib_received_telegram[0]){
|
|
modbus_lib_send_error(MBUS_RESPONSE_NONE);
|
|
return;
|
|
}
|
|
|
|
// Telegram is okay, call the relevant handler
|
|
// -------------------------------------------
|
|
uint8_t outgoing_telegram[MODBUS_LIB_MAX_BUFFER];
|
|
uint16_t oindex = 0;
|
|
|
|
outgoing_telegram[oindex++] = config->address;
|
|
|
|
volatile MbDataField start_addr, count, res, addr, value;
|
|
|
|
switch (g_modbus_lib_received_telegram[1]){
|
|
case MB_FUNC_READ_HOLDING_REGISTERS:
|
|
start_addr.bytes.high = g_modbus_lib_received_telegram[2];
|
|
start_addr.bytes.low = g_modbus_lib_received_telegram[3];
|
|
count.bytes.high = g_modbus_lib_received_telegram[4];
|
|
count.bytes.low = g_modbus_lib_received_telegram[5];
|
|
|
|
outgoing_telegram[oindex++] = g_modbus_lib_received_telegram[1]; // function code
|
|
outgoing_telegram[oindex++] = count.value * 2; // byte count
|
|
|
|
for (uint16_t i=0; i < count.value; i++){
|
|
res.value = modbus_lib_read_handler(start_addr.value + i + MB_ADDRESS_HOLDING_REGISTER_OFFSET);
|
|
if (g_modbus_lib_exception_occurred){
|
|
g_modbus_lib_exception_occurred = 0;
|
|
return;
|
|
}
|
|
outgoing_telegram[oindex++] = res.bytes.high;
|
|
outgoing_telegram[oindex++] = res.bytes.low;
|
|
}
|
|
|
|
CRC_t crc = usMBCRC16(outgoing_telegram, oindex);
|
|
outgoing_telegram[oindex++] = crc.bytes.low;
|
|
outgoing_telegram[oindex++] = crc.bytes.high;
|
|
|
|
modbus_lib_transport_write(outgoing_telegram, oindex);
|
|
break;
|
|
case MB_FUNC_WRITE_REGISTER:
|
|
addr.bytes.high = g_modbus_lib_received_telegram[2];
|
|
addr.bytes.low = g_modbus_lib_received_telegram[3];
|
|
value.bytes.high = g_modbus_lib_received_telegram[4];
|
|
value.bytes.low = g_modbus_lib_received_telegram[5];
|
|
|
|
if (modbus_lib_write_handler(addr.value + MB_ADDRESS_HOLDING_REGISTER_OFFSET, value.value) == 0){
|
|
// success
|
|
// normal response is the echo of received telegram
|
|
modbus_lib_transport_write(g_modbus_lib_received_telegram, g_modbus_lib_received_length);
|
|
} else {
|
|
// error
|
|
g_modbus_lib_exception_occurred = 0;
|
|
}
|
|
break;
|
|
default:
|
|
// unimplemented
|
|
modbus_lib_send_error(MBUS_RESPONSE_SERVICE_DEVICE_FAILURE);
|
|
g_modbus_lib_exception_occurred = 0;
|
|
return;
|
|
}
|
|
|
|
g_modbus_lib_received_length = 0;
|
|
}
|
|
|
|
#define MB_EXCEPTION_LENGTH 5
|
|
|
|
uint16_t modbus_lib_send_error(int error_code){
|
|
if (error_code != MBUS_RESPONSE_NONE){
|
|
g_modbus_lib_exception_occurred = 1;
|
|
uint8_t res[MB_EXCEPTION_LENGTH] = {
|
|
config->address,
|
|
g_modbus_lib_received_telegram[1] | 0x80,
|
|
error_code
|
|
};
|
|
CRC_t crc = usMBCRC16(res, MB_EXCEPTION_LENGTH - 2);
|
|
res[MB_EXCEPTION_LENGTH - 2] = crc.bytes.low;
|
|
res[MB_EXCEPTION_LENGTH - 1] = crc.bytes.high;
|
|
|
|
modbus_lib_transport_write(res, MB_EXCEPTION_LENGTH);
|
|
}
|
|
g_modbus_lib_received_length = 0;
|
|
return -1;
|
|
}
|