Modbus library: Fix responses to write commands

diff --git a/lib/modbus.c b/lib/modbus.c
index 41757d310e66af3e2d56db061213fa3d4587caa9..cd1d34eed28a5abd29f3c24f97f412159c4d4c70 100644 (file)
--- a/lib/modbus.c
+++ b/lib/modbus.c
@@ -478,6 +478,9 @@ static void func_write_single_coil(void)
                return report_error(ERR_ILLEGAL_DATA_VALUE);
 
        modbus_set_coil(addr, value);
+
+       write_u16(addr);
+       write_u16(value);
 }
 
 static void func_write_single_register(void)
@@ -492,6 +495,9 @@ static void func_write_single_register(void)
                return report_error(ERR_ILLEGAL_DATA_ADDRESS);
 
        modbus_set_holding_register(addr, value);
+
+       write_u16(addr);
+       write_u16(value);
 }
 
 static void func_write_multiple_coils(void)
@@ -512,6 +518,9 @@ static void func_write_multiple_coils(void)
 
        for (u16 i = 0; i < count; i++)
                modbus_set_coil(start + i, rx_frame[i/8] & (1U << (i%8)));
+
+       write_u16(start);
+       write_u16(count);
 }
 
 static void func_write_multiple_registers(void)
@@ -532,6 +541,9 @@ static void func_write_multiple_registers(void)
 
        for (u16 i = 0; i < count; i++)
                modbus_set_holding_register(start + i, read_u16());
+
+       write_u16(start);
+       write_u16(count);
 }
 
 static void func_mask_write_register(void)
@@ -549,6 +561,10 @@ static void func_mask_write_register(void)
        u16 reg = modbus_get_holding_register(addr);
        reg = (reg & and_mask) | (or_mask & ~and_mask);
        modbus_set_holding_register(addr, reg);
+
+       write_u16(addr);
+       write_u16(and_mask);
+       write_u16(or_mask);
 }
 
 static void func_read_write_multiple_registers(void)