From a72fefaedcf8c5d1faf25a2babb95a8913e91084 Mon Sep 17 00:00:00 2001
From: Martin Mares <mj@ucw.cz>
Date: Sun, 14 Jul 2019 20:49:06 +0200
Subject: [PATCH] MODBUS: Debugging and hooks

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

diff --git a/lib/modbus.c b/lib/modbus.c
index c633646..41757d3 100644
--- a/lib/modbus.c
+++ b/lib/modbus.c
@@ -66,6 +66,14 @@
 #endif
 #endif
 
+// Debugging
+// #define MODBUS_DEBUG
+
+#ifdef MODBUS_DEBUG
+#define DEBUG debug_printf
+#else
+#define DEBUG(xxx, ...) do { } while (0)
+#endif
 /*** State ***/
 
 enum mb_state {
@@ -101,6 +109,7 @@ static void rx_init(void)
 	rx_bad = 0;
 	usart_set_mode(MODBUS_USART, USART_MODE_RX);
 	usart_enable_rx_interrupt(MODBUS_USART);
+	modbus_ready_hook();
 }
 
 static void rx_done(void)
@@ -127,6 +136,8 @@ static void tx_done(void)
 
 void modbus_init(void)
 {
+	DEBUG("MODBUS: Init\n");
+
 	timer_set_prescaler(MODBUS_TIMER, CPU_CLOCK_MHZ-1);	// 1 tick = 1 μs
 	timer_set_mode(MODBUS_TIMER, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_DOWN);
 	timer_update_on_overflow(MODBUS_TIMER);
@@ -159,6 +170,8 @@ void MODBUS_USART_ISR(void)
 			if (status & (USART_SR_FE | USART_SR_ORE | USART_SR_NE)) {
 				rx_bad = 1;
 			} else if (rx_size < MODBUS_RX_BUFSIZE) {
+				if (!rx_size)
+					modbus_frame_start_hook();
 				rx_buf[rx_size++] = ch;
 			} else {
 				// Frame too long
@@ -212,9 +225,12 @@ void modbus_loop(void)
 		return;
 	}
 
+	DEBUG("MODBUS: < dest=%02x func=%02x len=%u\n", rx_buf[0], rx_buf[1], rx_size);
+
 	if (rx_buf[0] == MODBUS_OUR_ADDRESS) {
 		// Frame addressed to us: process and reply
 		process_frame();
+		DEBUG("MODBUS: > status=%02x len=%u\n", tx_buf[1], tx_size);
 		tx_init();
 	} else if (rx_buf[0] == 0x00) {
 		// Broadcast frame: process, but do not reply
@@ -305,11 +321,13 @@ static bool check_frame(void)
 {
 	if (rx_bad) {
 		// FIXME: Error counters?
+		DEBUG("MODBUS: RX bad\n");
 		return false;
 	}
 	
 	if (rx_size < 4) {
 		// FIXME: Error counters?
+		DEBUG("MODBUS: RX undersize\n");
 		return false;
 	}
 
@@ -317,6 +335,7 @@ static bool check_frame(void)
 	u16 rx_crc = (rx_buf[rx_size-2] << 8) | rx_buf[rx_size-1];
 	if (crc != rx_crc) {
 		// FIXME: Error counters?
+		DEBUG("MODBUS: Bad CRC\n");
 		return false;
 	}
 
diff --git a/lib/modbus.h b/lib/modbus.h
index 913a149..cb926e3 100644
--- a/lib/modbus.h
+++ b/lib/modbus.h
@@ -26,6 +26,9 @@ bool modbus_check_holding_register(u16 addr);
 u16 modbus_get_holding_register(u16 addr);
 void modbus_set_holding_register(u16 addr, u16 value);
 
+void modbus_ready_hook(void);
+void modbus_frame_start_hook(void);
+
 enum modbus_id_object {
 	MODBUS_ID_VENDOR_NAME,		// first three must be always defined
 	MODBUS_ID_PRODUCT_CODE,
-- 
2.39.2