Added function read_write_multiple_registers (#6)

CMakePresets.json

@@ -21,6 +21,17 @@
         "conf-base"
       ]
     },
+    {
+      "name": "conf-gcc-debug",
+      "cacheVariables": {
+        "VCPKG_TARGET_TRIPLET": "x64-linux-gcc",
+        "CMAKE_INSTALL_PREFIX": "${fileDir}/install/${presetName}",
+        "CMAKE_BUILD_TYPE": "Debug"
+      },
+      "inherits": [
+        "conf-base"
+      ]
+    },
     {
       "name": "conf-clang",
       "cacheVariables": {

examples/CMakeLists.txt

@@ -1,6 +1,9 @@
 
 add_executable(client_example client_example.cpp)
-target_link_libraries(client_example PRIVATE modbus )
+target_link_libraries(client_example PRIVATE modbus)
 
 add_executable(server_example server_example.cpp)
-target_link_libraries(server_example PRIVATE modbus )
+target_link_libraries(server_example PRIVATE modbus)
+
+add_executable(client_example_read_write_registers client_example_read_write_registers.cpp)
+target_link_libraries(client_example_read_write_registers PRIVATE modbus)

examples/client_example_read_write_registers.cpp

@@ -0,0 +1,57 @@
+// Copyright (c) 2023, Skaginn3x (https://skaginn3x.com)
+
+#include <iostream>
+#include <thread>
+#include <vector>
+
+#include <modbus/client.hpp>
+
+void on_io_error(std::error_code const& error) {
+  std::cout << "Read error: " << error.message() << "\n";
+}
+
+int main(int argc, const char** argv) {
+  if (argc < 3) {
+    std::cout << "Usage: " << argv[0] << " <hostname> <port>\n";
+    return -1;
+  }
+
+  std::vector<std::thread> pool;
+  asio::io_context ctx;
+
+  std::string_view hostname = argv[1];
+  std::string_view port = argv[2];
+
+  modbus::client client{ ctx };
+
+  co_spawn(
+      ctx,
+      [&]() -> asio::awaitable<void> {
+        auto [error] =
+            co_await client.connect(std::string(hostname), std::string(port), asio::as_tuple(asio::use_awaitable));
+        if (error) {
+          std::cerr << "Error connecting: " << error.message() << '\n';
+          exit(-1);
+        }
+        std::cout << "Connected!" << '\n';
+
+        for (size_t i = 0; i < 25; i++) {
+          auto response =
+              co_await client.read_write_multiple_registers(0, 1000, 10, 1010, { 1, 2, 3, 4, 5, 6 }, asio::use_awaitable);
+          if (!response) {
+            std::cerr << "Error reading: " << response.error().message() << '\n';
+            exit(-1);
+          }
+          std::cout << "Read registers: ";
+          for (unsigned short i : response->values) {
+            std::cout << "\t\t" << i << " ";
+          }
+          std::cout << '\n';
+          std::this_thread::sleep_for(std::chrono::milliseconds{ 150 });
+        }
+        client.close();
+      },
+      asio::detached);
+
+  ctx.run();
+}

include/modbus/client.hpp

@@ -215,6 +215,23 @@ class client {
                                           std::forward<decltype(token)>(token));
   }
 
+  /// Perform a read_write_multiple_registers on the connected server.
+  /**
+   * Compliant servers will set the value of the register to:
+   * ((old_value AND and_mask) OR (or_mask AND NOT and_MASK))
+   */
+  template <typename completion_token>
+  auto read_write_multiple_registers(std::uint8_t unit,
+                                     std::uint16_t read_address,
+                                     std::uint16_t read_count,
+                                     std::uint16_t write_address,
+                                     std::vector<std::uint16_t> values,
+                                     completion_token&& token) {
+    return send_message<completion_token>(
+        unit, request::read_write_multiple_registers{ read_address, read_count, write_address, values },
+        std::forward<decltype(token)>(token));
+  }
+
 protected:
   /// Send a Modbus request to the server.
   template <typename completion_token>

include/modbus/default_handler.hpp

@@ -85,6 +85,20 @@ struct default_handler {
     return resp;
   }
 
+  modbus::response::read_write_multiple_registers handle(uint8_t,
+                                                         const modbus::request::read_write_multiple_registers& req,
+                                                         modbus::errc_t&) {
+    modbus::response::read_write_multiple_registers resp{};
+    auto iit = req.values.begin();
+    auto oit = registers.begin() + req.write_address;
+    while (iit < req.values.end() && oit < registers.end()) {
+      *oit++ = *iit++;
+    }
+    resp.values.insert(resp.values.end(), registers.cbegin() + req.read_address,
+                       registers.cbegin() + req.read_address + req.read_count);
+    return resp;
+  }
+
   // TODO: Verify this method
   modbus::response::mask_write_register handle(uint8_t, const modbus::request::mask_write_register& req, modbus::errc_t&) {
     modbus::response::mask_write_register resp{};

include/modbus/impl/deserialize_request.hpp

@@ -31,14 +31,15 @@ auto request_from_function(function_e func) -> std::expected<request::requests,
       return request::write_multiple_registers{};
     case function_e::mask_write_register:
       return request::mask_write_register{};
+    case function_e::read_write_multiple_registers:
+      return request::read_write_multiple_registers{};
     case function_e::read_exception_status:
     case function_e::diagnostic:
     case function_e::get_com_event_log:
     case function_e::get_com_event_counter:
     case function_e::report_server_id:
     case function_e::read_file_record:
     case function_e::write_file_record:
-    case function_e::read_write_multiple_registers:
     case function_e::read_fifo_record:
     default:
       return std::unexpected(modbus_error(errc_t::illegal_function));

include/modbus/impl/deserialize_response.hpp

@@ -30,14 +30,15 @@ auto response_from_function(function_e func) -> std::expected<response::response
       return response::write_multiple_registers{};
     case function_e::mask_write_register:
       return response::mask_write_register{};
+    case function_e::read_write_multiple_registers:
+      return response::read_write_multiple_registers{};
     case function_e::read_exception_status:
     case function_e::diagnostic:
     case function_e::get_com_event_log:
     case function_e::get_com_event_counter:
     case function_e::report_server_id:
     case function_e::read_file_record:
     case function_e::write_file_record:
-    case function_e::read_write_multiple_registers:
     case function_e::read_fifo_record:
     default:
       return std::unexpected(modbus_error(errc_t::illegal_function));

include/modbus/request.hpp

@@ -45,6 +45,7 @@ struct write_single_register;
 struct write_multiple_coils;
 struct write_multiple_registers;
 struct mask_write_register;
+struct read_write_multiple_registers;
 }  // namespace response
 
 namespace request {
@@ -437,6 +438,65 @@ struct mask_write_register {
   }
 };
 
+/// Message representing a read_write_multiple_registers request.
+struct read_write_multiple_registers {
+  /// Response type.
+  using response = response::read_write_multiple_registers;
+
+  /// The function code.
+  static constexpr function_e function = function_e::read_write_multiple_registers;
+
+  /// The address of the register to read from
+  std::uint16_t read_address;
+
+  /// The amount of words to read
+  std::uint16_t read_count;
+
+  /// The address of the register to read from
+  std::uint16_t write_address;
+
+  /// The values to write.
+  std::vector<std::uint16_t> values;
+
+  /// The length of the serialized ADU in bytes.
+  [[nodiscard]] auto length() const -> std::size_t { return 10 + values.size() * 2; }
+
+  [[nodiscard]] auto serialize() const -> std::vector<uint8_t> {
+    std::vector<uint8_t> ret_value;
+    ret_value.emplace_back(impl::serialize_function(function));
+
+    auto read_address_arr = impl::serialize_16_array(impl::serialize_be16(read_address));
+    ret_value.insert(ret_value.end(), read_address_arr.begin(), read_address_arr.end());
+
+    auto read_count_arr = impl::serialize_16_array(impl::serialize_be16(read_count));
+    ret_value.insert(ret_value.end(), read_count_arr.begin(), read_count_arr.end());
+
+    auto write_address_arr = impl::serialize_16_array(impl::serialize_be16(write_address));
+    ret_value.insert(ret_value.end(), write_address_arr.begin(), write_address_arr.end());
+
+    auto arr_values = impl::serialize_words_request(values);
+    ret_value.insert(ret_value.end(), arr_values.begin(), arr_values.end());
+
+    return ret_value;
+  }
+
+  /// Deserialize request.
+  [[nodiscard]] auto deserialize(std::ranges::range auto data) -> std::error_code {
+    if (auto error = impl::check_length(data.size(), length())) {
+      return error;
+    }
+    read_address = impl::deserialize_be16(std::span(data).subspan(1, 2));
+    read_count = impl::deserialize_be16(std::span(data).subspan(3, 2));
+    write_address = impl::deserialize_be16(std::span(data).subspan(5, 2));
+    auto expected = impl::deserialize_words_request(std::span(data).subspan(7));
+    if (!expected) {
+      return expected.error();
+    }
+    values = expected.value();
+    return {};
+  }
+};
+
 using requests = std::variant<read_coils,
                               read_discrete_inputs,
                               read_holding_registers,
@@ -445,6 +505,7 @@ using requests = std::variant<read_coils,
                               write_single_register,
                               write_multiple_coils,
                               write_multiple_registers,
-                              mask_write_register>;
+                              mask_write_register,
+                              read_write_multiple_registers>;
 }  // namespace request
 }  // namespace modbus

