+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 func_encapsulated_interface_transport(void)
+{
+ if (read_remains() < 3 ||
+ read_byte() != 0x0e)
+ return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+ byte action = read_byte();
+ byte id = read_byte();
+
+ byte range_min, range_max;
+ switch (action) {
+ case 1:
+ // Streaming access to basic identification
+ range_min = MODBUS_ID_VENDOR_NAME;
+ range_max = MODBUS_ID_MAJOR_MINOR_REVISION;
+ break;
+ case 2:
+ // Streaming access to regular identification
+ range_min = MODBUS_ID_VENDOR_URL;
+ range_max = MODBUS_ID_USER_APP_NAME;
+ break;
+ case 4:
+ // Individual access
+ if (id >= MODBUS_ID_MAX || !modbus_id_strings[id])
+ return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+ range_min = range_max = id;
+ break;
+ default:
+ return report_error(ERR_ILLEGAL_DATA_VALUE);
+ }
+
+ if (action != 4) {
+ if (id < range_min || id > range_max)
+ id = range_min;
+ }
+
+ write_byte(0x0e); // Repeat a part of the request
+ write_byte(action);
+
+ // Conformity level
+ if (modbus_id_strings[MODBUS_ID_VENDOR_URL] ||
+ modbus_id_strings[MODBUS_ID_PRODUCT_NAME] ||
+ modbus_id_strings[MODBUS_ID_USER_APP_NAME])
+ write_byte(0x82); // Regular identification, both stream and individual access supported
+ else
+ write_byte(0x81); // Basic identification only
+
+ u16 more_follows_at = tx_size;
+ write_byte(0); // More follows: so far not
+ write_byte(0); // Next object ID: so far none
+ write_byte(0); // Number of objects
+
+ for (id = range_min; id <= range_max; id++) {
+ if (modbus_id_strings[id]) {
+ byte len = strlen(modbus_id_strings[id]);
+ byte remains = MODBUS_TX_BUFSIZE - 4 - tx_size; // 2 for CRC, 2 for object header
+ if (len > remains) {
+ // If it is the only object, cut it
+ if (!tx_buf[more_follows_at + 2])
+ len = remains;
+ else {
+ // More follows, report the next ID
+ tx_buf[more_follows_at] = 0xff;
+ tx_buf[more_follows_at + 1] = id;
+ break;
+ }
+ }
+ tx_buf[more_follows_at + 2] ++;
+ write_byte(id);
+ write_byte(len);
+ memcpy(tx_buf + tx_size, modbus_id_strings[id], len);
+ tx_size += len;
+ }
+ }
+}
+