modbus_helpers.h

Go to the documentation of this file.

#pragma once
 
#include <cmath>
#include <optional>
#include <span>
#include <string>
#include <vector>
 
#include "esphome/core/helpers.h"
#include "esphome/components/modbus/modbus_definitions.h"
 
namespace esphome::modbus::helpers {
 
// Pure read codes (0x01-0x04): they only read, so they are idempotent and safe to retry.
inline bool is_function_code_read_only(uint8_t function_code) {
  FunctionCode masked_function_code = static_cast<FunctionCode>(function_code & FUNCTION_CODE_MASK);
  return masked_function_code == FunctionCode::READ_COILS ||
         masked_function_code == FunctionCode::READ_DISCRETE_INPUTS ||
         masked_function_code == FunctionCode::READ_HOLDING_REGISTERS ||
         masked_function_code == FunctionCode::READ_INPUT_REGISTERS;
}
 
// Codes whose response carries read-back data: the pure reads plus 0x17, which reads and writes at once.
inline bool is_function_code_read(uint8_t function_code) {
  return is_function_code_read_only(function_code) ||
         static_cast<FunctionCode>(function_code & FUNCTION_CODE_MASK) == FunctionCode::READ_WRITE_MULTIPLE_REGISTERS;
}
 
// Codes that mutate registers or coils: the pure writes, 0x16 mask-write, and 0x17 read/write multiple.
inline bool is_function_code_write(uint8_t function_code) {
  FunctionCode masked_function_code = static_cast<FunctionCode>(function_code & FUNCTION_CODE_MASK);
  return masked_function_code == FunctionCode::WRITE_SINGLE_COIL ||
         masked_function_code == FunctionCode::WRITE_SINGLE_REGISTER ||
         masked_function_code == FunctionCode::WRITE_MULTIPLE_COILS ||
         masked_function_code == FunctionCode::WRITE_MULTIPLE_REGISTERS ||
         masked_function_code == FunctionCode::MASK_WRITE_REGISTER ||
         masked_function_code == FunctionCode::READ_WRITE_MULTIPLE_REGISTERS;
}
 
// True if [start_address, start_address + count) fits within the 16-bit Modbus address space. The 32-bit
// promotion is the overflow guard - a 16-bit sum could wrap and pass.
inline bool address_range_fits(uint16_t start_address, size_t count) {
  return uint32_t(start_address) + count <= 0x10000u;
}
 
inline bool is_function_code_exception(uint8_t function_code) {
  return (static_cast<uint8_t>(function_code) & FUNCTION_CODE_EXCEPTION_MASK) != 0;
}
 
inline bool is_function_code_custom(uint8_t function_code) {
  uint8_t masked_function_code = function_code & FUNCTION_CODE_MASK;
  return (masked_function_code >= FUNCTION_CODE_USER_DEFINED_SPACE_1_INIT &&
          masked_function_code <= FUNCTION_CODE_USER_DEFINED_SPACE_1_END) ||
         (masked_function_code >= FUNCTION_CODE_USER_DEFINED_SPACE_2_INIT &&
          masked_function_code <= FUNCTION_CODE_USER_DEFINED_SPACE_2_END);
}
 
inline bool is_function_code_unknown_length(uint8_t function_code) {
  if (is_function_code_exception(function_code))
    return false;
  switch (static_cast<FunctionCode>(function_code)) {
    case FunctionCode::READ_COILS:
    case FunctionCode::READ_DISCRETE_INPUTS:
    case FunctionCode::READ_HOLDING_REGISTERS:
    case FunctionCode::READ_INPUT_REGISTERS:
    case FunctionCode::WRITE_SINGLE_COIL:
    case FunctionCode::WRITE_SINGLE_REGISTER:
    case FunctionCode::WRITE_MULTIPLE_COILS:
    case FunctionCode::WRITE_MULTIPLE_REGISTERS:
    case FunctionCode::READ_FILE_RECORD:
    case FunctionCode::WRITE_FILE_RECORD:
    case FunctionCode::MASK_WRITE_REGISTER:
    case FunctionCode::READ_WRITE_MULTIPLE_REGISTERS:
    case FunctionCode::READ_FIFO_QUEUE:
      return false;
    default:
      return true;
  }
}
 
inline bool is_function_code_broadcastable(uint8_t function_code) {
  uint8_t masked_function_code = function_code & FUNCTION_CODE_MASK;
  if (is_function_code_read(masked_function_code))
    return false;
  return is_function_code_write(masked_function_code) || is_function_code_unknown_length(masked_function_code);
}
 
// Returns the expected length of a server response PDU based on the function code.
// If too few bytes have arrived to determine the length, returns the minimum length. `size` is the
// number of bytes available so far, which may exceed the eventual PDU (e.g. include the frame's CRC
// bytes): only fixed header positions are interpreted, so surplus bytes are never misread.
uint16_t server_pdu_length(const uint8_t *frame, size_t size);
// Frame counterpart: address(1) + PDU + CRC(2). Passes every received byte after the address through,
// so header fields (e.g. a byte count) are interpreted as soon as they arrive.
inline uint16_t server_frame_length(const uint8_t *frame, size_t size) {
  if (size < 2)
    return MIN_FRAME_SIZE;  // function code not received yet
  return server_pdu_length(frame + 1, size - 1) + 3;
}
 
// Returns the expected length of a client request PDU based on the function code.
// Same contract as server_pdu_length(): `size` is bytes available so far, may exceed the PDU.
uint16_t client_pdu_length(const uint8_t *frame, size_t size);
inline uint16_t client_frame_length(const uint8_t *frame, size_t size) {
  if (size < 2)
    return MIN_FRAME_SIZE;  // function code not received yet
  return client_pdu_length(frame + 1, size - 1) + 3;
}
 
// Returns true if pdu is a complete transaction whose shape is consistent with its function code.
// Unlike *_pdu_length(), `size` here is the exact PDU length: a size mismatch is non-conformant.
// Function codes with nothing variable to cross-check are validated by their fixed length alone: the
// single writes (except 0x05's value field, which must be 0x0000 or 0xFF00), mask-write and FIFO, and
// - deliberately - custom/unknown codes and exception responses, so a dispatcher can still route
// them by function code rather than reject them outright. Tests pin this contract.
bool is_server_pdu_standard(const uint8_t *pdu, size_t size);
 
// Client counterpart: additionally checks quantity bounds and address-range arithmetic per function code.
// The same acceptance rule applies to custom/unknown function codes.
bool is_client_pdu_standard(const uint8_t *pdu, size_t size);
 
// Remove before 2027.2.0
ESPDEPRECATED("Use server_pdu_payload() on the response PDU instead. Removed in 2027.2.0", "2026.8.0")
inline uint8_t server_frame_data_offset(const uint8_t *frame, size_t size) {
  if (size < 2)
    return 0;
  switch (static_cast<FunctionCode>(frame[1])) {
    case FunctionCode::READ_COILS:
    case FunctionCode::READ_DISCRETE_INPUTS:
    case FunctionCode::READ_HOLDING_REGISTERS:
    case FunctionCode::READ_INPUT_REGISTERS:
      return 3;  // address(1) + function(1) + byte count(1) + data + CRC(2)
    default:
      return 2;
  }
}
 
inline std::span<const uint8_t> server_pdu_payload(std::span<const uint8_t> pdu) {
  if (pdu.empty())
    return {};
  const size_t offset = (!is_function_code_exception(pdu[0]) && is_function_code_read(pdu[0])) ? 2 : 1;
  return pdu.size() > offset ? pdu.subspan(offset) : std::span<const uint8_t>();
}
 
inline uint8_t client_frame_data_offset(const uint8_t *, size_t) { return 2; }
 
template<typename T> T get_data(const uint8_t *data, size_t buffer_offset) {
  if (sizeof(T) == sizeof(uint8_t)) {
    return T(data[buffer_offset]);
  }
  if (sizeof(T) == sizeof(uint16_t)) {
    return T((uint16_t(data[buffer_offset + 0]) << 8) | (uint16_t(data[buffer_offset + 1]) << 0));
  }
  if (sizeof(T) == sizeof(uint32_t)) {
    return static_cast<uint32_t>(get_data<uint16_t>(data, buffer_offset)) << 16 |
           static_cast<uint32_t>(get_data<uint16_t>(data, buffer_offset + 2));
  }
  if (sizeof(T) == sizeof(uint64_t)) {
    return static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset)) << 32 |
           (static_cast<uint64_t>(get_data<uint32_t>(data, buffer_offset + 4)));
  }
  static_assert(sizeof(T) == sizeof(uint8_t) || sizeof(T) == sizeof(uint16_t) || sizeof(T) == sizeof(uint32_t) ||
                    sizeof(T) == sizeof(uint64_t),
                "Unsupported type size in get_data; only 1, 2, 4, or 8-byte integer types are supported.");
  return T{};
}
 
