1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
#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)
{
uint16_t adu[256], adu_currsize=0;
MB_StatusTypeDef MB_status = MB_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 (uint8_t i=0; i < adu_currsize; i++) {
MB_status = MB_UART_Tx(&device->modbus_uart, &adu[i], (uint8_t*)1, &device->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) {
MB_status = MB_UART_Rx(&device->modbus_uart, &adu[adu_currsize++],
(uint8_t*)1, &device->modbus_timeout);
if (MB_status == MB_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;
}
}
}
}
if (adu_currsize > 5 && MB_status == MB_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) {
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;
}
}
return MB_status;
}
|
