From e57053a2eab2217a4a7bd3c12ee7b8b1d7704117 Mon Sep 17 00:00:00 2001 From: Graham Date: Wed, 6 May 2020 10:46:54 -0400 Subject: [PATCH] Refactored RTUMaster --- Code/ESP32/include/Enumerations.h | 7 - Code/ESP32/include/RtuMaster.h | 21 +-- Code/ESP32/include/TcpSlave.h | 4 +- Code/ESP32/platformio.ini | 2 +- Code/ESP32/src/RtuMaster.cpp | 288 +++++++++++++++--------------- Code/ESP32/src/TcpSlave.cpp | 100 +++-------- 6 files changed, 183 insertions(+), 239 deletions(-) diff --git a/Code/ESP32/include/Enumerations.h b/Code/ESP32/include/Enumerations.h index d866722..bf5285f 100644 --- a/Code/ESP32/include/Enumerations.h +++ b/Code/ESP32/include/Enumerations.h @@ -37,11 +37,4 @@ enum ResultCode { EX_CANCEL = 0xE6 // Custom. Transaction/request canceled }; -enum RtuReplyState { - RTU_IDLE = 0x01, - RTU_PENDING = 0x02, - RTU_COMPLETE = 0x03, - RTU_FAILED = 0x04 -}; - } \ No newline at end of file diff --git a/Code/ESP32/include/RtuMaster.h b/Code/ESP32/include/RtuMaster.h index 54ce0a3..47fcd28 100644 --- a/Code/ESP32/include/RtuMaster.h +++ b/Code/ESP32/include/RtuMaster.h @@ -3,34 +3,27 @@ #include "Enumerations.h" #define InterFrameDelay 2 -#define MODBUSRTU_TIMEOUT 3000 +#define MODBUSRTU_TIMEOUT 2000 namespace ModbusAdapter { class RtuMaster { - - byte _slaveId; // unit ID sent to slave, cleared after byte _unitId; // configured unit ID from setup Stream* _port; int16_t _rtsPin = -1; - uint32_t _timestamp = 0; - uint8_t _sentFunctionCode = 0; void* _callBackFunction = nullptr; - unsigned int _interFrameDelay; // inter-frame delay in mS - uint32_t _lastTimeStamp = 0; - uint16_t _len = 0; - + uint8_t _requestFrame[16]; + uint8_t _requestFrameLen; public: RtuMaster(); void Init(long baudRate, uint8_t mosbussAddress); - bool Transfer(byte* frame, uint16_t len, void* cb); - void run(); - void reset(); + void Transfer(byte* frame, uint16_t len, void* cb); + void Run(); private: - bool rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len); - bool cleanup(); uint16_t crc16(uint8_t address, uint8_t* frame, uint8_t pdulen); }; + + } \ No newline at end of file diff --git a/Code/ESP32/include/TcpSlave.h b/Code/ESP32/include/TcpSlave.h index 4981876..c33dc19 100644 --- a/Code/ESP32/include/TcpSlave.h +++ b/Code/ESP32/include/TcpSlave.h @@ -39,11 +39,11 @@ class TcpSlave uint8_t* _frame = nullptr; uint16_t _len = 0; RtuMaster _rtuMaster; + ResultCode _rtuResultCode; int8_t _clientIndex = -1; uint16_t _slavePort = 0; WiFiServer* _pServer; WiFiClient* _pClients[MODBUSIP_MAX_CLIENTS]; - RtuReplyState _rtuState; void cleanup(); int8_t getFreeClient(); void exceptionResponse(FunctionCode fn, ResultCode excode); @@ -52,7 +52,7 @@ class TcpSlave public: TcpSlave(); - void cbResponse(ResultCode res, void *data, uint16_t len); + void cbResponse(ResultCode res, uint8_t* requestFrame, uint8_t requestFrameLen, uint8_t *data, uint16_t len); void init(long baudRate, long tcpPort, uint8_t mosbusAddress); void close(); void run(); diff --git a/Code/ESP32/platformio.ini b/Code/ESP32/platformio.ini index b549f76..de36c14 100644 --- a/Code/ESP32/platformio.ini +++ b/Code/ESP32/platformio.ini @@ -22,7 +22,7 @@ lib_deps = esp32ModbusRTU build_flags = - -D 'CONFIG_VERSION="V2.0.1"' ; major.minor.build (major or minor will invalidate the configuration) + -D 'CONFIG_VERSION="V2.0.2"' ; major.minor.build (major or minor will invalidate the configuration) -D WIFI_STATUS_PIN=2 ;LED Pin on the Dev board -D WIFI_AP_PIN=15 ; force AP mode diff --git a/Code/ESP32/src/RtuMaster.cpp b/Code/ESP32/src/RtuMaster.cpp index 749bd57..b7f75e2 100644 --- a/Code/ESP32/src/RtuMaster.cpp +++ b/Code/ESP32/src/RtuMaster.cpp @@ -8,25 +8,24 @@ namespace ModbusAdapter { // Table of CRC values static const uint16_t _auchCRC[] PROGMEM = { - 0x0000, 0xC1C0, 0x81C1, 0x4001, 0x01C3, 0xC003, 0x8002, 0x41C2, 0x01C6, 0xC006, 0x8007, 0x41C7, 0x0005, 0xC1C5, 0x81C4, - 0x4004, 0x01CC, 0xC00C, 0x800D, 0x41CD, 0x000F, 0xC1CF, 0x81CE, 0x400E, 0x000A, 0xC1CA, 0x81CB, 0x400B, 0x01C9, 0xC009, - 0x8008, 0x41C8, 0x01D8, 0xC018, 0x8019, 0x41D9, 0x001B, 0xC1DB, 0x81DA, 0x401A, 0x001E, 0xC1DE, 0x81DF, 0x401F, 0x01DD, - 0xC01D, 0x801C, 0x41DC, 0x0014, 0xC1D4, 0x81D5, 0x4015, 0x01D7, 0xC017, 0x8016, 0x41D6, 0x01D2, 0xC012, 0x8013, 0x41D3, - 0x0011, 0xC1D1, 0x81D0, 0x4010, 0x01F0, 0xC030, 0x8031, 0x41F1, 0x0033, 0xC1F3, 0x81F2, 0x4032, 0x0036, 0xC1F6, 0x81F7, - 0x4037, 0x01F5, 0xC035, 0x8034, 0x41F4, 0x003C, 0xC1FC, 0x81FD, 0x403D, 0x01FF, 0xC03F, 0x803E, 0x41FE, 0x01FA, 0xC03A, - 0x803B, 0x41FB, 0x0039, 0xC1F9, 0x81F8, 0x4038, 0x0028, 0xC1E8, 0x81E9, 0x4029, 0x01EB, 0xC02B, 0x802A, 0x41EA, 0x01EE, - 0xC02E, 0x802F, 0x41EF, 0x002D, 0xC1ED, 0x81EC, 0x402C, 0x01E4, 0xC024, 0x8025, 0x41E5, 0x0027, 0xC1E7, 0x81E6, 0x4026, - 0x0022, 0xC1E2, 0x81E3, 0x4023, 0x01E1, 0xC021, 0x8020, 0x41E0, 0x01A0, 0xC060, 0x8061, 0x41A1, 0x0063, 0xC1A3, 0x81A2, - 0x4062, 0x0066, 0xC1A6, 0x81A7, 0x4067, 0x01A5, 0xC065, 0x8064, 0x41A4, 0x006C, 0xC1AC, 0x81AD, 0x406D, 0x01AF, 0xC06F, - 0x806E, 0x41AE, 0x01AA, 0xC06A, 0x806B, 0x41AB, 0x0069, 0xC1A9, 0x81A8, 0x4068, 0x0078, 0xC1B8, 0x81B9, 0x4079, 0x01BB, - 0xC07B, 0x807A, 0x41BA, 0x01BE, 0xC07E, 0x807F, 0x41BF, 0x007D, 0xC1BD, 0x81BC, 0x407C, 0x01B4, 0xC074, 0x8075, 0x41B5, - 0x0077, 0xC1B7, 0x81B6, 0x4076, 0x0072, 0xC1B2, 0x81B3, 0x4073, 0x01B1, 0xC071, 0x8070, 0x41B0, 0x0050, 0xC190, 0x8191, - 0x4051, 0x0193, 0xC053, 0x8052, 0x4192, 0x0196, 0xC056, 0x8057, 0x4197, 0x0055, 0xC195, 0x8194, 0x4054, 0x019C, 0xC05C, - 0x805D, 0x419D, 0x005F, 0xC19F, 0x819E, 0x405E, 0x005A, 0xC19A, 0x819B, 0x405B, 0x0199, 0xC059, 0x8058, 0x4198, 0x0188, - 0xC048, 0x8049, 0x4189, 0x004B, 0xC18B, 0x818A, 0x404A, 0x004E, 0xC18E, 0x818F, 0x404F, 0x018D, 0xC04D, 0x804C, 0x418C, - 0x0044, 0xC184, 0x8185, 0x4045, 0x0187, 0xC047, 0x8046, 0x4186, 0x0182, 0xC042, 0x8043, 0x4183, 0x0041, 0xC181, 0x8180, - 0x4040, 0x0000 -}; + 0x0000, 0xC1C0, 0x81C1, 0x4001, 0x01C3, 0xC003, 0x8002, 0x41C2, 0x01C6, 0xC006, 0x8007, 0x41C7, 0x0005, 0xC1C5, 0x81C4, + 0x4004, 0x01CC, 0xC00C, 0x800D, 0x41CD, 0x000F, 0xC1CF, 0x81CE, 0x400E, 0x000A, 0xC1CA, 0x81CB, 0x400B, 0x01C9, 0xC009, + 0x8008, 0x41C8, 0x01D8, 0xC018, 0x8019, 0x41D9, 0x001B, 0xC1DB, 0x81DA, 0x401A, 0x001E, 0xC1DE, 0x81DF, 0x401F, 0x01DD, + 0xC01D, 0x801C, 0x41DC, 0x0014, 0xC1D4, 0x81D5, 0x4015, 0x01D7, 0xC017, 0x8016, 0x41D6, 0x01D2, 0xC012, 0x8013, 0x41D3, + 0x0011, 0xC1D1, 0x81D0, 0x4010, 0x01F0, 0xC030, 0x8031, 0x41F1, 0x0033, 0xC1F3, 0x81F2, 0x4032, 0x0036, 0xC1F6, 0x81F7, + 0x4037, 0x01F5, 0xC035, 0x8034, 0x41F4, 0x003C, 0xC1FC, 0x81FD, 0x403D, 0x01FF, 0xC03F, 0x803E, 0x41FE, 0x01FA, 0xC03A, + 0x803B, 0x41FB, 0x0039, 0xC1F9, 0x81F8, 0x4038, 0x0028, 0xC1E8, 0x81E9, 0x4029, 0x01EB, 0xC02B, 0x802A, 0x41EA, 0x01EE, + 0xC02E, 0x802F, 0x41EF, 0x002D, 0xC1ED, 0x81EC, 0x402C, 0x01E4, 0xC024, 0x8025, 0x41E5, 0x0027, 0xC1E7, 0x81E6, 0x4026, + 0x0022, 0xC1E2, 0x81E3, 0x4023, 0x01E1, 0xC021, 0x8020, 0x41E0, 0x01A0, 0xC060, 0x8061, 0x41A1, 0x0063, 0xC1A3, 0x81A2, + 0x4062, 0x0066, 0xC1A6, 0x81A7, 0x4067, 0x01A5, 0xC065, 0x8064, 0x41A4, 0x006C, 0xC1AC, 0x81AD, 0x406D, 0x01AF, 0xC06F, + 0x806E, 0x41AE, 0x01AA, 0xC06A, 0x806B, 0x41AB, 0x0069, 0xC1A9, 0x81A8, 0x4068, 0x0078, 0xC1B8, 0x81B9, 0x4079, 0x01BB, + 0xC07B, 0x807A, 0x41BA, 0x01BE, 0xC07E, 0x807F, 0x41BF, 0x007D, 0xC1BD, 0x81BC, 0x407C, 0x01B4, 0xC074, 0x8075, 0x41B5, + 0x0077, 0xC1B7, 0x81B6, 0x4076, 0x0072, 0xC1B2, 0x81B3, 0x4073, 0x01B1, 0xC071, 0x8070, 0x41B0, 0x0050, 0xC190, 0x8191, + 0x4051, 0x0193, 0xC053, 0x8052, 0x4192, 0x0196, 0xC056, 0x8057, 0x4197, 0x0055, 0xC195, 0x8194, 0x4054, 0x019C, 0xC05C, + 0x805D, 0x419D, 0x005F, 0xC19F, 0x819E, 0x405E, 0x005A, 0xC19A, 0x819B, 0x405B, 0x0199, 0xC059, 0x8058, 0x4198, 0x0188, + 0xC048, 0x8049, 0x4189, 0x004B, 0xC18B, 0x818A, 0x404A, 0x004E, 0xC18E, 0x818F, 0x404F, 0x018D, 0xC04D, 0x804C, 0x418C, + 0x0044, 0xC184, 0x8185, 0x4045, 0x0187, 0xC047, 0x8046, 0x4186, 0x0182, 0xC042, 0x8043, 0x4183, 0x0041, 0xC181, 0x8180, + 0x4040, 0x0000}; RtuMaster::RtuMaster() { @@ -34,151 +33,156 @@ RtuMaster::RtuMaster() void RtuMaster::Init(long baudRate, uint8_t modbusAddress) { - Serial1.begin(baudRate, SERIAL_8N1, GPIO_NUM_16, GPIO_NUM_17, false); // Modbus connection - while (!Serial1) - { - ; // wait for serial port to connect. - } - _port = &Serial1; - _unitId = modbusAddress; - _rtsPin = GPIO_NUM_5; - if (_rtsPin >= 0) { + Serial1.begin(baudRate, SERIAL_8N1, GPIO_NUM_16, GPIO_NUM_17, false); // Modbus connection + while (!Serial1) + { + ; // wait for serial port to connect. + } + _port = &Serial1; + _unitId = modbusAddress; + _rtsPin = GPIO_NUM_5; + if (_rtsPin >= 0) + { pinMode(_rtsPin, OUTPUT); digitalWrite(_rtsPin, LOW); } - if (baudRate > 19200) { - _interFrameDelay = 2; - } else { - _interFrameDelay = (35000/baudRate) + 1; - } } -bool RtuMaster::Transfer(byte* frame, uint16_t len, void* cb) +static uint16_t crc16_update(uint16_t crc, uint8_t a) { - if (_slaveId) return false; // Break if waiting for previous request result - _slaveId = _unitId; - _sentFunctionCode = frame[0]; - rawSend(_slaveId, frame, len); - _timestamp = millis(); - _callBackFunction = cb; - _len = 0; - // logd("RTU Transfer id: %d, len: %d", _slaveId, len); - // printHexString(frame, len); - return true; + int i; + crc ^= a; + for (i = 0; i < 8; ++i) + { + if (crc & 1) + crc = (crc >> 1) ^ 0xA001; + else + crc = (crc >> 1); + } + return crc; } -void RtuMaster::run() +void RtuMaster::Transfer(byte *frame, uint16_t len, void *cb) { - if (_port->available() > _len) { - _len = _port->available(); - _lastTimeStamp = millis(); - return; - } - if (_len == 0) { // No data - cleanup(); - return; - } - if (millis() - _lastTimeStamp < _interFrameDelay) return; // Wait data whitespace - uint8_t address = _port->read(); //first byte of frame = address - _len--; // Decrease by slaveId byte - if (_len == 0) { - return; - } - if (_slaveId == 0) { // Check if slaveId is set - for (uint8_t i=0 ; i < _len ; i++) _port->read(); // Skip packet if is not expected - _len = 0; - return; + _callBackFunction = cb; + memcpy(_requestFrame, frame, len); + _requestFrameLen = len; + uint16_t newCrc = crc16(_unitId, frame, len); + while (_port->read() != -1) + ; // flush receive buffer before transmitting request + if (_rtsPin >= 0) + { + digitalWrite(_rtsPin, HIGH); + delay(1); } - if (address != _slaveId) { // SlaveId Check - while (_len != 0) { - address = _port->read(); // filter out non-modbus crap - _len--; - if (address == _slaveId) { + _port->write(_unitId); + _port->write(frame, len); // Send PDU + _port->write(newCrc >> 8); //Send CRC + _port->write(newCrc & 0xFF); //Send CRC + _port->flush(); + if (_rtsPin >= 0) + digitalWrite(_rtsPin, LOW); + return; +} + +void RtuMaster::Run() +{ + uint8_t u8ModbusADU[256]; + uint8_t u8ModbusADUSize = 0; + uint16_t u16CRC; + uint32_t u32StartTime; + uint8_t u8BytesLeft = 8; + ResultCode u8MBStatus = EX_SUCCESS; + + u32StartTime = millis(); + while (u8BytesLeft && !u8MBStatus) + { + if (_port->available()) + { + u8ModbusADU[u8ModbusADUSize++] = _port->read(); + u8BytesLeft--; + } + // evaluate slave ID, function code once enough bytes have been read + if (u8ModbusADUSize == 5) + { + // verify response is for correct Modbus slave + if (u8ModbusADU[0] != _unitId) + { + u8MBStatus = EX_ILLEGAL_ADDRESS; + break; + } + // verify response is for correct Modbus function code (mask exception bit 7) + if ((u8ModbusADU[1] & 0x7F) != _requestFrame[0]) + { + u8MBStatus = EX_ILLEGAL_FUNCTION; + break; + } + // check whether Modbus exception occurred; return Modbus Exception Code + if (bitRead(u8ModbusADU[1], 7)) + { + u8MBStatus = (ResultCode)u8ModbusADU[2]; + break; + } + + // evaluate returned Modbus function code + switch (u8ModbusADU[1]) + { + case FC_READ_COILS: + case FC_READ_INPUT_STAT: + case FC_READ_REGS: + case FC_READ_INPUT_REGS: + u8BytesLeft = u8ModbusADU[2]; + break; + + default: + u8MBStatus = EX_ILLEGAL_FUNCTION; break; } } - if (_len == 0) { - return; + if ((millis() - u32StartTime) > MODBUSRTU_TIMEOUT) + { + u8MBStatus = EX_TIMEOUT; } } - uint8_t* frame = (uint8_t*) malloc(_len); - if (!frame) { // Fail to allocate buffer - for (uint8_t i=0 ; i < _len ; i++) _port->read(); // Skip packet if can't allocate buffer - _len = 0; - logw("Fail to allocate buffer"); - return; - } - for (uint8_t i=0 ; i < _len ; i++) - frame[i] = _port->read(); // read data + crc - u_int frameCrc = ((frame[_len - 2] << 8) | frame[_len - 1]); // Last two byts = crc - _len = _len - 2; // Decrease by CRC 2 bytes - uint16_t crc = crc16(address, frame, _len); - if (frameCrc != crc) { // CRC Check - // logw("wrong crc for %d: [0x%x, 0x%x] len %d", address, frameCrc, crc, _len); - // printHexString(frame, _len); - free(frame); - frame = nullptr; - _len = 0; // Cleanup if wrong crc + // verify response is large enough to inspect further + if (u8MBStatus == EX_SUCCESS && u8ModbusADUSize >= 5) + { + // calculate CRC + u16CRC = 0xFFFF; + for (int i = 0; i < (u8ModbusADUSize - 2); i++) + { + u16CRC = crc16_update(u16CRC, u8ModbusADU[i]); + } - return; + // verify CRC + if (!u8MBStatus && (lowByte(u16CRC) != u8ModbusADU[u8ModbusADUSize - 2] || + highByte(u16CRC) != u8ModbusADU[u8ModbusADUSize - 1])) + { + u8MBStatus = EX_UNEXPECTED_RESPONSE; + } + } + if (_callBackFunction) + { + TcpSlave *p = (TcpSlave *)_callBackFunction; + p->cbResponse(u8MBStatus, _requestFrame, _requestFrameLen, &u8ModbusADU[1], u8ModbusADUSize - 3); // response size minus CRC & FunctionCode (3) } - if ((frame[0] & 0x7F) == _sentFunctionCode) { // Check if function code the same as requested - if (_callBackFunction) { - TcpSlave* p =(TcpSlave*) _callBackFunction; - p->cbResponse(EX_SUCCESS, frame, _len); - } - _slaveId = 0; - } - // Cleanup - free(frame); - _len = 0; + return; } -uint16_t RtuMaster::crc16(uint8_t address, uint8_t* frame, uint8_t pduLen) { - uint8_t i = 0xFF ^ address; - uint16_t val = pgm_read_word(_auchCRC + i); - uint8_t CRCHi = 0xFF ^ highByte(val); // Hi - uint8_t CRCLo = lowByte(val); //Low - while (pduLen--) { +uint16_t RtuMaster::crc16(uint8_t address, uint8_t *frame, uint8_t pduLen) +{ + uint8_t i = 0xFF ^ address; + uint16_t val = pgm_read_word(_auchCRC + i); + uint8_t CRCHi = 0xFF ^ highByte(val); // Hi + uint8_t CRCLo = lowByte(val); //Low + while (pduLen--) + { i = CRCHi ^ *frame++; val = pgm_read_word(_auchCRC + i); - CRCHi = CRCLo ^ highByte(val); // Hi - CRCLo = lowByte(val); //Low + CRCHi = CRCLo ^ highByte(val); // Hi + CRCLo = lowByte(val); //Low } return (CRCHi << 8) | CRCLo; } -bool RtuMaster::rawSend(uint8_t slaveId, uint8_t* frame, uint8_t len) { - uint16_t newCrc = crc16(slaveId, frame, len); - if (_rtsPin >= 0) { - digitalWrite(_rtsPin, HIGH); - delay(1); - } - _port->write(slaveId); //Send slaveId - _port->write(frame, len); // Send PDU - _port->write(newCrc >> 8); //Send CRC - _port->write(newCrc & 0xFF); //Send CRC - _port->flush(); - if (_rtsPin >= 0) - digitalWrite(_rtsPin, LOW); - delay(_interFrameDelay); - return true; -} - -bool RtuMaster::cleanup() { - if (_slaveId && ((millis() - _timestamp) > MODBUSRTU_TIMEOUT)) { - if (_callBackFunction) { - TcpSlave* p =(TcpSlave*) _callBackFunction; - p->cbResponse(EX_TIMEOUT, nullptr, 0); - } - _slaveId = 0; - return true; - } - return false; -} - -void RtuMaster::reset() { - _slaveId = 0; - return; -} -} \ No newline at end of file +} // namespace ModbusAdapter \ No newline at end of file diff --git a/Code/ESP32/src/TcpSlave.cpp b/Code/ESP32/src/TcpSlave.cpp index 8016737..80df59f 100644 --- a/Code/ESP32/src/TcpSlave.cpp +++ b/Code/ESP32/src/TcpSlave.cpp @@ -20,7 +20,7 @@ void TcpSlave::init(long baudRate, long tcpPort, uint8_t mosbusAddress) _rtuMaster.Init(baudRate, mosbusAddress); _pServer = new WiFiServer(tcpPort); _pServer->begin(); - _rtuState = RTU_IDLE; + _rtuResultCode = EX_SLAVE_FAILURE; } void TcpSlave::close() @@ -28,24 +28,21 @@ void TcpSlave::close() _pServer->close(); } -void TcpSlave::cbResponse(ResultCode res, void *data, uint16_t len) +void TcpSlave::cbResponse(ResultCode res, uint8_t* requestFrame, uint8_t requestFrameLen, uint8_t *responseFrame, uint16_t responseFrameLen) { - if (res != EX_SUCCESS) + _rtuResultCode= res; + if (res == EX_SUCCESS) { - _rtuState = RTU_FAILED; - } - else - { - Transaction *trans = searchTransaction(_frame); + Transaction *trans = searchTransaction(requestFrame); if (trans) { - uint8_t *mem = (uint8_t *)malloc(len); + uint8_t *mem = (uint8_t *)malloc(responseFrameLen); if (mem) { free(trans->_responseFrame); trans->_responseFrame = mem; - memcpy(trans->_responseFrame, data, len); - trans->_responseFrameLen = len; + memcpy(trans->_responseFrame, responseFrame, responseFrameLen); + trans->_responseFrameLen = responseFrameLen; trans->timestamp = millis(); } } @@ -53,18 +50,18 @@ void TcpSlave::cbResponse(ResultCode res, void *data, uint16_t len) { Transaction newEntry; newEntry.timestamp = millis(); - uint8_t *memReq = (uint8_t *)malloc(_len); + uint8_t *memReq = (uint8_t *)malloc(requestFrameLen); if (memReq) { newEntry._requestFrame = memReq; - memcpy(newEntry._requestFrame, _frame, _len); - newEntry._requestFrameLen = _len; - uint8_t *memResp = (uint8_t *)malloc(len); + memcpy(newEntry._requestFrame, requestFrame, requestFrameLen); + newEntry._requestFrameLen = requestFrameLen; + uint8_t *memResp = (uint8_t *)malloc(responseFrameLen); if (memResp) { newEntry._responseFrame = memResp; - memcpy(newEntry._responseFrame, data, len); - newEntry._responseFrameLen = len; + memcpy(newEntry._responseFrame, responseFrame, responseFrameLen); + newEntry._responseFrameLen = responseFrameLen; _trans.push_back(newEntry); } else @@ -73,16 +70,6 @@ void TcpSlave::cbResponse(ResultCode res, void *data, uint16_t len) } } } - free(_frame); - _len = len; - _frame = (uint8_t *)malloc(len); - if (!_frame) - { - _rtuState = RTU_FAILED; - return; - } - memcpy(_frame, data, len); - _rtuState = RTU_COMPLETE; } return; } @@ -157,57 +144,23 @@ void TcpSlave::run() exceptionResponse((FunctionCode)_frame[0], EX_ILLEGAL_FUNCTION); } // forward incoming frame to RTU master - else if (_rtuMaster.Transfer(_frame, _len, this) == false) + _rtuMaster.Transfer(_frame, _len, this); + _rtuMaster.Run(); // read RTU responses + Transaction *trans = searchTransaction(_frame); + if (trans) { - Transaction *trans = searchTransaction(_frame); - if (trans) - { - uint8_t *mem = (uint8_t *)malloc(trans->_responseFrameLen); - if (mem) - { - _len = trans->_responseFrameLen; - free(_frame); - _frame = mem; - memcpy(_frame, trans->_responseFrame, _len); - _rtuState = RTU_COMPLETE; - } - } - else + uint8_t *mem = (uint8_t *)malloc(trans->_responseFrameLen); + if (mem) { - exceptionResponse((FunctionCode)_frame[0], EX_DEVICE_FAILED_TO_RESPOND); - _rtuState = RTU_IDLE; + _len = trans->_responseFrameLen; + free(_frame); + _frame = mem; + memcpy(_frame, trans->_responseFrame, _len); } } else { - _rtuState = RTU_PENDING; - uint32_t transferStart = millis(); - while (millis() - transferStart < MODBUSIP_MAX_RESPMS && _rtuState == RTU_PENDING) - { - _rtuMaster.run(); - } - if (_rtuState != RTU_COMPLETE) - { // did not complete? - Transaction *trans = searchTransaction(_frame); - if (trans) - { - uint8_t *mem = (uint8_t *)malloc(trans->_responseFrameLen); - if (mem) - { - _len = trans->_responseFrameLen; - free(_frame); - _frame = mem; - memcpy(_frame, trans->_responseFrame, _len); - _rtuState = RTU_COMPLETE; - } - } - else - { - exceptionResponse((FunctionCode)_frame[0], _rtuState == RTU_FAILED ? EX_GENERAL_FAILURE : EX_DATA_MISMACH); - _rtuState = RTU_IDLE; - _rtuMaster.reset(); - } - } + exceptionResponse((FunctionCode)_frame[0], EX_TIMEOUT); } } } @@ -222,6 +175,7 @@ void TcpSlave::run() memcpy(sbuf + sizeof(_MBAP.raw), _frame, _len); _pClients[_clientIndex]->write(sbuf, send_len); _pClients[_clientIndex]->flush(); + } if (_frame) { @@ -232,7 +186,7 @@ void TcpSlave::run() } } _clientIndex = -1; - _rtuMaster.run(); // read crap + } void TcpSlave::exceptionResponse(FunctionCode fn, ResultCode excode)