inline uint8_t pdu_function_code(std::span<const uint8_t> pdu) {
  return pdu.empty() ? 0 : (pdu[0] & FUNCTION_CODE_MASK);
}
 
inline std::optional<uint16_t> client_pdu_start_address(std::span<const uint8_t> pdu) {
  if (pdu.size() < 3 || is_function_code_unknown_length(pdu[0]))
    return std::nullopt;
  const auto fc = static_cast<FunctionCode>(pdu[0]);
  // The file-record PDUs are known-length but carry a byte count, not a start address.
  if (fc == FunctionCode::READ_FILE_RECORD || fc == FunctionCode::WRITE_FILE_RECORD)
    return std::nullopt;
  return get_data<uint16_t>(pdu.data(), 1);
}
 
enum class SensorValueType : uint8_t {
  RAW = 0x00,     // variable length
  U_WORD = 0x1,   // 1 Register unsigned
  U_DWORD = 0x2,  // 2 Registers unsigned
  S_WORD = 0x3,   // 1 Register signed
  S_DWORD = 0x4,  // 2 Registers signed
  BIT = 0x5,
  U_DWORD_R = 0x6,  // 2 Registers unsigned
  S_DWORD_R = 0x7,  // 2 Registers signed
  U_QWORD = 0x8,
  S_QWORD = 0x9,
  U_QWORD_R = 0xA,
  S_QWORD_R = 0xB,
  FP32 = 0xC,
  FP32_R = 0xD,
  U_WORD_S = 0xE,  // 1 Register unsigned, bytes swapped
  S_WORD_S = 0xF,  // 1 Register signed, bytes swapped
};
 
