ch32v203-eth-node/modbus.c

#include "modbus.h"

#include <string.h>

#include "debug.h"
#include "systick.h"
#include "uart.h"
#include "utils.h"

// modbus ctx
typedef struct {
  uint8_t buffer[256];
  uint16_t rx_len;
  uint32_t last_send_time;
  uint32_t response_timeout;
  modbus_state_t state;
  void (*on_response)(uint16_t value);
  void (*on_error)(uint8_t error);
} modbus_t;

// handler ctx
typedef struct {
  uint8_t current_device_idx;
  char current_property[16];
  void (*value_cb)(uint8_t device_idx, const char* property, uint16_t value);
} modbus_handler_t;

static modbus_t mb;
static modbus_handler_t handler;

static uint16_t modbus_crc16(const unsigned char* buf, unsigned int len) {
  static const uint16_t table[256] = {
      0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, 0xC601,
      0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440, 0xCC01, 0x0CC0,
      0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, 0x0A00, 0xCAC1, 0xCB81,
      0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841, 0xD801, 0x18C0, 0x1980, 0xD941,
      0x1B00, 0xDBC1, 0xDA81, 0x1A40, 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01,
      0x1DC0, 0x1C80, 0xDC41, 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0,
      0x1680, 0xD641, 0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081,
      0x1040, 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
      0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, 0x3C00,
      0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, 0xFA01, 0x3AC0,
      0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, 0x2800, 0xE8C1, 0xE981,
      0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, 0xEE01, 0x2EC0, 0x2F80, 0xEF41,
      0x2D00, 0xEDC1, 0xEC81, 0x2C40, 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700,
      0xE7C1, 0xE681, 0x2640, 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0,
      0x2080, 0xE041, 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281,
      0x6240, 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
      0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41, 0xAA01,
      0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, 0x7800, 0xB8C1,
      0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41, 0xBE01, 0x7EC0, 0x7F80,
      0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, 0xB401, 0x74C0, 0x7580, 0xB541,
      0x7700, 0xB7C1, 0xB681, 0x7640, 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101,
      0x71C0, 0x7080, 0xB041, 0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0,
      0x5280, 0x9241, 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481,
      0x5440, 0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
      0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, 0x8801,
      0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, 0x4E00, 0x8EC1,
      0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, 0x4400, 0x84C1, 0x8581,
      0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x8201, 0x42C0, 0x4380, 0x8341,
      0x4100, 0x81C1, 0x8081, 0x4040};

  uint8_t xor = 0;
  uint16_t crc = 0xFFFF;

  while (len--) {
    xor = (*buf++) ^ crc;
    crc >>= 8;
    crc ^= table[xor];
  }

  return crc;
}

static bool modbus_send(uint8_t slave_id, uint8_t function, uint16_t addr,
                        uint16_t value) {
  if (mb.state != MODBUS_IDLE) {
    return false;
  }

  uint8_t* req = mb.buffer;
  req[0] = slave_id;
  req[1] = function;

  uint8_t hi, lo;
  to_bytes(addr, &hi, &lo);
  req[2] = hi;
  req[3] = lo;

  to_bytes(value, &hi, &lo);
  req[4] = hi;
  req[5] = lo;

  uint16_t crc = modbus_crc16(req, 6);
  req[6] = crc & 0xFF;
  req[7] = (crc >> 8) & 0xFF;

  rs485_send(req, 8);
  mb.last_send_time = millis();
  mb.rx_len = 0;
  mb.state = MODBUS_WAITING_RESPONSE;

  return true;
}

static void process_response(void) {
  uint8_t* buf = mb.buffer;
  uint16_t len = mb.rx_len;

  // check minimum length and CRC
  if (len < 5 ||
      modbus_crc16(buf, len - 2) != ((buf[len - 1] << 8) | buf[len - 2])) {
    mb.on_error(MODBUS_ERROR_CRC);
    return;
  }

  // check for error response
  if (buf[1] & 0x80) {
    mb.on_error(buf[2]);
    return;
  }

  // extract value based on function code
  uint16_t value;
  switch (buf[1]) {
    case MODBUS_FC_READ_HOLDING_REGISTERS:
      if (len >= 7) {
        value = (buf[3] << 8) | buf[4];
        mb.on_response(value);
      }
      break;

    case MODBUS_FC_WRITE_SINGLE_REGISTER:
      if (len == 8) {
        value = (buf[4] << 8) | buf[5];
        mb.on_response(value);
      }
      break;

    default:
      mb.on_error(MODBUS_ERROR_ILLEGAL_FUNCTION);
  }
}

