From 7b8a56d8c1f16f9bf4377ba4597050f7e194538b Mon Sep 17 00:00:00 2001
From: Martin Mares <mj@ucw.cz>
Date: Thu, 18 Jul 2019 01:27:18 +0200
Subject: [PATCH] Modbus: Slave errors

---
 lib/modbus.c | 12 ++++++++++++
 lib/modbus.h |  3 +++
 2 files changed, 15 insertions(+)

diff --git a/lib/modbus.c b/lib/modbus.c
index cd1d34e..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)
@@ -685,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:
@@ -724,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;
+}
diff --git a/lib/modbus.h b/lib/modbus.h
index cb926e3..8fd3751 100644
--- a/lib/modbus.h
+++ b/lib/modbus.h
@@ -10,6 +10,9 @@
 void modbus_init(void);
 void modbus_loop(void);
 
+// If a call-back wants to signal a slave error in the reply
+void modbus_slave_error(void);
+
 // Callbacks
 
 bool modbus_check_discrete_input(u16 addr);
-- 
2.39.2