1 #include "modbus.hpp"
2
3 #include "modbus_commands.hpp"
4
5 #include <termios.h>
6
7 #include <phosphor-logging/lg2.hpp>
8 #include <sdbusplus/async.hpp>
9
10 namespace phosphor::modbus::rtu
11 {
12
13 PHOSPHOR_LOG2_USING;
14
15 const std::unordered_map<int, speed_t> baudRateMap = {
16 {0, B0}, {50, B50}, {75, B75}, {110, B110},
17 {134, B134}, {150, B150}, {200, B200}, {300, B300},
18 {600, B600}, {1200, B1200}, {1800, B1800}, {2400, B2400},
19 {4800, B4800}, {9600, B9600}, {19200, B19200}, {38400, B38400},
20 {57600, B57600}, {115200, B115200}};
21
Modbus(sdbusplus::async::context & ctx,int fd,uint32_t baudRate,uint16_t rtsDelay)22 Modbus::Modbus(sdbusplus::async::context& ctx, int fd, uint32_t baudRate,
23 uint16_t rtsDelay) :
24 ctx(ctx), fd(fd), rtsDelay(rtsDelay), fdioInstance(ctx, fd)
25 {
26 if (!setProperties(baudRate, Parity::even))
27 {
28 throw std::runtime_error("Failed to set port properties");
29 }
30
31 info("Modbus created successfully");
32 }
33
applyParitySettings(Parity parity,termios & tty)34 static auto applyParitySettings(Parity parity, termios& tty) -> bool
35 {
36 switch (parity)
37 {
38 case Parity::none:
39 tty.c_cflag &= ~PARENB;
40 tty.c_iflag &= ~INPCK;
41 break;
42 case Parity::odd:
43 tty.c_cflag |= PARENB;
44 tty.c_cflag |= PARODD;
45 tty.c_iflag |= INPCK;
46 break;
47 case Parity::even:
48 tty.c_cflag |= PARENB;
49 tty.c_cflag &= ~PARODD;
50 tty.c_iflag |= INPCK;
51 break;
52 default:
53 error("Invalid parity");
54 return false;
55 }
56
57 return true;
58 }
59
setProperties(uint32_t inBaudRate,Parity inParity)60 auto Modbus::setProperties(uint32_t inBaudRate, Parity inParity) -> bool
61 {
62 if (inBaudRate == baudRate && inParity == parity)
63 {
64 return true;
65 }
66
67 termios tty;
68 if (tcgetattr(fd, &tty) != 0)
69 {
70 error("Error getting termios");
71 return false;
72 }
73
74 if (inBaudRate != baudRate)
75 {
76 if (cfsetspeed(&tty, baudRateMap.at(inBaudRate)) != 0)
77 {
78 error("Error setting baud rate");
79 return false;
80 }
81 }
82
83 if (inParity != parity)
84 {
85 if (!applyParitySettings(inParity, tty))
86 {
87 error("Invalid parity");
88 return false;
89 }
90 }
91
92 // TODO: We might not need these again.
93 tty.c_cflag |= CS8 | CLOCAL | CREAD;
94 // Set non-blocking read behavior
95 tty.c_cc[VMIN] = 1; // Minimum characters to read
96 tty.c_cc[VTIME] = 0; // Timeout in deciseconds (0 for no timeout)
97
98 if (tcsetattr(fd, TCSAFLUSH, &tty) != 0)
99 {
100 error("Error setting termios");
101 return false;
102 }
103
104 parity = inParity;
105 baudRate = inBaudRate;
106
107 debug("Properties set successfully");
108
109 return true;
110 }
111
printMessage(uint8_t * data,size_t len)112 static auto printMessage(uint8_t* data, size_t len) -> void
113 {
114 std::stringstream ss;
115 ss << std::hex << std::setfill('0');
116
117 for (size_t i = 0; i < len; ++i)
118 {
119 ss << std::setw(2) << static_cast<int>(data[i]) << " ";
120 }
121
122 debug("{MSG}", "MSG", ss.str());
123 }
124
readHoldingRegisters(uint8_t deviceAddress,uint16_t registerOffset,std::vector<uint16_t> & registers)125 auto Modbus::readHoldingRegisters(uint8_t deviceAddress,
126 uint16_t registerOffset,
127 std::vector<uint16_t>& registers)
128 -> sdbusplus::async::task<bool>
129 {
130 try
131 {
132 ReadHoldingRegistersRequest request(deviceAddress, registerOffset,
133 registers.size());
134 ReadHoldingRegistersResponse response(deviceAddress, registers);
135
136 request.encode();
137
138 debug(
139 "Sending read holding registers request for {REGISTER_OFFSET} {DEVICE_ADDRESS}",
140 "REGISTER_OFFSET", registerOffset, "DEVICE_ADDRESS", deviceAddress);
141
142 if (!co_await writeRequest(deviceAddress, request))
143 {
144 co_return false;
145 }
146
147 debug(
148 "Waiting for read holding registers response for {REGISTER_OFFSET} {DEVICE_ADDRESS}",
149 "REGISTER_OFFSET", registerOffset, "DEVICE_ADDRESS", deviceAddress);
150
151 if (!co_await readResponse(deviceAddress, response,
152 request.functionCode))
153 {
154 co_return false;
155 }
156
157 response.decode();
158 }
159 catch (std::exception& e)
160 {
161 error(
162 "Failed to read holding registers for {DEVICE_ADDRESS} with {ERROR}",
163 "DEVICE_ADDRESS", deviceAddress, "ERROR", e);
164 co_return false;
165 }
166
167 co_return true;
168 }
169
writeRequest(uint8_t deviceAddress,Message & request)170 auto Modbus::writeRequest(uint8_t deviceAddress, Message& request)
171 -> sdbusplus::async::task<bool>
172 {
173 printMessage(request.raw.data(), request.len);
174
175 // Flush the input & output buffers for the fd
176 tcflush(fd, TCIOFLUSH);
177 auto ret = write(fd, request.raw.data(), request.len);
178 if ((size_t)ret != request.len)
179 {
180 error("Failed to send request to device {DEVICE_ADDRESS} with {ERROR}",
181 "DEVICE_ADDRESS", deviceAddress, "ERROR", strerror(errno));
182 co_return false;
183 }
184
185 co_return true;
186 }
187
readResponse(uint8_t deviceAddress,Message & response,uint8_t expectedResponseCode)188 auto Modbus::readResponse(uint8_t deviceAddress, Message& response,
189 uint8_t expectedResponseCode)
190 -> sdbusplus::async::task<bool>
191 {
192 int expectedLen = response.len;
193
194 do
195 {
196 debug("Waiting for response for {DEVICE_ADDRESS} with {EXPECTED} bytes",
197 "DEVICE_ADDRESS", deviceAddress, "EXPECTED", expectedLen);
198 co_await fdioInstance.next();
199 // TODO: Handle FD timeout in case of no response
200 auto ret = read(fd, response.raw.data() + response.len - expectedLen,
201 expectedLen);
202 if (ret < 0)
203 {
204 error(
205 "Failed to read response for device {DEVICE_ADDRESS} with {ERROR}",
206 "DEVICE_ADDRESS", deviceAddress, "ERROR", strerror(errno));
207 co_return false;
208 }
209
210 debug("Received response for {DEVICE_ADDRESS} with {SIZE}",
211 "DEVICE_ADDRESS", deviceAddress, "SIZE", ret);
212
213 printMessage(response.raw.data() + response.len - expectedLen, ret);
214
215 expectedLen -= ret;
216
217 if (expectedLen > 0)
218 {
219 debug(
220 "Read response for device {DEVICE_ADDRESS} with {EXPECTED} bytes remaining",
221 "DEVICE_ADDRESS", deviceAddress, "EXPECTED", expectedLen);
222 }
223 if (ret >= 2 && response.functionCode != expectedResponseCode)
224 {
225 // Update the length of the expected response to received error
226 // message size
227 response.len = ret;
228 error("Received error response {CODE} for device {DEVICE_ADDRESS}",
229 "CODE", response.raw[1], "DEVICE_ADDRESS", deviceAddress);
230 co_return false;
231 }
232 } while (expectedLen > 0);
233
234 if (rtsDelay)
235 {
236 // Asynchronously sleep for rts_delay milliseconds in case bus needs
237 // to be idle after each transaction
238 co_await sdbusplus::async::sleep_for(
239 ctx, std::chrono::milliseconds(rtsDelay));
240 }
241
242 co_return true;
243 }
244
245 } // namespace phosphor::modbus::rtu
246