Skip to content

A Hardware Hacking Tool with Web-Based CLI That Speaks Every Protocol

create modbus service

Geoccbcf9d1ad9c
diff --git a/src/Services/ModbusService.cpp b/src/Services/ModbusService.cpp
new file mode 100644
index 00000000..844bc7da
--- /dev/null
+++ b/src/Services/ModbusService.cpp
@@ -0,0 +1,181 @@
+#include "ModbusService.h"
+
+ModbusService* ModbusService::s_self = nullptr;
+ModbusService::ModbusService() { s_self = this; }
+ModbusService::~ModbusService() { if (s_self == this) s_self = nullptr; }
+
+bool ModbusService::setTarget(const std::string& hostOrIp, uint16_t port) {
+ IPAddress ip;
+ if (!resolveIPv4(hostOrIp, ip)) return false;
+ _host = ip;
+ _port = port ? port : 502;
+ _mb.reset(new ModbusClientTCPasync(_host, _port));
+
+ if (_mb) {
+ _mb->onDataHandler(&ModbusService::s_onData);
+ _mb->onErrorHandler(&ModbusService::s_onError);
+ _mb->setTimeout(_timeoutMs);
+ _mb->setIdleTimeout(_idleCloseMs);
+ _mb->setMaxInflightRequests(_maxInflight);
+ }
+ return true;
+}
+
+void ModbusService::begin(uint32_t reqTimeoutMs, uint32_t idleCloseMs, uint32_t maxInflight) {
+ _timeoutMs = reqTimeoutMs;
+ _idleCloseMs = idleCloseMs;
+ _maxInflight = maxInflight;
+
+ if (_mb) {
+ _mb->onDataHandler(&ModbusService::s_onData);
+ _mb->onErrorHandler(&ModbusService::s_onError);
+ _mb->setTimeout(_timeoutMs);
+ _mb->setIdleTimeout(_idleCloseMs);
+ _mb->setMaxInflightRequests(_maxInflight);
+ }
+}
+
+// FC01 - Read Coils
+Error ModbusService::readCoils(uint8_t unit, uint16_t addr0, uint16_t qty) {
+ if (!_mb) return INVALID_SERVER;
+ return _mb->addRequest(millis(), unit, READ_COIL, addr0, qty);
+}
+
+// FC02 - Read Discrete Inputs
+Error ModbusService::readDiscreteInputs(uint8_t unit, uint16_t addr0, uint16_t qty) {
+ if (!_mb) return INVALID_SERVER;
+ return _mb->addRequest(millis(), unit, READ_DISCR_INPUT, addr0, qty);
+}
+
+// FC03 - Read Holding Registers
+Error ModbusService::readHolding(uint8_t unit, uint16_t addr0, uint16_t qty) {
+ if (!_mb) return INVALID_SERVER;
+ return _mb->addRequest(millis(), unit, READ_HOLD_REGISTER, addr0, qty);
+}
+
+// FC04 - Read Input Registers
+Error ModbusService::readInputRegisters(uint8_t unit, uint16_t addr0, uint16_t qty) {
+ if (!_mb) return INVALID_SERVER;
+ return _mb->addRequest(millis(), unit, READ_INPUT_REGISTER, addr0, qty);
+}
+
+// FC05 - Write Single Coil
+Error ModbusService::writeSingleCoil(uint8_t unit, uint16_t addr0, bool on) {
+ if (!_mb) return INVALID_SERVER;
+ const uint16_t val = on ? 0xFF00 : 0x0000;
+ return _mb->addRequest(millis(), unit, WRITE_COIL, addr0, val);
+}
+
+// FC06 - Write Single Holding Register
+Error ModbusService::writeHoldingSingle(uint8_t unit, uint16_t addr0, uint16_t value) {
+ if (!_mb) return INVALID_SERVER;
+ return _mb->addRequest(millis(), unit, WRITE_HOLD_REGISTER, addr0, value);
+}
+
+// FC10/0x16 - Write Multiple Holding Registers
+Error ModbusService::writeHoldingMultiple(uint8_t unit, uint16_t addr0, const std::vector<uint16_t>& values) {
+ if (!_mb) return INVALID_SERVER;
+ std::vector<uint16_t> tmp(values.begin(), values.end());
+ const uint16_t qty = static_cast<uint16_t>(tmp.size());
+ const uint8_t byteCnt = static_cast<uint8_t>(qty * 2);
+ return _mb->addRequest(millis(), unit, WRITE_MULT_REGISTERS, addr0, qty, byteCnt, tmp.data());
+}
+
+// FC0F - Write Multiple Coils
+Error ModbusService::writeMultipleCoils(uint8_t unit, uint16_t addr0,
+ const std::vector<uint8_t>& packedBytes,
+ uint16_t coilQty) {
+ if (!_mb) return INVALID_SERVER;
+ std::vector<uint8_t> tmp(packedBytes.begin(), packedBytes.end());
+ const uint8_t byteCnt = static_cast<uint8_t>(tmp.size());
+ return _mb->addRequest(millis(), unit, WRITE_MULT_COILS, addr0, coilQty, byteCnt, tmp.data());
+}
+
+void ModbusService::s_onData(ModbusMessage resp, uint32_t token) {
+ if (s_self) s_self->onData(resp, token);
+}
+
+void ModbusService::s_onError(Error error, uint32_t token) {
+ if (s_self) s_self->onError(error, token);
+}
+
+void ModbusService::onData(ModbusMessage& resp, uint32_t token) {
+ Reply r;
+ r.fc = resp.getFunctionCode();
+ r.raw.assign(resp.begin(), resp.end()); // debug
+
+ if (r.fc & 0x80) {
+ r.ok = false;
+ if (resp.size() >= 2) r.exception = resp[1];
+ } else {
+ r.ok = true;
+
+ // Read Holding/Input Registers
+ if (r.fc == READ_HOLD_REGISTER || r.fc == READ_INPUT_REGISTER) {
+ if (!parseFC03or04(resp, r.regs)) {
+ r.ok = false; r.error = "parse FC03/04 failed";
+ }
+
+ // Read Coils / Discrete Inputs
+ } else if (r.fc == READ_COIL || r.fc == READ_DISCR_INPUT) {
+ if (!parseFC01or02(resp, r.coilBytes, r.byteCount)) {
+ r.ok = false; r.error = "parse FC01/02 failed";
+ }
+ }
+ // other FCs...
+ }
+
+ if (_onReply) _onReply(r, token);
+}
+
+void ModbusService::onError(Error error, uint32_t token) {
+ if (_onError) _onError(error, token);
+
+ Reply r;
+ r.ok = false;
+ ModbusError me(error);
+ r.error = (const char*)me;
+
+ if (_onReply) _onReply(r, token);
+}
+
+bool ModbusService::resolveIPv4(const std::string& host, IPAddress& outIp) {
+ addrinfo hints{}; hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM;
+ addrinfo* res = nullptr;
+ if (getaddrinfo(host.c_str(), nullptr, &hints, &res) != 0 || !res) return false;
+ auto* sin = reinterpret_cast<sockaddr_in*>(res->ai_addr);
+ outIp = IPAddress(sin->sin_addr.s_addr);
+ freeaddrinfo(res);
+ return true;
+}
+
+bool ModbusService::parseFC03or04(ModbusMessage& msg, std::vector<uint16_t>& outRegs) {
+ if (msg.size() < 3) return false;
+ uint8_t fc = msg.getFunctionCode();
+ if (fc != 0x03 && fc != 0x04) return false;
+ uint8_t bc = msg[2];
+ if (msg.size() < static_cast<size_t>(3 + bc)) return false;
+ if (bc % 2) return false;
+
+ outRegs.resize(bc / 2);
+ for (size_t i = 0; i < outRegs.size(); ++i) {
+ outRegs[i] = static_cast<uint16_t>((msg[3 + 2*i] << 8) | msg[3 + 2*i + 1]);
+ }
+ return true;
+}
+
+bool ModbusService::parseFC01or02(ModbusMessage& msg,
+ std::vector<uint8_t>& outBytes,
+ uint8_t& outByteCount) {
+ if (msg.size() < 3) return false;
+ uint8_t fc = msg.getFunctionCode();
+ if (fc != READ_COIL && fc != READ_DISCR_INPUT) return false;
+
+ uint8_t bc = msg[2];
+ if (bc == 0) return false;
+ if (msg.size() < static_cast<size_t>(3 + bc)) return false;
+
+ outByteCount = bc;
+ outBytes.assign(msg.begin() + 3, msg.begin() + 3 + bc); // LSB first
+ return true;
+}
\ No newline at end of file
diff --git a/src/Services/ModbusService.h b/src/Services/ModbusService.h
new file mode 100644
index 00000000..c927ad1e
--- /dev/null
+++ b/src/Services/ModbusService.h
@@ -0,0 +1,94 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+#include <vector>
+#include <memory>
+#include <functional>
+
+extern "C" {
+ #include <lwip/sockets.h>
+ #include <lwip/netdb.h>
+ #include <lwip/inet.h>
+}
+
+#undef INADDR_NONE // conflicting
+#undef IPADDR_NONE
+
+#include <Arduino.h>
+#include <ModbusClientTCPasync.h>
+#include <ModbusMessage.h>
+
+class ModbusService {
+public:
+ ModbusService();
+ ~ModbusService();
+
+ struct Reply {
+ bool ready = false;
+ bool ok = false;
+ uint8_t fc = 0;
+ uint8_t exception = 0;
+ uint8_t byteCount = 0; // FC01/FC02
+ std::string error;
+ std::vector<uint16_t> regs; // FC03/FC04
+ std::vector<uint8_t> coilBytes; // FC01/FC02
+ std::vector<uint8_t> raw;
+ };
+
+ // Configure target host
+ bool setTarget(const std::string& hostOrIp, uint16_t port);
+
+ // Apply config
+ void begin(uint32_t reqTimeoutMs = 10000,
+ uint32_t idleCloseMs = 60000,
+ uint32_t maxInflight = 4);
+
+ // Basic requests
+ Error readHolding(uint8_t unit, uint16_t addr0, uint16_t qty);
+ Error writeHoldingMultiple(uint8_t unit, uint16_t addr0, const std::vector<uint16_t>& values);
+ Error writeHoldingSingle(uint8_t unit, uint16_t addr0, uint16_t value);
+ Error readInputRegisters(uint8_t unit, uint16_t addr0, uint16_t qty);
+ Error readCoils(uint8_t unit, uint16_t addr0, uint16_t qty);
+ Error readDiscreteInputs(uint8_t unit, uint16_t addr0, uint16_t qty);
+ Error writeSingleCoil(uint8_t unit, uint16_t addr0, bool on);
+ Error writeMultipleCoils(uint8_t unit, uint16_t addr0,
+ const std::vector<uint8_t>& packedBytes,
+ uint16_t coilQty);
+
+ // Callbacks
+ using ReplyHandler = std::function<void(const Reply&, uint32_t token)>;
+ void setOnReply(ReplyHandler h) { _onReply = std::move(h); }
+ void setOnData(std::function<void(const ModbusMessage&, uint32_t)> cb) { _onData = std::move(cb); }
+ void setOnError(std::function<void(Error, uint32_t)> cb) { _onError = std::move(cb); }
+ void clearCallbacks() { _onData = {}; _onError = {}; _onReply = {}; }
+
+ // Helpers
+ static bool parseFC03or04(ModbusMessage& msg, std::vector<uint16_t>& outRegs);
+ static bool parseFC01or02(ModbusMessage& msg,
+ std::vector<uint8_t>& outBytes,
+ uint8_t& outByteCount);
+
+private:
+ static ModbusService* s_self;
+
+ static void s_onData(ModbusMessage resp, uint32_t token);
+ static void s_onError(Error error, uint32_t token);
+ void onData(ModbusMessage& resp, uint32_t token);
+ void onError(Error error, uint32_t token);
+ static bool resolveIPv4(const std::string& host, IPAddress& outIp);
+
+private:
+ std::unique_ptr<ModbusClientTCPasync> _mb;
+ IPAddress _host = IPAddress(0,0,0,0);
+ uint16_t _port = 502;
+
+ uint32_t _timeoutMs = 10000;
+ uint32_t _idleCloseMs = 60000;
+ uint32_t _maxInflight = 4;
+
+ ReplyHandler _onReply;
+
+ std::function<void(const ModbusMessage&, uint32_t)> _onData;
+ std::function<void(Error, uint32_t)> _onError;
+};