ModBus: Implementation of all common commands

diff --git a/test-modbus/modbus.c b/test-modbus/modbus.c
index 2e8320ee2ca2cefcd6f57fc301e70182340cc4d8..24a2589eb5e1770e6af7a51b32b0dc4d532df380 100644 (file)
--- a/test-modbus/modbus.c
+++ b/test-modbus/modbus.c
@@ -23,6 +23,9 @@ static u16 rx_size;
 static byte rx_bad;
 static byte state;             // STATE_xxx
 
+static byte *rx_frame;
+static byte *rx_frame_end;
+
 static byte tx_buf[TX_BUFSIZE];
 static u16 tx_size;
 static u16 tx_pos;
@@ -259,11 +262,31 @@ static bool check_frame(void)
                return false;
        }
 
+       rx_frame = rx_buf + 1;
+       rx_frame_end = rx_frame + rx_size - 2;
        return true;
 }
 
 enum mb_function {
+       FUNC_READ_COILS = 0x01,
+       FUNC_READ_DISCRETE_INPUTS = 0x02,
        FUNC_READ_HOLDING_REGISTERS = 0x03,
+       FUNC_READ_INPUT_REGISTERS = 0x04,
+       FUNC_WRITE_SINGLE_COIL = 0x05,
+       FUNC_WRITE_SINGLE_REGISTER = 0x06,
+       FUNC_READ_EXCEPTION_STATUS = 0x07,
+       FUNC_DIAGNOSTICS = 0x08,
+       FUNC_GET_COMM_EVENT_COUNTER = 0x0b,
+       FUNC_GET_COMM_EVENT_LOG = 0x0c,
+       FUNC_WRITE_MULTIPLE_COILS = 0x0f,
+       FUNC_WRITE_MULTIPLE_REGISTERS = 0x10,
+       FUNC_REPORT_SLAVE_ID = 0x11,
+       FUNC_READ_FILE_RECORD = 0x14,
+       FUNC_WRITE_FILE_RECORD = 0x15,
+       FUNC_MASK_WRITE_REGISTER = 0x16,
+       FUNC_READ_WRITE_MULTIPLE_REGISTERS = 0x17,
+       FUNC_READ_FIFO_QUEUE = 0x18,
+       FUNC_ENCAPSULATED_INTERFACE_TRANSPORT = 0x2b,
 };
 
 enum mb_error {
@@ -272,6 +295,40 @@ enum mb_error {
        ERR_ILLEGAL_DATA_VALUE = 0x03,
 };
 
+static uint read_remains(void)
+{
+       return rx_frame_end - rx_frame;
+}
+
+static byte read_byte(void)
+{
+       return *rx_frame++;
+}
+
+static u16 read_u16(void)
+{
+       byte hi = *rx_frame++;
+       byte lo = *rx_frame++;
+       return (hi << 8) | lo;
+}
+
+static void write_byte(byte v)
+{
+       tx_buf[tx_size++] = v;
+}
+
+static void write_u16(u16 v)
+{
+       write_byte(v >> 8);
+       write_byte(v);
+}
+
+static bool body_fits(uint body_len)
+{
+       // body_len excludes slave address, function code, and CRC
+       return (2 + body_len + 2 <= TX_BUFSIZE);
+}
+
 static void report_error(byte code)
 {
        // Discard the partially constructed body of the reply and rewrite the header
@@ -280,9 +337,180 @@ static void report_error(byte code)
        tx_size = 3;
 }
 
+static void func_read_bits(bool coils)
+{
+       if (read_remains() < 4)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 start = read_u16();
+       u16 count = read_u16();
+
+       uint bytes = (count+7) / 8;
+       if (!body_fits(1 + bytes))
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       for (u16 i = 0; i < count; i++)
+               if (!(coils ? modbus_check_coil : modbus_check_discrete_input)(start + i))
+                       return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       write_byte(bytes);
+       for (u16 i = 0; i < bytes; i++) {
+               byte b = 0;
+               for (byte j = 0; j < 8 && 8*i + j < count; j++) {
+                       uint addr = start + 8*i + j;
+                       if ((coils ? modbus_get_coil : modbus_get_discrete_input)(addr))
+                               b |= 1 << j;
+               }
+               write_byte(b);
+       }
+}
+
+static void func_read_registers(byte holding)
+{
+       if (read_remains() < 4)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 start = read_u16();
+       u16 count = read_u16();
+
+       uint bytes = 2*count;
+       if (!body_fits(1 + bytes))
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       for (u16 i = 0; i < count; i++)
+               if (!(holding ? modbus_check_holding_register : modbus_check_input_register)(start + i))
+                       return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       // FIXME: Reporting of slave failures?
+       write_byte(bytes);
+       for (u16 i = 0; i < count; i++)
+               write_u16((holding ? modbus_get_holding_register : modbus_get_input_register)(start + i));
+}
+
+static void func_write_single_coil(void)
+{
+       if (read_remains() < 4)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 addr = read_u16();
+       u16 value = read_u16();
+
+       if (!modbus_check_coil(addr))
+               return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+       if (value != 0x0000 && value != 0xff00)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       modbus_set_coil(addr, value);
+}
+
+static void func_write_single_register(void)
+{
+       if (read_remains() < 4)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 addr = read_u16();
+       u16 value = read_u16();
+
+       if (!modbus_check_holding_register(addr))
+               return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       modbus_set_holding_register(addr, value);
+}
+
+static void func_write_multiple_coils(void)
+{
+       if (read_remains() < 5)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 start = read_u16();
+       u16 count = read_u16();
+       byte bytes = read_byte();
+
+       if (read_remains() < bytes || bytes != (count+7) / 8)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       for (u16 i = 0; i < count; i++)
+               if (!modbus_check_coil(start + i))
+                       return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       for (u16 i = 0; i < count; i++)
+               modbus_set_coil(start + i, rx_frame[i/8] & (1U << (i%8)));
+}
+
+static void func_write_multiple_registers(void)
+{
+       if (read_remains() < 5)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 start = read_u16();
+       u16 count = read_u16();
+       byte bytes = read_byte();
+
+       if (read_remains() < bytes || bytes != 2*count)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       for (u16 i = 0; i < count; i++)
+               if (!modbus_check_holding_register(start + i))
+                       return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       for (u16 i = 0; i < count; i++)
+               modbus_set_holding_register(start + i, read_u16());
+}
+
+static void func_mask_write_register(void)
+{
+       if (read_remains() < 6)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 addr = read_u16();
+       u16 and_mask = read_u16();
+       u16 or_mask = read_u16();
+
+       if (!modbus_check_holding_register(addr))
+               return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       u16 reg = modbus_get_holding_register(addr);
+       reg = (reg & and_mask) | (or_mask & ~and_mask);
+       modbus_set_holding_register(addr, reg);
+}
+
+static void func_read_write_multiple_registers(void)
+{
+       if (read_remains() < 9)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       u16 read_start = read_u16();
+       u16 read_count = read_u16();
+       u16 write_start = read_u16();
+       u16 write_count = read_u16();
+       byte write_bytes = read_byte();
+
+       if (read_remains() < write_bytes || write_bytes != 2*write_count)
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       for (u16 i = 0; i < read_count; i++)
+               if (!modbus_check_holding_register(read_start + i))
+                       return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       for (u16 i = 0; i < write_count; i++)
+               if (!modbus_check_holding_register(write_start + i))
+                       return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+
+       byte read_bytes = 2*write_count;
+       if (!body_fits(1 + read_bytes))
+               return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+       for (u16 i = 0; i < write_count; i++)
+               modbus_set_holding_register(write_start + i, read_u16());
+
+       write_byte(read_bytes);
+       for (u16 i = 0; i < read_count; i++)
+               modbus_get_holding_register(read_start + i);
+}
+
 static void process_frame(void)
 {
-       byte func = rx_buf[1];
+       byte func = read_byte();
 
        // Prepare reply frame
        tx_buf[0] = MB_OUR_ADDRESS;
@@ -290,19 +518,42 @@ static void process_frame(void)
        tx_size = 2;
 
        switch (func) {
+               case FUNC_READ_COILS:
+                       func_read_bits(true);
+                       break;
+               case FUNC_READ_DISCRETE_INPUTS:
+                       func_read_bits(false);
+                       break;
                case FUNC_READ_HOLDING_REGISTERS:
-                       tx_buf[tx_size++] = 2;
-                       tx_buf[tx_size++] = 0x12;
-                       tx_buf[tx_size++] = 0x34;
+                       func_read_registers(true);
+                       break;
+               case FUNC_READ_INPUT_REGISTERS:
+                       func_read_registers(false);
+                       break;
+               case FUNC_WRITE_SINGLE_COIL:
+                       func_write_single_coil();
+                       break;
+               case FUNC_WRITE_SINGLE_REGISTER:
+                       func_write_single_register();
+                       break;
+               case FUNC_WRITE_MULTIPLE_COILS:
+                       func_write_multiple_coils();
+                       break;
+               case FUNC_WRITE_MULTIPLE_REGISTERS:
+                       func_write_multiple_registers();
+                       break;
+               case FUNC_MASK_WRITE_REGISTER:
+                       func_mask_write_register();
+                       break;
+               case FUNC_READ_WRITE_MULTIPLE_REGISTERS:
+                       func_read_write_multiple_registers();
                        break;
                default:
                        report_error(ERR_ILLEGAL_FUNCTION);
        }
 
        // Finish reply frame
-       u16 crc = crc16(tx_buf, tx_size);
-       tx_buf[tx_size++] = crc >> 8;
-       tx_buf[tx_size++] = crc;
+       write_u16(crc16(tx_buf, tx_size));
 }
 
 void modbus_loop(void)
@@ -332,3 +583,53 @@ void modbus_loop(void)
                rx_init();
        }
 }
