From f8c650828fe1cedb50bc66b636ab76756ec67837 Mon Sep 17 00:00:00 2001 From: Martin Mares Date: Mon, 8 Jul 2019 12:42:25 +0200 Subject: [PATCH] ModBus test --- test-modbus/Makefile | 81 +++++++++ test-modbus/client/Makefile | 10 ++ test-modbus/client/try.c | 30 ++++ test-modbus/kerm | 6 + test-modbus/modbus.c | 334 ++++++++++++++++++++++++++++++++++++ test-modbus/modbus.h | 2 + test-modbus/test.c | 111 ++++++++++++ test-modbus/util-debug.c | 166 ++++++++++++++++++ test-modbus/util.h | 71 ++++++++ 9 files changed, 811 insertions(+) create mode 100644 test-modbus/Makefile create mode 100644 test-modbus/client/Makefile create mode 100644 test-modbus/client/try.c create mode 100644 test-modbus/kerm create mode 100644 test-modbus/modbus.c create mode 100644 test-modbus/modbus.h create mode 100644 test-modbus/test.c create mode 100644 test-modbus/util-debug.c create mode 100644 test-modbus/util.h 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 + +#include +#include + +#include + +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 +#include +#include +#include + +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 +#include +#include +#include +#include +#include +#include + +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 + +#include +#include + +#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 +#include + +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); -- 2.39.2