From c1ef688cedfbc6f4c2a9a25ed6a73becf92b8083 Mon Sep 17 00:00:00 2001 From: Jasem Mutlaq Date: Wed, 24 Jul 2024 11:04:20 +0300 Subject: [PATCH 1/4] Preliminary driver --- drivers/auxiliary/CMakeLists.txt | 10 + drivers/auxiliary/waveshare_modbus_relay.cpp | 208 ++ drivers/auxiliary/waveshare_modbus_relay.h | 62 + libs/indibase/defaultdevice.cpp | 4 +- libs/indibase/defaultdevice.h | 4 +- libs/indibase/indirelayinterface.cpp | 4 +- libs/modbus/nanomodbus.c | 2404 ++++++++++++++++++ libs/modbus/nanomodbus.h | 528 ++++ libs/modbus/platform.h | 126 + 9 files changed, 3344 insertions(+), 6 deletions(-) create mode 100644 drivers/auxiliary/waveshare_modbus_relay.cpp create mode 100644 drivers/auxiliary/waveshare_modbus_relay.h create mode 100644 libs/modbus/nanomodbus.c create mode 100644 libs/modbus/nanomodbus.h create mode 100644 libs/modbus/platform.h diff --git a/drivers/auxiliary/CMakeLists.txt b/drivers/auxiliary/CMakeLists.txt index 3f2281f38c..60985e75bd 100644 --- a/drivers/auxiliary/CMakeLists.txt +++ b/drivers/auxiliary/CMakeLists.txt @@ -333,3 +333,13 @@ SET(dragonlight_SRC add_executable(indi_dragon_light ${dragonlight_SRC}) target_link_libraries(indi_dragon_light indidriver ${HTTPLIB_LIBRARY}) install(TARGETS indi_dragon_light RUNTIME DESTINATION bin) + +# ########## Waveshare Relay ############### +SET(wavesharerelay_SRC + waveshare_modbus_relay.cpp + ../../libs/modbus/nanomodbus.c + ) + +add_executable(indi_wavesharemodbus_relay ${wavesharerelay_SRC}) +target_link_libraries(indi_wavesharemodbus_relay indidriver) +install(TARGETS indi_wavesharemodbus_relay RUNTIME DESTINATION bin) diff --git a/drivers/auxiliary/waveshare_modbus_relay.cpp b/drivers/auxiliary/waveshare_modbus_relay.cpp new file mode 100644 index 0000000000..2df45ca143 --- /dev/null +++ b/drivers/auxiliary/waveshare_modbus_relay.cpp @@ -0,0 +1,208 @@ +/* + Waveshare ModBUS POE Relay + Copyright (C) 2024 Jasem Mutlaq + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#include "waveshare_modbus_relay.h" +#include "../../libs/modbus/platform.h" + +#include +#include +#include + +static std::unique_ptr sesto(new WaveshareRelay()); + +WaveshareRelay::WaveshareRelay() : RelayInterface(this) +{ + setVersion(1, 0); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::initProperties() +{ + INDI::DefaultDevice::initProperties(); + + INDI::RelayInterface::initProperties(MAIN_CONTROL_TAB, 8); + + setDriverInterface(AUX_INTERFACE | RELAY_INTERFACE); + + addAuxControls(); + + setDefaultPollingPeriod(2000); + tcpConnection = new Connection::TCP(this); + tcpConnection->setDefaultHost("192.168.1.1"); + tcpConnection->setDefaultPort(502); + tcpConnection->registerHandshake([&]() + { + return Handshake(); + }); + + registerConnection(tcpConnection); + + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::updateProperties() +{ + INDI::DefaultDevice::updateProperties(); + INDI::RelayInterface::updateProperties(); + + if (isConnected()) + { + SetTimer(getPollingPeriod()); + } + else + { + } + + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::Handshake() +{ + PortFD = tcpConnection->getPortFD(); + + nmbs_platform_conf platform_conf; + platform_conf.transport = NMBS_TRANSPORT_TCP; + platform_conf.read = read_fd_linux; + platform_conf.write = write_fd_linux; + platform_conf.arg = &PortFD; + + // Create the modbus client + nmbs_error err = nmbs_client_create(&nmbs, &platform_conf); + if (err != NMBS_ERROR_NONE) + { + LOGF_ERROR("Error creating modbus client: %d", err); + if (!nmbs_error_is_exception(err)) + return false; + } + + // Set only the response timeout. Byte timeout will be handled by the TCP connection + nmbs_set_read_timeout(&nmbs, 1000); + + INDI::RelayInterface::Status status; + return QueryRelay(0, status); +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +const char *WaveshareRelay::getDefaultName() +{ + return "Waveshare Relay"; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::ISNewText(const char *dev, const char *name, char *texts[], char *names[], int n) +{ + // Check Relay Properties + if (INDI::RelayInterface::processText(dev, name, texts, names, n)) + return true; + + return INDI::DefaultDevice::ISNewText(dev, name, texts, names, n); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::ISNewSwitch(const char *dev, const char *name, ISState *states, char *names[], int n) +{ + if (INDI::RelayInterface::processSwitch(dev, name, states, names, n)) + return true; + + return INDI::DefaultDevice::ISNewSwitch(dev, name, states, names, n); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::saveConfigItems(FILE * fp) +{ + INDI::RelayInterface::saveConfigItems(fp); + return INDI::DefaultDevice::saveConfigItems(fp); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +void WaveshareRelay::TimerHit() +{ + if (!isConnected()) + return; + + for (int i = 0; i < 8; i++) + { + INDI::RelayInterface::Status status; + if (QueryRelay(i, status)) + { + RelaysSP[i].reset(); + RelaysSP[i][status].setState(ISS_ON); + RelaysSP[i].setState(IPS_OK); + RelaysSP[i].apply(); + } + } + + SetTimer(getCurrentPollingPeriod()); +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::QueryRelay(uint32_t index, Status &status) +{ + nmbs_bitfield coils = {0}; + auto err = nmbs_read_coils(&nmbs, 0, 8, coils); + if (err != NMBS_ERROR_NONE) + { + LOGF_ERROR("Error reading coils at address 0: %s", nmbs_strerror(err)); + return false; + } + else + { + status = static_cast(nmbs_bitfield_read(coils, index)); + return true; + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////// +/// +//////////////////////////////////////////////////////////////////////////////////////////////////////// +bool WaveshareRelay::CommandRelay(uint32_t index, Command command) +{ + uint16_t value = 0; + if (command == Open) + value = 0xFF00; + else if (command == Flip) + value = 0x5500; + + auto err = nmbs_write_single_coil(&nmbs, index, value); + if (err != NMBS_ERROR_NONE) + { + LOGF_ERROR("Error reading coils at address 0: %s", nmbs_strerror(err)); + return false; + } + + return true; +} diff --git a/drivers/auxiliary/waveshare_modbus_relay.h b/drivers/auxiliary/waveshare_modbus_relay.h new file mode 100644 index 0000000000..98bc005f41 --- /dev/null +++ b/drivers/auxiliary/waveshare_modbus_relay.h @@ -0,0 +1,62 @@ +/* + Waveshare ModBUS POE Relay + Copyright (C) 2024 Jasem Mutlaq + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA +*/ + +#pragma once + +#include "indirelayinterface.h" +#include "defaultdevice.h" +#include "../../libs/modbus/nanomodbus.h" + +class WaveshareRelay : public INDI::DefaultDevice, public INDI::RelayInterface +{ + public: + WaveshareRelay(); + virtual ~WaveshareRelay() override = default; + + const char *getDefaultName() override; + virtual bool initProperties() override; + virtual bool updateProperties() override; + + virtual bool ISNewSwitch(const char *dev, const char *name, ISState *states, char *names[], int n) override; + virtual bool ISNewText(const char *dev, const char *name, char *texts[], char *names[], int n) override; + + protected: + bool Handshake(); + + // Relay + + /** + * \brief Query single relay status + * \param index Relay index + * \param status Store relay status in this variable. + * \return True if operation is successful, false otherwise + */ + virtual bool QueryRelay(uint32_t index, Status &status); + + /** + * \brief Send command to relay + * \return True if operation is successful, false otherwise + */ + virtual bool CommandRelay(uint32_t index, Command command); + + virtual void TimerHit() override; + virtual bool saveConfigItems(FILE *fp) override; + + private: + Connection::TCP *tcpConnection {nullptr}; + int PortFD{-1}; + nmbs_t nmbs; +}; diff --git a/libs/indibase/defaultdevice.cpp b/libs/indibase/defaultdevice.cpp index 68dbe95b01..75a609751e 100644 --- a/libs/indibase/defaultdevice.cpp +++ b/libs/indibase/defaultdevice.cpp @@ -838,13 +838,13 @@ bool DefaultDevice::updateProperties() // The situation occurs in the case of drivers where the class that inherits from `DefaultDevice` // will call the `setDriverInterface` function in the constructor -uint16_t DefaultDevice::getDriverInterface() const +uint32_t DefaultDevice::getDriverInterface() const { D_PTR(const DefaultDevice); return atoi(d->DriverInfoTP[3 /* DRIVER_INTERFACE */].getText()); } -void DefaultDevice::setDriverInterface(uint16_t value) +void DefaultDevice::setDriverInterface(uint32_t value) { D_PTR(DefaultDevice); d->DriverInfoTP[3 /* DRIVER_INTERFACE */].setText(std::to_string(value)); diff --git a/libs/indibase/defaultdevice.h b/libs/indibase/defaultdevice.h index 725c509e21..fe4e511a7c 100644 --- a/libs/indibase/defaultdevice.h +++ b/libs/indibase/defaultdevice.h @@ -309,7 +309,7 @@ class DefaultDevice : public ParentDevice /** * @return getInterface Return the interface declared by the driver. */ - uint16_t getDriverInterface() const; + uint32_t getDriverInterface() const; /** * @brief setInterface Set driver interface. By default the driver interface is set to GENERAL_DEVICE. @@ -318,7 +318,7 @@ class DefaultDevice : public ParentDevice * @warning This only updates the internal driver interface property and does not send it to the * client. To synchronize the client, use syncDriverInfo function. */ - void setDriverInterface(uint16_t value); + void setDriverInterface(uint32_t value); public: /** @brief Add a device to the watch list. diff --git a/libs/indibase/indirelayinterface.cpp b/libs/indibase/indirelayinterface.cpp index 3cd2be8e3d..36d05fa820 100644 --- a/libs/indibase/indirelayinterface.cpp +++ b/libs/indibase/indirelayinterface.cpp @@ -63,7 +63,7 @@ void RelayInterface::initProperties(const char *groupName, uint8_t relays) RelaysSP.reserve(relays); // Initialize switches, use labels if loaded. - for (auto i = 0; i < relays; i++) + for (size_t i = 0; i < relays; i++) { auto name = "RELAY_" + std::to_string(i); auto label = "Relay #" + std::to_string(i); @@ -108,7 +108,7 @@ bool RelayInterface::processSwitch(const char *dev, const char *name, ISState st { if (dev && !strcmp(dev, m_defaultDevice->getDeviceName())) { - for (auto i = 0; i < RelaysSP.size(); i++) + for (size_t i = 0; i < RelaysSP.size(); i++) { if (RelaysSP[i].isNameMatch(name)) { diff --git a/libs/modbus/nanomodbus.c b/libs/modbus/nanomodbus.c new file mode 100644 index 0000000000..21266feaeb --- /dev/null +++ b/libs/modbus/nanomodbus.c @@ -0,0 +1,2404 @@ +/* + nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers + + MIT License + + Copyright (c) 2024 Valerio De Benedetto (@debevv) + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + +#include "nanomodbus.h" + +#include +#include +#include + +#ifdef NMBS_DEBUG +#include +#define NMBS_DEBUG_PRINT(...) printf(__VA_ARGS__) +#else +#define NMBS_DEBUG_PRINT(...) (void) (0) +#endif + + +static uint8_t get_1(nmbs_t* nmbs) { + uint8_t result = nmbs->msg.buf[nmbs->msg.buf_idx]; + nmbs->msg.buf_idx++; + return result; +} + + +static void put_1(nmbs_t* nmbs, uint8_t data) { + nmbs->msg.buf[nmbs->msg.buf_idx] = data; + nmbs->msg.buf_idx++; +} + + +static void discard_1(nmbs_t* nmbs) { + nmbs->msg.buf_idx++; +} + + +#ifndef NMBS_SERVER_DISABLED +static void discard_n(nmbs_t* nmbs, uint16_t n) { + nmbs->msg.buf_idx += n; +} +#endif + + +static uint16_t get_2(nmbs_t* nmbs) { + uint16_t result = + ((uint16_t) nmbs->msg.buf[nmbs->msg.buf_idx]) << 8 | (uint16_t) nmbs->msg.buf[nmbs->msg.buf_idx + 1]; + nmbs->msg.buf_idx += 2; + return result; +} + + +static void put_2(nmbs_t* nmbs, uint16_t data) { + nmbs->msg.buf[nmbs->msg.buf_idx] = (uint8_t) ((data >> 8) & 0xFFU); + nmbs->msg.buf[nmbs->msg.buf_idx + 1] = (uint8_t) data; + nmbs->msg.buf_idx += 2; +} + + +#ifndef NMBS_SERVER_DISABLED +static void set_1(nmbs_t* nmbs, uint8_t data, uint8_t index) { + nmbs->msg.buf[index] = data; +} + + +static void set_2(nmbs_t* nmbs, uint16_t data, uint8_t index) { + nmbs->msg.buf[index] = (uint8_t) ((data >> 8) & 0xFFU); + nmbs->msg.buf[index + 1] = (uint8_t) data; +} +#endif + + +static uint8_t* get_n(nmbs_t* nmbs, uint16_t n) { + uint8_t* msg_buf_ptr = nmbs->msg.buf + nmbs->msg.buf_idx; + nmbs->msg.buf_idx += n; + return msg_buf_ptr; +} + + +#ifndef NMBS_SERVER_DISABLED +static void put_n(nmbs_t* nmbs, const uint8_t* data, uint8_t size) { + memcpy(&nmbs->msg.buf[nmbs->msg.buf_idx], data, size); + nmbs->msg.buf_idx += size; +} + + +static uint16_t* get_regs(nmbs_t* nmbs, uint16_t n) { + uint16_t* msg_buf_ptr = (uint16_t*) (nmbs->msg.buf + nmbs->msg.buf_idx); + nmbs->msg.buf_idx += n * 2; + while (n--) { + msg_buf_ptr[n] = (msg_buf_ptr[n] << 8) | ((msg_buf_ptr[n] >> 8) & 0xFF); + } + return msg_buf_ptr; +} +#endif + + +#ifndef NMBS_CLIENT_DISABLED +static void put_regs(nmbs_t* nmbs, const uint16_t* data, uint16_t n) { + uint16_t* msg_buf_ptr = (uint16_t*) (nmbs->msg.buf + nmbs->msg.buf_idx); + nmbs->msg.buf_idx += n * 2; + while (n--) { + msg_buf_ptr[n] = (data[n] << 8) | ((data[n] >> 8) & 0xFF); + } +} +#endif + + +static void swap_regs(uint16_t* data, uint16_t n) { + while (n--) { + data[n] = (data[n] << 8) | ((data[n] >> 8) & 0xFF); + } +} + + +static void msg_buf_reset(nmbs_t* nmbs) { + nmbs->msg.buf_idx = 0; +} + + +static void msg_state_reset(nmbs_t* nmbs) { + msg_buf_reset(nmbs); + nmbs->msg.unit_id = 0; + nmbs->msg.fc = 0; + nmbs->msg.transaction_id = 0; + nmbs->msg.broadcast = false; + nmbs->msg.ignored = false; +} + + +#ifndef NMBS_CLIENT_DISABLED +static void msg_state_req(nmbs_t* nmbs, uint8_t fc) { + if (nmbs->current_tid == UINT16_MAX) + nmbs->current_tid = 1; + else + nmbs->current_tid++; + + // Flush the remaining data on the line before sending the request + nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg); + + msg_state_reset(nmbs); + nmbs->msg.unit_id = nmbs->dest_address_rtu; + nmbs->msg.fc = fc; + nmbs->msg.transaction_id = nmbs->current_tid; + if (nmbs->msg.unit_id == 0 && nmbs->platform.transport == NMBS_TRANSPORT_RTU) + nmbs->msg.broadcast = true; +} +#endif + + +nmbs_error nmbs_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf) { + if (!nmbs) + return NMBS_ERROR_INVALID_ARGUMENT; + + memset(nmbs, 0, sizeof(nmbs_t)); + + nmbs->byte_timeout_ms = -1; + nmbs->read_timeout_ms = -1; + + if (!platform_conf) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (platform_conf->transport != NMBS_TRANSPORT_RTU && platform_conf->transport != NMBS_TRANSPORT_TCP) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (!platform_conf->read || !platform_conf->write) + return NMBS_ERROR_INVALID_ARGUMENT; + + nmbs->platform = *platform_conf; + + return NMBS_ERROR_NONE; +} + + +void nmbs_set_read_timeout(nmbs_t* nmbs, int32_t timeout_ms) { + nmbs->read_timeout_ms = timeout_ms; +} + + +void nmbs_set_byte_timeout(nmbs_t* nmbs, int32_t timeout_ms) { + nmbs->byte_timeout_ms = timeout_ms; +} + + +void nmbs_set_destination_rtu_address(nmbs_t* nmbs, uint8_t address) { + nmbs->dest_address_rtu = address; +} + + +void nmbs_set_platform_arg(nmbs_t* nmbs, void* arg) { + nmbs->platform.arg = arg; +} + + +uint16_t nmbs_crc_calc(const uint8_t* data, uint32_t length) { + uint16_t crc = 0xFFFF; + for (uint32_t i = 0; i < length; i++) { + crc ^= (uint16_t) data[i]; + for (int j = 8; j != 0; j--) { + if ((crc & 0x0001) != 0) { + crc >>= 1; + crc ^= 0xA001; + } + else + crc >>= 1; + } + } + + return (uint16_t) (crc << 8) | (uint16_t) (crc >> 8); +} + + +static nmbs_error recv(nmbs_t* nmbs, uint16_t count) { + int32_t ret = + nmbs->platform.read(nmbs->msg.buf + nmbs->msg.buf_idx, count, nmbs->byte_timeout_ms, nmbs->platform.arg); + + if (ret == count) + return NMBS_ERROR_NONE; + + if (ret < count) { + if (ret < 0) + return NMBS_ERROR_TRANSPORT; + + return NMBS_ERROR_TIMEOUT; + } + + return NMBS_ERROR_TRANSPORT; +} + + +static nmbs_error send(nmbs_t* nmbs, uint16_t count) { + int32_t ret = nmbs->platform.write(nmbs->msg.buf, count, nmbs->byte_timeout_ms, nmbs->platform.arg); + + if (ret == count) + return NMBS_ERROR_NONE; + + if (ret < count) { + if (ret < 0) + return NMBS_ERROR_TRANSPORT; + + return NMBS_ERROR_TIMEOUT; + } + + return NMBS_ERROR_TRANSPORT; +} + + +static nmbs_error recv_msg_footer(nmbs_t* nmbs) { + NMBS_DEBUG_PRINT("\n"); + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + uint16_t crc = nmbs_crc_calc(nmbs->msg.buf, nmbs->msg.buf_idx); + + nmbs_error err = recv(nmbs, 2); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t recv_crc = get_2(nmbs); + + if (recv_crc != crc) + return NMBS_ERROR_CRC; + } + + return NMBS_ERROR_NONE; +} + + +static nmbs_error recv_msg_header(nmbs_t* nmbs, bool* first_byte_received) { + // We wait for the read timeout here, just for the first message byte + int32_t old_byte_timeout = nmbs->byte_timeout_ms; + nmbs->byte_timeout_ms = nmbs->read_timeout_ms; + + msg_state_reset(nmbs); + + *first_byte_received = false; + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + nmbs_error err = recv(nmbs, 1); + + nmbs->byte_timeout_ms = old_byte_timeout; + + if (err != NMBS_ERROR_NONE) + return err; + + *first_byte_received = true; + + nmbs->msg.unit_id = get_1(nmbs); + + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + nmbs->msg.fc = get_1(nmbs); + } + else if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + nmbs_error err = recv(nmbs, 1); + + nmbs->byte_timeout_ms = old_byte_timeout; + + if (err != NMBS_ERROR_NONE) + return err; + + *first_byte_received = true; + + // Advance buf_idx + discard_1(nmbs); + + err = recv(nmbs, 7); + if (err != NMBS_ERROR_NONE) + return err; + + // Starting over + msg_buf_reset(nmbs); + + nmbs->msg.transaction_id = get_2(nmbs); + uint16_t protocol_id = get_2(nmbs); + uint16_t length = get_2(nmbs); // We should actually check the length of the request against this value + nmbs->msg.unit_id = get_1(nmbs); + nmbs->msg.fc = get_1(nmbs); + + if (protocol_id != 0) + return NMBS_ERROR_INVALID_TCP_MBAP; + + if (length > 255) + return NMBS_ERROR_INVALID_TCP_MBAP; + } + + return NMBS_ERROR_NONE; +} + + +static void put_msg_header(nmbs_t* nmbs, uint16_t data_length) { + msg_buf_reset(nmbs); + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + put_1(nmbs, nmbs->msg.unit_id); + } + else if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + put_2(nmbs, nmbs->msg.transaction_id); + put_2(nmbs, 0); + put_2(nmbs, (uint16_t) (1 + 1 + data_length)); + put_1(nmbs, nmbs->msg.unit_id); + } + + put_1(nmbs, nmbs->msg.fc); +} + + +#ifndef NMBS_SERVER_DISABLED +static void set_msg_header_size(nmbs_t* nmbs, uint16_t data_length) { + if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + data_length += 2; + set_2(nmbs, data_length, 4); + } +} +#endif + + +static nmbs_error send_msg(nmbs_t* nmbs) { + NMBS_DEBUG_PRINT("\n"); + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + uint16_t crc = nmbs_crc_calc(nmbs->msg.buf, nmbs->msg.buf_idx); + put_2(nmbs, crc); + } + + nmbs_error err = send(nmbs, nmbs->msg.buf_idx); + + return err; +} + + +#ifndef NMBS_SERVER_DISABLED +static nmbs_error recv_req_header(nmbs_t* nmbs, bool* first_byte_received) { + nmbs_error err = recv_msg_header(nmbs, first_byte_received); + if (err != NMBS_ERROR_NONE) + return err; + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + // Check if request is for us + if (nmbs->msg.unit_id == NMBS_BROADCAST_ADDRESS) + nmbs->msg.broadcast = true; + else if (nmbs->msg.unit_id != nmbs->address_rtu) + nmbs->msg.ignored = true; + else + nmbs->msg.ignored = false; + } + + return NMBS_ERROR_NONE; +} + + +static void put_res_header(nmbs_t* nmbs, uint16_t data_length) { + put_msg_header(nmbs, data_length); + NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->address_rtu, nmbs->msg.fc); +} + + +static nmbs_error send_exception_msg(nmbs_t* nmbs, uint8_t exception) { + nmbs->msg.fc += 0x80; + put_msg_header(nmbs, 1); + put_1(nmbs, exception); + + NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\texception %d", nmbs->address_rtu, nmbs->address_rtu, exception); + + return send_msg(nmbs); +} +#endif + + +static nmbs_error recv_res_header(nmbs_t* nmbs) { + uint16_t req_transaction_id = nmbs->msg.transaction_id; + uint8_t req_unit_id = nmbs->msg.unit_id; + uint8_t req_fc = nmbs->msg.fc; + + bool first_byte_received = false; + nmbs_error err = recv_msg_header(nmbs, &first_byte_received); + if (err != NMBS_ERROR_NONE) + return err; + + if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + if (nmbs->msg.transaction_id != req_transaction_id) + return NMBS_ERROR_INVALID_TCP_MBAP; + } + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU && nmbs->msg.unit_id != req_unit_id) + return NMBS_ERROR_INVALID_UNIT_ID; + + if (nmbs->msg.fc != req_fc) { + if (nmbs->msg.fc - 0x80 == req_fc) { + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t exception = get_1(nmbs); + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (exception < 1 || exception > 4) + return NMBS_ERROR_INVALID_RESPONSE; + + NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\texception %d\n", nmbs->address_rtu, nmbs->msg.unit_id, + exception); + return (nmbs_error) exception; + } + + return NMBS_ERROR_INVALID_RESPONSE; + } + + NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->msg.unit_id, nmbs->msg.fc); + + return NMBS_ERROR_NONE; +} + + +#ifndef NMBS_CLIENT_DISABLED +static void put_req_header(nmbs_t* nmbs, uint16_t data_length) { + put_msg_header(nmbs, data_length); +#ifdef NMBS_DEBUG + printf("%d ", nmbs->address_rtu); + printf("NMBS req -> "); + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + if (nmbs->msg.broadcast) + printf("broadcast\t"); + else + printf("address_rtu %d\t", nmbs->dest_address_rtu); + } + + printf("fc %d\t", nmbs->msg.fc); +#endif +} +#endif + + +static nmbs_error recv_read_discrete_res(nmbs_t* nmbs, nmbs_bitfield values) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t coils_bytes = get_1(nmbs); + NMBS_DEBUG_PRINT("b %d\t", coils_bytes); + + if (coils_bytes > 250) { + return NMBS_ERROR_INVALID_RESPONSE; + } + + err = recv(nmbs, coils_bytes); + if (err != NMBS_ERROR_NONE) + return err; + + NMBS_DEBUG_PRINT("coils "); + for (int i = 0; i < coils_bytes; i++) { + uint8_t coil = get_1(nmbs); + if (values) + values[i] = coil; + NMBS_DEBUG_PRINT("%d ", coil); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return NMBS_ERROR_NONE; +} + + +static nmbs_error recv_read_registers_res(nmbs_t* nmbs, uint16_t quantity, uint16_t* registers) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t registers_bytes = get_1(nmbs); + NMBS_DEBUG_PRINT("b %d\t", registers_bytes); + + if (registers_bytes > 250) + return NMBS_ERROR_INVALID_RESPONSE; + + err = recv(nmbs, registers_bytes); + if (err != NMBS_ERROR_NONE) + return err; + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < registers_bytes / 2; i++) { + uint16_t reg = get_2(nmbs); + if (registers) + registers[i] = reg; + NMBS_DEBUG_PRINT("%d ", reg); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers_bytes != quantity * 2) + return NMBS_ERROR_INVALID_RESPONSE; + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_write_single_coil_res(nmbs_t* nmbs, uint16_t address, uint16_t value_req) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address_res = get_2(nmbs); + uint16_t value_res = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value_res); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (address_res != address) + return NMBS_ERROR_INVALID_RESPONSE; + + if (value_res != value_req) + return NMBS_ERROR_INVALID_RESPONSE; + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_write_single_register_res(nmbs_t* nmbs, uint16_t address, uint16_t value_req) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address_res = get_2(nmbs); + uint16_t value_res = get_2(nmbs); + NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_res); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (address_res != address) + return NMBS_ERROR_INVALID_RESPONSE; + + if (value_res != value_req) + return NMBS_ERROR_INVALID_RESPONSE; + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_write_multiple_coils_res(nmbs_t* nmbs, uint16_t address, uint16_t quantity) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address_res = get_2(nmbs); + uint16_t quantity_res = get_2(nmbs); + NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (address_res != address) + return NMBS_ERROR_INVALID_RESPONSE; + + if (quantity_res != quantity) + return NMBS_ERROR_INVALID_RESPONSE; + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_write_multiple_registers_res(nmbs_t* nmbs, uint16_t address, uint16_t quantity) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address_res = get_2(nmbs); + uint16_t quantity_res = get_2(nmbs); + NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (address_res != address) + return NMBS_ERROR_INVALID_RESPONSE; + + if (quantity_res != quantity) + return NMBS_ERROR_INVALID_RESPONSE; + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_read_file_record_res(nmbs_t* nmbs, uint16_t* registers, uint16_t count) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t response_size = get_1(nmbs); + if (response_size > 250) { + return NMBS_ERROR_INVALID_RESPONSE; + } + + err = recv(nmbs, response_size); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t subreq_data_size = get_1(nmbs) - 1; + uint8_t subreq_reference_type = get_1(nmbs); + uint16_t* subreq_record_data = (uint16_t*) get_n(nmbs, subreq_data_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers) { + if (subreq_reference_type != 6) + return NMBS_ERROR_INVALID_RESPONSE; + + if (count != (subreq_data_size / 2)) + return NMBS_ERROR_INVALID_RESPONSE; + + swap_regs(subreq_record_data, subreq_data_size / 2); + memcpy(registers, subreq_record_data, subreq_data_size); + } + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_write_file_record_res(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, + const uint16_t* registers, uint16_t count) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t response_size = get_1(nmbs); + if (response_size > 251) + return NMBS_ERROR_INVALID_RESPONSE; + + err = recv(nmbs, response_size); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t subreq_reference_type = get_1(nmbs); + uint16_t subreq_file_number = get_2(nmbs); + uint16_t subreq_record_number = get_2(nmbs); + uint16_t subreq_record_length = get_2(nmbs); + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", subreq_file_number, subreq_record_number, subreq_record_length); + + uint16_t subreq_data_size = subreq_record_length * 2; + uint16_t* subreq_record_data = (uint16_t*) get_n(nmbs, subreq_data_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers) { + if (subreq_reference_type != 6) + return NMBS_ERROR_INVALID_RESPONSE; + + if (subreq_file_number != file_number) + return NMBS_ERROR_INVALID_RESPONSE; + + if (subreq_record_number != record_number) + return NMBS_ERROR_INVALID_RESPONSE; + + if (subreq_record_length != count) + return NMBS_ERROR_INVALID_RESPONSE; + + swap_regs(subreq_record_data, subreq_record_length); + if (memcmp(registers, subreq_record_data, subreq_data_size) != 0) + return NMBS_ERROR_INVALID_RESPONSE; + } + + return NMBS_ERROR_NONE; +} + +nmbs_error recv_read_device_identification_res(nmbs_t* nmbs, uint8_t buffers_count, char** buffers_out, + uint8_t buffers_length, const uint8_t* order, uint8_t* ids_out, + uint8_t* next_object_id_out, uint8_t* objects_count_out) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, 6); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t mei_type = get_1(nmbs); + if (mei_type != 0x0E) + return NMBS_ERROR_INVALID_RESPONSE; + + uint8_t read_device_id_code = get_1(nmbs); + if (read_device_id_code < 1 || read_device_id_code > 4) + return NMBS_ERROR_INVALID_RESPONSE; + + uint8_t conformity_level = get_1(nmbs); + if (conformity_level < 1 || (conformity_level > 3 && conformity_level < 0x81) || conformity_level > 0x83) + return NMBS_ERROR_INVALID_RESPONSE; + + uint8_t more_follows = get_1(nmbs); + if (more_follows != 0 && more_follows != 0xFF) + return NMBS_ERROR_INVALID_RESPONSE; + + uint8_t next_object_id = get_1(nmbs); + + uint8_t objects_count = get_1(nmbs); + if (objects_count_out) + *objects_count_out = objects_count; + + if (buffers_count == 0) { + buffers_out = NULL; + } + else if (objects_count > buffers_count) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (more_follows == 0) + next_object_id = 0x7F; // This value is reserved in the spec, we use it to signal stream is finished + + if (next_object_id_out) + *next_object_id_out = next_object_id; + + uint8_t res_size_left = 253 - 7; + for (int i = 0; i < objects_count; i++) { + err = recv(nmbs, 2); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t object_id = get_1(nmbs); + uint8_t object_length = get_1(nmbs); + res_size_left -= 2; + + if (object_length > res_size_left) + return NMBS_ERROR_INVALID_RESPONSE; + + err = recv(nmbs, object_length); + if (err != NMBS_ERROR_NONE) + return err; + + const char* str = (const char*) get_n(nmbs, object_length); + + if (ids_out) + ids_out[i] = object_id; + + uint8_t buf_index = i; + if (order) + buf_index = order[object_id]; + if (buffers_out) { + strncpy(buffers_out[buf_index], str, buffers_length); + buffers_out[buf_index][object_length] = 0; + } + } + + return recv_msg_footer(nmbs); +} + + +#ifndef NMBS_SERVER_DISABLED +#if !defined(NMBS_SERVER_READ_COILS_DISABLED) || !defined(NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED) +static nmbs_error handle_read_discrete(nmbs_t* nmbs, + nmbs_error (*callback)(uint16_t, uint16_t, nmbs_bitfield, uint8_t, void*)) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 2000) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (callback) { + nmbs_bitfield bitfield = {0}; + err = callback(address, quantity, bitfield, nmbs->msg.unit_id, nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if (!nmbs->msg.broadcast) { + uint8_t discrete_bytes = (quantity + 7) / 8; + put_res_header(nmbs, 1 + discrete_bytes); + + put_1(nmbs, discrete_bytes); + + NMBS_DEBUG_PRINT("b %d\t", discrete_bytes); + + NMBS_DEBUG_PRINT("coils "); + for (int i = 0; i < discrete_bytes; i++) { + put_1(nmbs, bitfield[i]); + NMBS_DEBUG_PRINT("%d ", bitfield[i]); + } + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_read_discrete_res(nmbs, NULL); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#if !defined(NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED) || !defined(NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED) +static nmbs_error handle_read_registers(nmbs_t* nmbs, + nmbs_error (*callback)(uint16_t, uint16_t, uint16_t*, uint8_t, void*)) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 125) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (callback) { + uint16_t regs[125] = {0}; + err = callback(address, quantity, regs, nmbs->msg.unit_id, nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + // TODO check all these read request broadcast use cases + if (!nmbs->msg.broadcast) { + uint8_t regs_bytes = quantity * 2; + put_res_header(nmbs, 1 + regs_bytes); + + put_1(nmbs, regs_bytes); + + NMBS_DEBUG_PRINT("b %d\t", regs_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < quantity; i++) { + put_2(nmbs, regs[i]); + NMBS_DEBUG_PRINT("%d ", regs[i]); + } + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_read_registers_res(nmbs, quantity, NULL); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_READ_COILS_DISABLED +static nmbs_error handle_read_coils(nmbs_t* nmbs) { + return handle_read_discrete(nmbs, nmbs->callbacks.read_coils); +} +#endif + + +#ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED +static nmbs_error handle_read_discrete_inputs(nmbs_t* nmbs) { + return handle_read_discrete(nmbs, nmbs->callbacks.read_discrete_inputs); +} +#endif + + +#ifndef NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED +static nmbs_error handle_read_holding_registers(nmbs_t* nmbs) { + return handle_read_registers(nmbs, nmbs->callbacks.read_holding_registers); +} +#endif + + +#ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED +static nmbs_error handle_read_input_registers(nmbs_t* nmbs) { + return handle_read_registers(nmbs, nmbs->callbacks.read_input_registers); +} +#endif + + +#ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED +static nmbs_error handle_write_single_coil(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t value = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (nmbs->callbacks.write_single_coil) { + if (value != 0 && value != 0xFF00) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + err = nmbs->callbacks.write_single_coil(address, value == 0 ? false : true, nmbs->msg.unit_id, + nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_single_coil_res(nmbs, address, value); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED +static nmbs_error handle_write_single_register(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t value = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (nmbs->callbacks.write_single_register) { + err = nmbs->callbacks.write_single_register(address, value, nmbs->msg.unit_id, nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_single_register_res(nmbs, address, value); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED +static nmbs_error handle_write_multiple_coils(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 5); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + uint8_t coils_bytes = get_1(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\tcoils ", address, quantity, coils_bytes); + + if (coils_bytes > 246) + return NMBS_ERROR_INVALID_REQUEST; + + err = recv(nmbs, coils_bytes); + if (err != NMBS_ERROR_NONE) + return err; + + nmbs_bitfield coils = {0}; + for (int i = 0; i < coils_bytes; i++) { + coils[i] = get_1(nmbs); + NMBS_DEBUG_PRINT("%d ", coils[i]); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 0x07B0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (coils_bytes == 0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((quantity + 7) / 8 != coils_bytes) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (nmbs->callbacks.write_multiple_coils) { + err = nmbs->callbacks.write_multiple_coils(address, quantity, coils, nmbs->msg.unit_id, + nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_multiple_coils_res(nmbs, address, quantity); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED +static nmbs_error handle_write_multiple_registers(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 5); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + uint8_t registers_bytes = get_1(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\tregs ", address, quantity, registers_bytes); + + err = recv(nmbs, registers_bytes); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers_bytes > 246) + return NMBS_ERROR_INVALID_REQUEST; + + uint16_t registers[0x007B]; + for (int i = 0; i < registers_bytes / 2; i++) { + registers[i] = get_2(nmbs); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 0x007B) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (registers_bytes == 0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (registers_bytes != quantity * 2) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (nmbs->callbacks.write_multiple_registers) { + err = nmbs->callbacks.write_multiple_registers(address, quantity, registers, nmbs->msg.unit_id, + nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_multiple_registers_res(nmbs, address, quantity); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED +static nmbs_error handle_read_file_record(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t request_size = get_1(nmbs); + if (request_size > 245) + return NMBS_ERROR_INVALID_REQUEST; + + err = recv(nmbs, request_size); + if (err != NMBS_ERROR_NONE) + return err; + + const uint8_t subreq_header_size = 7; + const uint8_t subreq_count = request_size / subreq_header_size; + + struct { + uint8_t reference_type; + uint16_t file_number; + uint16_t record_number; + uint16_t record_length; + } +#ifdef __STDC_NO_VLA__ + subreq[35]; // 245 / subreq_header_size +#else + subreq[subreq_count]; +#endif + + uint8_t response_data_size = 0; + + for (uint8_t i = 0; i < subreq_count; i++) { + subreq[i].reference_type = get_1(nmbs); + subreq[i].file_number = get_2(nmbs); + subreq[i].record_number = get_2(nmbs); + subreq[i].record_length = get_2(nmbs); + + response_data_size += 2 + subreq[i].record_length * 2; + } + + discard_n(nmbs, request_size % subreq_header_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (request_size % subreq_header_size) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (request_size < 0x07 || request_size > 0xF5) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + for (uint8_t i = 0; i < subreq_count; i++) { + if (subreq[i].reference_type != 0x06) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq[i].file_number == 0x0000) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq[i].record_number > 0x270F) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq[i].record_length > 124) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fread ", subreq[i].file_number, subreq[i].record_number, + subreq[i].record_length); + } + + put_res_header(nmbs, response_data_size); + put_1(nmbs, response_data_size); + + if (nmbs->callbacks.read_file_record) { + for (uint8_t i = 0; i < subreq_count; i++) { + uint16_t subreq_data_size = subreq[i].record_length * 2; + put_1(nmbs, subreq_data_size + 1); + put_1(nmbs, 0x06); // add Reference Type const + uint16_t* subreq_data = (uint16_t*) get_n(nmbs, subreq_data_size); + + err = nmbs->callbacks.read_file_record(subreq[i].file_number, subreq[i].record_number, subreq_data, + subreq[i].record_length, nmbs->msg.unit_id, nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + swap_regs(subreq_data, subreq[i].record_length); + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + + if (!nmbs->msg.broadcast) { + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return recv_read_file_record_res(nmbs, NULL, 0); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_WRITE_FILE_RECORD_DISABLED +static nmbs_error handle_write_file_record(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t request_size = get_1(nmbs); + if (request_size > 251) { + return NMBS_ERROR_INVALID_REQUEST; + } + + err = recv(nmbs, request_size); + if (err != NMBS_ERROR_NONE) + return err; + + // We can save msg.buf index and use it later for context recovery. + uint16_t msg_buf_idx = nmbs->msg.buf_idx; + discard_n(nmbs, request_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + const uint8_t subreq_header_size = 7; + uint16_t size = request_size; + nmbs->msg.buf_idx = msg_buf_idx; // restore context + + if (request_size < 0x07 || request_size > 0xFB) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + do { + uint8_t subreq_reference_type = get_1(nmbs); + uint16_t subreq_file_number_c = get_2(nmbs); + uint16_t subreq_record_number_c = get_2(nmbs); + uint16_t subreq_record_length_c = get_2(nmbs); + discard_n(nmbs, subreq_record_length_c * 2); + + if (subreq_reference_type != 0x06) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq_file_number_c == 0x0000) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq_record_number_c > 0x270F) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq_record_length_c > 122) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", subreq_file_number_c, subreq_record_number_c, + subreq_record_length_c); + size -= (subreq_header_size + subreq_record_length_c * 2); + } while (size >= subreq_header_size); + + if (size) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + // checks completed + + size = request_size; + nmbs->msg.buf_idx = msg_buf_idx; // restore context + + do { + discard_1(nmbs); + uint16_t subreq_file_number = get_2(nmbs); + uint16_t subreq_record_number = get_2(nmbs); + uint16_t subreq_record_length = get_2(nmbs); + uint16_t* subreq_data = get_regs(nmbs, subreq_record_length); + + if (nmbs->callbacks.write_file_record) { + err = nmbs->callbacks.write_file_record(subreq_file_number, subreq_record_number, subreq_data, + subreq_record_length, nmbs->msg.unit_id, nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + swap_regs(subreq_data, subreq_record_length); // restore swapping + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + + size -= (subreq_header_size + subreq_record_length * 2); + } while (size >= subreq_header_size); + + if (!nmbs->msg.broadcast) { + // The normal response to 'Write File' is an echo of the request. + // We can restore buffer index and response msg. + nmbs->msg.buf_idx = msg_buf_idx; + discard_n(nmbs, request_size); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return recv_write_file_record_res(nmbs, 0, 0, NULL, 0); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED +static nmbs_error handle_read_write_registers(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 9); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t read_address = get_2(nmbs); + uint16_t read_quantity = get_2(nmbs); + uint16_t write_address = get_2(nmbs); + uint16_t write_quantity = get_2(nmbs); + + uint8_t byte_count_write = get_1(nmbs); + + NMBS_DEBUG_PRINT("ra %d\trq %d\t wa %d\t wq %d\t b %d\tregs ", read_address, read_quantity, write_address, + write_quantity, byte_count_write); + + if (byte_count_write > 242) + return NMBS_ERROR_INVALID_REQUEST; + + err = recv(nmbs, byte_count_write); + if (err != NMBS_ERROR_NONE) + return err; + +#ifdef __STDC_NO_VLA__ + uint16_t registers[0x007B]; +#else + uint16_t registers[byte_count_write / 2]; +#endif + for (int i = 0; i < byte_count_write / 2; i++) { + registers[i] = get_2(nmbs); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (read_quantity < 1 || read_quantity > 0x007D) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (write_quantity < 1 || write_quantity > 0x007B) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (byte_count_write != write_quantity * 2) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) read_address + (uint32_t) read_quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if ((uint32_t) write_address + (uint32_t) write_quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (!nmbs->callbacks.write_multiple_registers || !nmbs->callbacks.read_holding_registers) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + + err = nmbs->callbacks.write_multiple_registers(write_address, write_quantity, registers, nmbs->msg.unit_id, + nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if (!nmbs->msg.broadcast) { +#ifdef __STDC_NO_VLA__ + uint16_t regs[125]; +#else + uint16_t regs[read_quantity]; +#endif + err = nmbs->callbacks.read_holding_registers(read_address, read_quantity, regs, nmbs->msg.unit_id, + nmbs->callbacks.arg); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + uint8_t regs_bytes = read_quantity * 2; + put_res_header(nmbs, 1 + regs_bytes); + + put_1(nmbs, regs_bytes); + + NMBS_DEBUG_PRINT("b %d\t", regs_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < read_quantity; i++) { + put_2(nmbs, regs[i]); + NMBS_DEBUG_PRINT("%d ", regs[i]); + } + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return recv_write_multiple_registers_res(nmbs, write_address, write_quantity); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED +static nmbs_error handle_read_device_identification(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 3); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t mei_type = get_1(nmbs); + uint8_t read_device_id_code = get_1(nmbs); + uint8_t object_id = get_1(nmbs); + + NMBS_DEBUG_PRINT("c %d\to %d", read_device_id_code, object_id); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (!nmbs->callbacks.read_device_identification_map || !nmbs->callbacks.read_device_identification) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + + if (mei_type != 0x0E) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + + if (read_device_id_code < 1 || read_device_id_code > 4) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (object_id > 6 && object_id < 0x80) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (!nmbs->msg.broadcast) { + char str[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH]; + + nmbs_bitfield_256 map; + nmbs_bitfield_reset(map); + + err = nmbs->callbacks.read_device_identification_map(map); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + put_res_header(nmbs, 0); // Length will be set later + put_1(nmbs, 0x0E); + put_1(nmbs, read_device_id_code); + put_1(nmbs, 0x83); + + if (read_device_id_code == 4) { + if (!nmbs_bitfield_read(map, object_id)) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + put_1(nmbs, 0); // More follows + put_1(nmbs, 0); // Next Object Id + put_1(nmbs, 1); // Number of objects + + str[0] = 0; + err = nmbs->callbacks.read_device_identification(object_id, str); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + size_t str_len = strlen(str); + + put_1(nmbs, object_id); // Object id + put_1(nmbs, str_len); // Object length + put_n(nmbs, (uint8_t*) str, str_len); + + set_msg_header_size(nmbs, 6 + 2 + str_len); + + return send_msg(nmbs); + } + + uint8_t more_follows_idx = nmbs->msg.buf_idx; + put_1(nmbs, 0); + uint8_t next_object_id_idx = nmbs->msg.buf_idx; + put_1(nmbs, 0); + uint8_t number_of_objects_idx = nmbs->msg.buf_idx; + put_1(nmbs, 0); + + int16_t res_size_left = 253 - 7; + + uint8_t last_id = 0; + uint8_t msg_size = 6; + uint8_t res_more_follows = 0; + uint8_t res_next_object_id = 0; + uint8_t res_number_of_objects = 0; + + switch (read_device_id_code) { + case 1: + if (object_id > 0x02) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + last_id = 0x02; + break; + case 2: + if (object_id < 0x03 || object_id > 0x07) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + last_id = 0x07; + break; + case 3: + if (object_id < 0x80) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + last_id = 0xFF; + break; + default: + // Unreachable + break; + } + + for (uint16_t id = object_id; id <= last_id; id++) { + if (!nmbs_bitfield_read(map, id)) { + if (id < 0x03) + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + continue; + } + + str[0] = 0; + err = nmbs->callbacks.read_device_identification((uint8_t) id, str); + if (err != NMBS_ERROR_NONE) { + if (nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + int16_t str_len = (int16_t) strlen(str); + + res_size_left = (int16_t) (res_size_left - 2 - str_len); + if (res_size_left < 0) { + res_more_follows = 0xFF; + res_next_object_id = id; + break; + } + + put_1(nmbs, (uint8_t) id); // Object id + put_1(nmbs, str_len); // Object length + put_n(nmbs, (uint8_t*) str, str_len); + + msg_size += (2 + str_len); + + res_number_of_objects++; + } + + set_1(nmbs, res_more_follows, more_follows_idx); + set_1(nmbs, res_next_object_id, next_object_id_idx); + set_1(nmbs, res_number_of_objects, number_of_objects_idx); + + set_msg_header_size(nmbs, msg_size); + + return send_msg(nmbs); + } + } + else { + return recv_read_device_identification_res(nmbs, 0, NULL, 0, NULL, NULL, NULL, NULL); + } + + return NMBS_ERROR_NONE; +} +#endif + + +static nmbs_error handle_req_fc(nmbs_t* nmbs) { + NMBS_DEBUG_PRINT("fc %d\t", nmbs->msg.fc); + + nmbs_error err = NMBS_ERROR_NONE; + switch (nmbs->msg.fc) { +#ifndef NMBS_SERVER_READ_COILS_DISABLED + case 1: + err = handle_read_coils(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED + case 2: + err = handle_read_discrete_inputs(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED + case 3: + err = handle_read_holding_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED + case 4: + err = handle_read_input_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED + case 5: + err = handle_write_single_coil(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED + case 6: + err = handle_write_single_register(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED + case 15: + err = handle_write_multiple_coils(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED + case 16: + err = handle_write_multiple_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED + case 20: + err = handle_read_file_record(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_FILE_RECORD_DISABLED + case 21: + err = handle_write_file_record(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED + case 23: + err = handle_read_write_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED + case 43: + err = handle_read_device_identification(nmbs); + break; +#endif + default: + err = send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + + return err; +} + + +nmbs_error nmbs_server_create(nmbs_t* nmbs, uint8_t address_rtu, const nmbs_platform_conf* platform_conf, + const nmbs_callbacks* callbacks) { + if (platform_conf->transport == NMBS_TRANSPORT_RTU && address_rtu == 0) + return NMBS_ERROR_INVALID_ARGUMENT; + + nmbs_error ret = nmbs_create(nmbs, platform_conf); + if (ret != NMBS_ERROR_NONE) + return ret; + + nmbs->address_rtu = address_rtu; + nmbs->callbacks = *callbacks; + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_server_poll(nmbs_t* nmbs) { + msg_state_reset(nmbs); + + bool first_byte_received = false; + nmbs_error err = recv_req_header(nmbs, &first_byte_received); + if (err != NMBS_ERROR_NONE) { + if (!first_byte_received && err == NMBS_ERROR_TIMEOUT) + return NMBS_ERROR_NONE; + + return err; + } + +#ifdef NMBS_DEBUG + printf("%d ", nmbs->address_rtu); + printf("NMBS req <- "); + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + if (nmbs->msg.broadcast) + printf("broadcast\t"); + else + printf("address_rtu %d\t", nmbs->msg.unit_id); + } +#endif + + err = handle_req_fc(nmbs); + if (err != NMBS_ERROR_NONE && !nmbs_error_is_exception(err)) { + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU && err != NMBS_ERROR_TIMEOUT && nmbs->msg.ignored) { + // Flush the remaining data on the line + nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg); + } + + return err; + } + + return NMBS_ERROR_NONE; +} + +void nmbs_set_callbacks_arg(nmbs_t* nmbs, void* arg) { + nmbs->callbacks.arg = arg; +} +#endif + + +#ifndef NMBS_CLIENT_DISABLED +nmbs_error nmbs_client_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf) { + return nmbs_create(nmbs, platform_conf); +} + + +static nmbs_error read_discrete(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint16_t quantity, nmbs_bitfield values) { + if (quantity < 1 || quantity > 2000) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, fc); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return recv_read_discrete_res(nmbs, values); +} + + +nmbs_error nmbs_read_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield coils_out) { + return read_discrete(nmbs, 1, address, quantity, coils_out); +} + + +nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out) { + return read_discrete(nmbs, 2, address, quantity, inputs_out); +} + +static nmbs_error read_registers(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint16_t quantity, uint16_t* registers) { + if (quantity < 1 || quantity > 125) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, fc); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + + NMBS_DEBUG_PRINT("a %d\tq %d ", address, quantity); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return recv_read_registers_res(nmbs, quantity, registers); +} + + +nmbs_error nmbs_read_holding_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out) { + return read_registers(nmbs, 3, address, quantity, registers_out); +} + + +nmbs_error nmbs_read_input_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out) { + return read_registers(nmbs, 4, address, quantity, registers_out); +} + +nmbs_error nmbs_write_single_coil(nmbs_t *nmbs, uint16_t address, uint16_t value) +{ + msg_state_req(nmbs, 5); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + + NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_single_coil_res(nmbs, address, value); + + return NMBS_ERROR_NONE; +} + +nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t value) { + msg_state_req(nmbs, 6); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_single_register_res(nmbs, address, value); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const nmbs_bitfield coils) { + if (quantity < 1 || quantity > 0x07B0) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t coils_bytes = (quantity + 7) / 8; + + msg_state_req(nmbs, 15); + put_req_header(nmbs, 5 + coils_bytes); + + put_2(nmbs, address); + put_2(nmbs, quantity); + put_1(nmbs, coils_bytes); + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\t", address, quantity, coils_bytes); + + NMBS_DEBUG_PRINT("coils "); + for (int i = 0; i < coils_bytes; i++) { + put_1(nmbs, coils[i]); + NMBS_DEBUG_PRINT("%d ", coils[i]); + } + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_multiple_coils_res(nmbs, address, quantity); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const uint16_t* registers) { + if (quantity < 1 || quantity > 0x007B) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t registers_bytes = quantity * 2; + + msg_state_req(nmbs, 16); + put_req_header(nmbs, 5 + registers_bytes); + + put_2(nmbs, address); + put_2(nmbs, quantity); + put_1(nmbs, registers_bytes); + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\t", address, quantity, registers_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < quantity; i++) { + put_2(nmbs, registers[i]); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_single_register_res(nmbs, address, quantity); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, uint16_t* registers, + uint16_t count) { + if (file_number == 0x0000) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (record_number > 0x270F) + return NMBS_ERROR_INVALID_ARGUMENT; + + // In expected response: max PDU length = 253, assuming a single file request, (253 - 1 - 1 - 1 - 1) / 2 = 124 + if (count > 124) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, 20); + put_req_header(nmbs, 8); + + put_1(nmbs, 7); // add Byte Count + put_1(nmbs, 6); // add Reference Type const + put_2(nmbs, file_number); + put_2(nmbs, record_number); + put_2(nmbs, count); + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fread ", file_number, record_number, count); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return recv_read_file_record_res(nmbs, registers, count); +} + + +nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, const uint16_t* registers, + uint16_t count) { + if (file_number == 0x0000) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (record_number > 0x270F) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (count > 122) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint16_t data_size = count * 2; + + msg_state_req(nmbs, 21); + put_req_header(nmbs, 8 + data_size); + + put_1(nmbs, 7 + data_size); // add Byte Count + put_1(nmbs, 6); // add Reference Type const + put_2(nmbs, file_number); + put_2(nmbs, record_number); + put_2(nmbs, count); + put_regs(nmbs, registers, count); + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", file_number, record_number, count); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_file_record_res(nmbs, file_number, record_number, registers, count); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16_t read_quantity, + uint16_t* registers_out, uint16_t write_address, uint16_t write_quantity, + const uint16_t* registers) { + if (read_quantity < 1 || read_quantity > 0x007D) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) read_address + (uint32_t) read_quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (write_quantity < 1 || write_quantity > 0x0079) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) write_address + (uint32_t) write_quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t registers_bytes = write_quantity * 2; + + msg_state_req(nmbs, 23); + put_req_header(nmbs, 9 + registers_bytes); + + put_2(nmbs, read_address); + put_2(nmbs, read_quantity); + put_2(nmbs, write_address); + put_2(nmbs, write_quantity); + put_1(nmbs, registers_bytes); + + NMBS_DEBUG_PRINT("read a %d\tq %d ", read_address, read_quantity); + NMBS_DEBUG_PRINT("write a %d\tq %d\tb %d\t", write_address, write_quantity, registers_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < write_quantity; i++) { + put_2(nmbs, registers[i]); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) { + return recv_read_registers_res(nmbs, read_quantity, registers_out); + } + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_device_identification_basic(nmbs_t* nmbs, char* vendor_name, char* product_code, + char* major_minor_revision, uint8_t buffers_length) { + const uint8_t order[3] = {0, 1, 2}; + char* buffers[3] = {vendor_name, product_code, major_minor_revision}; + uint8_t total_received = 0; + uint8_t next_object_id = 0x00; + + while (next_object_id != 0x7F) { + msg_state_req(nmbs, 43); + put_msg_header(nmbs, 3); + put_1(nmbs, 0x0E); + put_1(nmbs, 1); + put_1(nmbs, next_object_id); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t objects_received = 0; + err = recv_read_device_identification_res(nmbs, 3, buffers, buffers_length, order, NULL, &next_object_id, + &objects_received); + if (err != NMBS_ERROR_NONE) + return err; + + total_received += objects_received; + if (total_received > 3) + return NMBS_ERROR_INVALID_RESPONSE; + + if (objects_received == 0) + return NMBS_ERROR_INVALID_RESPONSE; + } + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_device_identification_regular(nmbs_t* nmbs, char* vendor_url, char* product_name, char* model_name, + char* user_application_name, uint8_t buffers_length) { + const uint8_t order[7] = {0, 0, 0, 0, 1, 2, 3}; + char* buffers[4] = {vendor_url, product_name, model_name, user_application_name}; + uint8_t total_received = 0; + uint8_t next_object_id = 0x03; + + while (next_object_id != 0x7F) { + msg_state_req(nmbs, 43); + put_req_header(nmbs, 3); + put_1(nmbs, 0x0E); + put_1(nmbs, 2); + put_1(nmbs, next_object_id); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t objects_received = 0; + err = recv_read_device_identification_res(nmbs, 4, buffers, buffers_length, order, NULL, &next_object_id, + &objects_received); + if (err != NMBS_ERROR_NONE) + return err; + + total_received += objects_received; + if (total_received > 4) + return NMBS_ERROR_INVALID_RESPONSE; + + if (objects_received == 0) + return NMBS_ERROR_INVALID_RESPONSE; + } + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_device_identification_extended(nmbs_t* nmbs, uint8_t object_id_start, uint8_t* ids, char** buffers, + uint8_t ids_length, uint8_t buffer_length, + uint8_t* objects_count_out) { + if (object_id_start < 0x80) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t total_received = 0; + uint8_t next_object_id = object_id_start; + + while (next_object_id != 0x7F) { + msg_state_req(nmbs, 43); + put_req_header(nmbs, 3); + put_1(nmbs, 0x0E); + put_1(nmbs, 3); + put_1(nmbs, next_object_id); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t objects_received = 0; + err = recv_read_device_identification_res(nmbs, ids_length - total_received, &buffers[total_received], + buffer_length, NULL, &ids[total_received], &next_object_id, + &objects_received); + if (err != NMBS_ERROR_NONE) + return err; + + total_received += objects_received; + } + + *objects_count_out = total_received; + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, uint8_t object_id, char* buffer, uint8_t buffer_length) { + if (object_id > 0x06 && object_id < 0x80) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, 43); + put_req_header(nmbs, 3); + put_1(nmbs, 0x0E); + put_1(nmbs, 4); + put_1(nmbs, object_id); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + char* buf[1] = {buffer}; + return recv_read_device_identification_res(nmbs, 1, buf, buffer_length, NULL, NULL, NULL, NULL); +} + + +nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint16_t data_len) { + msg_state_req(nmbs, fc); + put_msg_header(nmbs, data_len); + + NMBS_DEBUG_PRINT("raw "); + for (uint16_t i = 0; i < data_len; i++) { + put_1(nmbs, data[i]); + NMBS_DEBUG_PRINT("%d ", data[i]); + } + + return send_msg(nmbs); +} + + +nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint8_t data_out_len) { + nmbs_error err = recv_res_header(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + err = recv(nmbs, data_out_len); + if (err != NMBS_ERROR_NONE) + return err; + + if (data_out) { + for (uint16_t i = 0; i < data_out_len; i++) + data_out[i] = get_1(nmbs); + } + else { + for (uint16_t i = 0; i < data_out_len; i++) + get_1(nmbs); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_STRERROR_DISABLED +const char* nmbs_strerror(nmbs_error error) { + switch (error) { + case NMBS_ERROR_INVALID_REQUEST: + return "invalid request received"; + + case NMBS_ERROR_INVALID_UNIT_ID: + return "invalid unit ID received"; + + case NMBS_ERROR_INVALID_TCP_MBAP: + return "invalid TCP MBAP received"; + + case NMBS_ERROR_CRC: + return "invalid CRC received"; + + case NMBS_ERROR_TRANSPORT: + return "transport error"; + + case NMBS_ERROR_TIMEOUT: + return "timeout"; + + case NMBS_ERROR_INVALID_RESPONSE: + return "invalid response received"; + + case NMBS_ERROR_INVALID_ARGUMENT: + return "invalid argument provided"; + + case NMBS_ERROR_NONE: + return "no error"; + + case NMBS_EXCEPTION_ILLEGAL_FUNCTION: + return "modbus exception 1: illegal function"; + + case NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS: + return "modbus exception 2: illegal data address"; + + case NMBS_EXCEPTION_ILLEGAL_DATA_VALUE: + return "modbus exception 3: data value"; + + case NMBS_EXCEPTION_SERVER_DEVICE_FAILURE: + return "modbus exception 4: server device failure"; + + default: + return "unknown error"; + } +} +#endif diff --git a/libs/modbus/nanomodbus.h b/libs/modbus/nanomodbus.h new file mode 100644 index 0000000000..195a1f5617 --- /dev/null +++ b/libs/modbus/nanomodbus.h @@ -0,0 +1,528 @@ +/* + nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers + + MIT License + + Copyright (c) 2024 Valerio De Benedetto (@debevv) + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + + +/** @file */ + +/*! \mainpage nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers + * nanoMODBUS is a small C library that implements the Modbus protocol. It is especially useful in resource-constrained + * system like microcontrollers. + * + * GtiHub: https://github.com/debevv/nanoMODBUS + * + * API reference: \link nanomodbus.h \endlink + * + */ + +#ifndef NANOMODBUS_H +#define NANOMODBUS_H + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * nanoMODBUS errors. + * Values <= 0 are library errors, > 0 are modbus exceptions. + */ +typedef enum nmbs_error { + // Library errors + NMBS_ERROR_INVALID_REQUEST = -8, /**< Received invalid request from client */ + NMBS_ERROR_INVALID_UNIT_ID = -7, /**< Received invalid unit ID in response from server */ + NMBS_ERROR_INVALID_TCP_MBAP = -6, /**< Received invalid TCP MBAP */ + NMBS_ERROR_CRC = -5, /**< Received invalid CRC */ + NMBS_ERROR_TRANSPORT = -4, /**< Transport error */ + NMBS_ERROR_TIMEOUT = -3, /**< Read/write timeout occurred */ + NMBS_ERROR_INVALID_RESPONSE = -2, /**< Received invalid response from server */ + NMBS_ERROR_INVALID_ARGUMENT = -1, /**< Invalid argument provided */ + NMBS_ERROR_NONE = 0, /**< No error */ + + // Modbus exceptions + NMBS_EXCEPTION_ILLEGAL_FUNCTION = 1, /**< Modbus exception 1 */ + NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2, /**< Modbus exception 2 */ + NMBS_EXCEPTION_ILLEGAL_DATA_VALUE = 3, /**< Modbus exception 3 */ + NMBS_EXCEPTION_SERVER_DEVICE_FAILURE = 4, /**< Modbus exception 4 */ +} nmbs_error; + + +/** + * Return whether the nmbs_error is a modbus exception + * @e nmbs_error to check + */ +#define nmbs_error_is_exception(e) ((e) > 0 && (e) < 5) + + +/** + * Bitfield consisting of 2000 coils/discrete inputs + */ +typedef uint8_t nmbs_bitfield[250]; + +/** + * Bitfield consisting of 256 values + */ +typedef uint8_t nmbs_bitfield_256[32]; + +/** + * Read a bit from the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_read(bf, b) ((bool) ((bf)[(b) / 8] & (0x1 << ((b) % 8)))) + +/** + * Set a bit of the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_set(bf, b) (((bf)[(b) / 8]) = (((bf)[(b) / 8]) | (0x1 << ((b) % 8)))) + +/** + * Reset a bit of the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_unset(bf, b) (((bf)[(b) / 8]) = (((bf)[(b) / 8]) & ~(0x1 << ((b) % 8)))) + +/** + * Write value v to the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_write(bf, b, v) \ + (((bf)[(b) / 8]) = ((v) ? (((bf)[(b) / 8]) | (0x1 << ((b) % 8))) : (((bf)[(b) / 8]) & ~(0x1 << ((b) % 8))))) + +/** + * Reset (zero) the whole bitfield + */ +#define nmbs_bitfield_reset(bf) memset(bf, 0, sizeof(bf)) + +/** + * Modbus transport type. + */ +typedef enum nmbs_transport { + NMBS_TRANSPORT_RTU = 1, + NMBS_TRANSPORT_TCP = 2, +} nmbs_transport; + + +/** + * nanoMODBUS platform configuration struct. + * Passed to nmbs_server_create() and nmbs_client_create(). + * + * read() and write() are the platform-specific methods that read/write data to/from a serial port or a TCP connection. + * + * Both methods should block until either: + * - `count` bytes of data are read/written + * - the byte timeout, with `byte_timeout_ms >= 0`, expires + * + * A value `< 0` for `byte_timeout_ms` means no timeout. + * + * Their return value should be the number of bytes actually read/written, or `< 0` in case of error. + * A return value between `0` and `count - 1` will be treated as if a timeout occurred on the transport side. All other + * values will be treated as transport errors. + * + * These methods accept a pointer to arbitrary user-data, which is the arg member of this struct. + * After the creation of an instance it can be changed with nmbs_set_platform_arg(). + */ +typedef struct nmbs_platform_conf { + nmbs_transport transport; /*!< Transport type */ + int32_t (*read)(uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, + void* arg); /*!< Bytes read transport function pointer */ + int32_t (*write)(const uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, + void* arg); /*!< Bytes write transport function pointer */ + void* arg; /*!< User data, will be passed to functions above */ +} nmbs_platform_conf; + + +/** + * Modbus server request callbacks. Passed to nmbs_server_create(). + * + * These methods accept a pointer to arbitrary user data, which is the arg member of the nmbs_platform_conf that was passed + * to nmbs_server_create together with this struct. + * + * `unit_id` is the RTU unit ID of the request sender. It is always 0 on TCP. + */ +typedef struct nmbs_callbacks { +#ifndef NMBS_SERVER_DISABLED +#ifndef NMBS_SERVER_READ_COILS_DISABLED + nmbs_error (*read_coils)(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED + nmbs_error (*read_discrete_inputs)(uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out, uint8_t unit_id, + void* arg); +#endif + +#ifndef NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED + nmbs_error (*read_holding_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, + void* arg); +#endif + +#ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED + nmbs_error (*read_input_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, + void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED + nmbs_error (*write_single_coil)(uint16_t address, bool value, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED + nmbs_error (*write_single_register)(uint16_t address, uint16_t value, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED + nmbs_error (*write_multiple_coils)(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id, + void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED + nmbs_error (*write_multiple_registers)(uint16_t address, uint16_t quantity, const uint16_t* registers, + uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED + nmbs_error (*read_file_record)(uint16_t file_number, uint16_t record_number, uint16_t* registers, uint16_t count, + uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_FILE_RECORD_DISABLED + nmbs_error (*write_file_record)(uint16_t file_number, uint16_t record_number, const uint16_t* registers, + uint16_t count, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED +#define NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH 128 + nmbs_error (*read_device_identification)(uint8_t object_id, char buffer[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH]); + nmbs_error (*read_device_identification_map)(nmbs_bitfield_256 map); +#endif +#endif + + void* arg; // User data, will be passed to functions above +} nmbs_callbacks; + + +/** + * nanoMODBUS client/server instance type. All struct members are to be considered private, + * it is not advisable to read/write them directly. + */ +typedef struct nmbs_t { + struct { + uint8_t buf[260]; + uint16_t buf_idx; + + uint8_t unit_id; + uint8_t fc; + uint16_t transaction_id; + bool broadcast; + bool ignored; + } msg; + + nmbs_callbacks callbacks; + + int32_t byte_timeout_ms; + int32_t read_timeout_ms; + + nmbs_platform_conf platform; + + uint8_t address_rtu; + uint8_t dest_address_rtu; + uint16_t current_tid; +} nmbs_t; + +/** + * Modbus broadcast address. Can be passed to nmbs_set_destination_rtu_address(). + */ +static const uint8_t NMBS_BROADCAST_ADDRESS = 0; + +/** Set the request/response timeout. + * If the target instance is a server, sets the timeout of the nmbs_server_poll() function. + * If the target instance is a client, sets the response timeout after sending a request. In case of timeout, + * the called method will return NMBS_ERROR_TIMEOUT. + * @param nmbs pointer to the nmbs_t instance + * @param timeout_ms timeout in milliseconds. If < 0, the timeout is disabled. + */ +void nmbs_set_read_timeout(nmbs_t* nmbs, int32_t timeout_ms); + +/** Set the timeout between the reception/transmission of two consecutive bytes. + * @param nmbs pointer to the nmbs_t instance + * @param timeout_ms timeout in milliseconds. If < 0, the timeout is disabled. + */ +void nmbs_set_byte_timeout(nmbs_t* nmbs, int32_t timeout_ms); + +/** Set the pointer to user data argument passed to platform functions. + * @param nmbs pointer to the nmbs_t instance + * @param arg user data argument + */ +void nmbs_set_platform_arg(nmbs_t* nmbs, void* arg); + +#ifndef NMBS_SERVER_DISABLED +/** Create a new Modbus server. + * @param nmbs pointer to the nmbs_t instance where the client will be created. + * @param address_rtu RTU address of this server. Can be 0 if transport is not RTU. + * @param platform_conf nmbs_platform_conf struct with platform configuration. It may be discarded after calling this method. + * @param callbacks nmbs_callbacks struct with server request callbacks. It may be discarded after calling this method. + * + * @return NMBS_ERROR_NONE if successful, NMBS_ERROR_INVALID_ARGUMENT otherwise. + */ +nmbs_error nmbs_server_create(nmbs_t* nmbs, uint8_t address_rtu, const nmbs_platform_conf* platform_conf, + const nmbs_callbacks* callbacks); + +/** Handle incoming requests to the server. + * This function should be called in a loop in order to serve any incoming request. Its maximum duration, in case of no + * received request, is the value set with nmbs_set_read_timeout() (unless set to < 0). + * @param nmbs pointer to the nmbs_t instance + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_server_poll(nmbs_t* nmbs); + +/** Set the pointer to user data argument passed to server request callbacks. + * @param nmbs pointer to the nmbs_t instance + * @param arg user data argument + */ +void nmbs_set_callbacks_arg(nmbs_t* nmbs, void* arg); +#endif + +#ifndef NMBS_CLIENT_DISABLED +/** Create a new Modbus client. + * @param nmbs pointer to the nmbs_t instance where the client will be created. + * @param platform_conf nmbs_platform_conf struct with platform configuration. It may be discarded after calling this method. + * +* @return NMBS_ERROR_NONE if successful, NMBS_ERROR_INVALID_ARGUMENT otherwise. + */ +nmbs_error nmbs_client_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf); + +/** Set the recipient server address of the next request on RTU transport. + * @param nmbs pointer to the nmbs_t instance + * @param address server address + */ +void nmbs_set_destination_rtu_address(nmbs_t* nmbs, uint8_t address); + +/** Send a FC 01 (0x01) Read Coils request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of coils + * @param coils_out nmbs_bitfield where the coils will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield coils_out); + +/** Send a FC 02 (0x02) Read Discrete Inputs request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of inputs + * @param inputs_out nmbs_bitfield where the discrete inputs will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out); + +/** Send a FC 03 (0x03) Read Holding Registers request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of registers + * @param registers_out array where the registers will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_holding_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out); + +/** Send a FC 04 (0x04) Read Input Registers request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of registers + * @param registers_out array where the registers will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_input_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out); + +/** Send a FC 05 (0x05) Write Single Coil request + * @param nmbs pointer to the nmbs_t instance + * @param address coil address + * @param value coil value + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, uint16_t value); + +/** Send a FC 06 (0x06) Write Single Register request + * @param nmbs pointer to the nmbs_t instance + * @param address register address + * @param value register value + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t value); + +/** Send a FC 15 (0x0F) Write Multiple Coils + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of coils + * @param coils bitfield of coils values + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const nmbs_bitfield coils); + +/** Send a FC 16 (0x10) Write Multiple Registers + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of registers + * @param registers array of registers values + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const uint16_t* registers); + +/** Send a FC 20 (0x14) Read File Record + * @param nmbs pointer to the nmbs_t instance + * @param file_number file number (1 to 65535) + * @param record_number record number from file (0000 to 9999) + * @param registers array of registers to read + * @param count count of registers + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, uint16_t* registers, + uint16_t count); + +/** Send a FC 21 (0x15) Write File Record + * @param nmbs pointer to the nmbs_t instance + * @param file_number file number (1 to 65535) + * @param record_number record number from file (0000 to 9999) + * @param registers array of registers to write + * @param count count of registers + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, const uint16_t* registers, + uint16_t count); + +/** Send a FC 23 (0x17) Read Write Multiple registers + * @param nmbs pointer to the nmbs_t instance + * @param read_address starting read address + * @param read_quantity quantity of registers to read + * @param registers_out array where the read registers will be stored + * @param write_address starting write address + * @param write_quantity quantity of registers to write + * @param registers array of registers values to write + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16_t read_quantity, + uint16_t* registers_out, uint16_t write_address, uint16_t write_quantity, + const uint16_t* registers); + +/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Basic Object Id values (Read Device ID code 1) + * @param nmbs pointer to the nmbs_t instance + * @param vendor_name char array where the read VendorName value will be stored + * @param product_code char array where the read ProductCode value will be stored + * @param major_minor_revision char array where the read MajorMinorRevision value will be stored + * @param buffers_length length of every char array + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_device_identification_basic(nmbs_t* nmbs, char* vendor_name, char* product_code, + char* major_minor_revision, uint8_t buffers_length); + +/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Regular Object Id values (Read Device ID code 2) + * @param nmbs pointer to the nmbs_t instance + * @param vendor_url char array where the read VendorUrl value will be stored + * @param product_name char array where the read ProductName value will be stored + * @param model_name char array where the read ModelName value will be stored + * @param user_application_name char array where the read UserApplicationName value will be stored + * @param buffers_length length of every char array + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_device_identification_regular(nmbs_t* nmbs, char* vendor_url, char* product_name, char* model_name, + char* user_application_name, uint8_t buffers_length); + +/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Extended Object Id values (Read Device ID code 3) + * @param nmbs pointer to the nmbs_t instance + * @param object_id_start Object Id to start reading from + * @param ids array where the read Object Ids will be stored + * @param buffers array of char arrays where the read values will be stored + * @param ids_length length of the ids array and buffers array + * @param buffer_length length of each char array + * @param objects_count_out retrieved Object Ids count + * + * @return NMBS_ERROR_NONE if successful, NMBS_INVALID_ARGUMENT if buffers_count is less than retrieved Object Ids count, + * other errors otherwise. + */ +nmbs_error nmbs_read_device_identification_extended(nmbs_t* nmbs, uint8_t object_id_start, uint8_t* ids, char** buffers, + uint8_t ids_length, uint8_t buffer_length, + uint8_t* objects_count_out); + +/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to retrieve a single Object Id value (Read Device ID code 4) + * @param nmbs pointer to the nmbs_t instance + * @param object_id requested Object Id + * @param buffer char array where the resulting value will be stored + * @param buffer_length length of the char array + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, uint8_t object_id, char* buffer, uint8_t buffer_length); + +/** Send a raw Modbus PDU. + * CRC on RTU will be calculated and sent by this function. + * @param nmbs pointer to the nmbs_t instance + * @param fc request function code + * @param data request data. It's up to the caller to convert this data to network byte order + * @param data_len length of the data parameter + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint16_t data_len); + +/** Receive a raw response Modbus PDU. + * @param nmbs pointer to the nmbs_t instance + * @param data_out response data. It's up to the caller to convert this data to host byte order. Can be NULL. + * @param data_out_len number of bytes to receive + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint8_t data_out_len); +#endif + +/** Calculate the Modbus CRC of some data. + * @param data Data + * @param length Length of the data + */ +uint16_t nmbs_crc_calc(const uint8_t* data, uint32_t length); + +#ifndef NMBS_STRERROR_DISABLED +/** Convert a nmbs_error to string + * @param error error to be converted + * + * @return string representation of the error + */ +const char* nmbs_strerror(nmbs_error error); +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif //NANOMODBUS_H diff --git a/libs/modbus/platform.h b/libs/modbus/platform.h new file mode 100644 index 0000000000..badbe15eb3 --- /dev/null +++ b/libs/modbus/platform.h @@ -0,0 +1,126 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "nanomodbus.h" + + +#define UNUSED_PARAM(x) ((x) = (x)) + +// Connection management +int client_connection = -1; +int client_read_fd = -1; +fd_set client_connections; + +void disconnect(void* conn) +{ + int fd = *(int*) conn; + FD_CLR(fd, &client_connections); + close(fd); +} + +// Read/write/sleep platform functions + +int32_t read_fd_linux(uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) +{ + int fd = *(int*) arg; + + uint16_t total = 0; + while (total != count) + { + fd_set rfds; + FD_ZERO(&rfds); + FD_SET(fd, &rfds); + + struct timeval* tv_p = NULL; + struct timeval tv; + if (timeout_ms >= 0) + { + tv_p = &tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (int64_t) (timeout_ms % 1000) * 1000; + } + + int ret = select(fd + 1, &rfds, NULL, NULL, tv_p); + if (ret == 0) + { + return total; + } + + if (ret == 1) + { + ssize_t r = read(fd, buf + total, 1); + if (r == 0) + { + disconnect(arg); + return -1; + } + + if (r < 0) + return -1; + + total += r; + } + else + return -1; + } + + return total; +} + + +int32_t write_fd_linux(const uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) +{ + int fd = *(int*) arg; + + uint16_t total = 0; + while (total != count) + { + fd_set wfds; + FD_ZERO(&wfds); + FD_SET(fd, &wfds); + + struct timeval* tv_p = NULL; + struct timeval tv; + if (timeout_ms >= 0) + { + tv_p = &tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (int64_t) (timeout_ms % 1000) * 1000; + } + + int ret = select(fd + 1, NULL, &wfds, NULL, tv_p); + if (ret == 0) + { + return 0; + } + + if (ret == 1) + { + ssize_t w = write(fd, buf + total, count); + if (w == 0) + { + disconnect(arg); + return -1; + } + + if (w <= 0) + return -1; + + total += (int32_t) w; + } + else + return -1; + } + + return total; +} From 9ac0c316ff69d91a93f8c639e12ef13b9efe1fdc Mon Sep 17 00:00:00 2001 From: Jasem Mutlaq Date: Wed, 24 Jul 2024 14:07:07 +0300 Subject: [PATCH 2/4] First relay should start from 1 --- libs/indibase/indirelayinterface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libs/indibase/indirelayinterface.cpp b/libs/indibase/indirelayinterface.cpp index 36d05fa820..ded4761d41 100644 --- a/libs/indibase/indirelayinterface.cpp +++ b/libs/indibase/indirelayinterface.cpp @@ -49,8 +49,8 @@ void RelayInterface::initProperties(const char *groupName, uint8_t relays) // Initialize labels for (auto i = 0; i < relays; i++) { - auto name = "RELAY_" + std::to_string(i); - auto label = "Relay #" + std::to_string(i); + auto name = "RELAY_" + std::to_string(i + 1); + auto label = "Relay #" + std::to_string(i + 1); INDI::WidgetText oneLabel; oneLabel.fill(name, label, label); @@ -65,8 +65,8 @@ void RelayInterface::initProperties(const char *groupName, uint8_t relays) // Initialize switches, use labels if loaded. for (size_t i = 0; i < relays; i++) { - auto name = "RELAY_" + std::to_string(i); - auto label = "Relay #" + std::to_string(i); + auto name = "RELAY_" + std::to_string(i + 1); + auto label = "Relay #" + std::to_string(i + 1); INDI::PropertySwitch oneRelay {3}; oneRelay[Open].fill("OPEN", "Open", ISS_OFF); From bb733f9f3f6a4743ff726ff47ff6fec33fc751e8 Mon Sep 17 00:00:00 2001 From: Jasem Mutlaq Date: Wed, 24 Jul 2024 14:07:29 +0300 Subject: [PATCH 3/4] Driver is operational --- drivers/auxiliary/waveshare_modbus_relay.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/drivers/auxiliary/waveshare_modbus_relay.cpp b/drivers/auxiliary/waveshare_modbus_relay.cpp index 2df45ca143..ab669d445a 100644 --- a/drivers/auxiliary/waveshare_modbus_relay.cpp +++ b/drivers/auxiliary/waveshare_modbus_relay.cpp @@ -181,7 +181,7 @@ bool WaveshareRelay::QueryRelay(uint32_t index, Status &status) } else { - status = static_cast(nmbs_bitfield_read(coils, index)); + status = (nmbs_bitfield_read(coils, index) == 0) ? Closed : Opened; return true; } } From d64dded83b7088f927c14b05b2e43de59232b017 Mon Sep 17 00:00:00 2001 From: Jasem Mutlaq Date: Wed, 24 Jul 2024 14:10:02 +0300 Subject: [PATCH 4/4] Add driver to list --- drivers.xml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/drivers.xml b/drivers.xml index 70a2363f82..4b09d9f687 100644 --- a/drivers.xml +++ b/drivers.xml @@ -792,7 +792,11 @@ indi_terrans_powerboxpro_v2 1.0 - + + + indi_wavesharemodbus_relay + 1.0 +