inline bool value_type_is_float(SensorValueType v) {
  return v == SensorValueType::FP32 || v == SensorValueType::FP32_R;
}
 
constexpr uint16_t register_width_for(SensorValueType v) {
  switch (v) {
    case SensorValueType::U_DWORD:
    case SensorValueType::S_DWORD:
    case SensorValueType::U_DWORD_R:
    case SensorValueType::S_DWORD_R:
    case SensorValueType::FP32:
    case SensorValueType::FP32_R:
      return 2;
    case SensorValueType::U_QWORD:
    case SensorValueType::S_QWORD:
    case SensorValueType::U_QWORD_R:
    case SensorValueType::S_QWORD_R:
      return 4;
    default:
      return 1;
  }
}
 
inline bool is_entity_type_binary(EntityType type) {
  return type == EntityType::COIL || type == EntityType::DISCRETE_INPUT;
}
 
inline FunctionCode modbus_register_read_function(EntityType reg_type) {
  switch (reg_type) {
    case EntityType::COIL:
      return FunctionCode::READ_COILS;
    case EntityType::DISCRETE_INPUT:
      return FunctionCode::READ_DISCRETE_INPUTS;
    case EntityType::HOLDING:
      return FunctionCode::READ_HOLDING_REGISTERS;
    case EntityType::INPUT_REGISTER:
      return FunctionCode::READ_INPUT_REGISTERS;
    default:
      return FunctionCode::INVALID;
  }
}
 
