X-Git-Url: http://mj.ucw.cz/gitweb/?a=blobdiff_plain;f=lib%2Fmodbus.c;h=28f976df7b8e43f2b619043421c44aa5b5f8d3e7;hb=5286b9d20dd896a010e0d1db9f0813f5c489951e;hp=c6336460d307d1a8fe814e77b3159409ddaeb113;hpb=59f04eaa0a96f06ca4b94474c8a3a6ee677541fa;p=home-hw.git

diff --git a/lib/modbus.c b/lib/modbus.c
index c633646..28f976d 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 {
@@ -88,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);
@@ -101,6 +110,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 +137,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 +171,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 +226,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 +322,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 +336,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;
 	}
 
@@ -351,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)
@@ -459,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)
@@ -473,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)
@@ -493,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)
@@ -513,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)
@@ -530,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)
@@ -650,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:
@@ -689,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;
+}