diff options
Diffstat (limited to 'src/modbus.c')
| -rw-r--r-- | src/modbus.c | 163 |
1 files changed, 163 insertions, 0 deletions
diff --git a/src/modbus.c b/src/modbus.c new file mode 100644 index 0000000..98f40bd --- /dev/null +++ b/src/modbus.c @@ -0,0 +1,163 @@ + +#include "modbus.h" + +/** @ingroup util_crc16 + Processor-independent CRC-16 calculation. + + Polynomial: x^16 + x^15 + x^2 + 1 (0xA001)<br> + Initial value: 0xFFFF + + This CRC is normally used in disk-drive controllers. + + @param uint16_t crc (0x0000..0xFFFF) + @param uint8_t a (0x00..0xFF) + @return calculated CRC (0x0000..0xFFFF) +*/ +uint16_t crc16_update(uint16_t crc, uint8_t a) +{ + int i; + crc ^= a; + for (i = 0; i < 8; ++i) { + if (crc & 1) + crc = (crc >> 1) ^ 0xA001; + else + crc = (crc >> 1); + } + + return crc; +} + +void ClearResponseBuffer(modbus_slave_device *device) +{ + memset(device->response_buffer, 0, sizeof(device->response_buffer)); +} + +MB_StatusTypeDef ReadHoldingRegisters(modbus_slave_device *device, uint16_t reg_addr, int read_num) +{ + return modbus_transaction(read_holding_regs, device, reg_addr, read_num, 0); +} + +MB_StatusTypeDef ReadInputRegisters(modbus_slave_device *device, uint16_t reg_addr, int read_num) +{ + return modbus_transaction(read_input_regs, device, reg_addr, read_num, 0); +} + +MB_StatusTypeDef WriteSingleRegister(modbus_slave_device *device, uint16_t reg_addr, uint16_t write_val) +{ + return modbus_transaction(write_single_reg, device, reg_addr, 1, write_val); +} + +MB_StatusTypeDef modbus_transaction(uint8_t func, modbus_slave_device *device, + uint16_t rw_addr, int read_num, uint16_t write_value) +{ + uint8_t adu[256], adu_currsize=0; + MB_StatusTypeDef MB_status = MB_OK; + HAL_StatusTypeDef UART_status = HAL_OK; + + /* Add the values that are relevant to all (supported) functions to the buffer */ + adu[adu_currsize++] = device->slave_id; + adu[adu_currsize++] = func; + adu[adu_currsize++] = highByte(rw_addr); + adu[adu_currsize++] = lowByte(rw_addr); + + /* Put data in the buffer based on function code */ + switch (func) { + case read_holding_regs: + case read_input_regs: + adu[adu_currsize++] = highByte(read_num); + adu[adu_currsize++] = lowByte(read_num); + break; + case write_single_reg: + adu[adu_currsize++] = highByte(write_value); + adu[adu_currsize++] = lowByte(write_value); + } + + + /* Calculate and append the CRC16 to the buffer */ + uint16_t crc = 0xffff; + for (int i=0; i < adu_currsize; i++) { + crc = crc16_update(crc, adu[i]); + } + + adu[adu_currsize++] = lowByte(crc); + adu[adu_currsize++] = highByte(crc); + adu[adu_currsize] = 0; + + /* Send data using HAL UART */ + for (int i=0; i < adu_currsize; i++) { + UART_status = HAL_UART_Transmit(&device->modbus_uart, &adu[i], 1, MODBUS_TIMEOUT); + } + + + /* Reset the ADU currsize pointer in preperation of data RX */ + adu_currsize = 0; + + int rx_curr_bytesize = 8; + while (rx_curr_bytesize > 0 && MB_status == MB_OK && UART_status == HAL_OK) { + UART_status = HAL_UART_Receive(&device->modbus_uart, &adu[adu_currsize++], + 1, MODBUS_TIMEOUT); + if (UART_status == HAL_OK) { + rx_curr_bytesize--; + if (adu_currsize == 5) { + if (adu[0] != device->slave_id) { + /* Return invalid slave ID error */ + MB_status = MB_SLAVE_ID_ERR; + break; + } + if ((adu[1] & 0x7f) != func) { + /* Return invalid function error */ + MB_status = MB_FUNC_ERR; + break; + } + + /* Need to read n more bytes based on the function code */ + switch (adu[1]) { + case read_holding_regs: + case read_input_regs: + rx_curr_bytesize = adu[2]; + break; + case write_single_reg: + rx_curr_bytesize = 3; + break; + } + } + } + else { + MB_status = UART_status + 3; + } + } + + if (adu_currsize > 5 && MB_status == MB_OK && UART_status == HAL_OK) { + crc = 0xffff; + for (int i=0; i < adu_currsize - 2; i++) { + crc = crc16_update(crc, adu[i]); + } + if((lowByte(crc) != adu[adu_currsize - 2] || + highByte(crc) != adu[adu_currsize - 1])) { + /* Invalid CRC error */ + MB_status = MB_CRC_ERR; + } + } + + /* For read operations: disassemble ADU into words */ + if (MB_status == MB_OK && UART_status == HAL_OK) { + switch (adu[1]) { + case read_input_regs: + case read_holding_regs: + for (int i=0; i < (adu[2] >> 1); i++) { + if (i < MAX_BUFSIZE) { + device->response_buffer[i] = word(adu[2*i+3], adu[2*i+4]); + } + device->rbuf_len = i; + } + break; + } + } + + if (!MB_status && UART_status) { + MB_status = UART_status + 3; + } + + return MB_status; +} + |