include/modbus/response.hpp

@@ -43,6 +43,7 @@ struct write_single_register;
 struct write_multiple_coils;
 struct write_multiple_registers;
 struct mask_write_register;
+struct read_write_multiple_registers;
 }  // namespace request
 
 namespace response {
@@ -356,6 +357,38 @@ struct mask_write_register {
   }
 };
 
+/// Message representing a read_write_multiple_registers response.
+struct read_write_multiple_registers {
+  /// Request type.
+  using request = request::read_write_multiple_registers;
+
+  /// The function code.
+  static constexpr function_e function = function_e::read_write_multiple_registers;
+
+  /// The read values.
+  std::vector<std::uint16_t> values;
+
+  /// The length of the serialized ADU in bytes.
+  [[nodiscard]] auto length() const -> std::size_t { return 2 + values.size() * 2; }
+
+  [[nodiscard]] auto deserialize(std::ranges::range auto data) -> std::error_code {
+    auto ex_values = impl::deserialize_words_response(std::span(data).subspan(1));
+    if (!ex_values) {
+      return ex_values.error();
+    }
+    values = ex_values.value();
+    return {};
+  }
+
+  [[nodiscard]] auto serialize() const -> std::vector<uint8_t> {
+    std::vector<uint8_t> ret_value;
+    ret_value.emplace_back(impl::serialize_function(function));
+    auto word_response = impl::serialize_words_response(values);
+    ret_value.insert(ret_value.end(), word_response.begin(), word_response.end());
+    return ret_value;
+  }
+};
+
 using responses = std::variant<mask_write_register,
                                read_holding_registers,
                                read_coils,
