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 {
 
inline bool is_function_code_read(uint8_t function_code) {
  ModbusFunctionCode masked_function_code = static_cast<ModbusFunctionCode>(function_code & FUNCTION_CODE_MASK);
  return masked_function_code == ModbusFunctionCode::READ_COILS ||
         masked_function_code == ModbusFunctionCode::READ_DISCRETE_INPUTS ||
         masked_function_code == ModbusFunctionCode::READ_HOLDING_REGISTERS ||
         masked_function_code == ModbusFunctionCode::READ_INPUT_REGISTERS;
}
 
inline bool is_function_code_write(uint8_t function_code) {
  ModbusFunctionCode masked_function_code = static_cast<ModbusFunctionCode>(function_code & FUNCTION_CODE_MASK);
  return masked_function_code == ModbusFunctionCode::WRITE_SINGLE_COIL ||
         masked_function_code == ModbusFunctionCode::WRITE_SINGLE_REGISTER ||
         masked_function_code == ModbusFunctionCode::WRITE_MULTIPLE_COILS ||
         masked_function_code == ModbusFunctionCode::WRITE_MULTIPLE_REGISTERS;
}
 
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);
}
 
// Returns the expected length of a server response frame based on the function code
// If the frame is too short to determine the length, returns the minimum length
uint16_t server_frame_length(const uint8_t *frame, size_t size);
 
// Returns the expected length of a client request frame based on the function code
// If the frame is too short to determine the length, returns the minimum length
uint16_t client_frame_length(const uint8_t *frame, size_t size);
 
inline uint8_t server_frame_data_offset(const uint8_t *frame, size_t size) {
  if (size < 2)
    return 0;
  switch (static_cast<ModbusFunctionCode>(frame[1])) {
    case ModbusFunctionCode::READ_COILS:
    case ModbusFunctionCode::READ_DISCRETE_INPUTS:
    case ModbusFunctionCode::READ_HOLDING_REGISTERS:
    case ModbusFunctionCode::READ_INPUT_REGISTERS:
      return 3;  // address(1) + function(1) + byte count(1) + data + CRC(2)
    default:
      return 2;
  }
}
 
inline uint8_t client_frame_data_offset(const uint8_t *, size_t) { return 2; }
 
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 unsigned
  U_QWORD = 0x8,
  S_QWORD = 0x9,
  U_QWORD_R = 0xA,
  S_QWORD_R = 0xB,
  FP32 = 0xC,
  FP32_R = 0xD
};
 
inline bool value_type_is_float(SensorValueType v) {
  return v == SensorValueType::FP32 || v == SensorValueType::FP32_R;
}
 
inline ModbusFunctionCode modbus_register_read_function(ModbusRegisterType reg_type) {
  switch (reg_type) {
    case ModbusRegisterType::COIL:
      return ModbusFunctionCode::READ_COILS;
    case ModbusRegisterType::DISCRETE_INPUT:
      return ModbusFunctionCode::READ_DISCRETE_INPUTS;
    case ModbusRegisterType::HOLDING:
      return ModbusFunctionCode::READ_HOLDING_REGISTERS;
    case ModbusRegisterType::INPUT_REGISTER:
      return ModbusFunctionCode::READ_INPUT_REGISTERS;
    default:
      return ModbusFunctionCode::INVALID;
  }
}
 
inline ModbusFunctionCode modbus_register_write_function(ModbusRegisterType reg_type, bool multiple = false) {
  switch (reg_type) {
    case ModbusRegisterType::COIL:
      return multiple ? ModbusFunctionCode::WRITE_MULTIPLE_COILS : ModbusFunctionCode::WRITE_SINGLE_COIL;
    case ModbusRegisterType::HOLDING:
      return multiple ? ModbusFunctionCode::WRITE_MULTIPLE_REGISTERS : ModbusFunctionCode::WRITE_SINGLE_REGISTER;
    // These register types can't be written (per spec)
    case ModbusRegisterType::INPUT_REGISTER:
    case ModbusRegisterType::DISCRETE_INPUT:
    default:
      return ModbusFunctionCode::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);
}
 
// Extract data from modbus response buffer
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{};
}
 
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 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_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);
 
StaticVector<uint8_t, MAX_PDU_SIZE> create_client_pdu(ModbusFunctionCode function_code, uint16_t start_address,
                                                      uint16_t number_of_entities, const uint8_t *values = nullptr,
                                                      size_t values_len = 0);
 
inline std::vector<uint16_t> float_to_payload(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);
  }
 
  std::vector<uint16_t> data;
  number_to_payload(data, val, value_type);
  return data;
}
 
}  // namespace esphome::modbus::helpers