From 3b2290c8a5ff066390768ffd8d5db05e8d84346f Mon Sep 17 00:00:00 2001
From: Martin Mares <mj@ucw.cz>
Date: Mon, 8 Jul 2019 18:24:53 +0200
Subject: [PATCH] ModBus: Identification

---
 test-modbus/modbus.c | 104 ++++++++++++++++++++++++++++++++++++++-----
 test-modbus/modbus.h |  12 +++++
 2 files changed, 104 insertions(+), 12 deletions(-)

diff --git a/test-modbus/modbus.c b/test-modbus/modbus.c
index 24a2589..285c152 100644
--- a/test-modbus/modbus.c
+++ b/test-modbus/modbus.c
@@ -1,6 +1,9 @@
 #include "util.h"
 #include "modbus.h"
 
+#include <stddef.h>
+#include <string.h>
+
 #include <libopencm3/cm3/nvic.h>
 #include <libopencm3/stm32/gpio.h>
 #include <libopencm3/stm32/usart.h>
@@ -101,14 +104,6 @@ void modbus_init(void)
 
 	nvic_enable_irq(NVIC_USART2_IRQ);
 	usart_enable(USART2);
-
-#if 0
-	u32 xxx = USART_CR1(USART2);
-	usart_set_mode(USART2, USART_MODE_TX);
-	gpio_set(GPIOA, GPIO1);
-	debug_printf("%08x\n", xxx);
-	xx_write_char('\n');
-#endif
 }
 
 void usart2_isr(void)
@@ -508,6 +503,82 @@ static void func_read_write_multiple_registers(void)
 		modbus_get_holding_register(read_start + i);
 }
 
+static void func_encapsulated_interface_transport(void)
+{
+	if (read_remains() < 3 ||
+	    read_byte() != 0x0e)
+		return report_error(ERR_ILLEGAL_DATA_VALUE);
+
+	byte action = read_byte();
+	byte id = read_byte();
+
+	byte range_min, range_max;
+	switch (action) {
+		case 1:
+			// Streaming access to basic identification
+			range_min = MODBUS_ID_VENDOR_NAME;
+			range_max = MODBUS_ID_MAJOR_MINOR_REVISION;
+			break;
+		case 2:
+			// Streaming access to regular identification
+			range_min = MODBUS_ID_VENDOR_URL;
+			range_max = MODBUS_ID_USER_APP_NAME;
+			break;
+		case 4:
+			// Individual access
+			if (id >= MODBUS_ID_MAX || !modbus_id_strings[id])
+				return report_error(ERR_ILLEGAL_DATA_ADDRESS);
+			range_min = range_max = id;
+			break;
+		default:
+			return report_error(ERR_ILLEGAL_DATA_VALUE);
+	}
+
+	if (action != 4) {
+		if (id < range_min || id > range_max)
+			id = range_min;
+	}
+
+	write_byte(0x0e);	// Repeat a part of the request
+	write_byte(action);
+
+	// Conformity level
+	if (modbus_id_strings[MODBUS_ID_VENDOR_URL] ||
+	    modbus_id_strings[MODBUS_ID_PRODUCT_NAME] ||
+	    modbus_id_strings[MODBUS_ID_USER_APP_NAME])
+		write_byte(0x82);	// Regular identification, both stream and individual access supported
+	else
+		write_byte(0x81);	// Basic identification only
+
+	u16 more_follows_at = tx_size;
+	write_byte(0);		// More follows: so far not
+	write_byte(0);		// Next object ID: so far none
+	write_byte(0);		// Number of objects
+
+	for (id = range_min; id <= range_max; id++) {
+		if (modbus_id_strings[id]) {
+			byte len = strlen(modbus_id_strings[id]);
+			byte remains = TX_BUFSIZE - 4 - tx_size;	// 2 for CRC, 2 for object header
+			if (len > remains) {
+				// If it is the only object, cut it
+				if (!tx_buf[more_follows_at + 2])
+					len = remains;
+				else {
+					// More follows, report the next ID
+					tx_buf[more_follows_at] = 0xff;
+					tx_buf[more_follows_at + 1] = id;
+					break;
+				}
+			}
+			tx_buf[more_follows_at + 2] ++;
+			write_byte(id);
+			write_byte(len);
+			memcpy(tx_buf + tx_size, modbus_id_strings[id], len);
+			tx_size += len;
+		}
+	}
+}
+
 static void process_frame(void)
 {
 	byte func = read_byte();
@@ -548,6 +619,9 @@ static void process_frame(void)
 		case FUNC_READ_WRITE_MULTIPLE_REGISTERS:
 			func_read_write_multiple_registers();
 			break;
+		case FUNC_ENCAPSULATED_INTERFACE_TRANSPORT:
+			func_encapsulated_interface_transport();
+			break;
 		default:
 			report_error(ERR_ILLEGAL_FUNCTION);
 	}
@@ -558,12 +632,9 @@ static void process_frame(void)
 
 void modbus_loop(void)
 {
-	if (state != STATE_RX_DONE) {
-		// gpio_toggle(GPIOC, GPIO13);
+	if (state != STATE_RX_DONE)
 		return;
-	}
 	state = STATE_PROCESSING;
-	gpio_toggle(GPIOC, GPIO13);
 
 	if (!check_frame()) {
 		rx_init();
@@ -633,3 +704,12 @@ u16 modbus_get_holding_register(u16 addr UNUSED)
 void modbus_set_holding_register(u16 addr UNUSED, u16 value UNUSED)
 {
 }
+
+const char * const modbus_id_strings[MODBUS_ID_MAX] = {
+	[MODBUS_ID_VENDOR_NAME] = "United Computer Wizards",
+	[MODBUS_ID_PRODUCT_CODE] = "42",
+	[MODBUS_ID_MAJOR_MINOR_REVISION] = "1.0",
+	[MODBUS_ID_VENDOR_URL] = "http://www.ucw.cz/",
+	[MODBUS_ID_PRODUCT_NAME] = "Magic Gadget",
+	[MODBUS_ID_USER_APP_NAME] = NULL,
+};
diff --git a/test-modbus/modbus.h b/test-modbus/modbus.h
index dd80a66..010fb3c 100644
--- a/test-modbus/modbus.h
+++ b/test-modbus/modbus.h
@@ -1,6 +1,16 @@
 void modbus_init(void);
 void modbus_loop(void);
 
+enum modbus_id_object {
+	MODBUS_ID_VENDOR_NAME,		// first three must be always defined
+	MODBUS_ID_PRODUCT_CODE,
+	MODBUS_ID_MAJOR_MINOR_REVISION,
+	MODBUS_ID_VENDOR_URL,		// the rest may be NULL
+	MODBUS_ID_PRODUCT_NAME,
+	MODBUS_ID_USER_APP_NAME,
+	MODBUS_ID_MAX,
+};
+
 // Callbacks
 
 bool modbus_check_discrete_input(u16 addr);
@@ -16,3 +26,5 @@ u16 modbus_get_input_register(u16 addr);
 bool modbus_check_holding_register(u16 addr);
 u16 modbus_get_holding_register(u16 addr);
 void modbus_set_holding_register(u16 addr, u16 value);
+
+extern const char * const modbus_id_strings[MODBUS_ID_MAX];
-- 
2.39.5