4#include "Stroika/Frameworks/StroikaPreComp.h"
11#include "Stroika/Foundation/IO/Network/Listener.h"
28using namespace Stroika::Foundation::Debug;
30using namespace Stroika::Foundation::Memory;
31using namespace Stroika::Foundation::IO;
33using namespace Stroika::Foundation::Streams;
34using namespace Stroika::Foundation::Traversal;
36using namespace Stroika::Frameworks;
46 enum FunctionCodeType_ : uint8_t {
48 kReadDiscreteInputs_ = 2,
49 kReadHoldingResisters_ = 3,
50 kReadInputRegister_ = 4,
51 kWriteSingleCoil_ = 5,
64 struct MBAPHeaderIsh_ {
65 alignas (2) uint16_t fTransactionID;
66 alignas (2) uint16_t fProtocolID;
67 alignas (2) uint16_t fLength;
68 alignas (1) uint8_t fUnitID;
69 alignas (1) FunctionCodeType_ fFunctionCode;
71 static constexpr size_t kExtraLengthFromThisHeaderAccountedInPayloadLength{2};
73 size_t GetPayloadLength ()
const
77 Require (fLength >= kExtraLengthFromThisHeaderAccountedInPayloadLength);
78 return fLength - kExtraLengthFromThisHeaderAccountedInPayloadLength;
82 static_assert (
sizeof (MBAPHeaderIsh_) == 8,
"");
83 static_assert (offsetof (MBAPHeaderIsh_, fTransactionID) == 0,
"");
84 static_assert (offsetof (MBAPHeaderIsh_, fProtocolID) == 2,
"");
85 static_assert (offsetof (MBAPHeaderIsh_, fLength) == 4,
"");
86 static_assert (offsetof (MBAPHeaderIsh_, fUnitID) == 6,
"");
87 static_assert (offsetof (MBAPHeaderIsh_, fFunctionCode) == 7,
"");
94 uint16_t FromNetwork_ (uint16_t v)
98 MBAPHeaderIsh_ FromNetwork_ (
const MBAPHeaderIsh_& v)
100 return MBAPHeaderIsh_{FromNetwork_ (v.fTransactionID), FromNetwork_ (v.fProtocolID), FromNetwork_ (v.fLength), v.fUnitID, v.fFunctionCode};
102 uint16_t ToNetwork_ (uint16_t v)
106 MBAPHeaderIsh_ ToNetwork_ (
const MBAPHeaderIsh_& v)
108 return MBAPHeaderIsh_{ToNetwork_ (v.fTransactionID), ToNetwork_ (v.fProtocolID), ToNetwork_ (v.fLength), v.fUnitID, v.fFunctionCode};
118 case FunctionCodeType_::kReadCoils_:
119 return "Read-Coils"_k;
120 case FunctionCodeType_::kReadDiscreteInputs_:
121 return "Read-Discrete-Inputs"_k;
122 case FunctionCodeType_::kReadHoldingResisters_:
123 return "Read-Holding-Resisters_"_k;
124 case FunctionCodeType_::kReadInputRegister_:
125 return "Read-Input-Register_"_k;
126 case FunctionCodeType_::kWriteSingleCoil_:
127 return "WriteSingleCoil"_k;
129 return Format (
"{}"_f, f);
136 sb <<
"TransactionID: "sv << mh.fTransactionID;
137 sb <<
", ProtocolID: "sv << mh.fProtocolID;
138 sb <<
", Length: "sv << mh.fLength;
139 sb <<
", UnitID: "sv << mh.fUnitID;
140 sb <<
", FunctionCode: "sv << mh.fFunctionCode;
148 const ServerOptions& options)
151#if qStroika_Foundation_Debug_DefaultTracingOn
152 static atomic<uint32_t> sConnectionNumber_;
153 uint32_t thisModbusConnectionNumber = ++sConnectionNumber_;
154 DbgTrace (
"Starting Modbus connection {}"_f, thisModbusConnectionNumber);
155 [[maybe_unused]]
auto&& cleanup =
156 Finally ([thisModbusConnectionNumber] () {
DbgTrace (
"Finishing Modbus connection {}"_f, thisModbusConnectionNumber); });
160 DbgTrace (
"Starting connection from peer: {}"_f, *p);
168 auto checkedReadHelperPayload2Shorts = [] (
const Memory::BLOB& requestPayload, uint16_t minSecondValue,
169 uint16_t maxSecondValue) -> pair<uint16_t, uint16_t> {
173 if (requestPayload.
size () != 4) {
174 DbgTrace (
"requestPayload={}"_f, requestPayload);
177 uint16_t startingAddress = FromNetwork_ (*
reinterpret_cast<const uint16_t*
> (requestPayload.
begin () + 0));
178 uint16_t quantity = FromNetwork_ (*
reinterpret_cast<const uint16_t*
> (requestPayload.
begin () + 2));
179 if (not(minSecondValue <= quantity and quantity <= maxSecondValue)) {
180 Throw (
Execution::Exception{
"Invalid quantity parameter ({}): expected value from {}..{}"_f(quantity, minSecondValue, maxSecondValue)});
182 return pair<uint16_t, uint16_t>{startingAddress, quantity};
191 MBAPHeaderIsh_ requestHeader;
192 size_t n = in.
ReadAll (as_writable_bytes (span{&requestHeader, 1})).
size ();
193 if (n !=
sizeof (requestHeader)) {
201 requestHeader = FromNetwork_ (requestHeader);
206 if (requestHeader.fProtocolID != 0) {
209 if (requestHeader.fLength < 2) {
215 auto zeroToOneBased = [] (uint16_t i) -> uint16_t {
return i + 1; };
216 auto oneBasedToZeroBased = [] (uint16_t i) -> uint16_t {
return i - 1; };
217 switch (requestHeader.fFunctionCode) {
218 case FunctionCodeType_::kReadCoils_: {
222 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).first;
223 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).second;
224 Assert (quantity >= 1);
225 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
226 size_t quantityBytes = (quantity + 7) / 8;
228 (void)::memset (results.begin (), 0, quantityBytes);
229#if USE_NOISY_TRACE_IN_THIS_MODULE_
230 DbgTrace (
"Processing kReadCoils_ (starting0Address: {}, quantity: {}) message with request-header={}",
231 startingAddress, quantity, requestHeader);
234 serviceHandler->ReadCoils (
DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
237 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) < endInclusiveAddress and i.fValue) {
238 results[(oneBasedToZeroBased (i.fKey) - startingAddress) / 8] |=
239 Memory::Bit ((oneBasedToZeroBased (i.fKey) - startingAddress) % 8);
242#if USE_NOISY_TRACE_IN_THIS_MODULE_
243 DbgTrace (
"results bitmask bytes={}",
Memory::BLOB{
reinterpret_cast<const byte*
> (results.begin ()),
244 reinterpret_cast<const byte*
> (results.end ())});
248 uint8_t responseLen =
static_cast<uint8_t
> (quantityBytes);
249 MBAPHeaderIsh_ responseHeader =
250 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
251 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
252 sizeof (responseLen) + responseLen),
253 requestHeader.fUnitID, requestHeader.fFunctionCode};
254 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
255 out.
Write (Memory::AsBytes (responseLen));
256 out.
Write (span{results.begin (), responseLen});
257#if USE_NOISY_TRACE_IN_THIS_MODULE_
258 DbgTrace (
"Sent response: header={}, responseLen={}, responsePayload={}"_f, responseHeader, responseLen,
263 case FunctionCodeType_::kReadDiscreteInputs_: {
267 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).first;
268 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).second;
269 Assert (quantity >= 1);
270 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
271 size_t quantityBytes = (quantity + 7) / 8;
273#if USE_NOISY_TRACE_IN_THIS_MODULE_
274 DbgTrace (
"Processing kReadDiscreteInputs_ (starting0Address: {}, quantity: {}) message with request-header={}"_f,
275 startingAddress, quantity, requestHeader);
277 for (
const auto& i : serviceHandler->ReadDiscreteInput (
278 DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
281 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) <= endInclusiveAddress) {
283 results[(oneBasedToZeroBased (i.fKey) - startingAddress) / 8] |=
284 Memory::Bit ((oneBasedToZeroBased (i.fKey) - startingAddress) % 8);
290 uint8_t responseLen =
static_cast<uint8_t
> (quantityBytes);
291 MBAPHeaderIsh_ responseHeader =
292 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
293 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
294 sizeof (responseLen) + responseLen),
295 requestHeader.fUnitID, requestHeader.fFunctionCode};
296 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
297 out.
Write (Memory::AsBytes (responseLen));
298 out.
Write (span{results.begin (), responseLen});
299#if USE_NOISY_TRACE_IN_THIS_MODULE_
300 DbgTrace (
"Sent response: header={}, responseLen={}"_f, responseHeader, responseLen);
304 case FunctionCodeType_::kReadHoldingResisters_: {
308 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).first;
309 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).second;
310 Assert (quantity >= 1);
311 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
313#if USE_NOISY_TRACE_IN_THIS_MODULE_
314 DbgTrace (
"Processing kReadHoldingResisters_ (starting0Address: {}, quantity: {}) message with request-header={}"_f,
315 startingAddress, quantity, requestHeader);
317 for (
const auto& i : serviceHandler->ReadHoldingRegisters (
318 DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
321 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) <= endInclusiveAddress) {
322 results[oneBasedToZeroBased (i.fKey) - startingAddress] = ToNetwork_ (i.fValue);
327 uint8_t responseLen =
static_cast<uint8_t
> (quantity *
sizeof (results[0]));
328 MBAPHeaderIsh_ responseHeader =
329 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
330 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
331 sizeof (responseLen) + responseLen),
332 requestHeader.fUnitID, requestHeader.fFunctionCode};
333 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
334 out.
Write (Memory::AsBytes (responseLen));
335 out.
Write (span{
reinterpret_cast<const byte*
> (results.begin ()), responseLen});
336#if USE_NOISY_TRACE_IN_THIS_MODULE_
337 DbgTrace (
"Sent response: header={}, responseLen={}"_f, responseHeader, responseLen);
341 case FunctionCodeType_::kReadInputRegister_: {
345 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).first;
346 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).second;
347 Assert (quantity >= 1);
348 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
350#if USE_NOISY_TRACE_IN_THIS_MODULE_
351 DbgTrace (
"Processing kReadInputRegister_ (starting0Address: {}, quantity: {}) message with request-header={}"_f,
352 startingAddress, quantity, requestHeader);
354 for (
const auto& i : serviceHandler->ReadInputRegisters (
355 DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
358 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) <= endInclusiveAddress) {
359 results[oneBasedToZeroBased (i.fKey) - startingAddress] = ToNetwork_ (i.fValue);
364 uint8_t responseLen =
static_cast<uint8_t
> (quantity *
sizeof (results[0]));
365 MBAPHeaderIsh_ responseHeader =
366 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
367 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
368 sizeof (responseLen) + responseLen),
369 requestHeader.fUnitID, requestHeader.fFunctionCode};
370 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
371 out.
Write (Memory::AsBytes (responseLen));
372 out.
Write (span{
reinterpret_cast<const byte*
> (results.begin ()), responseLen});
373#if USE_NOISY_TRACE_IN_THIS_MODULE_
374 DbgTrace (
"Sent response: header={}, responseLen={}"_f, responseHeader, responseLen);
378 case FunctionCodeType_::kWriteSingleCoil_: {
382 uint16_t outputAddress = checkedReadHelperPayload2Shorts (requestPayload, 0, 0xff00).first;
383 uint16_t value = checkedReadHelperPayload2Shorts (requestPayload, 0, 0xff00).second;
384#if USE_NOISY_TRACE_IN_THIS_MODULE_
385 DbgTrace (
"Processing kWriteSingleCoil_ (outputAddress: {}, value: {}) message with request-header={}"_f,
386 outputAddress, value, requestHeader);
388 serviceHandler->WriteCoils ({{zeroToOneBased (outputAddress), value == 0 ? false :
true}});
391 out.
Write (Memory::AsBytes (requestHeader));
392 out.
Write (Memory::AsBytes (ToNetwork_ (outputAddress)));
393 out.
Write (Memory::AsBytes (ToNetwork_ (value)));
394#if USE_NOISY_TRACE_IN_THIS_MODULE_
395 DbgTrace (
"Sent response: header={}"_f, requestHeader);
400 DbgTrace (
"UNREGONIZED FunctionCode (NYI probably) - {} - so echo ILLEGAL_FUNCTION code"_f, requestHeader.fFunctionCode);
401 if (options.fLogger) {
402 options.fLogger.value ()->Log (Logger::eWarning,
"ModbusTCP unrecognized function code '{}'- rejected as ILLEGAL_FUNCTION"_f,
403 requestHeader.fFunctionCode);
405 MBAPHeaderIsh_ responseHeader = requestHeader;
406 responseHeader.fFunctionCode =
static_cast<FunctionCodeType_
> (responseHeader.fFunctionCode | 0x80);
407 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
408 out.
Write (Memory::AsBytes (
static_cast<uint8_t
> (ExceptionCode::ILLEGAL_FUNCTION)));
409#if USE_NOISY_TRACE_IN_THIS_MODULE_
410 DbgTrace (
"Sent UNREGONIZED_FUNCTION response: header={}, and exceptionCode={}"_f, responseHeader, exceptionCode);
417 catch (
const Thread::AbortException&) {
422 if (options.fLogger) {
423 options.fLogger.value ()->Log (Logger::eWarning,
"ModbusTCP connection ended abnormally: {}"_f, current_exception ());
437 shared_ptr<ThreadPool> usingThreadPool = options.fThreadPool;
438 if (usingThreadPool ==
nullptr) {
439 usingThreadPool = make_shared<ThreadPool> (ThreadPool::Options{.fThreadCount = 1});
444 usingThreadPool->AddTask ([serviceHandler, options, s] () { ConnectionHandler_ (s, serviceHandler, options); });
447 [onModbusConnection, options] () {
448#if USE_NOISY_TRACE_IN_THIS_MODULE_
451 uint16_t usingPortNumber = options.fListenPort.value_or (502);
452 if (options.fLogger) {
453 options.fLogger.value ()->Log (Logger::eInfo,
"Listening for ModbusTCP requests on port {}"_f, usingPortNumber);
457 "Modbus-Listener"_k);
#define qStroika_Foundation_Debug_AssertionsChecked
The qStroika_Foundation_Debug_AssertionsChecked flag determines if assertions are checked and validat...
Similar to String, but intended to more efficiently construct a String. Mutable type (String is large...
String is like std::u32string, except it is much easier to use, often much more space efficient,...
nonvirtual size_t size() const noexcept
Set<T> is a container of T, where once an item is added, additionally adds () do nothing.
Exception<> is a replacement (subclass) for any std c++ exception class (e.g. the default 'std::excep...
Thread::Ptr is a (unsynchronized) smart pointer referencing an internally synchronized std::thread ob...
nonvirtual void Wait(Time::DurationSeconds timeout=Time::kInfinity)
nonvirtual optional< IO::Network::SocketAddress > GetPeerAddress() const
nonvirtual const byte * begin() const
nonvirtual size_t size() const
Logically halfway between std::array and std::vector; Smart 'direct memory array' - which when needed...
OutputStream<>::Ptr is Smart pointer to a stream-based sink of data.
nonvirtual void Write(span< ELEMENT_TYPE2, EXTENT_2 > elts) const
nonvirtual void Flush() const
forces any data contained in this stream to be written.
A DiscreteRange is a Range where the underlying endpoints are integral (discrete, not continuous); th...
nonvirtual Iterable< T > Elements() const
String ToString(T &&t, ARGS... args)
Return a debug-friendly, display version of the argument: not guaranteed parsable or usable except fo...
constexpr Endian GetEndianness()
returns native (std::endian::native) Endianness flag. Can be complicated (mixed, etc)....
constexpr T EndianConverter(T value, Endian from, Endian to)
Ptr New(const function< void()> &fun2CallOnce, const optional< Characters::String > &name, const optional< Configuration > &configuration)
void Throw(T &&e2Throw)
identical to builtin C++ 'throw' except that it does helpful, type dependent DbgTrace() messages firs...
auto Finally(FUNCTION &&f) -> Private_::FinallySentry< FUNCTION >
Execution::Thread::Ptr MakeModbusTCPServerThread(const shared_ptr< IModbusService > &serviceHandler, const ServerOptions &options=ServerOptions{})