First stable compilation with ModbusRTU and ModbusTCP
This commit is contained in:
931
lib/modbus-esp8266/Modbus.cpp
Normal file
931
lib/modbus-esp8266/Modbus.cpp
Normal file
@@ -0,0 +1,931 @@
|
||||
/*
|
||||
Modbus Library for Arduino
|
||||
Core functions
|
||||
Copyright (C) 2014 Andr<64> Sarmento Barbosa
|
||||
2017-2023 Alexander Emelianov (a.m.emelianov@gmail.com)
|
||||
*/
|
||||
#include "Modbus.h"
|
||||
|
||||
#if defined(MODBUS_GLOBAL_REGS)
|
||||
#if defined(MODBUS_USE_STL)
|
||||
std::vector<TRegister> Modbus::_regs;
|
||||
std::vector<TCallback> Modbus::_callbacks;
|
||||
#if defined(MODBUS_FILES)
|
||||
std::function<Modbus::ResultCode(Modbus::FunctionCode, uint16_t, uint16_t, uint16_t, uint8_t*)> Modbus::_onFile;
|
||||
#endif
|
||||
#else
|
||||
DArray<TRegister, 1, 1> Modbus::_regs;
|
||||
DArray<TCallback, 1, 1> Modbus::_callbacks;
|
||||
#if defined(MODBUS_FILES)
|
||||
cbModbusFileOp Modbus::_onFile = nullptr;
|
||||
#endif
|
||||
#endif
|
||||
#endif
|
||||
|
||||
uint16_t Modbus::callback(TRegister* reg, uint16_t val, TCallback::CallbackType t) {
|
||||
#define MODBUS_COMPARE_CB [reg, t](TCallback& cb){return cb.address == reg->address && cb.type == t;}
|
||||
uint16_t newVal = val;
|
||||
#if defined(MODBUS_USE_STL)
|
||||
std::vector<TCallback>::iterator it = _callbacks.begin();
|
||||
do {
|
||||
it = std::find_if(it, _callbacks.end(), MODBUS_COMPARE_CB);
|
||||
if (it != _callbacks.end()) {
|
||||
newVal = it->cb(reg, newVal);
|
||||
it++;
|
||||
}
|
||||
} while (it != _callbacks.end());
|
||||
#else
|
||||
size_t r = 0;
|
||||
do {
|
||||
r = _callbacks.find(MODBUS_COMPARE_CB, r);
|
||||
if (r < _callbacks.size())
|
||||
newVal = _callbacks[r].cb(reg, newVal);
|
||||
r++;
|
||||
} while (r < _callbacks.size());
|
||||
#endif
|
||||
return newVal;
|
||||
}
|
||||
|
||||
TRegister* Modbus::searchRegister(TAddress address) {
|
||||
#define MODBUS_COMPARE_REG [address](TRegister& addr){return (addr.address == address);}
|
||||
#if defined(MODBUS_USE_STL)
|
||||
std::vector<TRegister>::iterator it = std::find_if(_regs.begin(), _regs.end(), MODBUS_COMPARE_REG);
|
||||
if (it != _regs.end()) return &*it;
|
||||
#else
|
||||
size_t r = _regs.find(MODBUS_COMPARE_REG);
|
||||
if (r < _regs.size()) return _regs.entry(r);
|
||||
#endif
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool Modbus::addReg(TAddress address, uint16_t value, uint16_t numregs) {
|
||||
#if defined(MODBUS_MAX_REGS)
|
||||
if (_regs.size() + numregs > MODBUS_MAX_REGS) return false;
|
||||
#endif
|
||||
if (0xFFFF - address.address < numregs)
|
||||
numregs = 0xFFFF - address.address;
|
||||
for (uint16_t i = 0; i < numregs; i++) {
|
||||
if (!searchRegister(address + i))
|
||||
_regs.push_back({address + i, value});
|
||||
}
|
||||
//std::sort(_regs.begin(), _regs.end());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Modbus::Reg(TAddress address, uint16_t value) {
|
||||
TRegister* reg;
|
||||
reg = searchRegister(address); //search for the register address
|
||||
if (reg) { //if found then assign the register value to the new value.
|
||||
if (cbEnabled) {
|
||||
reg->value = callback(reg, value, TCallback::ON_SET);
|
||||
} else {
|
||||
reg->value = value;
|
||||
}
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t Modbus::Reg(TAddress address) {
|
||||
TRegister* reg;
|
||||
reg = searchRegister(address);
|
||||
if(reg)
|
||||
if (cbEnabled) {
|
||||
return callback(reg, reg->value, TCallback::ON_GET);
|
||||
} else {
|
||||
return reg->value;
|
||||
}
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool Modbus::removeReg(TAddress address, uint16_t numregs) {
|
||||
TRegister* reg;
|
||||
bool atLeastOne = false;
|
||||
if (0xFFFF - address.address < numregs)
|
||||
numregs = 0xFFFF - address.address;
|
||||
for (uint16_t i = 0; i < numregs; i++) {
|
||||
reg = searchRegister(address + i);
|
||||
if (reg) {
|
||||
atLeastOne = true;
|
||||
removeOnSet(address + i);
|
||||
removeOnGet(address + i);
|
||||
#if defined(MODBUS_USE_STL)
|
||||
_regs.erase(std::remove( _regs.begin(), _regs.end(), *reg), _regs.end() );
|
||||
#else
|
||||
_regs.remove(_regs.find(MODBUS_COMPARE_REG));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
return atLeastOne;
|
||||
}
|
||||
|
||||
bool Modbus::addReg(TAddress address, uint16_t* value, uint16_t numregs) {
|
||||
if (0xFFFF - address.address < numregs)
|
||||
numregs = 0xFFFF - address.address;
|
||||
for (uint16_t k = 0; k < numregs; k++)
|
||||
addReg(address + k, value[k]);
|
||||
return true;
|
||||
}
|
||||
|
||||
void Modbus::slavePDU(uint8_t* frame) {
|
||||
FunctionCode fcode = (FunctionCode)frame[0];
|
||||
uint16_t field1 = (uint16_t)frame[1] << 8 | (uint16_t)frame[2];
|
||||
uint16_t field2 = (uint16_t)frame[3] << 8 | (uint16_t)frame[4];
|
||||
uint16_t field3 = 0;
|
||||
uint16_t field4 = 0;
|
||||
uint16_t bytecount_calc;
|
||||
uint16_t k;
|
||||
ResultCode ex;
|
||||
switch (fcode) {
|
||||
case FC_WRITE_REG:
|
||||
//field1 = reg, field2 = value
|
||||
ex = _onRequest(fcode, {HREG(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
if (!Reg(HREG(field1), field2)) { //Check Address and execute (reg exists?)
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
if (Reg(HREG(field1)) != field2) { //Check for failure
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
_reply = REPLY_ECHO;
|
||||
_onRequestSuccess(fcode, {HREG(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_READ_REGS:
|
||||
//field1 = startreg, field2 = numregs, header len = 3
|
||||
ex = _onRequest(fcode, {HREG(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
ex = readWords(HREG(field1), field2, fcode);
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
_onRequestSuccess(fcode, {HREG(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_WRITE_REGS:
|
||||
//field1 = startreg, field2 = numregs, frame[5] = data lenght, header len = 6
|
||||
ex = _onRequest(fcode, {HREG(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
if (field2 < 0x0001 || field2 > MODBUS_MAX_WORDS || 0xFFFF - field1 < field2 || frame[5] != 2 * field2) { //Check constrains
|
||||
exceptionResponse(fcode, EX_ILLEGAL_VALUE);
|
||||
return;
|
||||
}
|
||||
for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
|
||||
if (!searchRegister(HREG(field1) + k)) {
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
}
|
||||
if (!setMultipleWords((uint16_t*)(frame + 6), HREG(field1), field2)) {
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
successResponce(HREG(field1), field2, fcode);
|
||||
_reply = REPLY_NORMAL;
|
||||
_onRequestSuccess(fcode, {HREG(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_READ_COILS:
|
||||
//field1 = startreg, field2 = numregs
|
||||
ex = _onRequest(fcode, {COIL(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
ex = readBits(COIL(field1), field2, fcode);
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
_onRequestSuccess(fcode, {COIL(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_READ_INPUT_STAT:
|
||||
//field1 = startreg, field2 = numregs
|
||||
ex = _onRequest(fcode, {ISTS(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
ex = readBits(ISTS(field1), field2, fcode);
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
_onRequestSuccess(fcode, {ISTS(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_READ_INPUT_REGS:
|
||||
//field1 = startreg, field2 = numregs
|
||||
ex = _onRequest(fcode, {IREG(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
ex = readWords(IREG(field1), field2, fcode);
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
_onRequestSuccess(fcode, {IREG(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_WRITE_COIL:
|
||||
//field1 = reg, field2 = status, header len = 3
|
||||
ex = _onRequest(fcode, {COIL(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
if (field2 != 0xFF00 && field2 != 0x0000) { //Check value (status)
|
||||
exceptionResponse(fcode, EX_ILLEGAL_VALUE);
|
||||
return;
|
||||
}
|
||||
if (!Reg(COIL(field1), field2)) { //Check Address and execute (reg exists?)
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
if (Reg(COIL(field1)) != field2) { //Check for failure
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
_reply = REPLY_ECHO;
|
||||
_onRequestSuccess(fcode, {COIL(field1), field2});
|
||||
break;
|
||||
|
||||
case FC_WRITE_COILS:
|
||||
//field1 = startreg, field2 = numregs, frame[5] = bytecount, header len = 6
|
||||
ex = _onRequest(fcode, {COIL(field1), field2});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
bytecount_calc = field2 / 8;
|
||||
if (field2%8) bytecount_calc++;
|
||||
if (field2 < 0x0001 || field2 > MODBUS_MAX_BITS || 0xFFFF - field1 < field2 || frame[5] != bytecount_calc) { //Check registers range and data size maches
|
||||
exceptionResponse(fcode, EX_ILLEGAL_VALUE);
|
||||
return;
|
||||
}
|
||||
for (k = 0; k < field2; k++) { //Check Address (startreg...startreg + numregs)
|
||||
if (!searchRegister(COIL(field1) + k)) {
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
}
|
||||
if (!setMultipleBits(frame + 6, COIL(field1), field2)) {
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
successResponce(COIL(field1), field2, fcode);
|
||||
_reply = REPLY_NORMAL;
|
||||
_onRequestSuccess(fcode, {COIL(field1), field2});
|
||||
break;
|
||||
#if defined(MODBUS_FILES)
|
||||
case FC_READ_FILE_REC:
|
||||
if (frame[1] < 0x07 || frame[1] > 0xF5) { // Wrong request data size
|
||||
exceptionResponse(fcode, EX_ILLEGAL_VALUE);
|
||||
return;
|
||||
}
|
||||
{
|
||||
uint8_t bufSize = 2; // 2 bytes for frame header
|
||||
uint8_t* recs = frame + 2; // Begin of sub-recs blocks
|
||||
uint8_t recsCount = frame[1] / 7; // Count of sub-rec blocks
|
||||
for (uint8_t p = 0; p < recsCount; p++) { // Calc output buffer size required
|
||||
//uint16_t fileNum = (uint16_t)recs[1] << 8 | (uint16_t)recs[2];
|
||||
uint16_t recNum = (uint16_t)recs[3] << 8 | (uint16_t)recs[4];
|
||||
uint16_t recLen = (uint16_t)recs[5] << 8 | (uint16_t)recs[6];
|
||||
//Serial.printf("%d, %d, %d\n", fileNum, recNum, recLen);
|
||||
if (recs[0] != 0x06 || recNum > 0x270F) { // Wrong ref type or count of records
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
bufSize += recLen * 2 + 2; // 4 bytes for header + data
|
||||
recs += 7;
|
||||
}
|
||||
// if (bufSize > MODBUS_MAX_FRAME) { // Frame to return too large
|
||||
// exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
// return;
|
||||
// }
|
||||
uint8_t* srcFrame = _frame;
|
||||
_frame = (uint8_t*)malloc(bufSize);
|
||||
if (!_frame) {
|
||||
free(srcFrame);
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
_len = bufSize;
|
||||
recs = frame + 2; // Begin of sub-recs blocks
|
||||
uint8_t* data = _frame + 2;
|
||||
for (uint8_t p = 0; p < recsCount; p++) {
|
||||
uint16_t fileNum = (uint16_t)recs[1] << 8 | (uint16_t)recs[2];
|
||||
uint16_t recNum = (uint16_t)recs[3] << 8 | (uint16_t)recs[4];
|
||||
uint16_t recLen = (uint16_t)recs[5] << 8 | (uint16_t)recs[6];
|
||||
ResultCode res = fileOp(fcode, fileNum, recNum, recLen, data + 2);
|
||||
if (res != EX_SUCCESS) { // File read failed
|
||||
free(srcFrame);
|
||||
exceptionResponse(fcode, res);
|
||||
return;
|
||||
}
|
||||
data[0] = recLen * 2 + 1;
|
||||
data[1] = 0x06;
|
||||
data += recLen * 2 + 2;
|
||||
recs += 7;
|
||||
}
|
||||
_frame[0] = fcode;
|
||||
_frame[1] = bufSize;
|
||||
_reply = REPLY_NORMAL;
|
||||
free(srcFrame);
|
||||
}
|
||||
break;
|
||||
case FC_WRITE_FILE_REC: {
|
||||
if (frame[1] < 0x09 || frame[1] > 0xFB) { // Wrong request data size
|
||||
exceptionResponse(fcode, EX_ILLEGAL_VALUE);
|
||||
return;
|
||||
}
|
||||
uint8_t* recs = frame + 2; // Begin of sub-recs blocks
|
||||
while (recs < frame + frame[1]) {
|
||||
if (recs[0] != 0x06) {
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
uint16_t fileNum = (uint16_t)recs[1] << 8 | (uint16_t)recs[2];
|
||||
uint16_t recNum = (uint16_t)recs[3] << 8 | (uint16_t)recs[4];
|
||||
uint16_t recLen = (uint16_t)recs[5] << 8 | (uint16_t)recs[6];
|
||||
if (recs + recLen * 2 > frame + frame[1]) {
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
ResultCode res = fileOp(fcode, fileNum, recNum, recLen, recs + 7);
|
||||
if (res != EX_SUCCESS) { // File write failed
|
||||
exceptionResponse(fcode, res);
|
||||
return;
|
||||
}
|
||||
recs += 7 + recLen * 2;
|
||||
}
|
||||
}
|
||||
_reply = REPLY_ECHO;
|
||||
break;
|
||||
#endif
|
||||
case FC_MASKWRITE_REG:
|
||||
//field1 = reg, field2 = AND mask, field3 = OR mask
|
||||
// Result = (Current Contents AND And_Mask) OR (Or_Mask AND (NOT And_Mask))
|
||||
field3 = (uint16_t)frame[5] << 8 | (uint16_t)frame[6];
|
||||
ex = _onRequest(fcode, {HREG(field1), field2, field3});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
field4 = Reg(HREG(field1));
|
||||
field4 = (field4 & field2) | (field3 & ~field2);
|
||||
if (!Reg(HREG(field1), field4)) { //Check Address and execute (reg exists?)
|
||||
exceptionResponse(fcode, EX_ILLEGAL_ADDRESS);
|
||||
return;
|
||||
}
|
||||
if (Reg(HREG(field1)) != field4) { //Check for failure
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
_reply = REPLY_ECHO;
|
||||
_onRequestSuccess(fcode, {HREG(field1), field2, field3});
|
||||
break;
|
||||
case FC_READWRITE_REGS:
|
||||
//field1 = readreg, field2 = read count, frame[9] = data lenght, header len = 10
|
||||
//field3 = wtitereg, field4 = write count
|
||||
field3 = (uint16_t)frame[5] << 8 | (uint16_t)frame[6];
|
||||
field4 = (uint16_t)frame[7] << 8 | (uint16_t)frame[8];
|
||||
ex = _onRequest(fcode, {HREG(field1), field2, HREG(field3), field4});
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
if (field2 < 0x0001 || field2 > MODBUS_MAX_WORDS ||
|
||||
field4 < 0x0001 || field4 > MODBUS_MAX_WORDS ||
|
||||
0xFFFF - field1 < field2 || 0xFFFF - field1 < field2 ||
|
||||
frame[9] != 2 * field4) { //Check value
|
||||
exceptionResponse(fcode, EX_ILLEGAL_VALUE);
|
||||
return;
|
||||
}
|
||||
if (!setMultipleWords((uint16_t*)(frame + 10), HREG(field3), field4)) {
|
||||
exceptionResponse(fcode, EX_SLAVE_FAILURE);
|
||||
return;
|
||||
}
|
||||
ex = readWords(HREG(field1), field2, fcode);
|
||||
if (ex != EX_SUCCESS) {
|
||||
exceptionResponse(fcode, ex);
|
||||
return;
|
||||
}
|
||||
_onRequestSuccess(fcode, {HREG(field1), field2, HREG(field3), field4});
|
||||
break;
|
||||
|
||||
default:
|
||||
ex = _onRequest(fcode, {frame + 1});
|
||||
if (ex != EX_PASSTHROUGH) {
|
||||
exceptionResponse(fcode, EX_ILLEGAL_FUNCTION);
|
||||
}
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
void Modbus::successResponce(TAddress startreg, uint16_t numoutputs, FunctionCode fn) {
|
||||
free(_frame);
|
||||
_len = 5;
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) {
|
||||
_reply = REPLY_OFF;
|
||||
return;
|
||||
}
|
||||
_frame[0] = fn;
|
||||
_frame[1] = startreg.address >> 8;
|
||||
_frame[2] = startreg.address & 0x00FF;
|
||||
_frame[3] = numoutputs >> 8;
|
||||
_frame[4] = numoutputs & 0x00FF;
|
||||
}
|
||||
|
||||
void Modbus::exceptionResponse(FunctionCode fn, ResultCode excode) {
|
||||
free(_frame);
|
||||
_len = 2;
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) {
|
||||
_reply = REPLY_OFF;
|
||||
return;
|
||||
}
|
||||
_frame[0] = fn + 0x80;
|
||||
_frame[1] = excode;
|
||||
_reply = REPLY_NORMAL;
|
||||
}
|
||||
|
||||
void Modbus::getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs) {
|
||||
uint8_t bitn = 0;
|
||||
uint16_t i = 0;
|
||||
while (numregs--) {
|
||||
if (BIT_BOOL(Reg(startreg)))
|
||||
bitSet(frame[i], bitn);
|
||||
else
|
||||
bitClear(frame[i], bitn);
|
||||
bitn++; //increment the bit index
|
||||
if (bitn == 8) {
|
||||
i++;
|
||||
bitn = 0;
|
||||
}
|
||||
startreg++; //increment the register
|
||||
}
|
||||
}
|
||||
|
||||
void Modbus::getMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs) {
|
||||
for (uint8_t i = 0; i < numregs; i++) {
|
||||
frame[i] = __swap_16(Reg(startreg + i));
|
||||
}
|
||||
}
|
||||
|
||||
Modbus::ResultCode Modbus::readBits(TAddress startreg, uint16_t numregs, FunctionCode fn) {
|
||||
if (numregs < 0x0001 || numregs > MODBUS_MAX_BITS || (0xFFFF - startreg.address) < numregs)
|
||||
return EX_ILLEGAL_ADDRESS;
|
||||
//Check Address
|
||||
//Check only startreg. Is this correct?
|
||||
//When I check all registers in range I got errors in ScadaBR
|
||||
//I think that ScadaBR request more than one in the single request
|
||||
//when you have more then one datapoint configured from same type.
|
||||
#if defined(MODBUS_STRICT_REG)
|
||||
for (k = 0; k < numregs; k++) { //Check Address (startreg...startreg + numregs)
|
||||
if (!searchRegister(startreg + k))
|
||||
return EX_ILLEGAL_ADDRESS;
|
||||
}
|
||||
#else
|
||||
if (!searchRegister(startreg))
|
||||
return EX_ILLEGAL_ADDRESS;
|
||||
#endif
|
||||
free(_frame);
|
||||
//Determine the message length = function type, byte count and
|
||||
//for each group of 8 registers the message length increases by 1
|
||||
_len = 2 + numregs/8;
|
||||
if (numregs % 8) _len++; //Add 1 to the message length for the partial byte.
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame)
|
||||
return EX_SLAVE_FAILURE;
|
||||
_frame[0] = fn;
|
||||
_frame[1] = _len - 2; //byte count (_len - function code and byte count)
|
||||
_frame[_len - 1] = 0; //Clean last probably partial byte
|
||||
getMultipleBits(_frame+2, startreg, numregs);
|
||||
_reply = REPLY_NORMAL;
|
||||
return EX_SUCCESS;
|
||||
}
|
||||
|
||||
Modbus::ResultCode Modbus::readWords(TAddress startreg, uint16_t numregs, FunctionCode fn) {
|
||||
//Check value (numregs)
|
||||
if (numregs < 0x0001 || numregs > MODBUS_MAX_WORDS || 0xFFFF - startreg.address < numregs)
|
||||
return EX_ILLEGAL_ADDRESS;
|
||||
#if defined(MODBUS_STRICT_REG)
|
||||
for (k = 0; k < numregs; k++) { //Check Address (startreg...startreg + numregs)
|
||||
if (!searchRegister(startreg + k))
|
||||
return EX_ILLEGAL_ADDRESS;
|
||||
}
|
||||
#else
|
||||
if (!searchRegister(startreg))
|
||||
return EX_ILLEGAL_ADDRESS;
|
||||
#endif
|
||||
free(_frame);
|
||||
_len = 2 + numregs * 2; //calculate the query reply message length. 2 bytes per register + 2 bytes for header
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame)
|
||||
return EX_SLAVE_FAILURE;
|
||||
_frame[0] = fn;
|
||||
_frame[1] = _len - 2; //byte count
|
||||
getMultipleWords((uint16_t*)(_frame + 2), startreg, numregs);
|
||||
_reply = REPLY_NORMAL;
|
||||
return EX_SUCCESS;
|
||||
}
|
||||
|
||||
bool Modbus::setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutputs) {
|
||||
uint8_t bitn = 0;
|
||||
uint16_t i = 0;
|
||||
bool result = true;
|
||||
while (numoutputs--) {
|
||||
Reg(startreg, BIT_VAL(bitRead(frame[i], bitn)));
|
||||
if (Reg(startreg) != BIT_VAL(bitRead(frame[i], bitn)))
|
||||
result = false;
|
||||
bitn++; //increment the bit index
|
||||
if (bitn == 8) {
|
||||
i++;
|
||||
bitn = 0;
|
||||
}
|
||||
startreg++; //increment the register
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Modbus::setMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs) {
|
||||
bool result = true;
|
||||
for (uint8_t i = 0; i < numregs; i++) {
|
||||
Reg(startreg + i, __swap_16(frame[i]));
|
||||
if (Reg(startreg + i) != __swap_16(frame[i]))
|
||||
result = false;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Modbus::onGet(TAddress address, cbModbus cb, uint16_t numregs) {
|
||||
TRegister* reg;
|
||||
bool atLeastOne = false;
|
||||
if (!cb) {
|
||||
return removeOnGet(address, nullptr, numregs);
|
||||
}
|
||||
while (numregs > 0) {
|
||||
reg = searchRegister(address);
|
||||
if (reg) {
|
||||
_callbacks.push_back({TCallback::ON_GET, address, cb});
|
||||
atLeastOne = true;
|
||||
}
|
||||
address++;
|
||||
numregs--;
|
||||
}
|
||||
return atLeastOne;
|
||||
}
|
||||
bool Modbus::onSet(TAddress address, cbModbus cb, uint16_t numregs) {
|
||||
TRegister* reg;
|
||||
bool atLeastOne = false;
|
||||
if (!cb) {
|
||||
return removeOnSet(address, nullptr, numregs);
|
||||
}
|
||||
while (numregs > 0) {
|
||||
reg = searchRegister(address);
|
||||
if (reg) {
|
||||
_callbacks.push_back({TCallback::ON_SET, address, cb});
|
||||
atLeastOne = true;
|
||||
}
|
||||
address++;
|
||||
numregs--;
|
||||
}
|
||||
return atLeastOne;
|
||||
}
|
||||
|
||||
bool Modbus::removeOn(TCallback::CallbackType t, TAddress address, cbModbus cb, uint16_t numregs) {
|
||||
size_t s = _callbacks.size();
|
||||
#if defined(MODBUS_USE_STL)
|
||||
#define MODBUS_COMPARE_ON [t, address, cb](const TCallback entry){\
|
||||
return entry.type == t && entry.address == address \
|
||||
&& (!cb || std::addressof(cb) == std::addressof(entry.cb));}
|
||||
while(numregs--) {
|
||||
_callbacks.erase(remove_if(_callbacks.begin(), _callbacks.end(), MODBUS_COMPARE_ON), _callbacks.end());
|
||||
address++;
|
||||
}
|
||||
#else
|
||||
#define MODBUS_COMPARE_ON [t, address, cb](const TCallback entry){ \
|
||||
return entry.type == t && entry.address == address \
|
||||
&& (!cb || entry.cb == cb);}
|
||||
while(numregs--) {
|
||||
size_t r = 0;
|
||||
do {
|
||||
r = _callbacks.find(MODBUS_COMPARE_ON);
|
||||
_callbacks.remove(r);
|
||||
} while (r < _callbacks.size());
|
||||
address++;
|
||||
}
|
||||
#endif
|
||||
return s == _callbacks.size();
|
||||
}
|
||||
bool Modbus::removeOnSet(TAddress address, cbModbus cb, uint16_t numregs) {
|
||||
return removeOn(TCallback::ON_SET, address, cb, numregs);
|
||||
}
|
||||
|
||||
bool Modbus::removeOnGet(TAddress address, cbModbus cb, uint16_t numregs) {
|
||||
return removeOn(TCallback::ON_GET, address, cb, numregs);
|
||||
}
|
||||
|
||||
bool Modbus::readSlave(uint16_t address, uint16_t numregs, FunctionCode fn) {
|
||||
free(_frame);
|
||||
_len = 5;
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) {
|
||||
_reply = REPLY_OFF;
|
||||
return false;
|
||||
}
|
||||
_frame[0] = fn;
|
||||
_frame[1] = address >> 8;
|
||||
_frame[2] = address & 0x00FF;
|
||||
_frame[3] = numregs >> 8;
|
||||
_frame[4] = numregs & 0x00FF;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Modbus::writeSlaveBits(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, bool* data) {
|
||||
free(_frame);
|
||||
_len = 6 + numregs/8;
|
||||
if (numregs % 8) _len++; //Add 1 to the message length for the partial byte.
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) {
|
||||
_reply = REPLY_OFF;
|
||||
return false;
|
||||
}
|
||||
_frame[0] = fn;
|
||||
_frame[1] = to >> 8;
|
||||
_frame[2] = to & 0x00FF;
|
||||
_frame[3] = numregs >> 8;
|
||||
_frame[4] = numregs & 0x00FF;
|
||||
_frame[5] = _len - 6;
|
||||
_frame[_len - 1] = 0; //Clean last probably partial byte
|
||||
if (data) {
|
||||
boolToBits(_frame + 6, data, numregs);
|
||||
} else {
|
||||
getMultipleBits(_frame + 6, startreg, numregs);
|
||||
}
|
||||
_reply = REPLY_NORMAL;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, FunctionCode fn, uint16_t* data) {
|
||||
free(_frame);
|
||||
_len = 6 + 2 * numregs;
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) {
|
||||
_reply = REPLY_OFF;
|
||||
return false;
|
||||
}
|
||||
_frame[0] = fn;
|
||||
_frame[1] = to >> 8;
|
||||
_frame[2] = to & 0x00FF;
|
||||
_frame[3] = numregs >> 8;
|
||||
_frame[4] = numregs & 0x00FF;
|
||||
_frame[5] = _len - 6;
|
||||
if (data) {
|
||||
uint16_t* frame = (uint16_t*)(_frame + 6);
|
||||
for (uint8_t i = 0; i < numregs; i++) {
|
||||
frame[i] = __swap_16(data[i]);
|
||||
}
|
||||
} else {
|
||||
getMultipleWords((uint16_t*)(_frame + 6), startreg, numregs);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void Modbus::boolToBits(uint8_t* dst, bool* src, uint16_t numregs) {
|
||||
uint8_t bitn = 0;
|
||||
uint16_t i = 0;
|
||||
uint16_t j = 0;
|
||||
while (numregs--) {
|
||||
if (src[j])
|
||||
bitSet(dst[i], bitn);
|
||||
else
|
||||
bitClear(dst[i], bitn);
|
||||
bitn++; //increment the bit index
|
||||
if (bitn == 8) {
|
||||
i++;
|
||||
bitn = 0;
|
||||
}
|
||||
j++; //increment the register
|
||||
}
|
||||
}
|
||||
|
||||
void Modbus::bitsToBool(bool* dst, uint8_t* src, uint16_t numregs) {
|
||||
uint8_t bitn = 0;
|
||||
uint16_t i = 0;
|
||||
uint16_t j = 0;
|
||||
while (numregs--) {
|
||||
dst[j] = bitRead(src[i], bitn);
|
||||
bitn++; //increment the bit index
|
||||
if (bitn == 8) {
|
||||
i++;
|
||||
bitn = 0;
|
||||
}
|
||||
j++; //increment the register
|
||||
}
|
||||
}
|
||||
|
||||
void Modbus::masterPDU(uint8_t* frame, uint8_t* sourceFrame, TAddress startreg, uint8_t* output) {
|
||||
uint8_t fcode = frame[0];
|
||||
if ((fcode & 0x80) != 0) { // Check if error responce
|
||||
_reply = frame[1];
|
||||
return;
|
||||
}
|
||||
if (fcode != sourceFrame[0]) { // Check if responce matches the request
|
||||
_reply = EX_DATA_MISMACH;
|
||||
return;
|
||||
}
|
||||
_reply = EX_SUCCESS;
|
||||
uint16_t field2 = (uint16_t)sourceFrame[3] << 8 | (uint16_t)sourceFrame[4];
|
||||
uint8_t bytecount_calc;
|
||||
switch (fcode) {
|
||||
case FC_READ_REGS:
|
||||
case FC_READ_INPUT_REGS:
|
||||
case FC_READWRITE_REGS:
|
||||
//field2 = numregs, frame[1] = data lenght, header len = 2
|
||||
if (frame[1] != 2 * field2) { //Check if data size matches
|
||||
_reply = EX_DATA_MISMACH;
|
||||
break;
|
||||
}
|
||||
if (output) {
|
||||
uint16_t* from = (uint16_t*)(frame + 2);
|
||||
uint16_t* to = (uint16_t*)output;
|
||||
while(field2--) {
|
||||
*(to++) = __swap_16(*(from++));
|
||||
}
|
||||
} else {
|
||||
setMultipleWords((uint16_t*)(frame + 2), startreg, field2);
|
||||
}
|
||||
break;
|
||||
case FC_READ_COILS:
|
||||
case FC_READ_INPUT_STAT:
|
||||
//field2 = numregs, frame[1] = data length, header len = 2
|
||||
bytecount_calc = field2 / 8;
|
||||
if (field2 % 8) bytecount_calc++;
|
||||
if (frame[1] != bytecount_calc) { // check if data size matches
|
||||
_reply = EX_DATA_MISMACH;
|
||||
break;
|
||||
}
|
||||
if (output) {
|
||||
bitsToBool((bool*)output, frame + 2, field2);
|
||||
} else {
|
||||
setMultipleBits(frame + 2, startreg, field2);
|
||||
}
|
||||
break;
|
||||
#if defined(MODBUS_FILES)
|
||||
case FC_READ_FILE_REC:
|
||||
// Should check if byte order swap needed
|
||||
if (frame[1] < 0x07 || frame[1] > 0xF5) { // Wrong request data size
|
||||
_reply = EX_ILLEGAL_VALUE;
|
||||
return;
|
||||
}
|
||||
{
|
||||
uint8_t* data = frame + 2;
|
||||
uint8_t* eoFrame = frame + frame[1];
|
||||
while (data < eoFrame) {
|
||||
//data[0] - sub-req length
|
||||
//data[1] = 0x06
|
||||
if (data[1] != 0x06 || data[0] < 0x07 || data[0] > 0xF5 || data + data[0] > eoFrame) { // Wrong request data size
|
||||
_reply = EX_ILLEGAL_VALUE;
|
||||
return;
|
||||
}
|
||||
memcpy(output, data + 2, data[0]);
|
||||
data += data[0] + 1;
|
||||
output += data[0] - 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case FC_WRITE_FILE_REC:
|
||||
#endif
|
||||
case FC_WRITE_REG:
|
||||
case FC_WRITE_REGS:
|
||||
case FC_WRITE_COIL:
|
||||
case FC_WRITE_COILS:
|
||||
case FC_MASKWRITE_REG:
|
||||
break;
|
||||
|
||||
default:
|
||||
_reply = EX_GENERAL_FAILURE;
|
||||
}
|
||||
}
|
||||
|
||||
bool Modbus::cbEnable(const bool state) {
|
||||
const bool old_state = state;
|
||||
cbEnabled = state;
|
||||
return old_state;
|
||||
}
|
||||
bool Modbus::cbDisable() {
|
||||
return cbEnable(false);
|
||||
}
|
||||
Modbus::~Modbus() {
|
||||
free(_frame);
|
||||
}
|
||||
|
||||
#if defined(MODBUS_FILES)
|
||||
#if defined(MODBUS_USE_STL)
|
||||
bool Modbus::onFile(std::function<Modbus::ResultCode(Modbus::FunctionCode, uint16_t, uint16_t, uint16_t, uint8_t*)> cb) {
|
||||
#else
|
||||
bool Modbus::onFile(Modbus::ResultCode (*cb)(Modbus::FunctionCode, uint16_t, uint16_t, uint16_t, uint8_t*)) {
|
||||
#endif
|
||||
_onFile = cb;
|
||||
return true;
|
||||
}
|
||||
Modbus::ResultCode Modbus::fileOp(Modbus::FunctionCode fc, uint16_t fileNum, uint16_t recNum, uint16_t recLen, uint8_t* frame) {
|
||||
if (!_onFile) return EX_ILLEGAL_ADDRESS;
|
||||
return _onFile(fc, fileNum, recNum, recLen, frame);
|
||||
}
|
||||
|
||||
bool Modbus::readSlaveFile(uint16_t* fileNum, uint16_t* startRec, uint16_t* len, uint8_t count, FunctionCode fn) {
|
||||
_len = count * 7 + 2;
|
||||
if (_len > MODBUS_MAX_FRAME) return false;
|
||||
free(_frame);
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) return false;
|
||||
_frame[0] = fn;
|
||||
_frame[1] = _len - 2;
|
||||
uint8_t* subReq = _frame + 2;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
subReq[0] = 0x06;
|
||||
subReq[1] = fileNum[i] >> 8;
|
||||
subReq[2] = fileNum[i] & 0x00FF;
|
||||
subReq[3] = startRec[i] >> 8;
|
||||
subReq[4] = startRec[i] & 0x00FF;
|
||||
subReq[5] = len[i] >> 8;
|
||||
subReq[6] = len[i] & 0x00FF;
|
||||
subReq += 7;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool Modbus::writeSlaveFile(uint16_t* fileNum, uint16_t* startRec, uint16_t* len, uint8_t count, FunctionCode fn, uint8_t* data) {
|
||||
_len = 2;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
_len += len[i] * 2 + 7;
|
||||
}
|
||||
if (_len > MODBUS_MAX_FRAME) return false;
|
||||
free(_frame);
|
||||
_frame = (uint8_t*) malloc(_len);
|
||||
if (!_frame) return false;
|
||||
_frame[0] = fn;
|
||||
_frame[1] = _len - 2;
|
||||
uint8_t* subReq = _frame + 2;
|
||||
for (uint8_t i = 0; i < count; i++) {
|
||||
subReq[0] = 0x06;
|
||||
subReq[1] = fileNum[i] >> 8;
|
||||
subReq[2] = fileNum[i] & 0x00FF;
|
||||
subReq[3] = startRec[i] >> 8;
|
||||
subReq[4] = startRec[i] & 0x00FF;
|
||||
subReq[5] = len[i] >> 8;
|
||||
subReq[6] = len[i] & 0x00FF;
|
||||
uint8_t clen = len[i] * 2;
|
||||
memcpy(subReq + 7, data, clen);
|
||||
subReq += 7 + clen;
|
||||
data += clen;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool Modbus::onRaw(cbRaw cb) {
|
||||
_cbRaw = cb;
|
||||
return true;
|
||||
}
|
||||
Modbus::ResultCode Modbus::_onRequestDefault(Modbus::FunctionCode fc, const RequestData data) {
|
||||
return EX_SUCCESS;
|
||||
}
|
||||
bool Modbus::onRequest(cbRequest cb) {
|
||||
_onRequest = cb;
|
||||
return true;
|
||||
}
|
||||
#if defined (MODBUSAPI_OPTIONAL)
|
||||
bool Modbus::onRequestSuccess(cbRequest cb) {
|
||||
_onRequestSuccess = cb;
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(ARDUINO_SAM_DUE_STL)
|
||||
namespace std {
|
||||
void __throw_bad_function_call() {
|
||||
Serial.println(F("STL ERROR - __throw_bad_function_call"));
|
||||
__builtin_unreachable();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user