Hopi-pico/modbus_rtu.c

#include "modbus_rtu.h"

#include <string.h>

// Standard Modbus RTU CRC16 (poly 0xA001)
uint16_t modbus_rtu_crc16(const uint8_t *data, size_t len) {
  uint16_t crc = 0xFFFF;
  for (size_t i = 0; i < len; i++) {
    crc ^= data[i];
    for (int j = 0; j < 8; j++) {
      if (crc & 1) {
        crc = (uint16_t)((crc >> 1) ^ 0xA001);
      } else {
        crc >>= 1;
      }
    }
  }
  return crc;
}

uint16_t modbus_rtu_get_u16_be(const uint8_t *p) {
  return (uint16_t)(((uint16_t)p[0] << 8) | (uint16_t)p[1]);
}

void modbus_rtu_master_init(modbus_rtu_master_t *m, modbus_rtu_master_config_t cfg) {
  memset(m, 0, sizeof(*m));
  m->cfg = cfg;
}

size_t modbus_rtu_build_read_holding(uint8_t *out, size_t out_cap,
                                    uint8_t slave_id, uint16_t start_addr, uint16_t quantity,
                                    bool disable_crc) {
  // Request: [id][0x03][start hi][start lo][qty hi][qty lo][crc lo][crc hi]
  const size_t base_len = 6;
  const size_t crc_len = disable_crc ? 0 : 2;
  if (out_cap < base_len + crc_len) return 0;

  out[0] = slave_id;
  out[1] = 0x03;
  out[2] = (uint8_t)(start_addr >> 8);
  out[3] = (uint8_t)(start_addr & 0xFF);
  out[4] = (uint8_t)(quantity >> 8);
  out[5] = (uint8_t)(quantity & 0xFF);

  if (!disable_crc) {
    uint16_t crc = modbus_rtu_crc16(out, base_len);
    out[6] = (uint8_t)(crc & 0xFF);
    out[7] = (uint8_t)(crc >> 8);
  }

  return base_len + crc_len;
}

void modbus_rtu_feed(modbus_rtu_master_t *m, const uint8_t *data, size_t len) {
  // Simple append with truncation on overflow.
  size_t space = sizeof(m->rx_buf) - m->rx_len;
  if (len > space) len = space;
  if (len == 0) return;
  memcpy(&m->rx_buf[m->rx_len], data, len);
  m->rx_len += len;
}

static void drop_prefix(modbus_rtu_master_t *m, size_t n) {
  if (n == 0 || n > m->rx_len) return;
  memmove(m->rx_buf, m->rx_buf + n, m->rx_len - n);
  m->rx_len -= n;
}

bool modbus_rtu_try_parse_read_holding_response(modbus_rtu_master_t *m,
                                               uint8_t expected_slave_id,
                                               uint16_t expected_quantity,
                                               size_t *out_data_offset,
                                               uint16_t *out_word_count) {
  // Response: [id][0x03][byte_count][data...][crc lo][crc hi]
  // Some devices always include CRC but may compute it incorrectly.
  // If m->cfg.disable_crc==true, we do NOT validate CRC, but we still accept frames that include CRC bytes.
  // We also accept CRC-less frames only when m->cfg.disable_crc==true.
  const size_t min_header = 3;

  // Try to resync if buffer has junk: look for [id][0x03].
  while (m->rx_len >= 2) {
    if (m->rx_buf[0] == expected_slave_id && m->rx_buf[1] == 0x03) break;
    drop_prefix(m, 1);
  }

  if (m->rx_len < min_header) return false;

  uint8_t byte_count = m->rx_buf[2];
  if ((byte_count % 2) != 0) {
    // invalid, resync
    drop_prefix(m, 1);
    return false;
  }

  const uint16_t word_count = (uint16_t)(byte_count / 2);
  if (word_count != expected_quantity) {
    // Could be a different request/response; resync minimally.
    // (You can relax this if you later add more Modbus queries.)
    drop_prefix(m, 1);
    return false;
  }

  const size_t data_len = (size_t)3 + (size_t)byte_count;
  if (m->rx_len < data_len) return false;

  // Prefer CRC-framed response when present.
  const bool has_crc = (m->rx_len >= (data_len + 2));
  const size_t total_len = has_crc ? (data_len + 2) : data_len;

  if (has_crc && !m->cfg.disable_crc) {
    uint16_t got_crc = (uint16_t)((uint16_t)m->rx_buf[total_len - 2] | ((uint16_t)m->rx_buf[total_len - 1] << 8));
    uint16_t calc_crc = modbus_rtu_crc16(m->rx_buf, total_len - 2);
    if (got_crc != calc_crc) {
      drop_prefix(m, 1);
      return false;
    }
  }

  if (!has_crc && !m->cfg.disable_crc) {
    // In strict mode we require CRC bytes to be present.
    return false;
  }

  *out_data_offset = 3;
  *out_word_count = word_count;

  // Leave data in buffer for caller to consume; caller should drop_prefix(total_len) afterwards.
  return true;
}

float modbus_float_dcba(uint16_t w1, uint16_t w2) {
  // Matches your ESPHome lambdas:
  // b0=low(w2), b1=high(w2), b2=low(w1), b3=high(w1)
  // u = b0<<24 | b1<<16 | b2<<8 | b3
  uint8_t b0 = (uint8_t)(w2 & 0xFF);
  uint8_t b1 = (uint8_t)((w2 >> 8) & 0xFF);
  uint8_t b2 = (uint8_t)(w1 & 0xFF);
  uint8_t b3 = (uint8_t)((w1 >> 8) & 0xFF);

  uint32_t u = ((uint32_t)b0 << 24) | ((uint32_t)b1 << 16) | ((uint32_t)b2 << 8) | (uint32_t)b3;
  float f;
  memcpy(&f, &u, sizeof(f));
  return f;
}