inline FunctionCode modbus_register_write_function(EntityType reg_type, bool multiple = false) {
  switch (reg_type) {
    case EntityType::COIL:
      return multiple ? FunctionCode::WRITE_MULTIPLE_COILS : FunctionCode::WRITE_SINGLE_COIL;
    case EntityType::HOLDING:
      return multiple ? FunctionCode::WRITE_MULTIPLE_REGISTERS : FunctionCode::WRITE_SINGLE_REGISTER;
    // These register types can't be written (per spec)
    case EntityType::INPUT_REGISTER:
    case EntityType::DISCRETE_INPUT:
    default:
      return FunctionCode::INVALID;
  }
}
 
inline uint8_t c_to_hex(char c) { return (c >= 'A') ? (c >= 'a') ? (c - 'a' + 10) : (c - 'A' + 10) : (c - '0'); }
 
inline uint8_t byte_from_hex_str(const std::string &value, uint8_t pos) {
  if (value.length() < pos * 2 + 2)
    return 0;
  return (c_to_hex(value[pos * 2]) << 4) | c_to_hex(value[pos * 2 + 1]);
}
 
inline uint16_t word_from_hex_str(const std::string &value, uint8_t pos) {
  return byte_from_hex_str(value, pos) << 8 | byte_from_hex_str(value, pos + 1);
}
 
inline uint32_t dword_from_hex_str(const std::string &value, uint8_t pos) {
  return word_from_hex_str(value, pos) << 16 | word_from_hex_str(value, pos + 2);
}
 
inline uint64_t qword_from_hex_str(const std::string &value, uint8_t pos) {
  return static_cast<uint64_t>(dword_from_hex_str(value, pos)) << 32 | dword_from_hex_str(value, pos + 4);
}
 
template<typename T> T get_data(const std::vector<uint8_t> &data, size_t buffer_offset) {
  return get_data<T>(data.data(), buffer_offset);
}
 
inline bool bit_from_packed(int bit, std::span<const uint8_t> data) {
  auto data_byte = bit / 8;
  return (data[data_byte] & (1 << (bit % 8))) > 0;
}
 
// Remove before 2027.2.0
ESPDEPRECATED("Use bit_from_packed() instead. Removed in 2027.2.0", "2026.8.0")
inline bool coil_from_vector(int coil, std::span<const uint8_t> data) { return bit_from_packed(coil, data); }
 
template<typename Out, typename Bits> void pack_bits(Out &out, const Bits &bits) {
  uint8_t byte = 0;
  uint8_t bit = 0;
  for (bool b : bits) {
    if (b)
      byte |= (1 << bit);
    if (++bit == 8) {
      out.push_back(byte);
      byte = 0;
      bit = 0;
    }
  }
  if (bit != 0)  // flush the final partial byte
    out.push_back(byte);
}
 
template<typename N> N mask_and_shift_by_rightbit(N data, uint32_t mask) {
  auto result = (mask & data);
  if (result == 0 || mask == 0xFFFFFFFF) {
    return result;
  }
  for (size_t pos = 0; pos < sizeof(N) << 3; pos++) {
    if (pos < 32 && (mask & (1UL << pos)) != 0)
      return result >> pos;
  }
  return 0;
}
 
// Logs an error for an unsupported value type. Defined in the .cpp so logging stays out of headers.
void log_unsupported_value_type(SensorValueType value_type);
 
template<typename Container> void number_to_payload(Container &data, int64_t value, SensorValueType value_type) {
  switch (value_type) {
    case SensorValueType::U_WORD:
    case SensorValueType::S_WORD:
      data.push_back(value & 0xFFFF);
      break;
    case SensorValueType::U_WORD_S:
    case SensorValueType::S_WORD_S:
      data.push_back(byteswap(static_cast<uint16_t>(value & 0xFFFF)));
      break;
    case SensorValueType::U_DWORD:
    case SensorValueType::S_DWORD:
    case SensorValueType::FP32:
      data.push_back((value & 0xFFFF0000) >> 16);
      data.push_back(value & 0xFFFF);
      break;
    case SensorValueType::U_DWORD_R:
    case SensorValueType::S_DWORD_R:
    case SensorValueType::FP32_R:
      data.push_back(value & 0xFFFF);
      data.push_back((value & 0xFFFF0000) >> 16);
      break;
    case SensorValueType::U_QWORD:
    case SensorValueType::S_QWORD:
      data.push_back((value & 0xFFFF000000000000) >> 48);
      data.push_back((value & 0xFFFF00000000) >> 32);
      data.push_back((value & 0xFFFF0000) >> 16);
      data.push_back(value & 0xFFFF);
      break;
    case SensorValueType::U_QWORD_R:
    case SensorValueType::S_QWORD_R:
      data.push_back(value & 0xFFFF);
      data.push_back((value & 0xFFFF0000) >> 16);
      data.push_back((value & 0xFFFF00000000) >> 32);
      data.push_back((value & 0xFFFF000000000000) >> 48);
      break;
    default:
      log_unsupported_value_type(value_type);
      break;
  }
}
 
