diff --git a/include/MB/modbusRequest.hpp b/include/MB/modbusRequest.hpp index 8f1487b..5858c20 100644 --- a/include/MB/modbusRequest.hpp +++ b/include/MB/modbusRequest.hpp @@ -95,6 +95,10 @@ class ModbusRequest { //! communication [[nodiscard]] std::vector toRaw() const; + //! Returns raw bytes representation of object, ready for modbus + //! communication + [[nodiscard]] std::vector toRawCRC() const; + //! Returns function type based on Modbus function code [[nodiscard]] utils::MBFunctionType functionType() const noexcept { return utils::functionType(_functionCode); diff --git a/include/MB/modbusResponse.hpp b/include/MB/modbusResponse.hpp index 5ee74a8..a42848a 100644 --- a/include/MB/modbusResponse.hpp +++ b/include/MB/modbusResponse.hpp @@ -27,9 +27,11 @@ class ModbusResponse { private: uint8_t _slaveID; utils::MBFunctionCode _functionCode; + bool _exception; uint16_t _address; uint16_t _registersNumber; + uint8_t _bytes; std::vector _values; diff --git a/include/MB/modbusUtils.hpp b/include/MB/modbusUtils.hpp index ec17edd..e0c5d67 100644 --- a/include/MB/modbusUtils.hpp +++ b/include/MB/modbusUtils.hpp @@ -47,6 +47,7 @@ enum MBErrorCode : uint8_t { // See issue: https://github.com/Mazurel/Modbus/issues/3 NumberOfRegistersInvalid = 0b01111000, NumberOfValuesInvalid = 0b01110111, + InputDataLengthInvalid = 0b01110110, }; //! Checks if error code is modbus standard error code @@ -71,6 +72,7 @@ inline bool isStandardErrorCode(MBErrorCode code) { case ConnectionClosed: case Timeout: case NumberOfRegistersInvalid: + case InputDataLengthInvalid: default: return false; } @@ -117,6 +119,8 @@ inline std::string mbErrorCodeToStr(MBErrorCode code) noexcept { return "Number of registers in response is too big - cannot be serialized"; case NumberOfValuesInvalid: return "Number of values is not valid"; + case InputDataLengthInvalid: + return "Encountered end of data during parsing"; } return "Unknown"; diff --git a/src/modbusRequest.cpp b/src/modbusRequest.cpp index 2c62b0a..0ae1e3c 100644 --- a/src/modbusRequest.cpp +++ b/src/modbusRequest.cpp @@ -201,3 +201,12 @@ std::vector ModbusRequest::toRaw() const { return result; } + +std::vector ModbusRequest::toRawCRC() const { + std::vector result {toRaw()}; + const auto crc = MB::utils::calculateCRC(result); + result.push_back(reinterpret_cast(&crc)[0]); + result.push_back(reinterpret_cast(&crc)[1]); + + return result; +} diff --git a/src/modbusResponse.cpp b/src/modbusResponse.cpp index 5b85a0f..7b30e30 100644 --- a/src/modbusResponse.cpp +++ b/src/modbusResponse.cpp @@ -50,67 +50,74 @@ ModbusResponse &ModbusResponse::operator=(const ModbusResponse &reference) { ModbusResponse::ModbusResponse(std::vector inputData, bool CRC) { try { if (inputData.size() < 3) - throw ModbusException(utils::InvalidByteOrder); + throw ModbusException(utils::InputDataLengthInvalid); - _slaveID = inputData[0]; - _functionCode = static_cast(inputData[1]); + _slaveID = inputData.at(0); + _functionCode = static_cast(0b01111111 & inputData.at(1)); + _exception = (inputData.at(1) & 0b10000000) != 0; if (functionType() != utils::Read) - _address = utils::bigEndianConv(&inputData[2]); + _address = utils::bigEndianConv(&inputData.at(2)); int crcIndex = -1; - uint8_t bytes; - - switch (_functionCode) { - case utils::ReadDiscreteOutputCoils: - case utils::ReadDiscreteInputContacts: - bytes = inputData[2]; - _registersNumber = bytes * 8; - _values = std::vector(_registersNumber); - for (auto i = 0; i < _registersNumber; i++) { - _values[i].coil() = inputData[3 + (i / 8)] & (1 << (i % 8)); - } - crcIndex = 2 + bytes + 1; - break; - case utils::ReadAnalogOutputHoldingRegisters: - case utils::ReadAnalogInputRegisters: - bytes = inputData[2]; - _registersNumber = bytes / 2; - for (auto i = 0; i < bytes / 2; i++) { - _values.emplace_back(utils::bigEndianConv(&inputData[3 + (i * 2)])); + + if(!_exception){ + switch (_functionCode) { + case utils::ReadDiscreteOutputCoils: + case utils::ReadDiscreteInputContacts: + _bytes = inputData.at(2); + _registersNumber = _bytes * 8; + _values = std::vector(_registersNumber); + for (auto i = 0; i < _registersNumber; i++) { + _values[i].coil() = inputData.at(3 + (i / 8)) & (1 << (i % 8)); + } + crcIndex = 2 + _bytes + 1; + break; + case utils::ReadAnalogOutputHoldingRegisters: + case utils::ReadAnalogInputRegisters: + _bytes = inputData.at(2); + _registersNumber = _bytes / 2; + for (auto i = 0; i < _bytes / 2; i++) { + _values.emplace_back(utils::bigEndianConv(&inputData.at(3 + (i * 2)))); + } + crcIndex = 2 + _bytes + 1; + break; + case utils::WriteSingleDiscreteOutputCoil: + _bytes = 8; + _registersNumber = 1; + _address = utils::bigEndianConv(&inputData.at(2)); + _values = {ModbusCell::initCoil(inputData.at(4) == 0xFF)}; + crcIndex = 6; + break; + case utils::WriteSingleAnalogOutputRegister: + _bytes = 8; + _registersNumber = 1; + _address = utils::bigEndianConv(&inputData.at(2)); + _values = {ModbusCell::initReg(utils::bigEndianConv(&inputData.at(4)))}; + crcIndex = 6; + break; + case utils::WriteMultipleDiscreteOutputCoils: + case utils::WriteMultipleAnalogOutputHoldingRegisters: + _bytes = 8; + _address = utils::bigEndianConv(&inputData.at(2)); + _registersNumber = utils::bigEndianConv(&inputData.at(4)); + crcIndex = 6; + break; + default: + throw ModbusException(utils::InvalidByteOrder); } - crcIndex = 2 + bytes + 1; - break; - case utils::WriteSingleDiscreteOutputCoil: - _registersNumber = 1; - _address = utils::bigEndianConv(&inputData[2]); - _values = {ModbusCell::initCoil(inputData[4] == 0xFF)}; - crcIndex = 6; - break; - case utils::WriteSingleAnalogOutputRegister: - _registersNumber = 1; - _address = utils::bigEndianConv(&inputData[2]); - _values = {ModbusCell::initReg(utils::bigEndianConv(&inputData[4]))}; - crcIndex = 6; - break; - case utils::WriteMultipleDiscreteOutputCoils: - case utils::WriteMultipleAnalogOutputHoldingRegisters: - _address = utils::bigEndianConv(&inputData[2]); - _registersNumber = utils::bigEndianConv(&inputData[4]); - crcIndex = 6; - break; - default: - throw ModbusException(utils::InvalidByteOrder); + }else{ + crcIndex = 3; } _values.resize(_registersNumber); if (CRC) { if (crcIndex == -1 || static_cast(crcIndex) + 2 > inputData.size()) - throw ModbusException(utils::InvalidByteOrder); + throw ModbusException(utils::InputDataLengthInvalid); const auto receivedCRC = - *reinterpret_cast(&inputData[crcIndex]); + *reinterpret_cast(&inputData.at(crcIndex)); const auto inputDataLen = static_cast(crcIndex); const auto calculatedCRC = MB::CRC::calculateCRC(inputData, inputDataLen); @@ -118,8 +125,14 @@ ModbusResponse::ModbusResponse(std::vector inputData, bool CRC) { throw ModbusException(utils::InvalidCRC, _slaveID); } } + if(_exception){ + throw ModbusException{inputData,CRC}; + } + } catch (const ModbusException &ex) { throw ex; + } catch (const std::out_of_range& inputDataInSufficient){ + throw ModbusException(utils::InputDataLengthInvalid); } catch (const std::exception &) { // TODO: Save the exception somewhere throw ModbusException(utils::InvalidByteOrder);