From: Martin Mares <mj@ucw.cz>
Date: Mon, 8 Jul 2019 10:42:25 +0000 (+0200)
Subject: ModBus test
X-Git-Url: http://mj.ucw.cz/gitweb/?a=commitdiff_plain;h=f8c650828fe1cedb50bc66b636ab76756ec67837;p=home-hw.git

ModBus test
---

diff --git a/test-modbus/Makefile b/test-modbus/Makefile
new file mode 100644
index 0000000..ecc5aec
--- /dev/null
+++ b/test-modbus/Makefile
@@ -0,0 +1,81 @@
+BINARY=test
+OBJS=test.o util-debug.o modbus.o
+
+OPENCM3_DIR=/home/mj/stm/libopencm3
+DEVICE=stm32f103x8
+
+all: $(BINARY).elf $(BINARY).bin
+
+flash: all
+	../bin/st-flash write $(BINARY).bin 0x8000000
+
+reset: all
+	../bin/st-flash reset
+
+ifneq ($(V),1)
+Q		:= @
+NULL		:= 2>/dev/null
+endif
+
+include $(OPENCM3_DIR)/mk/genlink-config.mk
+
+PREFIX		?= arm-none-eabi
+
+CC		:= $(PREFIX)-gcc
+CXX		:= $(PREFIX)-g++
+LD		:= $(PREFIX)-gcc
+AR		:= $(PREFIX)-ar
+AS		:= $(PREFIX)-as
+OBJCOPY		:= $(PREFIX)-objcopy
+OBJDUMP		:= $(PREFIX)-objdump
+GDB		:= $(PREFIX)-gdb
+OPT		:= -Os
+DEBUG		:= -ggdb3
+CSTD		?= -std=c99
+
+TGT_CFLAGS	+= $(OPT) $(CSTD) $(DEBUG)
+TGT_CFLAGS	+= $(ARCH_FLAGS)
+TGT_CFLAGS	+= -Wall -Wextra -Wshadow -Wimplicit-function-declaration
+TGT_CFLAGS	+= -Wredundant-decls -Wmissing-prototypes -Wstrict-prototypes
+TGT_CFLAGS	+= -fno-common -ffunction-sections -fdata-sections
+
+TGT_CPPFLAGS	+= -MD
+
+TGT_LDFLAGS	+= --static -nostartfiles
+TGT_LDFLAGS	+= -T$(LDSCRIPT)
+TGT_LDFLAGS	+= $(ARCH_FLAGS) $(DEBUG)
+TGT_LDFLAGS	+= -Wl,-Map=$(*).map -Wl,--cref
+TGT_LDFLAGS	+= -Wl,--gc-sections
+ifeq ($(V),99)
+TGT_LDFLAGS	+= -Wl,--print-gc-sections
+endif
+
+LDLIBS		+= -Wl,--start-group -lc -lgcc -lnosys -Wl,--end-group
+
+include $(OPENCM3_DIR)/mk/genlink-rules.mk
+
+%.bin: %.elf
+	@printf "  OBJCOPY $(*).bin\n"
+	$(Q)$(OBJCOPY) -Obinary $(*).elf $(*).bin
+
+%.elf: $(OBJS) $(LDSCRIPT)
+	@printf "  LD      $(*).elf\n"
+	$(Q)$(LD) $(TGT_LDFLAGS) $(LDFLAGS) $(OBJS) $(LDLIBS) -o $*.elf
+
+%.o: %.c
+	@printf "  CC      $(*).c\n"
+	$(Q)$(CC) $(TGT_CFLAGS) $(CFLAGS) $(TGT_CPPFLAGS) $(CPPFLAGS) -o $*.o -c $*.c
+
+.PHONY: clean
+clean:
+	@printf "  CLEAN\n"
+	$(Q)rm -f *.elf *.bin *.o *.d *.map $(LDSCRIPT)
+
+%.stlink-flash: %.bin
+	@printf "  FLASH  $<\n"
+	$(STFLASH) write $(*).bin 0x8000000
+
+.SECONDEXPANSION:
+.SECONDARY:
+
+-include $(OBJS:.o=.d)
diff --git a/test-modbus/client/Makefile b/test-modbus/client/Makefile
new file mode 100644
index 0000000..4397ff0
--- /dev/null
+++ b/test-modbus/client/Makefile
@@ -0,0 +1,10 @@
+PC := pkg-config
+MODBUS_CFLAGS := $(shell $(PC) --cflags libmodbus)
+MODBUS_LDFLAGS := $(shell $(PC) --libs libmodbus)
+UCW_CFLAGS := $(shell $(PC) --cflags libucw)
+UCW_LDFLAGS := $(shell $(PC) --libs libucw)
+
+CFLAGS=-std=gnu99 -Wall -Wextra -Wno-parentheses $(MODBUS_CFLAGS) $(UCW_CFLAGS)
+LDFLAGS=$(MODBUS_LDFLAGS) $(UCW_LDFLAGS)
+
+all: try
diff --git a/test-modbus/client/try.c b/test-modbus/client/try.c
new file mode 100644
index 0000000..e77adc7
--- /dev/null
+++ b/test-modbus/client/try.c
@@ -0,0 +1,30 @@
+#include <ucw/lib.h>
+
+#include <errno.h>
+#include <stdio.h>
+
+#include <modbus/modbus.h>
+
+int main(void)
+{
+	modbus_t *mb = modbus_new_rtu("/dev/ttyUSB0", 19200, 'N', 8, 1);
+	if (!mb)
+		die("Cannot open modbus");
+
+	int rc = modbus_connect(mb);
+	if (rc < 0)
+		die("Cannot connect: %d", rc);
+
+	if (modbus_set_slave(mb, 42) < 0)
+		die("Cannot set slave address");
+
+	u16 dest;
+	rc = modbus_read_registers(mb, 0, 1, &dest);
+	if (rc < 0)
+		die("Cannot read: %s", modbus_strerror(errno));
+
+	printf("%04x\n", dest);
+
+	modbus_close(mb);
+	modbus_free(mb);
+}
diff --git a/test-modbus/kerm b/test-modbus/kerm
new file mode 100644
index 0000000..e2aaa97
--- /dev/null
+++ b/test-modbus/kerm
@@ -0,0 +1,6 @@
+set port /dev/ttyUSB0
+set speed 19200
+set flow-control none
+set carrier-watch off
+set parity even
+connect
diff --git a/test-modbus/modbus.c b/test-modbus/modbus.c
new file mode 100644
index 0000000..da788a2
--- /dev/null
+++ b/test-modbus/modbus.c
@@ -0,0 +1,334 @@
+#include "util.h"
+#include "modbus.h"
+
+#include <libopencm3/cm3/nvic.h>
+#include <libopencm3/stm32/gpio.h>
+#include <libopencm3/stm32/usart.h>
+#include <libopencm3/stm32/timer.h>
+
+enum mb_state {
+	STATE_RX,
+	STATE_RX_DONE,
+	STATE_PROCESSING,
+	STATE_TX,
+	STATE_TX_LAST,
+	STATE_TX_DONE,
+};
+
+#define RX_BUFSIZE 256
+#define TX_BUFSIZE 256
+
+static byte rx_buf[RX_BUFSIZE];
+static u16 rx_size;
+static byte rx_bad;
+static byte state;		// STATE_xxx
+
+static byte tx_buf[TX_BUFSIZE];
+static u16 tx_size;
+static u16 tx_pos;
+
+#define MB_OUR_ADDRESS 42
+
+static void UNUSED xx_write_char(uint c)	// FIXME
+{
+	usart_set_mode(USART2, USART_MODE_TX);
+	gpio_set(GPIOA, GPIO1);
+	usart_send_blocking(USART2, c);
+	while (!usart_get_flag(USART2, USART_SR_TC))
+		;
+	gpio_clear(GPIOA, GPIO1);
+	usart_set_mode(USART2, USART_MODE_RX);
+}
+
+static void rx_init(void)
+{
+	state = STATE_RX;
+	rx_size = 0;
+	rx_bad = 0;
+	usart_set_mode(USART2, USART_MODE_RX);
+	usart_enable_rx_interrupt(USART2);
+}
+
+static void rx_done(void)
+{
+	state = STATE_RX_DONE;
+	usart_disable_rx_interrupt(USART2);
+}
+
+static void tx_init(void)
+{
+	state = STATE_TX;
+	tx_pos = 0;
+	gpio_set(GPIOA, GPIO1);
+	usart_set_mode(USART2, USART_MODE_TX);
+	usart_enable_tx_interrupt(USART2);
+}
+
+static void tx_done(void)
+{
+	state = STATE_TX_DONE;
+	// usart_disable_tx_interrupt(USART2);		// Already done by irq handler
+	gpio_clear(GPIOA, GPIO1);
+}
+
+void modbus_init(void)
+{
+	timer_set_prescaler(TIM2, 71);		// 1 tick = 1 μs
+	timer_set_mode(TIM2, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_DOWN);
+	timer_update_on_overflow(TIM2);
+	timer_disable_preload(TIM2);
+	timer_one_shot_mode(TIM2);
+	timer_enable_irq(TIM2, TIM_DIER_UIE);
+	nvic_enable_irq(NVIC_TIM2_IRQ);
+
+	gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO_USART2_RX);
+	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART2_TX);
+
+	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO1);
+	gpio_clear(GPIOA, GPIO1);
+
+	usart_set_baudrate(USART2, 19200);
+	usart_set_databits(USART2, 8);
+	usart_set_stopbits(USART2, USART_STOPBITS_1);
+	usart_set_parity(USART2, USART_PARITY_NONE);
+	// usart_set_parity(USART2, USART_PARITY_EVEN);	// FIXME
+	usart_set_flow_control(USART2, USART_FLOWCONTROL_NONE);
+
+	rx_init();
+
+	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)
+{
+	u32 status = USART_SR(USART2);
+
+	// FIXME: Optimize
+
+	if (status & USART_SR_RXNE) {
+		uint ch = usart_recv(USART2);
+		if (state == STATE_RX) {
+#if 0
+			if (status & (USART_SR_FE | USART_SR_ORE | USART_SR_NE)) {
+				rx_bad = 1;
+			} else
+#endif
+			if (rx_size < RX_BUFSIZE) {
+				rx_buf[rx_size++] = ch;
+			} else {
+				// Frame too long
+				rx_bad = 2;
+			}
+			timer_set_period(TIM2, 7500);	// 0.75 ms timeout for end of frame (FIXME: right value?)
+			timer_generate_event(TIM2, TIM_EGR_UG);
+			timer_enable_counter(TIM2);
+		}
+	}
+
+	if (state == STATE_TX) {
+		if (status & USART_SR_TXE) {
+			if (tx_pos < tx_size) {
+				usart_send(USART2, tx_buf[tx_pos++]);
+			} else {
+				// The transmitter is double-buffered, so at this moment, it is transmitting
+				// the last byte of the frame. Wait until transfer is completed.
+				usart_disable_tx_interrupt(USART2);
+				USART_CR1(USART2) |= USART_CR1_TCIE;
+				state = STATE_TX_LAST;
+			}
+		}
+	}
+
+	if (state == STATE_TX_LAST) {
+		if (status & USART_SR_TC) {
+			// Transfer of the last byte is complete. Release the bus.
+			USART_CR1(USART2) &= ~USART_CR1_TCIE;
+			tx_done();
+			rx_init();
+		}
+	}
+}
+
+void tim2_isr(void)
+{
+	if (TIM_SR(TIM2) & TIM_SR_UIF) {
+		TIM_SR(TIM2) &= ~TIM_SR_UIF;
+		if (state == STATE_RX)
+			rx_done();
+	}
+}
+
+// CRC tables
+
+static const byte crc_hi[] = {
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
+    0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
+    0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
+    0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41,
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
+    0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
+    0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
+    0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
+    0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
+    0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40,
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
+    0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
+    0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40, 0x00, 0xc1, 0x81, 0x40,
+    0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0, 0x80, 0x41, 0x00, 0xc1,
+    0x81, 0x40, 0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41,
+    0x00, 0xc1, 0x81, 0x40, 0x01, 0xc0, 0x80, 0x41, 0x01, 0xc0,
+    0x80, 0x41, 0x00, 0xc1, 0x81, 0x40
+};
+
+static const byte crc_lo[] = {
+    0x00, 0xc0, 0xc1, 0x01, 0xc3, 0x03, 0x02, 0xc2, 0xc6, 0x06,
+    0x07, 0xc7, 0x05, 0xc5, 0xc4, 0x04, 0xcc, 0x0c, 0x0d, 0xcd,
+    0x0f, 0xcf, 0xce, 0x0e, 0x0a, 0xca, 0xcb, 0x0b, 0xc9, 0x09,
+    0x08, 0xc8, 0xd8, 0x18, 0x19, 0xd9, 0x1b, 0xdb, 0xda, 0x1a,
+    0x1e, 0xde, 0xdf, 0x1f, 0xdd, 0x1d, 0x1c, 0xdc, 0x14, 0xd4,
+    0xd5, 0x15, 0xd7, 0x17, 0x16, 0xd6, 0xd2, 0x12, 0x13, 0xd3,
+    0x11, 0xd1, 0xd0, 0x10, 0xf0, 0x30, 0x31, 0xf1, 0x33, 0xf3,
+    0xf2, 0x32, 0x36, 0xf6, 0xf7, 0x37, 0xf5, 0x35, 0x34, 0xf4,
+    0x3c, 0xfc, 0xfd, 0x3d, 0xff, 0x3f, 0x3e, 0xfe, 0xfa, 0x3a,
+    0x3b, 0xfb, 0x39, 0xf9, 0xf8, 0x38, 0x28, 0xe8, 0xe9, 0x29,
+    0xeb, 0x2b, 0x2a, 0xea, 0xee, 0x2e, 0x2f, 0xef, 0x2d, 0xed,
+    0xec, 0x2c, 0xe4, 0x24, 0x25, 0xe5, 0x27, 0xe7, 0xe6, 0x26,
+    0x22, 0xe2, 0xe3, 0x23, 0xe1, 0x21, 0x20, 0xe0, 0xa0, 0x60,
+    0x61, 0xa1, 0x63, 0xa3, 0xa2, 0x62, 0x66, 0xa6, 0xa7, 0x67,
+    0xa5, 0x65, 0x64, 0xa4, 0x6c, 0xac, 0xad, 0x6d, 0xaf, 0x6f,
+    0x6e, 0xae, 0xaa, 0x6a, 0x6b, 0xab, 0x69, 0xa9, 0xa8, 0x68,
+    0x78, 0xb8, 0xb9, 0x79, 0xbb, 0x7b, 0x7a, 0xba, 0xbe, 0x7e,
+    0x7f, 0xbf, 0x7d, 0xbd, 0xbc, 0x7c, 0xb4, 0x74, 0x75, 0xb5,
+    0x77, 0xb7, 0xb6, 0x76, 0x72, 0xb2, 0xb3, 0x73, 0xb1, 0x71,
+    0x70, 0xb0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
+    0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9c, 0x5c,
+    0x5d, 0x9d, 0x5f, 0x9f, 0x9e, 0x5e, 0x5a, 0x9a, 0x9b, 0x5b,
+    0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4b, 0x8b,
+    0x8a, 0x4a, 0x4e, 0x8e, 0x8f, 0x4f, 0x8d, 0x4d, 0x4c, 0x8c,
+    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
+    0x43, 0x83, 0x41, 0x81, 0x80, 0x40
+};
+
+static u16 crc16(byte *buf, u16 len)
+{
+	byte hi = 0xff, lo = 0xff;
+
+	while (len--) {
+		byte i = hi ^ *buf++;
+		hi = lo ^ crc_hi[i];
+		lo = crc_lo[i];
+	}
+
+	return (hi << 8 | lo);
+}
+
+static bool check_frame(void)
+{
+	if (rx_bad) {
+		// FIXME: Error counters?
+		return false;
+	}
+	
+	if (rx_size < 4) {
+		// FIXME: Error counters?
+		return false;
+	}
+
+	u16 crc = crc16(rx_buf, rx_size - 2);
+	u16 rx_crc = (rx_buf[rx_size-2] << 8) | rx_buf[rx_size-1];
+	if (crc != rx_crc) {
+		// FIXME: Error counters?
+		return false;
+	}
+
+	return true;
+}
+
+enum mb_function {
+	FUNC_READ_HOLDING_REGISTERS = 0x03,
+};
+
+enum mb_error {
+	ERR_ILLEGAL_FUNCTION = 0x01,
+	ERR_ILLEGAL_DATA_ADDRESS = 0x02,
+	ERR_ILLEGAL_DATA_VALUE = 0x03,
+};
+
+static void report_error(byte code)
+{
+	// Discard the partially constructed body of the reply and rewrite the header
+	tx_buf[1] |= 0x80;
+	tx_buf[2] = code;
+	tx_size = 3;
+}
+
+static void process_frame(void)
+{
+	byte func = rx_buf[1];
+
+	// Prepare reply frame
+	tx_buf[0] = MB_OUR_ADDRESS;
+	tx_buf[1] = rx_buf[1];
+	tx_size = 2;
+
+	switch (func) {
+		case FUNC_READ_HOLDING_REGISTERS:
+			tx_buf[tx_size++] = 2;
+			tx_buf[tx_size++] = 0x12;
+			tx_buf[tx_size++] = 0x34;
+			break;
+		default:
+			report_error(ERR_ILLEGAL_FUNCTION);
+	}
+
+	// Finish reply frame
+	u16 crc = crc16(tx_buf, tx_size);
+	tx_buf[tx_size++] = crc >> 8;
+	tx_buf[tx_size++] = crc;
+}
+
+void modbus_loop(void)
+{
+	if (state != STATE_RX_DONE) {
+		// gpio_toggle(GPIOC, GPIO13);
+		return;
+	}
+	state = STATE_PROCESSING;
+	gpio_toggle(GPIOC, GPIO13);
+
+	if (!check_frame()) {
+		rx_init();
+		return;
+	}
+
+	if (rx_buf[0] == MB_OUR_ADDRESS) {
+		// Frame addressed to us: process and reply
+		process_frame();
+		tx_init();
+	} else if (rx_buf[0] == 0x00) {
+		// Broadcast frame: process, but do not reply
+		process_frame();
+		rx_init();
+	} else {
+		// Somebody else's frame: discard
+		rx_init();
+	}
+}
diff --git a/test-modbus/modbus.h b/test-modbus/modbus.h
new file mode 100644
index 0000000..1c2fe62
--- /dev/null
+++ b/test-modbus/modbus.h
@@ -0,0 +1,2 @@
+void modbus_init(void);
+void modbus_loop(void);
diff --git a/test-modbus/test.c b/test-modbus/test.c
new file mode 100644
index 0000000..4dc1f41
--- /dev/null
+++ b/test-modbus/test.c
@@ -0,0 +1,111 @@
+#include "util.h"
+#include "modbus.h"
+
+#include <libopencm3/cm3/cortex.h>
+#include <libopencm3/cm3/nvic.h>
+#include <libopencm3/cm3/systick.h>
+#include <libopencm3/stm32/rcc.h>
+#include <libopencm3/stm32/gpio.h>
+#include <libopencm3/stm32/usart.h>
+#include <libopencm3/stm32/timer.h>
+
+static void clock_setup(void)
+{
+	rcc_clock_setup_in_hse_8mhz_out_72mhz();
+
+	rcc_periph_clock_enable(RCC_GPIOA);
+	rcc_periph_clock_enable(RCC_GPIOB);
+	rcc_periph_clock_enable(RCC_GPIOC);
+	rcc_periph_clock_enable(RCC_USART2);
+	rcc_periph_clock_enable(RCC_TIM2);
+	rcc_periph_clock_enable(RCC_TIM4);
+
+	rcc_periph_reset_pulse(RST_GPIOA);
+	rcc_periph_reset_pulse(RST_GPIOB);
+	rcc_periph_reset_pulse(RST_GPIOC);
+	rcc_periph_reset_pulse(RST_USART2);
+	rcc_periph_reset_pulse(RST_TIM2);
+	rcc_periph_reset_pulse(RST_TIM4);
+}
+
+static void gpio_setup(void)
+{
+	// PC13 = BluePill LED
+	gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO13);
+	gpio_clear(GPIOC, GPIO13);
+}
+
+static volatile u32 ms_ticks;
+
+void sys_tick_handler(void)
+{
+	ms_ticks++;
+}
+
+static void tick_setup(void)
+{
+	systick_set_frequency(1000, 72000000);
+	systick_counter_enable();
+	systick_interrupt_enable();
+}
+
+static void delay_ms(uint ms)
+{
+	u32 start_ticks = ms_ticks;
+	while (ms_ticks - start_ticks < ms)
+		;
+}
+
+static void usart_setup(void)
+{
+#if 0
+	gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO_USART2_TX);
+
+	usart_set_baudrate(USART2, 9600);
+	usart_set_databits(USART2, 8);
+	usart_set_stopbits(USART2, USART_STOPBITS_1);
+	usart_set_mode(USART2, USART_MODE_TX_RX);
+	usart_set_parity(USART2, USART_PARITY_NONE);
+	usart_set_flow_control(USART2, USART_FLOWCONTROL_NONE);
+
+	usart_enable(USART2);
+#endif
+}
+
+int main(void)
+{
+	clock_setup();
+	gpio_setup();
+	tick_setup();
+	usart_setup();
+
+	modbus_init();
+	cm_enable_interrupts();		// FIXME: Needed?
+
+#if 0
+	timer_set_prescaler(TIM4, 7);	// clock = 72 MHz / 8 = 18 MHz	FIXME!
+	timer_set_mode(TIM4, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP);
+	timer_disable_preload(TIM4);
+	timer_set_period(TIM4, 255);	// PWM frequency = 18 MHz / 256 = 70.3125 kHz
+	timer_set_oc_mode(TIM4, TIM_OC1, TIM_OCM_PWM1);
+	timer_set_oc_value(TIM4, TIM_OC1, 128);
+	timer_set_oc_polarity_high(TIM4, TIM_OC1);
+	timer_enable_counter(TIM4);
+	timer_enable_oc_output(TIM4, TIM_OC1);
+#endif
+
+	gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO6);
+	// gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO8);
+
+	for (;;) {
+		//gpio_toggle(GPIOC, GPIO13);
+		delay_ms(50);
+		// gpio_toggle(GPIOA, GPIO8);
+		//timer_set_oc_mode(TIM4, TIM_OC1, TIM_OCM_FORCE_LOW);
+		//delay_ms(50);
+		//timer_set_oc_mode(TIM4, TIM_OC1, TIM_OCM_FORCE_HIGH);
+		modbus_loop();
+	}
+
+	return 0;
+}
diff --git a/test-modbus/util-debug.c b/test-modbus/util-debug.c
new file mode 100644
index 0000000..0367182
--- /dev/null
+++ b/test-modbus/util-debug.c
@@ -0,0 +1,166 @@
+#include "util.h"
+
+#include <libopencm3/stm32/usart.h>
+
+#include <stdarg.h>
+#include <string.h>
+
+#ifdef DEBUG_SEMIHOSTING
+
+void semi_put_char(char c)
+{
+	// This is tricky, we need to work around GCC bugs
+	volatile char cc = c;
+	asm volatile (
+		"mov r0, #0x03\n"   /* SYS_WRITEC */
+		"mov r1, %[msg]\n"
+		"bkpt #0xAB\n"
+		:
+		: [msg] "r" (&cc)
+		: "r0", "r1"
+	);
+}
+
+void semi_write_string(char *c)
+{
+	asm volatile (
+		"mov r0, #0x04\n"   /* SYS_WRITE0 */
+		"mov r1, %[msg]\n"
+		"bkpt #0xAB\n"
+		:
+		: [msg] "r" (c)
+		: "r0", "r1"
+	);
+}
+
+#endif
+
+void debug_putc(int c)
+{
+#ifdef DEBUG_SEMIHOSTING
+	static char debug_buf[128];
+	static int debug_i;
+	debug_buf[debug_i++] = c;
+	if (c == '\n' || debug_i >= sizeof(debug_buf) - 1) {
+		debug_buf[debug_i] = 0;
+		semi_write_string(debug_buf);
+		debug_i = 0;
+	}
+#endif
+#ifdef DEBUG_USART
+	if (c == '\n')
+		usart_send_blocking(USART2, '\r');
+	usart_send_blocking(USART2, c);
+#endif
+}
+
+void debug_puts(const char *s)
+{
+	while (*s)
+		debug_putc(*s++);
+}
+
+enum printf_flags {
+	PF_ZERO_PAD = 1,
+	PF_SIGNED = 2,
+	PF_NEGATIVE = 4,
+	PF_UPPERCASE = 8,
+	PF_LEFT = 16,
+};
+
+static void printf_string(const char *s, uint width, uint flags)
+{
+	uint len = strlen(s);
+	uint pad = (len < width) ? width - len : 0;
+	char pad_char = (flags & PF_ZERO_PAD) ? '0' : ' ';
+
+	if (flags & PF_LEFT)
+		debug_puts(s);
+	while (pad--)
+		debug_putc(pad_char);
+	if (!(flags & PF_LEFT))
+		debug_puts(s);
+}
+
+static void printf_number(uint i, uint width, uint flags, uint base)
+{
+	char buf[16];
+	char *w = buf + sizeof(buf);
+
+	if (flags & PF_SIGNED) {
+		if ((int) i < 0) {
+			i = - (int) i;
+			flags |= PF_NEGATIVE;
+		}
+	}
+
+	*--w = 0;
+	do {
+		uint digit = i % base;
+		if (digit < 10)
+			*--w = '0' + digit;
+		else
+			*--w = ((flags & PF_UPPERCASE) ? 'A' : 'a') + digit - 10;
+		i /= base;
+	}
+	while (i);
+
+	if (flags & PF_NEGATIVE)
+		*--w = '-';
+
+	printf_string(w, width, flags);
+}
+
+void debug_printf(const char *fmt, ...)
+{
+	va_list args;
+	va_start(args, fmt);
+
+	while (*fmt) {
+		int c = *fmt++;
+		if (c != '%') {
+			debug_putc(c);
+			continue;
+		}
+
+		uint width = 0;
+		uint flags = 0;
+
+		if (*fmt == '-') {
+			fmt++;
+			flags |= PF_LEFT;
+		}
+
+		if (*fmt == '0') {
+			fmt++;
+			flags |= PF_ZERO_PAD;
+		}
+
+		while (*fmt >= '0' && *fmt <= '9')
+			width = 10*width + *fmt++ - '0';
+
+		c = *fmt++;
+		switch (c) {
+			case 'd':
+				printf_number(va_arg(args, int), width, flags | PF_SIGNED, 10);
+				break;
+			case 'u':
+				printf_number(va_arg(args, int), width, flags, 10);
+				break;
+			case 'X':
+				flags |= PF_UPPERCASE;
+				// fall-thru
+			case 'x':
+				printf_number(va_arg(args, int), width, flags, 16);
+				break;
+			case 's':
+				printf_string(va_arg(args, char *), width, flags);
+				break;
+			default:
+				debug_putc(c);
+				continue;
+		}
+	}
+
+	va_end(args);
+}
diff --git a/test-modbus/util.h b/test-modbus/util.h
new file mode 100644
index 0000000..65eb435
--- /dev/null
+++ b/test-modbus/util.h
@@ -0,0 +1,71 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef unsigned int uint;
+typedef uint8_t byte;
+typedef uint16_t u16;
+typedef int16_t s16;
+typedef uint32_t u32;
+typedef int32_t s32;
+
+#define MIN(x,y) ((x) < (y) ? (x) : (y))
+#define MAX(x,y) ((x) > (y) ? (x) : (y))
+
+#define UNUSED __attribute__((unused))
+
+static inline uint get_u16_le(byte *p)
+{
+	return (p[1] << 8) | p[0];
+}
+
+static inline uint get_u16_be(byte *p)
+{
+	return (p[0] << 8) | p[1];
+}
+
+static inline uint get_u32_le(byte *p)
+{
+	return (p[3] << 24) | (p[2] << 16) | (p[1] << 8) | p[0];
+}
+
+static inline uint get_u32_be(byte *p)
+{
+	return (p[0] << 24) | (p[1] << 16) | (p[2] << 8) | p[3];
+}
+
+static inline void put_u16_le(byte *p, u16 x)
+{
+	p[0] = x;
+	p[1] = x >> 8;
+}
+
+static inline void put_u16_be(byte *p, u16 x)
+{
+	p[0] = x >> 8;
+	p[1] = x;
+}
+
+static inline void put_u32_be(byte *p, u32 x)
+{
+	p[0] = x >> 24;
+	p[1] = (x >> 16) & 0xff;
+	p[2] = (x >> 8) & 0xff;
+	p[3] = x & 0xff;
+}
+
+static inline void put_u32_le(byte *p, u32 x)
+{
+	p[3] = x >> 24;
+	p[2] = (x >> 16) & 0xff;
+	p[1] = (x >> 8) & 0xff;
+	p[0] = x & 0xff;
+}
+
+// debug.c
+
+// #define DEBUG_SEMIHOSTING
+#define DEBUG_USART USART1
+
+void debug_printf(const char *fmt, ...);
+void debug_puts(const char *s);
+void debug_putc(int c);