std::optional<int64_t> payload_to_number(const uint8_t *data, size_t size, SensorValueType sensor_value_type,
                                         uint8_t offset, uint32_t bitmask);
 
inline std::optional<int64_t> payload_to_number(std::span<const uint8_t> data, SensorValueType sensor_value_type,
                                                uint8_t offset, uint32_t bitmask) {
  return payload_to_number(data.data(), data.size(), sensor_value_type, offset, bitmask);
}
 
// Remove before 2027.2.0
ESPDEPRECATED("Use the std::span overload returning std::optional<int64_t> instead. Removed in 2027.2.0", "2026.8.0")
inline int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
                                 uint32_t bitmask) {
  // Released behavior: a too-short payload logs an error and decodes to 0.
  return payload_to_number(std::span<const uint8_t>(data), sensor_value_type, offset, bitmask).value_or(0);
}
 
std::optional<int64_t> registers_to_number(const uint16_t *registers, size_t count, SensorValueType sensor_value_type);
 
constexpr uint32_t registers_to_uint32(uint16_t high_word, uint16_t low_word) {
  return (static_cast<uint32_t>(high_word) << 16) | low_word;
}
 
constexpr uint64_t registers_to_uint64(uint16_t word0, uint16_t word1, uint16_t word2, uint16_t word3) {
  return (static_cast<uint64_t>(registers_to_uint32(word0, word1)) << 32) | registers_to_uint32(word2, word3);
}
 
// Always false, whatever the type: it exists only to make the static_assert below depend on the
// template argument. Not a queryable trait.
template<SensorValueType> inline constexpr bool VALUE_TYPE_SUPPORTED = false;
 
template<SensorValueType VALUE_TYPE> constexpr auto registers_to_value(const uint16_t *registers) {
  if constexpr (VALUE_TYPE == SensorValueType::U_WORD) {
    return registers[0];
  } else if constexpr (VALUE_TYPE == SensorValueType::S_WORD) {
    return static_cast<int16_t>(registers[0]);
  } else if constexpr (VALUE_TYPE == SensorValueType::U_WORD_S) {
    return byteswap(registers[0]);
  } else if constexpr (VALUE_TYPE == SensorValueType::S_WORD_S) {
    return static_cast<int16_t>(byteswap(registers[0]));
  } else if constexpr (VALUE_TYPE == SensorValueType::U_DWORD) {
    return registers_to_uint32(registers[0], registers[1]);
  } else if constexpr (VALUE_TYPE == SensorValueType::U_DWORD_R) {
    return registers_to_uint32(registers[1], registers[0]);
  } else if constexpr (VALUE_TYPE == SensorValueType::S_DWORD) {
    return static_cast<int32_t>(registers_to_uint32(registers[0], registers[1]));
  } else if constexpr (VALUE_TYPE == SensorValueType::S_DWORD_R) {
    return static_cast<int32_t>(registers_to_uint32(registers[1], registers[0]));
  } else if constexpr (VALUE_TYPE == SensorValueType::FP32) {
    return bit_cast<float>(registers_to_uint32(registers[0], registers[1]));
  } else if constexpr (VALUE_TYPE == SensorValueType::FP32_R) {
    return bit_cast<float>(registers_to_uint32(registers[1], registers[0]));
  } else if constexpr (VALUE_TYPE == SensorValueType::U_QWORD) {
    return registers_to_uint64(registers[0], registers[1], registers[2], registers[3]);
  } else if constexpr (VALUE_TYPE == SensorValueType::U_QWORD_R) {
    return registers_to_uint64(registers[3], registers[2], registers[1], registers[0]);
  } else if constexpr (VALUE_TYPE == SensorValueType::S_QWORD) {
    return static_cast<int64_t>(registers_to_uint64(registers[0], registers[1], registers[2], registers[3]));
  } else if constexpr (VALUE_TYPE == SensorValueType::S_QWORD_R) {
    return static_cast<int64_t>(registers_to_uint64(registers[3], registers[2], registers[1], registers[0]));
  } else {
    static_assert(VALUE_TYPE_SUPPORTED<VALUE_TYPE>, "registers_to_value() does not support this value type");
  }
}
 
