SP8EBC-ParaTNC/system/src/modbus_rtu/rtu_parser.c

89 wiersze
2.2 KiB
C

/*
* rtu_parser.c
*
* Created on: 27.08.2020
* Author: mateusz
*/
#include "./modbus_rtu/rtu_parser.h"
#include "./modbus_rtu/rtu_return_values.h"
#define MODBUS_RTU_MIN_03_04_RESP_LN 7 // one register to read
int32_t rtu_parser_03_04_registers(uint8_t* input, uint16_t input_ln, rtu_register_data_t* output, rtu_exception_t* exception) {
uint32_t retval = MODBUS_RET_UNINITIALIZED;
uint16_t data = 0;
uint8_t slave_address_from_frame = 0;
// iterator through input table and registers table
int i = 0, j = 3;
// rewind the input buffer if the first byte is not valid modbus rtu slave address
if (*input < 1 || *input > 0xF7) {
// don't loop here and assume that the modbus response is shifted only
// by one byte
input++;
}
// 7 bytes is the shortest meaningful Modbus RTU frame
// with a value of single register
if (input_ln < MODBUS_RTU_MIN_03_04_RESP_LN) {
retval = MODBUS_RET_TOO_SHORT;
}
else {
// fetch slave address
data = *input;
// TODO: store slave address
slave_address_from_frame = data;
// fetch function code
data = *(input + 1);
// if the exception flag is set
if ((data & 0x80) > 0) {
// parse the exception value
*exception = rtu_exception_from_frame_data(data);
// and set the return value
retval = MODBUS_RET_GOT_EXCEPTION;
}
// check if the function code is correct or not
else if (data == 0x03 || data == 0x04) {
// check if this is an answer from the slave we expect
if (slave_address_from_frame == output->slave_address) {
// fetch the function result lenght in bytes
data = *(input + 2);
// store amount of registers in this response
output->number_of_registers = data / 2;
// get all registers values
for (int i = 0; i < output->number_of_registers && i < MODBUS_RTU_MAX_REGISTERS_AT_ONCE; i++) {
output->registers_values[i] = *(input + j) << 8 | *(input + j + 1);
// moving to next 16bit word with next register
j += 2;
}
retval = MODBUS_RET_OK;
}
else {
retval = MODBUS_RET_UNEXP_SLAVE_ADR;
}
}
else {
// if not exit with an error as this isn't
// correct parser for this modbus function
retval = MODBUS_RET_WRONG_FUNCTION;
}
}
return retval;
}