static void modbus_process(void) {
  if (mb.state != MODBUS_WAITING_RESPONSE) {
    return;
  }

  // chec for timeout
  if (millis() - mb.last_send_time >= mb.response_timeout) {
    mb.on_error(MODBUS_ERROR_TIMEOUT);
    mb.state = MODBUS_IDLE;
    return;
  }

  // read available data
  while (rs485_available() && mb.rx_len < sizeof(mb.buffer)) {
    mb.buffer[mb.rx_len++] = rs485_read();

    // complete message?
    if (mb.rx_len >= 8 || (mb.rx_len >= 5 && mb.buffer[1] & 0x80) ||
        (mb.rx_len >= 5 && mb.buffer[1] == MODBUS_FC_READ_HOLDING_REGISTERS &&
         mb.rx_len >= (3 + mb.buffer[2] + 2))) {
      process_response();
      mb.state = MODBUS_IDLE;
      break;
    }
  }
}

static void modbus_init(void (*response_cb)(uint16_t),
                        void (*error_cb)(uint8_t)) {
  memset(&mb, 0, sizeof(mb));
  mb.state = MODBUS_IDLE;
  mb.response_timeout = 200;
  mb.on_response = response_cb;
  mb.on_error = error_cb;

  // flush any stale data - we get 0x00 on startup??
  while (rs485_available()) {
    rs485_read();
  }
}

// internal response cb
static void on_modbus_response(uint16_t value) {
  if (handler.current_device_idx >= RS485_DEVICE_COUNT || !handler.value_cb) {
    return;
  }

  // forward value to mqtt cb
  handler.value_cb(handler.current_device_idx, handler.current_property, value);

  handler.current_device_idx = 0xFF;
  handler.current_property[0] = '\0';
}

// internal error cb
static void on_modbus_error(__attribute__((unused)) uint8_t error_code) {
  handler.current_device_idx = 0xFF;
  handler.current_property[0] = '\0';
}

static uint16_t get_register_address(device_type_t type, const char* property) {
  // todo: more device types
  if (type == DEVICE_RELAY && strcmp(property, "state") == 0) {
    return 0x0000;
  }

  return 0xFFFF;  // invalid reg
}

void modbus_handler_init(modbus_value_cb value_callback) {
  memset(&handler, 0xFF, sizeof(handler));
  handler.current_property[0] = '\0';
  handler.value_cb = value_callback;

  modbus_init(on_modbus_response, on_modbus_error);
}

void modbus_handler_process(void) { modbus_process(); }

bool modbus_handler_send_request(uint8_t device_idx, const char* property,
                                 bool is_write, uint16_t value) {
  if (device_idx >= RS485_DEVICE_COUNT) {
    return false;
  }

  uint16_t reg = get_register_address(RS485_DEVICES[device_idx].type, property);
  if (reg == 0xFFFF) {
    return false;
  }

  uint8_t function = is_write ? MODBUS_FC_WRITE_SINGLE_REGISTER
                              : MODBUS_FC_READ_HOLDING_REGISTERS;

  if (modbus_send(RS485_DEVICES[device_idx].slave_id, function, reg, value)) {
    // store ctx for resp handling
    handler.current_device_idx = device_idx;
    strncpy(handler.current_property, property,
            sizeof(handler.current_property) - 1);
    handler.current_property[sizeof(handler.current_property) - 1] = '\0';
    return true;
  }

  return false;
}

void modbus_publish_value(MQTTClient* client, const char* device_name,
                          const char* property, uint16_t value) {
  char topic[MAX_TOPIC_LENGTH];
  char payload[17];  // 16 bits + null terminator

  snprintf(topic, sizeof(topic), "homie/%s/%s/%s", NODE_CONFIG.id, device_name,
           property);

  // format based on property type
  if (strcmp(property, "state") == 0) {
    for (int i = 15; i >= 0; i--) {
      payload[15 - i] = '0' + ((value >> i) & 1);
    }
    payload[16] = '\0';
  } else {
    // todo:
    return;
  }

  MQTTMessage message = {.qos = QOS1,
                         .retained = 1,
                         .payload = payload,
                         .payloadlen = strlen(payload)};

  if (MQTTPublish(client, topic, &message) != 0) {
    DEBUG_PRINT("Failed to publish to %s\n", topic);
  }
}