template<SensorValueType VALUE_TYPE>
using RegisterValueType = decltype(registers_to_value<VALUE_TYPE>(static_cast<const uint16_t *>(nullptr)));
 
template<SensorValueType VALUE_TYPE>
constexpr std::optional<RegisterValueType<VALUE_TYPE>> value_at(std::span<const uint16_t> registers,
                                                                uint16_t start_address, uint16_t address) {
  if (address < start_address)
    return std::nullopt;
  const size_t offset = static_cast<size_t>(address) - start_address;
  if (offset + register_width_for(VALUE_TYPE) > registers.size())
    return std::nullopt;
  return registers_to_value<VALUE_TYPE>(registers.data() + offset);
}
 
static constexpr uint16_t MAX_FEW_REGISTERS = 4;
 
// Named PDU buffer types: the builders' storage strategy (currently stack-allocated StaticVector,
// right-sized per shape) can be swapped in one place without touching every signature.
using PduBuffer = StaticVector<uint8_t, MAX_PDU_SIZE>;
using ReadPdu = StaticVector<uint8_t, READ_PDU_SIZE>;
using WriteSinglePdu = StaticVector<uint8_t, WRITE_SINGLE_PDU_SIZE>;
using WriteFewRegistersPdu = StaticVector<uint8_t, WRITE_MULTIPLE_HEADER_SIZE + 2 * MAX_FEW_REGISTERS>;
using CoilPackBuffer = StaticVector<uint8_t, packed_bit_bytes(MAX_NUM_OF_COILS_TO_WRITE)>;
 
ReadPdu create_read_pdu(FunctionCode function_code, uint16_t start_address, uint16_t number_of_entities);
 
PduBuffer create_client_pdu(FunctionCode function_code, uint16_t start_address, uint16_t number_of_entities,
                            const uint8_t *values = nullptr, size_t values_len = 0);
 
PduBuffer create_write_registers_pdu(uint16_t start_address, std::span<const uint16_t> values);
 
WriteFewRegistersPdu create_write_few_registers_pdu(uint16_t start_address, std::span<const uint16_t> values);
 
PduBuffer create_read_write_multiple_registers_pdu(uint16_t read_start_address, uint16_t read_count,
                                                   uint16_t write_start_address,
                                                   std::span<const uint16_t> write_values);
 
WriteSinglePdu create_write_single_register_pdu(uint16_t start_address, uint16_t value);
 
WriteSinglePdu create_write_single_coil_pdu(uint16_t address, bool value);
 
PduBuffer create_write_coils_pdu(uint16_t start_address, std::span<const bool> values);
 
PduBuffer create_write_coils_pdu(uint16_t start_address, const std::vector<bool> &values);
 
PduBuffer create_write_coils_pdu(uint16_t start_address, PackedBits bits);
 
template<typename Container> void float_to_payload(Container &data, float value, SensorValueType value_type) {
  int64_t val;
 
  if (value_type_is_float(value_type)) {
    val = bit_cast<uint32_t>(value);
  } else {
    val = llroundf(value);
  }
 
  number_to_payload(data, val, value_type);
}
 
// Remove before 2027.2.0
ESPDEPRECATED("Use the container overload of float_to_payload() instead. Removed in 2027.2.0", "2026.8.0")
inline std::vector<uint16_t> float_to_payload(float value, SensorValueType value_type) {
  std::vector<uint16_t> data;
  float_to_payload(data, value, value_type);
  return data;
}
 
}  // namespace esphome::modbus::helpers