@@ -364,6 +397,7 @@ using responses = std::variant<mask_write_register,
                                write_multiple_coils,
                                write_multiple_registers,
                                write_single_coil,
-                               write_single_register>;
+                               write_single_register,
+                               read_write_multiple_registers>;
 }  // namespace response
 }  // namespace modbus

include/modbus/server.hpp

@@ -134,6 +134,8 @@ auto handle_connection(tcp::socket client, auto&& handler) -> awaitable<void> {
       std::array<asio::const_buffer, 2> buffs{ asio::buffer(header_bytes), asio::buffer(resp.value()) };
       co_await async_write(state->client_, buffs, use_awaitable);
     } else {
+      std::cerr << "error client: " << state->client_.remote_endpoint() << " error " << modbus_error(resp.error()).message()
+                << '\n';
       co_await async_write(state->client_, asio::buffer(build_error_buffer(header, 0, resp.error()), count), use_awaitable);
     }
   }

tests/deserialization.cpp

@@ -233,6 +233,24 @@ int main() {
     expect(request.or_mask == 16);
   };
 
+  "deserialize request read_write_multiple_registers"_test = []() {
+    // Request captured from M580 PLC
+    std::array<std::uint8_t, 21> data = { 0x04, 0x8c, 0x00, 0x00, 0x00, 0x0d, 0xff, 0x17, 0x00, 0x00,
+                                          0x00, 0x27, 0x00, 0x00, 0x00, 0x01, 0x02, 0x00, 0x00 };
+
+    auto expected_request = deserialize_request(std::span(data).subspan(modbus::tcp_mbap::size),
+                                                modbus::function_e::read_write_multiple_registers);
+    expect(expected_request.has_value());
+    expect(holds_alternative<modbus::request::read_write_multiple_registers>(expected_request.value()));
+    auto request = std::get<modbus::request::read_write_multiple_registers>(expected_request.value());
+    expect(request.function == modbus::function_e::read_write_multiple_registers);
+    expect(request.values.size() == 1);
+    expect(request.values[0] == 0);
+    expect(request.read_address == 0);
+    expect(request.read_count == 39);
+    expect(request.write_address == 0);
+  };
+
   // ATH, mbpoll -r parameter is from 1 but the modbus addresses
   // are from 0. So the address 1 in mbpoll is 0 in modbus.
 
@@ -388,5 +406,25 @@ int main() {
     expect(response.and_mask == 15);
     expect(response.or_mask == 16);
   };
+
+  "deserialize response read_write_multiple_registers"_test = []() {
+    // Response captured from M580 PLC
+    auto data =
+        std::array<uint8_t, 29>{ 0x00, 0x0e, 0x00, 0x00, 0x00, 0x17, 0xfc, 0x17, 0x14, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03,
+                                 0x00, 0x04, 0x00, 0x05, 0x00, 0x06, 0x00, 0x07, 0x00, 0x08, 0x00, 0x09, 0x00, 0x0a };
+
+    auto expected_response = deserialize_response(std::span(data).subspan(modbus::tcp_mbap::size),
+                                                  modbus::function_e::read_write_multiple_registers);
+    expect(expected_response.has_value());
+    expect(holds_alternative<modbus::response::read_write_multiple_registers>(expected_response.value()));
+    auto response = std::get<modbus::response::read_write_multiple_registers>(expected_response.value());
+    expect(response.function == modbus::function_e::read_write_multiple_registers);
+    expect(response.values.size() == 10);
+    std::uint16_t i = 0;
+    for (auto& elem : response.values) {
+      expect(elem == ++i);
+    }
+  };
+
   return 0;
 }