ModBus test

diff --git a/test-modbus/Makefile b/test-modbus/Makefile
new file mode 100644 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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 (file)
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);