X-Git-Url: http://mj.ucw.cz/gitweb/?a=blobdiff_plain;f=lib%2Fmodbus.c;h=28f976df7b8e43f2b619043421c44aa5b5f8d3e7;hb=3038b627489dae62ce54d415a4bcfc4cfd28f084;hp=41757d310e66af3e2d56db061213fa3d4587caa9;hpb=a72fefaedcf8c5d1faf25a2babb95a8913e91084;p=home-hw.git

diff --git a/lib/modbus.c b/lib/modbus.c
index 41757d3..28f976d 100644
--- a/lib/modbus.c
+++ b/lib/modbus.c
@@ -96,6 +96,7 @@ static byte *rx_frame_end;
 static byte tx_buf[MODBUS_TX_BUFSIZE];
 static u16 tx_size;
 static u16 tx_pos;
+static byte pending_error;
 
 static bool check_frame(void);
 static void process_frame(void);
@@ -370,6 +371,7 @@ enum mb_error {
 	ERR_ILLEGAL_FUNCTION = 0x01,
 	ERR_ILLEGAL_DATA_ADDRESS = 0x02,
 	ERR_ILLEGAL_DATA_VALUE = 0x03,
+	ERR_SLAVE_DEVICE_FAILURE = 0x04,
 };
 
 static uint read_remains(void)
@@ -478,6 +480,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 +497,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 +520,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 +543,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 +563,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)
@@ -669,6 +687,7 @@ static void process_frame(void)
 	tx_buf[0] = MODBUS_OUR_ADDRESS;
 	tx_buf[1] = rx_buf[1];
 	tx_size = 2;
+	pending_error = 0;
 
 	switch (func) {
 		case FUNC_READ_COILS:
@@ -708,6 +727,15 @@ static void process_frame(void)
 			report_error(ERR_ILLEGAL_FUNCTION);
 	}
 
+	// Is there a deferred error pending?
+	if (pending_error)
+		report_error(pending_error);
+
 	// Finish reply frame
 	write_u16(crc16(tx_buf, tx_size));
 }
+
+void modbus_slave_error(void)
+{
+	pending_error = ERR_SLAVE_DEVICE_FAILURE;
+}