1 #include "modbus_message.hpp"
2
3 #include "modbus_exception.hpp"
4
5 #include <phosphor-logging/lg2.hpp>
6
7 namespace phosphor::modbus::rtu
8 {
9
10 PHOSPHOR_LOG2_USING;
11
operator <<(uint8_t d)12 Message& Message::operator<<(uint8_t d)
13 {
14 if ((len + 1) > raw.size())
15 {
16 throw std::overflow_error("Encoding Failed");
17 }
18 raw[len++] = d;
19 return *this;
20 }
21
operator <<(uint16_t d)22 Message& Message::operator<<(uint16_t d)
23 {
24 uint8_t upper = d >> 8, lower = d & 0xffff;
25 *this << upper; // Big-endian
26 *this << lower;
27 return *this;
28 }
29
operator <<(uint32_t d)30 Message& Message::operator<<(uint32_t d)
31 {
32 uint16_t upper = d >> 16, lower = d & 0xffff;
33 *this << upper; // Big-endian
34 *this << lower;
35 return *this;
36 }
37
operator >>(uint8_t & d)38 Message& Message::operator>>(uint8_t& d)
39 {
40 if (len < 1)
41 {
42 throw std::underflow_error("Decoding Failed");
43 }
44 d = raw[--len];
45 return *this;
46 }
47
operator >>(uint16_t & d)48 Message& Message::operator>>(uint16_t& d)
49 {
50 uint8_t upper, lower;
51 *this >> lower; // Big-endian
52 *this >> upper;
53 d = upper << 8 | lower;
54 return *this;
55 }
56
operator >>(uint32_t & d)57 Message& Message::operator>>(uint32_t& d)
58 {
59 uint16_t upper, lower;
60 *this >> lower; // Big-endian
61 *this >> upper;
62 d = upper << 16 | lower;
63 return *this;
64 }
65
appendCRC()66 auto Message::appendCRC() -> void
67 {
68 *this << generateCRC();
69 }
70
validate()71 auto Message::validate() -> void
72 {
73 uint16_t crc;
74 *this >> crc;
75 uint16_t expectedCRC = generateCRC();
76 if (expectedCRC != crc)
77 {
78 throw ModbusCRCException(expectedCRC, crc);
79 }
80 }
81
verifyValue(const std::string & name,uint32_t currentValue,uint32_t expectedValue)82 auto Message::verifyValue(const std::string& name, uint32_t currentValue,
83 uint32_t expectedValue) -> void
84 {
85 if (currentValue != expectedValue)
86 {
87 throw ModbusBadResponseException(name, expectedValue, currentValue);
88 }
89 }
90
91 static const uint16_t crc16Table[] = {
92 0x0, 0xc0c1, 0xc181, 0x140, 0xc301, 0x3c0, 0x280, 0xc241, 0xc601,
93 0x6c0, 0x780, 0xc741, 0x500, 0xc5c1, 0xc481, 0x440, 0xcc01, 0xcc0,
94 0xd80, 0xcd41, 0xf00, 0xcfc1, 0xce81, 0xe40, 0xa00, 0xcac1, 0xcb81,
95 0xb40, 0xc901, 0x9c0, 0x880, 0xc841, 0xd801, 0x18c0, 0x1980, 0xd941,
96 0x1b00, 0xdbc1, 0xda81, 0x1a40, 0x1e00, 0xdec1, 0xdf81, 0x1f40, 0xdd01,
97 0x1dc0, 0x1c80, 0xdc41, 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0,
98 0x1680, 0xd641, 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081,
99 0x1040, 0xf001, 0x30c0, 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240,
100 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501, 0x35c0, 0x3480, 0xf441, 0x3c00,
101 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41, 0xfa01, 0x3ac0,
102 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 0x2800, 0xe8c1, 0xe981,
103 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 0xee01, 0x2ec0, 0x2f80, 0xef41,
104 0x2d00, 0xedc1, 0xec81, 0x2c40, 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700,
105 0xe7c1, 0xe681, 0x2640, 0x2200, 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0,
106 0x2080, 0xe041, 0xa001, 0x60c0, 0x6180, 0xa141, 0x6300, 0xa3c1, 0xa281,
107 0x6240, 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480, 0xa441,
108 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 0xaa01,
109 0x6ac0, 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 0x7800, 0xb8c1,
110 0xb981, 0x7940, 0xbb01, 0x7bc0, 0x7a80, 0xba41, 0xbe01, 0x7ec0, 0x7f80,
111 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40, 0xb401, 0x74c0, 0x7580, 0xb541,
112 0x7700, 0xb7c1, 0xb681, 0x7640, 0x7200, 0xb2c1, 0xb381, 0x7340, 0xb101,
113 0x71c0, 0x7080, 0xb041, 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0,
114 0x5280, 0x9241, 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481,
115 0x5440, 0x9c01, 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40,
116 0x5a00, 0x9ac1, 0x9b81, 0x5b40, 0x9901, 0x59c0, 0x5880, 0x9841, 0x8801,
117 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81, 0x4a40, 0x4e00, 0x8ec1,
118 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 0x4400, 0x84c1, 0x8581,
119 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 0x8201, 0x42c0, 0x4380, 0x8341,
120 0x4100, 0x81c1, 0x8081, 0x4040};
121
generateCRC()122 auto Message::generateCRC() -> uint16_t
123 {
124 uint16_t crc = 0xFFFF; // Initial value for Modbus CRC
125
126 for (const auto& data : *this)
127 {
128 // XOR the current byte with the low byte of the CRC,
129 // then use the result as an index into the table.
130 crc = (crc >> 8) ^ crc16Table[(crc ^ data) & 0xFF];
131 }
132
133 // Modbus CRC requires byte swapping of the final result
134 return ((crc & 0xFF) << 8) | ((crc >> 8) & 0xFF);
135 }
136
137 } // namespace phosphor::modbus::rtu
138