#include "modbus.h"

int modbus_protocol;

struct intstr modbus_function[] = {
  { 0x01, "Read Coils" },			/* Old: "Read Coil Status" */
  { 0x02, "Read Discrete Inputs" },		/* Old: "Read Input Status" */
  { 0x03, "Read Holding Registers" },
  { 0x04, "Read Input Registers" },
  { 0x05, "Write Single Coil" },		/* Old: "Force Single Coil" */
  { 0x06, "Write Single Register" },		/* Old: "Preset Single Register" */
  { 0x07, "Read Exception Status" },		/* 				Serial Line only */
  { 0x08, "Diagnostics" },			/* 				Serial Line only */
  { 0x09, "Program 484" },			/* Reserved in v1.1 */
  { 0x0A, "Poll 484" },				/* Reserved in v1.1 */
  { 0x0B, "Get Comm Event Counter" },		/* Old: "Fetch Comm Event Ctr", Serial Line only */
  { 0x0C, "Get Comm Event Log" },		/* Old: "Fetch Comm Event Log", Serial Line only */
  { 0x0D, "Program Controller" },		/* Reserved in v1.1 */
  { 0x0E, "Poll Controller" },			/* Reserved in v1.1 */
  { 0x0F, "Write Multiple Coils" },		/* Old: "Force Multiple Coils" */
  { 0x10, "Write Multiple registers" }, 	/* Old: "Preset Multiple Regs" */
  { 0x11, "Report Slave ID" },			/* 				Serial Line only */
  { 0x12, "Program 884/M84" },			/* Ancient */
  { 0x13, "Reset Comm. Link" },			/* Ancient */
  { 0x14, "Read File Record" },			/* Old: "Read General Reference" */
  { 0x15, "Write File Record" },		/* Old: "Write General Reference" */
  { 0x16, "Mask Write Register" },		/* Old: "Mask Write 4X Register" */
  { 0x17, "Read/Write Multiple registers" },	/* Old: "Read/Write 4X Registers" */
  { 0x18, "Read FIFO Queue" },
  { 0x2B, "Encapsulated Interface Transport" },
};

struct intstr modbus_Diagnostic_Subfunctions[] = { /* used for function 0x08 */
  { 0x00, "Return Query Data" },
  { 0x01, "Restart Communications Option" },
  { 0x02, "Return Diagnostic Register" },
  { 0x03, "Change ASCII Input Delimiter" },
  { 0x04, "Force Listen Only Mode" },
  /* 0x05 .. 0x09 Reserved */
  { 0x0A, "Clear Counters and Diagnostic Register" },
  { 0x0B, "Return Bus Message Count" },
  { 0x0C, "Return Bus Communication Error Count" },
  { 0x0D, "Return Bus Exception Error Count" },
  { 0x0E, "Return Slave Message Count" },
  { 0x0F, "Return Slave No Response Count" },
  { 0x10, "Return Slave NAK Count" },
  { 0x11, "Return Slave Busy Count" },
  { 0x12, "Return Bus Character Overrun Count" },
  { 0x13, "Return IOP Overrun Count" },			/* Reserved in v1.1 */
  { 0x14, "Clear Overrun Counter and Flag" },
  { 0x15, "Get/Clear Modbus Plus Statistics" },		/* Reserved in v1.1 */
  /* 0x16 .. 0xffff Reserved */
};

struct intstr modbus_MEI_Subfunctions[] = { /* used for function 0x2B */
  { 0x0D, "CANopen General Reference Request and Response PDU" },
  { 0x0E, "Read Device Identification" },
};

struct intstr modbus_Exception_Codes[] = {
  { 0x01, "ILLEGAL FUNCTION" },
  { 0x02, "ILLEGAL DATA ADDRESS" },
  { 0x03, "ILLEGAL DATA VALUE" },
  { 0x04, "SLAVE DEVICE FAILURE" },
  { 0x05, "ACKNOWLEDGE" },
  { 0x06, "SLAVE DEVICE BUSY" },
  { 0x07, "NEGATIVE ACKNOWLEDGE" },			/* Not in v1.1 */
  { 0x08, "MEMORY PARITY ERROR" },
  { 0x0A, "GATEWAY PATH UNAVAILABLE" },
  { 0x0B, "GATEWAY TARGET DEVICE FAILED TO RESPOND" },
};

int modbus_mbapcheck(int sz, uint8_t *buf, uint8_t uid) {
  uint16_t tid;
  uint16_t pid;
  uint16_t len;

  LOG_IF(sz < MODBUS_HDRSZ_MBAP, LOG_ERR) return -1;
  LOG_IF(buf == NULL, LOG_ERR) return -1;

  tid = buf[0] << 8 | buf[1];
  pid = buf[2] << 8 | buf[3];
  len = buf[4] << 8 | buf[5];
  LOG_IF(pid != 0, LOG_ERR) return -1;
  LOG_IF(len != sz - MODBUS_HDRSZ_MBAP + 1, LOG_ERR) return -1;
  LOG_IF(uid != buf[6], LOG_ERR) return -1;

  return tid;
}

int modbus_mbap_mkhdr( size_t buflen, uint8_t *buf, uint16_t tid, uint16_t len, uint8_t uid) {
  LOG_IF(buflen < 7, LOG_ERR) return -1;
  LOG_IF(!buf, LOG_ERR) return -1;
  
  MODBUS_WRREG(buf, tid);
  buf[2] = buf[3] = 0;
  MODBUS_WRREG(buf+4, len);
  buf[6] = uid;
  return 0;
}

int modbus_req_read( size_t buflen, uint8_t *buf, uint8_t func, uint16_t starting_address, uint16_t count) {
  LOG_IF(buflen < 5, LOG_ERR) return -1;
  LOG_IF(!buf, LOG_ERR) return -1;

  LOG_IF( func == 0 || func > 4, LOG_ERR) return -1;
  LOG_IF( count == 0, LOG_ERR) return -1;

  switch (func) {
  case 1:
  case 2:
    LOG_IF(count > 2000, LOG_ERR) return -1;
    break;
  case 3:
  case 4:
    LOG_IF(count > 125, LOG_ERR) return -1;
  }

  buf[0] = func;
  MODBUS_WRREG(buf+1, starting_address);
  MODBUS_WRREG(buf+3, count);

  return 0;
}