+
+/*** Callbacks ***/
+
+bool modbus_check_discrete_input(u16 addr UNUSED)
+{
+       return false;
+}
+
+bool modbus_get_discrete_input(u16 addr UNUSED)
+{
+       return false;
+}
+
+bool modbus_check_coil(u16 addr UNUSED)
+{
+       return false;
+}
+
+bool modbus_get_coil(u16 addr UNUSED)
+{
+       return false;
+}
+
+void modbus_set_coil(u16 addr UNUSED, bool value UNUSED)
+{
+}
+
+bool modbus_check_input_register(u16 addr UNUSED)
+{
+       return false;
+}
+
+u16 modbus_get_input_register(u16 addr UNUSED)
+{
+       return 0;
+}
+
+bool modbus_check_holding_register(u16 addr UNUSED)
+{
+       return (addr == 0);
+}
+
+u16 modbus_get_holding_register(u16 addr UNUSED)
+{
+       return 0xbeef;
+}
+
+void modbus_set_holding_register(u16 addr UNUSED, u16 value UNUSED)
+{
+}

diff --git a/test-modbus/modbus.h b/test-modbus/modbus.h
index 1c2fe624684abc5755b4cad015c827f956976d0d..dd80a661ac725b12bd3211397bd29bcce055ecf0 100644 (file)
--- a/test-modbus/modbus.h
+++ b/test-modbus/modbus.h
@@ -1,2 +1,18 @@
 void modbus_init(void);
 void modbus_loop(void);
+
+// Callbacks
+
+bool modbus_check_discrete_input(u16 addr);
+bool modbus_get_discrete_input(u16 addr);
+
+bool modbus_check_coil(u16 addr);
+bool modbus_get_coil(u16 addr);
+void modbus_set_coil(u16 addr, bool value);
+
+bool modbus_check_input_register(u16 addr);
+u16 modbus_get_input_register(u16 addr);
+
+bool modbus_check_holding_register(u16 addr);
+u16 modbus_get_holding_register(u16 addr);
+void modbus_set_holding_register(u16 addr, u16 value);