Skip to content

Instantly share code, notes, and snippets.

@TurkeyMan

TurkeyMan/ex.d Secret

Last active August 28, 2024 09:40
Show Gist options
  • Save TurkeyMan/0e49da245cc0086f852ac18deed21a9c to your computer and use it in GitHub Desktop.
Save TurkeyMan/0e49da245cc0086f852ac18deed21a9c to your computer and use it in GitHub Desktop.
Input validation...
int parseFrame(const(ubyte)[] data, out ModbusFrameInfo frameInfo)
{
if (data.length < 4) // unlikely
return 0;
// check the address is in the valid range
ubyte address = data[0];
if (address >= 248 && address <= 255) // unlikely
return 0;
frameInfo.address = address;
// frames must start with a valid function code...
ubyte f = data[1];
FunctionCode fc = cast(FunctionCode)(f & 0x7F);
ushort fnData = fc < functionLens.length ? functionLens[f] : fc == 0x2B ? 0xFFFF : 0;
if (fnData == 0) // unlikely
return 0;
frameInfo.functionCode = fc;
// exceptions are always 3 bytes
ubyte reqLength = void;
ubyte resLength = void;
if (f & 0x80) // unlikely
{
frameInfo.exceptionCode = cast(ExceptionCode)data[2];
frameInfo.frameType = ModbusFrameType.Response;
reqLength = 3;
resLength = 3;
}
// zero bytes (broadcast address) are common in data streams, and if the function code can't broadcast, we can exclude this packet
// NOTE: this can only catch 10 bad bytes in the second byte position... maybe not worth the if()?
// else if (address == 0 && (fFlags & 2) == 0) // unlikely
// {
// frameId.invalidFrame = true;
// return false;
// }
// if the function code can determine the length...
else if (fnData != 0xFFFF)
{
// TODO: we can instead load these bytes separately if the bit-shifting is worse than loads...
reqLength = fnData & 0xF;
ubyte reqExtra = (fnData >> 4) & 0xF;
resLength = (fnData >> 8) & 0xF;
ubyte resExtra = fnData >> 12;
if (reqExtra && reqExtra < data.length)
reqLength += data[reqExtra];
if (resExtra)
resLength += data[resExtra];
}
else
{
// scan for a CRC...
assert(false);
}
int failResult = 0;
if (reqLength != resLength)
{
ubyte length = void, smallerLength = void;
ModbusFrameType type = void, smallerType = void;
if (reqLength > resLength)
{
length = reqLength;
smallerLength = resLength;
type = ModbusFrameType.Request;
smallerType = ModbusFrameType.Response;
}
else
{
length = resLength;
smallerLength = reqLength;
type = ModbusFrameType.Response;
smallerType = ModbusFrameType.Request;
}
if (data.length >= length + 2)
{
uint crc2 = calculateModbusCRC2(data[0 .. length], smallerLength);
if ((crc2 >> 16) == (data[smallerLength] | cast(ushort)data[smallerLength + 1] << 8))
{
frameInfo.frameType = smallerType;
return smallerLength;
}
if ((crc2 & 0xFFFF) == (data[length] | cast(ushort)data[length + 1] << 8))
{
frameInfo.frameType = type;
return length;
}
return 0;
}
else
{
failResult = -1;
reqLength = smallerLength;
frameInfo.frameType = smallerType;
}
}
// check we have enough data...
if (data.length < reqLength + 2) // unlikely
return -1;
ushort crc = calculateModbusCRC(data[0 .. reqLength]);
if (crc == (data[reqLength] | cast(ushort)data[reqLength + 1] << 8))
return reqLength;
return failResult;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment