4#include "Stroika/Frameworks/StroikaPreComp.h"
11#include "Stroika/Foundation/IO/Network/Listener.h"
29using namespace Stroika::Foundation::Debug;
31using namespace Stroika::Foundation::Memory;
32using namespace Stroika::Foundation::IO;
34using namespace Stroika::Foundation::Streams;
35using namespace Stroika::Foundation::Traversal;
37using Memory::MakeSharedPtr;
39using namespace Stroika::Frameworks;
49 enum FunctionCodeType_ : uint8_t {
51 kReadDiscreteInputs_ = 2,
52 kReadHoldingResisters_ = 3,
53 kReadInputRegister_ = 4,
54 kWriteSingleCoil_ = 5,
67 struct MBAPHeaderIsh_ {
68 alignas (2) uint16_t fTransactionID;
69 alignas (2) uint16_t fProtocolID;
70 alignas (2) uint16_t fLength;
71 alignas (1) uint8_t fUnitID;
72 alignas (1) FunctionCodeType_ fFunctionCode;
74 static constexpr size_t kExtraLengthFromThisHeaderAccountedInPayloadLength{2};
76 size_t GetPayloadLength ()
const
80 Require (fLength >= kExtraLengthFromThisHeaderAccountedInPayloadLength);
81 return fLength - kExtraLengthFromThisHeaderAccountedInPayloadLength;
85 static_assert (
sizeof (MBAPHeaderIsh_) == 8,
"");
86 static_assert (offsetof (MBAPHeaderIsh_, fTransactionID) == 0,
"");
87 static_assert (offsetof (MBAPHeaderIsh_, fProtocolID) == 2,
"");
88 static_assert (offsetof (MBAPHeaderIsh_, fLength) == 4,
"");
89 static_assert (offsetof (MBAPHeaderIsh_, fUnitID) == 6,
"");
90 static_assert (offsetof (MBAPHeaderIsh_, fFunctionCode) == 7,
"");
97 uint16_t FromNetwork_ (uint16_t v)
101 MBAPHeaderIsh_ FromNetwork_ (
const MBAPHeaderIsh_& v)
103 return MBAPHeaderIsh_{FromNetwork_ (v.fTransactionID), FromNetwork_ (v.fProtocolID), FromNetwork_ (v.fLength), v.fUnitID, v.fFunctionCode};
105 uint16_t ToNetwork_ (uint16_t v)
109 MBAPHeaderIsh_ ToNetwork_ (
const MBAPHeaderIsh_& v)
111 return MBAPHeaderIsh_{ToNetwork_ (v.fTransactionID), ToNetwork_ (v.fProtocolID), ToNetwork_ (v.fLength), v.fUnitID, v.fFunctionCode};
121 case FunctionCodeType_::kReadCoils_:
122 return "Read-Coils"_k;
123 case FunctionCodeType_::kReadDiscreteInputs_:
124 return "Read-Discrete-Inputs"_k;
125 case FunctionCodeType_::kReadHoldingResisters_:
126 return "Read-Holding-Resisters_"_k;
127 case FunctionCodeType_::kReadInputRegister_:
128 return "Read-Input-Register_"_k;
129 case FunctionCodeType_::kWriteSingleCoil_:
130 return "WriteSingleCoil"_k;
132 return Format (
"{}"_f, f);
139 sb <<
"TransactionID: "sv << mh.fTransactionID;
140 sb <<
", ProtocolID: "sv << mh.fProtocolID;
141 sb <<
", Length: "sv << mh.fLength;
142 sb <<
", UnitID: "sv << mh.fUnitID;
143 sb <<
", FunctionCode: "sv << mh.fFunctionCode;
151 const ServerOptions& options)
154#if qStroika_Foundation_Debug_DefaultTracingOn
155 static atomic<uint32_t> sConnectionNumber_;
156 uint32_t thisModbusConnectionNumber = ++sConnectionNumber_;
157 DbgTrace (
"Starting Modbus connection {}"_f, thisModbusConnectionNumber);
158 [[maybe_unused]]
auto&& cleanup =
159 Finally ([thisModbusConnectionNumber] () {
DbgTrace (
"Finishing Modbus connection {}"_f, thisModbusConnectionNumber); });
163 DbgTrace (
"Starting connection from peer: {}"_f, *p);
171 auto checkedReadHelperPayload2Shorts = [] (
const Memory::BLOB& requestPayload, uint16_t minSecondValue,
172 uint16_t maxSecondValue) -> pair<uint16_t, uint16_t> {
176 if (requestPayload.
size () != 4) {
177 DbgTrace (
"requestPayload={}"_f, requestPayload);
180 uint16_t startingAddress = FromNetwork_ (*
reinterpret_cast<const uint16_t*
> (requestPayload.
begin () + 0));
181 uint16_t quantity = FromNetwork_ (*
reinterpret_cast<const uint16_t*
> (requestPayload.
begin () + 2));
182 if (not(minSecondValue <= quantity and quantity <= maxSecondValue)) {
183 Throw (
Execution::Exception{
"Invalid quantity parameter ({}): expected value from {}..{}"_f(quantity, minSecondValue, maxSecondValue)});
185 return pair<uint16_t, uint16_t>{startingAddress, quantity};
194 MBAPHeaderIsh_ requestHeader;
195 size_t n = in.
ReadAll (as_writable_bytes (span{&requestHeader, 1})).
size ();
196 if (n !=
sizeof (requestHeader)) {
204 requestHeader = FromNetwork_ (requestHeader);
209 if (requestHeader.fProtocolID != 0) {
212 if (requestHeader.fLength < 2) {
218 auto zeroToOneBased = [] (uint16_t i) -> uint16_t {
return i + 1; };
219 auto oneBasedToZeroBased = [] (uint16_t i) -> uint16_t {
return i - 1; };
220 switch (requestHeader.fFunctionCode) {
221 case FunctionCodeType_::kReadCoils_: {
225 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).first;
226 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).second;
227 Assert (quantity >= 1);
228 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
229 size_t quantityBytes = (quantity + 7) / 8;
231 (void)::memset (results.begin (), 0, quantityBytes);
232#if USE_NOISY_TRACE_IN_THIS_MODULE_
233 DbgTrace (
"Processing kReadCoils_ (starting0Address: {}, quantity: {}) message with request-header={}",
234 startingAddress, quantity, requestHeader);
237 serviceHandler->ReadCoils (
DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
240 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) < endInclusiveAddress and i.fValue) {
241 results[(oneBasedToZeroBased (i.fKey) - startingAddress) / 8] |=
242 Memory::Bit ((oneBasedToZeroBased (i.fKey) - startingAddress) % 8);
245#if USE_NOISY_TRACE_IN_THIS_MODULE_
246 DbgTrace (
"results bitmask bytes={}",
Memory::BLOB{
reinterpret_cast<const byte*
> (results.begin ()),
247 reinterpret_cast<const byte*
> (results.end ())});
251 uint8_t responseLen =
static_cast<uint8_t
> (quantityBytes);
252 MBAPHeaderIsh_ responseHeader =
253 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
254 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
255 sizeof (responseLen) + responseLen),
256 requestHeader.fUnitID, requestHeader.fFunctionCode};
257 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
258 out.
Write (Memory::AsBytes (responseLen));
259 out.
Write (span{results.begin (), responseLen});
260#if USE_NOISY_TRACE_IN_THIS_MODULE_
261 DbgTrace (
"Sent response: header={}, responseLen={}, responsePayload={}"_f, responseHeader, responseLen,
266 case FunctionCodeType_::kReadDiscreteInputs_: {
270 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).first;
271 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d0).second;
272 Assert (quantity >= 1);
273 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
274 size_t quantityBytes = (quantity + 7) / 8;
276#if USE_NOISY_TRACE_IN_THIS_MODULE_
277 DbgTrace (
"Processing kReadDiscreteInputs_ (starting0Address: {}, quantity: {}) message with request-header={}"_f,
278 startingAddress, quantity, requestHeader);
280 for (
const auto& i : serviceHandler->ReadDiscreteInput (
281 DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
284 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) <= endInclusiveAddress) {
286 results[(oneBasedToZeroBased (i.fKey) - startingAddress) / 8] |=
287 Memory::Bit ((oneBasedToZeroBased (i.fKey) - startingAddress) % 8);
293 uint8_t responseLen =
static_cast<uint8_t
> (quantityBytes);
294 MBAPHeaderIsh_ responseHeader =
295 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
296 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
297 sizeof (responseLen) + responseLen),
298 requestHeader.fUnitID, requestHeader.fFunctionCode};
299 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
300 out.
Write (Memory::AsBytes (responseLen));
301 out.
Write (span{results.begin (), responseLen});
302#if USE_NOISY_TRACE_IN_THIS_MODULE_
303 DbgTrace (
"Sent response: header={}, responseLen={}"_f, responseHeader, responseLen);
307 case FunctionCodeType_::kReadHoldingResisters_: {
311 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).first;
312 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).second;
313 Assert (quantity >= 1);
314 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
316#if USE_NOISY_TRACE_IN_THIS_MODULE_
317 DbgTrace (
"Processing kReadHoldingResisters_ (starting0Address: {}, quantity: {}) message with request-header={}"_f,
318 startingAddress, quantity, requestHeader);
320 for (
const auto& i : serviceHandler->ReadHoldingRegisters (
321 DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
324 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) <= endInclusiveAddress) {
325 results[oneBasedToZeroBased (i.fKey) - startingAddress] = ToNetwork_ (i.fValue);
330 uint8_t responseLen =
static_cast<uint8_t
> (quantity *
sizeof (results[0]));
331 MBAPHeaderIsh_ responseHeader =
332 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
333 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
334 sizeof (responseLen) + responseLen),
335 requestHeader.fUnitID, requestHeader.fFunctionCode};
336 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
337 out.
Write (Memory::AsBytes (responseLen));
338 out.
Write (span{
reinterpret_cast<const byte*
> (results.begin ()), responseLen});
339#if USE_NOISY_TRACE_IN_THIS_MODULE_
340 DbgTrace (
"Sent response: header={}, responseLen={}"_f, responseHeader, responseLen);
344 case FunctionCodeType_::kReadInputRegister_: {
348 uint16_t startingAddress = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).first;
349 uint16_t quantity = checkedReadHelperPayload2Shorts (requestPayload, 1, 0x7d).second;
350 Assert (quantity >= 1);
351 uint16_t endInclusiveAddress = startingAddress + quantity - 1u;
353#if USE_NOISY_TRACE_IN_THIS_MODULE_
354 DbgTrace (
"Processing kReadInputRegister_ (starting0Address: {}, quantity: {}) message with request-header={}"_f,
355 startingAddress, quantity, requestHeader);
357 for (
const auto& i : serviceHandler->ReadInputRegisters (
358 DiscreteRange<uint16_t>{zeroToOneBased (startingAddress), zeroToOneBased (endInclusiveAddress)}
361 if (startingAddress <= oneBasedToZeroBased (i.fKey) and oneBasedToZeroBased (i.fKey) <= endInclusiveAddress) {
362 results[oneBasedToZeroBased (i.fKey) - startingAddress] = ToNetwork_ (i.fValue);
367 uint8_t responseLen =
static_cast<uint8_t
> (quantity *
sizeof (results[0]));
368 MBAPHeaderIsh_ responseHeader =
369 MBAPHeaderIsh_{requestHeader.fTransactionID, requestHeader.fProtocolID,
370 static_cast<uint16_t
> (MBAPHeaderIsh_::kExtraLengthFromThisHeaderAccountedInPayloadLength +
371 sizeof (responseLen) + responseLen),
372 requestHeader.fUnitID, requestHeader.fFunctionCode};
373 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
374 out.
Write (Memory::AsBytes (responseLen));
375 out.
Write (span{
reinterpret_cast<const byte*
> (results.begin ()), responseLen});
376#if USE_NOISY_TRACE_IN_THIS_MODULE_
377 DbgTrace (
"Sent response: header={}, responseLen={}"_f, responseHeader, responseLen);
381 case FunctionCodeType_::kWriteSingleCoil_: {
385 uint16_t outputAddress = checkedReadHelperPayload2Shorts (requestPayload, 0, 0xff00).first;
386 uint16_t value = checkedReadHelperPayload2Shorts (requestPayload, 0, 0xff00).second;
387#if USE_NOISY_TRACE_IN_THIS_MODULE_
388 DbgTrace (
"Processing kWriteSingleCoil_ (outputAddress: {}, value: {}) message with request-header={}"_f,
389 outputAddress, value, requestHeader);
391 serviceHandler->WriteCoils ({{zeroToOneBased (outputAddress), value == 0 ? false :
true}});
394 out.
Write (Memory::AsBytes (requestHeader));
395 out.
Write (Memory::AsBytes (ToNetwork_ (outputAddress)));
396 out.
Write (Memory::AsBytes (ToNetwork_ (value)));
397#if USE_NOISY_TRACE_IN_THIS_MODULE_
398 DbgTrace (
"Sent response: header={}"_f, requestHeader);
403 DbgTrace (
"UNREGONIZED FunctionCode (NYI probably) - {} - so echo ILLEGAL_FUNCTION code"_f, requestHeader.fFunctionCode);
404 if (options.fLogger) {
405 options.fLogger.value ()->Log (Logger::eWarning,
"ModbusTCP unrecognized function code '{}'- rejected as ILLEGAL_FUNCTION"_f,
406 requestHeader.fFunctionCode);
408 MBAPHeaderIsh_ responseHeader = requestHeader;
409 responseHeader.fFunctionCode =
static_cast<FunctionCodeType_
> (responseHeader.fFunctionCode | 0x80);
410 out.
Write (Memory::AsBytes (ToNetwork_ (responseHeader)));
411 out.
Write (Memory::AsBytes (
static_cast<uint8_t
> (ExceptionCode::ILLEGAL_FUNCTION)));
412#if USE_NOISY_TRACE_IN_THIS_MODULE_
413 DbgTrace (
"Sent UNREGONIZED_FUNCTION response: header={}, and exceptionCode={}"_f, responseHeader, exceptionCode);
420 catch (
const Thread::AbortException&) {
425 if (options.fLogger) {
426 options.fLogger.value ()->Log (Logger::eWarning,
"ModbusTCP connection ended abnormally: {}"_f, current_exception ());
440 shared_ptr<ThreadPool> usingThreadPool = options.fThreadPool;
441 if (usingThreadPool ==
nullptr) {
442 usingThreadPool = MakeSharedPtr<ThreadPool> (ThreadPool::Options{.fThreadCount = 1});
447 usingThreadPool->AddTask ([serviceHandler, options, s] () { ConnectionHandler_ (s, serviceHandler, options); });
450 [onModbusConnection, options] () {
451#if USE_NOISY_TRACE_IN_THIS_MODULE_
454 uint16_t usingPortNumber = options.fListenPort.value_or (502);
455 if (options.fLogger) {
456 options.fLogger.value ()->Log (Logger::eInfo,
"Listening for ModbusTCP requests on port {}"_f, usingPortNumber);
460 "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{})