diff --git a/ucw-stm32lib/README.md b/ucw-stm32lib/README.md new file mode 100644 index 0000000..14589f7 --- /dev/null +++ b/ucw-stm32lib/README.md @@ -0,0 +1,2 @@ +Source: https://github.com/ucw-gadgets/stm32lib +Licence: GPL3 diff --git a/ucw-stm32lib/lib/dfu-bootloader.c b/ucw-stm32lib/lib/dfu-bootloader.c new file mode 100644 index 0000000..4703984 --- /dev/null +++ b/ucw-stm32lib/lib/dfu-bootloader.c @@ -0,0 +1,485 @@ +/* + * Generic DFU Bootloader + * + * (c) 2020 Martin Mareš + * + * Based on example code from the libopencm3 project, which is + * Copyright (C) 2010 Gareth McMullin + * + * Licensed under the GNU LGPL v3 or any later version. + */ + +#include "util.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef BOOTLOADER_DEBUG +#define DEBUG(x...) debug_printf(x) +#else +#define DEBUG(x...) do { } while (0) +#endif + +// Offsets to firmware header fields (see tools/dfu-sign.c) +#define HDR_LENGTH 0x1c +#define HDR_FLASH_IN_PROGRESS 0x20 + +// DFU blocks should be equal to erase blocks of the flash +#define BLOCK_SIZE 1024 +byte usbd_control_buffer[BLOCK_SIZE]; + +static enum dfu_state dfu_state = STATE_DFU_IDLE; +static uint timeout; +#define DEFAULT_TIMEOUT 5000 // ms +#define DFU_TIMEOUT 2000 + +static struct { + byte buf[sizeof(usbd_control_buffer)]; + u16 blocknum; + u16 len; +} prog; + +static char usb_serial_number[13]; + +enum usb_string { + STR_MANUFACTURER = 1, + STR_PRODUCT, + STR_SERIAL, +}; + +static const char *usb_strings[] = { + BOOTLOADER_MFG_NAME, + BOOTLOADER_PROD_NAME, + usb_serial_number, +}; + +const struct usb_device_descriptor dev = { + .bLength = USB_DT_DEVICE_SIZE, + .bDescriptorType = USB_DT_DEVICE, + .bcdUSB = 0x0200, + .bDeviceClass = 0, + .bDeviceSubClass = 0, + .bDeviceProtocol = 0, + .bMaxPacketSize0 = 64, + .idVendor = BOOTLOADER_MFG_ID, + .idProduct = BOOTLOADER_PROD_ID, + .bcdDevice = BOOTLOADER_PROD_VERSION, + .iManufacturer = STR_MANUFACTURER, + .iProduct = STR_PRODUCT, + .iSerialNumber = STR_SERIAL, + .bNumConfigurations = 1, +}; + +const struct usb_dfu_descriptor dfu_function = { + .bLength = sizeof(struct usb_dfu_descriptor), + .bDescriptorType = DFU_FUNCTIONAL, + .bmAttributes = USB_DFU_CAN_DOWNLOAD | USB_DFU_WILL_DETACH, + .wDetachTimeout = 255, + .wTransferSize = BLOCK_SIZE, + .bcdDFUVersion = 0x0100, +}; + +const struct usb_interface_descriptor iface = { + .bLength = USB_DT_INTERFACE_SIZE, + .bDescriptorType = USB_DT_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 0, + .bInterfaceClass = 0xFE, /* Device Firmware Upgrade */ + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 2, + .extra = &dfu_function, + .extralen = sizeof(dfu_function), +}; + +const struct usb_interface ifaces[] = {{ + .num_altsetting = 1, + .altsetting = &iface, +}}; + +const struct usb_config_descriptor config = { + .bLength = USB_DT_CONFIGURATION_SIZE, + .bDescriptorType = USB_DT_CONFIGURATION, + .wTotalLength = 0, + .bNumInterfaces = 1, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = USB_CONFIG_ATTR_DEFAULT, // bus-powered + .bMaxPower = 50, // multiplied by 2 mA + .interface = ifaces, +}; + +static inline u32 get_u32(u32 addr) +{ + return *(u32*)addr; +} + +static inline u16 get_u16(u32 addr) +{ + return *(u16*)addr; +} + +static bool verify_firmware(void) +{ + u32 len = get_u32(BOOTLOADER_APP_START + HDR_LENGTH); + u16 flash_in_progress = get_u16(BOOTLOADER_APP_START + HDR_FLASH_IN_PROGRESS); + DEBUG("DFU: len=%u\n", (uint) len); + + // FIXME: Should check if len is reasonable + + crc_reset(); + u32 crc = crc_calculate_block((u32 *)BOOTLOADER_APP_START, len/4); + u32 want_crc = get_u32(BOOTLOADER_APP_START + len); + DEBUG("DFU: fip=%04x crc=%08x/%08x len=%u\n", (uint) flash_in_progress, (uint) crc, (uint) want_crc, (uint) len); + if (flash_in_progress || crc != want_crc) { + DEBUG("DFU: Bad firmware\n"); + return 0; + } + + return 1; +} + +static byte dfu_getstatus(usbd_device *usbd_dev UNUSED, u32 *bwPollTimeout) +{ + switch (dfu_state) { + case STATE_DFU_DNLOAD_SYNC: + dfu_state = STATE_DFU_DNBUSY; + *bwPollTimeout = 100; + return DFU_STATUS_OK; + case STATE_DFU_MANIFEST_SYNC: + /* Device will reset when read is complete. */ + dfu_state = STATE_DFU_MANIFEST; + return DFU_STATUS_OK; + case STATE_DFU_ERROR: + return DFU_STATUS_ERR_VERIFY; + default: + return DFU_STATUS_OK; + } +} + +static void dfu_getstatus_complete(usbd_device *usbd_dev UNUSED, struct usb_setup_data *req UNUSED) +{ + switch (dfu_state) { + case STATE_DFU_DNBUSY: + if (prog.blocknum == 0) { + // The "flash in progress" word is programmed as 0xffff first and reset later + *(u16*)(prog.buf + HDR_FLASH_IN_PROGRESS) = 0xffff; + } + u32 baseaddr = BOOTLOADER_APP_START + prog.blocknum * BLOCK_SIZE; + DEBUG("DFU: Block %u -> %08x + %u\n", prog.blocknum, (uint) baseaddr, prog.len); + flash_unlock(); + flash_erase_page(baseaddr); + for (uint i = 0; i < prog.len; i += 2) + flash_program_half_word(baseaddr + i, *(u16*)(prog.buf + i)); + flash_lock(); + for (uint i = 0; i < prog.len; i++) { + if (*(byte *)(baseaddr + i) != prog.buf[i]) { + DEBUG("DFU: Verification failed\n"); + dfu_state = STATE_DFU_ERROR; + } + } + dfu_state = STATE_DFU_DNLOAD_IDLE; + return; + case STATE_DFU_MANIFEST: + // At the very end, re-flash the "flash in progress" word + flash_unlock(); + flash_program_half_word(BOOTLOADER_APP_START + 0x20, 0); + flash_lock(); + if (verify_firmware()) + dfu_state = STATE_DFU_MANIFEST_WAIT_RESET; + else + dfu_state = STATE_DFU_ERROR; + return; + default: + return; + } +} + +static enum usbd_request_return_codes dfu_control_request(usbd_device *usbd_dev, + struct usb_setup_data *req, + byte **buf, + u16 *len, + void (**complete)(usbd_device *usbd_dev, struct usb_setup_data *req)) +{ + if ((req->bmRequestType & 0x7F) != 0x21) + return USBD_REQ_NOTSUPP; /* Only accept class request. */ + + DEBUG("DFU: Request %02x in state %d\n", req->bRequest, dfu_state); + timeout = DFU_TIMEOUT; + + switch (req->bRequest) { + case DFU_DNLOAD: + if (len == NULL || *len == 0) { + dfu_state = STATE_DFU_MANIFEST_SYNC; + } else { + /* Copy download data for use on GET_STATUS. */ + prog.blocknum = req->wValue; + prog.len = *len; + memcpy(prog.buf, *buf, *len); + dfu_state = STATE_DFU_DNLOAD_SYNC; + } + return USBD_REQ_HANDLED; + case DFU_CLRSTATUS: + /* Clear error and return to dfuIDLE. */ + if (dfu_state == STATE_DFU_ERROR) + dfu_state = STATE_DFU_IDLE; + return USBD_REQ_HANDLED; + case DFU_ABORT: + /* Abort returns to dfuIDLE state. */ + dfu_state = STATE_DFU_IDLE; + return USBD_REQ_HANDLED; + case DFU_UPLOAD: + /* Upload not supported for now. */ + return USBD_REQ_NOTSUPP; + case DFU_GETSTATUS: { + u32 bwPollTimeout = 0; /* 24-bit number of milliseconds */ + (*buf)[0] = dfu_getstatus(usbd_dev, &bwPollTimeout); + (*buf)[1] = bwPollTimeout & 0xFF; + (*buf)[2] = (bwPollTimeout >> 8) & 0xFF; + (*buf)[3] = (bwPollTimeout >> 16) & 0xFF; + (*buf)[4] = dfu_state; + (*buf)[5] = 0; /* iString not used here */ + *len = 6; + *complete = dfu_getstatus_complete; + return USBD_REQ_HANDLED; + } + case DFU_GETSTATE: + /* Return state with no state transition. */ + *buf[0] = dfu_state; + *len = 1; + return USBD_REQ_HANDLED; + } + + return USBD_REQ_NOTSUPP; +} + +static void dfu_set_config(usbd_device *usbd_dev, u16 wValue UNUSED) +{ + usbd_register_control_callback( + usbd_dev, + USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE, + USB_REQ_TYPE_TYPE | USB_REQ_TYPE_RECIPIENT, + dfu_control_request); +} + +static void dfu_reset(void) +{ + dfu_state = STATE_DFU_IDLE; +} + +/* + * This is a modified version of rcc_clock_setup_in_hsi_out_48mhz(), + * which properly turns off the PLL before setting its parameters. + */ +static void my_rcc_clock_setup_in_hsi_out_48mhz(void) +{ + /* Enable internal high-speed oscillator. */ + rcc_osc_on(RCC_HSI); + rcc_wait_for_osc_ready(RCC_HSI); + + /* Select HSI as SYSCLK source. */ + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSICLK); + + // XXX: Disable PLL + rcc_osc_off(RCC_PLL); + + /* + * Set prescalers for AHB, ADC, ABP1, ABP2. + * Do this before touching the PLL (TODO: why?). + */ + rcc_set_hpre(RCC_CFGR_HPRE_SYSCLK_NODIV); /*Set.48MHz Max.72MHz */ + rcc_set_adcpre(RCC_CFGR_ADCPRE_PCLK2_DIV8); /*Set. 6MHz Max.14MHz */ + rcc_set_ppre1(RCC_CFGR_PPRE1_HCLK_DIV2); /*Set.24MHz Max.36MHz */ + rcc_set_ppre2(RCC_CFGR_PPRE2_HCLK_NODIV); /*Set.48MHz Max.72MHz */ + rcc_set_usbpre(RCC_CFGR_USBPRE_PLL_CLK_NODIV); /*Set.48MHz Max.48MHz */ + + /* + * Sysclk runs with 48MHz -> 1 waitstates. + * 0WS from 0-24MHz + * 1WS from 24-48MHz + * 2WS from 48-72MHz + */ + flash_set_ws(FLASH_ACR_LATENCY_1WS); + + /* + * Set the PLL multiplication factor to 12. + * 8MHz (internal) * 12 (multiplier) / 2 (PLLSRC_HSI_CLK_DIV2) = 48MHz + */ + rcc_set_pll_multiplication_factor(RCC_CFGR_PLLMUL_PLL_CLK_MUL12); + + /* Select HSI/2 as PLL source. */ + rcc_set_pll_source(RCC_CFGR_PLLSRC_HSI_CLK_DIV2); + + /* Enable PLL oscillator and wait for it to stabilize. */ + rcc_osc_on(RCC_PLL); + rcc_wait_for_osc_ready(RCC_PLL); + + /* Select PLL as SYSCLK source. */ + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_PLLCLK); + + /* Set the peripheral clock frequencies used */ + rcc_ahb_frequency = 48000000; + rcc_apb1_frequency = 24000000; + rcc_apb2_frequency = 48000000; +} + +static void clock_plain_hsi(void) +{ + // Select HSI as SYSCLK source + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSICLK); + + // Disable PLL + rcc_osc_off(RCC_PLL); + + // Set prescalers for AHB, ADC, ABP1, ABP2, USB to defaults + rcc_set_hpre(RCC_CFGR_HPRE_SYSCLK_NODIV); + rcc_set_adcpre(RCC_CFGR_ADCPRE_PCLK2_DIV2); + rcc_set_ppre1(RCC_CFGR_PPRE1_HCLK_NODIV); + rcc_set_ppre2(RCC_CFGR_PPRE2_HCLK_NODIV); + rcc_set_usbpre(RCC_CFGR_USBPRE_PLL_VCO_CLK_DIV3); +} + +static void reset_peripherals(void) +{ + // Turn off clock to all peripherals and reset them + RCC_AHBENR = 0x00000014; + RCC_APB1ENR = 0; + RCC_APB2ENR = 0; + RCC_APB1RSTR = 0x22fec9ff; + RCC_APB2RSTR = 0x0038fffd; + RCC_APB1RSTR = 0; + RCC_APB2RSTR = 0; +} + +static void configure_hardware(void) +{ + rcc_periph_clock_enable(RCC_GPIOA); + rcc_periph_clock_enable(RCC_GPIOB); + rcc_periph_clock_enable(RCC_GPIOC); + rcc_periph_clock_enable(RCC_USB); + rcc_periph_clock_enable(RCC_CRC); + +#ifdef DEBUG_USART +#if DEBUG_USART == USART1 + rcc_periph_clock_enable(RCC_USART1); + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); +#elif DEBUG_USART == USART2 + rcc_periph_clock_enable(RCC_USART2); + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO2); +#elif DEBUG_USART == USART3 + rcc_periph_clock_enable(RCC_USART3); + gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO10); +#else +#error "Unknown USART for debugging" +#endif + usart_set_baudrate(DEBUG_USART, 115200); + usart_set_databits(DEBUG_USART, 8); + usart_set_stopbits(DEBUG_USART, USART_STOPBITS_1); + usart_set_mode(DEBUG_USART, USART_MODE_TX); + usart_set_parity(DEBUG_USART, USART_PARITY_NONE); + usart_set_flow_control(DEBUG_USART, USART_FLOWCONTROL_NONE); + usart_enable(DEBUG_USART); +#endif + +#ifdef DEBUG_LED_BLUEPILL + // BluePill LED + gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO13); + debug_led(1); +#endif + + // Systick: set to overflow in 1 ms, will use only the overflow flag, no interrupts + systick_set_frequency(1000, CPU_CLOCK_MHZ * 1000000); + systick_clear(); + systick_counter_enable(); +} + +#ifndef BOOTLOADER_CUSTOM_HW_INIT + +static void usb_disconnect(void) +{ + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_OPENDRAIN, GPIO11 | GPIO12); + gpio_clear(GPIOA, GPIO11 | GPIO12); + for (uint i=0; i<100; i++) { + while (!systick_get_countflag()) + ; + } +} + +#endif + +int main(void) +{ + usbd_device *usbd_dev; + + reset_peripherals(); + + // Flash programming requires running on the internal oscillator + my_rcc_clock_setup_in_hsi_out_48mhz(); + + configure_hardware(); + desig_get_unique_id_as_dfu(usb_serial_number); + + DEBUG("DFU: Started (SN %s)\n", usb_serial_number); + +#ifdef BOOTLOADER_CUSTOM_HW_INIT + custom_hw_init(); +#else + usb_disconnect(); +#endif + + DEBUG("DFU: Ready\n"); + debug_led(0); + + usbd_dev = usbd_init(&st_usbfs_v1_usb_driver, &dev, &config, usb_strings, ARRAY_SIZE(usb_strings), usbd_control_buffer, sizeof(usbd_control_buffer)); + usbd_register_reset_callback(usbd_dev, dfu_reset); + usbd_register_set_config_callback(usbd_dev, dfu_set_config); + +restart: ; + + timeout = DEFAULT_TIMEOUT; + while (timeout || (dfu_state != STATE_DFU_IDLE && dfu_state != STATE_DFU_MANIFEST_WAIT_RESET)) { + usbd_poll(usbd_dev); + if (timeout && systick_get_countflag()) { + timeout--; + // FIXME: Blink LED even after timeout + if (!(timeout & 0x3f)) + debug_led_toggle(); + } + } + + if (!verify_firmware()) + goto restart; + + u32 sp = get_u32(BOOTLOADER_APP_START); + u32 pc = get_u32(BOOTLOADER_APP_START + 4); + DEBUG("DFU: Boot (sp=%08x pc=%08x)\n", (uint) sp, (uint) pc); + +#ifdef DEBUG_USART + debug_flush(); +#endif + debug_led(0); + + reset_peripherals(); + clock_plain_hsi(); + + /* Set vector table base address. */ + SCB_VTOR = BOOTLOADER_APP_START; + + /* Initialize master stack pointer. */ + asm volatile("msr msp, %0"::"g" (sp)); + + /* Jump to application. */ + ((void (*)(void)) pc)(); +} diff --git a/ucw-stm32lib/lib/ds18b20.c b/ucw-stm32lib/lib/ds18b20.c new file mode 100644 index 0000000..02a62cf --- /dev/null +++ b/ucw-stm32lib/lib/ds18b20.c @@ -0,0 +1,414 @@ +/* + * Interface to DS18B20 Temperature Sensors + * + * (c) 2019 Martin Mareš + */ + +#include "util.h" +#include "ds18b20.h" +#include "ext-timer.h" + +#include +#include +#include +#include +#include + +/*** Configuration ***/ + +// You should set the following parameters in config.h + +// #define DS_TIMER TIM3 +// #define DS_GPIO GPIOA +// #define DS_PIN GPIO7 +// #define DS_DMA DMA1 +// #define DS_DMA_CH 6 + +// #undef DS_DEBUG +// #undef DS_DEBUG2 + +// Maximum number of supported sensors +// #define DS_NUM_SENSORS 8 + +#ifdef DS_DEBUG +#define DEBUG debug_printf +#else +#define DEBUG(xxx, ...) do { } while (0) +#endif + +#ifdef DS_DEBUG2 +#define DEBUG2 debug_printf +#else +#define DEBUG2(xxx, ...) do { } while (0) +#endif + +static volatile u32 ds_dma_buffer; + +static bool ds_reset(void) +{ + DEBUG2("DS18B20: Reset\n"); + timer_disable_counter(DS_TIMER); + timer_one_shot_mode(DS_TIMER); + + // DMA for reading pin state + ds_dma_buffer = 0xdeadbeef; + dma_set_memory_address(DS_DMA, DS_DMA_CH, (u32) &ds_dma_buffer); + dma_set_peripheral_address(DS_DMA, DS_DMA_CH, (u32) &GPIO_IDR(DS_GPIO)); + dma_set_number_of_data(DS_DMA, DS_DMA_CH, 1); + dma_enable_channel(DS_DMA, DS_DMA_CH); + + // CC1 is used to drive the DMA (read line state at specified time) + timer_disable_oc_output(DS_TIMER, TIM_OC1); + timer_set_oc_mode(DS_TIMER, TIM_OC1, TIM_OCM_FROZEN); + timer_set_oc_value(DS_TIMER, TIM_OC1, 560); + timer_set_dma_on_compare_event(DS_TIMER); + timer_enable_dma_cc1(DS_TIMER); + + // CC2 is used to generate pulses (return line to idle state at specified time) + timer_set_oc_mode(DS_TIMER, TIM_OC2, TIM_OCM_FORCE_HIGH); + timer_enable_oc_output(DS_TIMER, TIM_OC2); + timer_set_oc_value(DS_TIMER, TIM_OC2, 480); + timer_set_oc_polarity_low(DS_TIMER, TIM_OC2); + + // Set timer period to the length of the whole transaction (1 ms) + timer_set_period(DS_TIMER, 999); + + // XXX: We do not know why this is needed... + static bool once; + if (!once) { + for (int i=0; i<10000; i++) __asm__ volatile ("nop"); + once = 1; + } + + // Pull line down and start timer + timer_generate_event(DS_TIMER, TIM_EGR_UG); + timer_set_oc_mode(DS_TIMER, TIM_OC2, TIM_OCM_INACTIVE); + timer_enable_counter(DS_TIMER); + + // Wait until the timer expires + while (timer_is_counter_enabled(DS_TIMER)) + ; + // Counter is automatically disabled at the end of cycle + + // Disable DMA + timer_disable_dma_cc1(DS_TIMER); + dma_disable_channel(DS_DMA, DS_DMA_CH); + + DEBUG2("Init DMA: %08x [%u] (%u remains)\n", + ds_dma_buffer, + !!(ds_dma_buffer & DS_PIN), + dma_get_number_of_data(DS_DMA, DS_DMA_CH)); + + // Did the device respond? + if (ds_dma_buffer & DS_PIN) { + DEBUG("DS18B20: Initialization failed\n"); + return 0; + } else + return 1; +} + +static void ds_send_bit(bool bit) +{ + timer_set_period(DS_TIMER, 99); // Each write slot takes 100 μs + timer_set_oc_mode(DS_TIMER, TIM_OC2, TIM_OCM_FORCE_HIGH); + timer_set_oc_value(DS_TIMER, TIM_OC2, (bit ? 3 : 89)); // 1: 3μs pulse, 0: 89μs pulse + timer_generate_event(DS_TIMER, TIM_EGR_UG); + timer_set_oc_mode(DS_TIMER, TIM_OC2, TIM_OCM_INACTIVE); + timer_enable_counter(DS_TIMER); + while (timer_is_counter_enabled(DS_TIMER)) + ; +} + +static void ds_send_byte(byte b) +{ + DEBUG2("DS write: %02x\n", b); + for (uint m = 1; m < 0x100; m <<= 1) + ds_send_bit(b & m); +} + +static bool ds_recv_bit(void) +{ + timer_set_period(DS_TIMER, 79); // Each read slot takes 80μs + timer_set_oc_value(DS_TIMER, TIM_OC2, 2); // Generate 2μs pulse to start read slot + timer_set_oc_value(DS_TIMER, TIM_OC1, 8); // Sample data 8μs after start of slot + timer_enable_dma_cc1(DS_TIMER); + + ds_dma_buffer = 0xdeadbeef; + dma_set_number_of_data(DS_DMA, DS_DMA_CH, 1); + dma_enable_channel(DS_DMA, DS_DMA_CH); + timer_set_oc_mode(DS_TIMER, TIM_OC2, TIM_OCM_FORCE_HIGH); + timer_generate_event(DS_TIMER, TIM_EGR_UG); + timer_set_oc_mode(DS_TIMER, TIM_OC2, TIM_OCM_INACTIVE); + timer_enable_counter(DS_TIMER); + while (timer_is_counter_enabled(DS_TIMER)) + ; + // DEBUG2("XXX %08x\n", ds_dma_buffer); + bool out = ds_dma_buffer & DS_PIN; + dma_disable_channel(DS_DMA, DS_DMA_CH); + + timer_disable_dma_cc1(DS_TIMER); + + return out; +} + +static byte ds_recv_byte(void) +{ + uint out = 0; + for (uint m = 1; m < 0x100; m <<= 1) { + if (ds_recv_bit()) + out |= m; + } + + DEBUG2("DS read: %02x\n", out); + return out; +} + +static byte ds_buf[10]; + +static byte ds_crc_block(uint n) +{ + /// XXX: This might be worth optimizing + uint crc = 0; + + for (uint i = 0; i < n; i++) { + byte b = ds_buf[i]; + for (uint j = 0; j < 8; j++) { + uint k = (b & 1) ^ (crc >> 7); + crc = (crc << 1) & 0xff; + if (k) + crc ^= 0x31; + b >>= 1; + } + } + + return crc; +} + +static bool ds_recv_block(uint n) +{ + for (uint i = 0; i < n; i++) + ds_buf[i] = ds_recv_byte(); + + byte crc = ds_crc_block(n); + if (crc) { + DEBUG("DS18B20: Invalid CRC %02x\n", crc); + return 0; + } + return 1; +} + +struct ds_sensor ds_sensors[DS_NUM_SENSORS]; + +#if DS_NUM_SENSORS == 1 + +static void ds_enumerate(void) +{ + if (!ds_reset()) + return; + + ds_send_byte(0x33); // READ_ROM + if (!ds_recv_block(8)) + return; + + DEBUG("DS18B20: Found sensor "); + for (uint i = 0; i < 8; i++) { + DEBUG("%02x", ds_buf[i]); + ds_sensors[0].address[i] = ds_buf[i]; + } + DEBUG("\n"); +} + +#else + +static void ds_enumerate(void) +{ + /* + * The enumeration algorithm roughly follows the one described in the + * Book of iButton Standards (Maxim Integrated Application Note 937). + * + * It simulates depth-first search on the trie of all device IDs. + * In each pass, it walks the trie from the root and recognizes branching nodes. + * + * The old_choice variable remembers the deepest left branch taken in the + * previous pass, new_choice is the same for the current pass. + */ + + DEBUG("DS18B20: Enumerate\n"); + + uint num_sensors = 0; + byte *addr = ds_buf; + byte old_choice = 0; + + for (;;) { + if (!ds_reset()) { + DEBUG("DS18B20: Enumeration found no sensor\n"); + return; + } + + ds_send_byte(0xf0); // SEARCH_ROM + byte new_choice = 0; + for (byte i=0; i<64; i++) { + bool have_one = ds_recv_bit(); + bool have_zero = ds_recv_bit(); + bool old_bit = addr[i/8] & (1U << (i%8)); + bool new_bit; + switch (2*have_one + have_zero) { + case 3: + // This should not happen + DEBUG("DS18B20: Enumeration failed\n"); + return; + case 1: + // Only 0 + new_bit = 0; + break; + case 2: + // Only 1 + new_bit = 1; + break; + default: + // Both + if (i == old_choice) + new_bit = 1; + else if (i > old_choice) { + new_bit = 0; + new_choice = i; + } else { + new_bit = old_bit; + if (!new_bit) + new_choice = i; + } + } + if (new_bit) + addr[i/8] |= 1U << (i%8); + else + addr[i/8] &= ~(1U << (i%8)); + ds_send_bit(new_bit); + } + + if (num_sensors >= DS_NUM_SENSORS) { + DEBUG("DS18B20: Too many sensors\n"); + return; + } + + DEBUG("DS18B20: Found sensor #%u: ", num_sensors); + for (byte i=0; i<8; i++) + DEBUG("%02x", addr[i]); + if (ds_crc_block(8)) { + DEBUG(" - invalid CRC!\n"); + } else if (ds_buf[0] == 0x28) { + DEBUG("\n"); + memcpy(ds_sensors[num_sensors].address, ds_buf, 8); + num_sensors++; + } else { + DEBUG(" - wrong type\n"); + } + + old_choice = new_choice; + if (!old_choice) + break; + } +} + +#endif + +void ds_init(void) +{ + DEBUG("DS18B20: Init\n"); + + for (uint i = 0; i < DS_NUM_SENSORS; i++) { + memset(ds_sensors[i].address, 0, 8); + ds_sensors[i].current_temp = DS_TEMP_UNKNOWN; + } + + dma_set_read_from_peripheral(DS_DMA, DS_DMA_CH); + dma_set_priority(DS_DMA, DS_DMA_CH, DMA_CCR_PL_VERY_HIGH); + dma_disable_peripheral_increment_mode(DS_DMA, DS_DMA_CH); + dma_enable_memory_increment_mode(DS_DMA, DS_DMA_CH); + dma_set_peripheral_size(DS_DMA, DS_DMA_CH, DMA_CCR_PSIZE_16BIT); + dma_set_memory_size(DS_DMA, DS_DMA_CH, DMA_CCR_MSIZE_16BIT); + + timer_set_prescaler(DS_TIMER, CPU_CLOCK_MHZ - 1); // 1 tick = 1 μs + timer_set_mode(DS_TIMER, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); + timer_disable_preload(DS_TIMER); + + gpio_set_mode(DS_GPIO, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN, DS_PIN); + + ds_enumerate(); + + // FIXME: Configure precision? +} + +#if DS_NUM_SENSORS == 1 +#define ds_current_id 0 +#else + static byte ds_current_id; +#endif + +static bool ds_activate(void) +{ + if (!ds_reset()) { + DEBUG("DS18B20: Reset failed\n"); + return false; + } +#if DS_NUM_SENSORS == 1 + ds_send_byte(0xcc); // SKIP_ROM +#else + ds_send_byte(0x55); // MATCH_ROM + for (uint i = 0; i < 8; i++) + ds_send_byte(ds_sensors[ds_current_id].address[i]); +#endif + return true; +} + +void ds_step(void) +{ + static byte ds_running; + static byte ds_timeout; + + if (!ds_running) { + // Start measurement +#if DS_NUM_SENSORS != 1 + uint maxn = DS_NUM_SENSORS; + do { + if (!maxn--) + return; + ds_current_id++; + if (ds_current_id >= DS_NUM_SENSORS) { + ds_current_id = 0; + } + } while (!ds_sensors[ds_current_id].address[0]); +#endif + if (!ds_activate()) { + ds_sensors[ds_current_id].current_temp = DS_TEMP_UNKNOWN; + return; + } + ds_send_byte(0x44); // CONVERT_T + ds_running = 1; + ds_timeout = 255; + } else { + // Still running? + if (!ds_recv_bit()) { + if (!ds_timeout--) { + DEBUG("DS18B20 #%u: Timeout\n", ds_current_id); + ds_sensors[ds_current_id].current_temp = DS_TEMP_UNKNOWN; + ds_running = 0; + } + return; + } + ds_running = 0; + + // Read scratch pad + if (!ds_activate()) + return; + ds_send_byte(0xbe); // READ_SCRATCHPAD + if (!ds_recv_block(9)) { + ds_sensors[ds_current_id].current_temp = DS_TEMP_UNKNOWN; + return; + } + int t = (int16_t) (ds_buf[0] | (ds_buf[1] << 8)); + t = t * 1000 / 16; + + DEBUG("DS18B20 #%u: %d.%03d degC\n", ds_current_id, t / 1000, t % 1000); + ds_sensors[ds_current_id].current_temp = t; + } +} diff --git a/ucw-stm32lib/lib/ds18b20.h b/ucw-stm32lib/lib/ds18b20.h new file mode 100644 index 0000000..3be92f3 --- /dev/null +++ b/ucw-stm32lib/lib/ds18b20.h @@ -0,0 +1,23 @@ +/* + * Interface to DS18B20 Temperature Sensors + * + * (c) 2019 Martin Mareš + */ + +#ifndef _DS18B20_H +#define _DS18B20_H + +struct ds_sensor { + byte address[8]; // All zeroes if sensor does not exist. + // Otherwise, address[0] is guaranteed to be non-zero. + int current_temp; // Temperature in m°C or DS_TEMP_UNKNOWN +}; + +extern struct ds_sensor ds_sensors[DS_NUM_SENSORS]; + +#define DS_TEMP_UNKNOWN 0x7fffffff + +void ds_init(void); +void ds_step(void); + +#endif diff --git a/ucw-stm32lib/lib/ext-timer.h b/ucw-stm32lib/lib/ext-timer.h new file mode 100644 index 0000000..e05736f --- /dev/null +++ b/ucw-stm32lib/lib/ext-timer.h @@ -0,0 +1,27 @@ +/* + * Timer Functions Missing from LibOpenCM3 + * + * (c) 2019 Martin Mareš + */ + +#ifndef _EXT_TIMER_H +#define _EXT_TIMER_H + +#include + +static inline bool timer_is_counter_enabled(u32 timer) +{ + return TIM_CR1(timer) & TIM_CR1_CEN; +} + +static inline void timer_enable_dma_cc1(u32 timer) +{ + TIM_DIER(timer) |= TIM_DIER_CC1DE; +} + +static inline void timer_disable_dma_cc1(u32 timer) +{ + TIM_DIER(timer) &= ~TIM_DIER_CC1DE; +} + +#endif diff --git a/ucw-stm32lib/lib/modbus-bootloader-proto.h b/ucw-stm32lib/lib/modbus-bootloader-proto.h new file mode 100644 index 0000000..1abb12c --- /dev/null +++ b/ucw-stm32lib/lib/modbus-bootloader-proto.h @@ -0,0 +1,88 @@ +/* + * MODBUS Bootloader -- Protocol + * + * (c) 2023 Martin Mareš + * + * Licensed under the GNU LGPL v3 or any later version. + */ + +enum bl_input_reg { + BL_INPUT_MAGIC_HI = 0xe000, + BL_INPUT_MAGIC_LO, + BL_INPUT_LOADER_VERSION, + BL_INPUT_STATUS, + // The following registers are not available in application mode + BL_INPUT_VENDOR_ID, + BL_INPUT_DEVICE_ID, + BL_INPUT_SERIAL_NUMBER, // 12 characters as in USB + BL_INPUT_BLOCK_SIZE = BL_INPUT_SERIAL_NUMBER + 6, + BL_INPUT_FLASH_SIZE, // kB + BL_INPUT_MAX, +}; + +enum bl_holding_reg { + BL_HOLD_COMMAND = 0xe000, + BL_HOLD_BLOCK_NUMBER, + BL_HOLD_MAX, + BL_HOLD_BLOCK_DATA = BL_HOLD_COMMAND + 0x100, // next BL_BLOCK_SIZE/2 registers contain block data +}; + +#define BL_MAGIC_HI 0x426f // "Bo" +#define BL_MAGIC_LO 0x6f54 // "oT" +#define BL_LOADER_VERSION 0x0001 + +enum bl_status { + BL_STATUS_APP = 1, // In application code, needs exit command + BL_STATUS_READY, // Boot loader ready + BL_STATUS_FLASHING, // Flashing in progress + BL_STATUS_ERROR, // Error occurred + BL_STATUS_CORRUPTED, // Corrupted firmware found + BL_STATUS_BOOTING, // About to boot current firmware +}; + +enum bl_command { + BL_COMMAND_BOOT = 1, // Boot current firmware + BL_COMMAND_FLASH_START, // Enter flash mode + BL_COMMAND_FLASH_END, // Quit flash mode and verify checksum + BL_COMMAND_FLASH_BLOCK, // Flash one block + BL_COMMAND_EXIT_APP, // Exit from application code to boot loader +}; + +static inline bool bl_app_check_input_register(u16 addr) +{ + switch (addr) { + case BL_INPUT_MAGIC_HI: + case BL_INPUT_MAGIC_LO: + case BL_INPUT_LOADER_VERSION: + case BL_INPUT_STATUS: + return true; + default: + return false; + } +} + +static inline u16 bl_app_get_input_register(u16 addr) +{ + switch (addr) { + case BL_INPUT_MAGIC_HI: + return BL_MAGIC_HI; + case BL_INPUT_MAGIC_LO: + return BL_MAGIC_LO; + case BL_INPUT_LOADER_VERSION: + return BL_LOADER_VERSION; + case BL_INPUT_STATUS: + return BL_STATUS_APP; + default: + return 0; + } +} + +static inline bool bl_app_check_holding_register(u16 addr) +{ + return (addr == BL_HOLD_COMMAND); +} + +static inline bool bl_app_set_holding_register(u16 addr, u16 value) +{ + return (addr == BL_HOLD_COMMAND && value == BL_COMMAND_EXIT_APP); +} diff --git a/ucw-stm32lib/lib/modbus-bootloader.c b/ucw-stm32lib/lib/modbus-bootloader.c new file mode 100644 index 0000000..63d7082 --- /dev/null +++ b/ucw-stm32lib/lib/modbus-bootloader.c @@ -0,0 +1,428 @@ +/* + * MODBUS Bootloader + * + * (c) 2023 Martin Mareš + * + * Based on example code from the libopencm3 project, which is + * Copyright (C) 2010 Gareth McMullin + * + * Licensed under the GNU LGPL v3 or any later version. + */ + +#include "util.h" +#include "modbus.h" +#include "modbus-bootloader-proto.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#ifdef BOOTLOADER_DEBUG +#define DEBUG(x...) debug_printf(x) +#else +#define DEBUG(x...) do { } while (0) +#endif + +// Offsets to firmware header fields (see tools/dfu-sign.c) +#define HDR_LENGTH 0x1c +#define HDR_FLASH_IN_PROGRESS 0x20 + +// Block size should be equal to erase block of the flash memory +#define BLOCK_SIZE 1024 +static byte current_block[BLOCK_SIZE]; +static uint current_block_number; + +static uint timeout; +#define DEFAULT_TIMEOUT 5000 // ms + +static char usb_serial_number[13]; +static uint flash_size_kb; +static uint status; + +static inline u32 get_u32(u32 addr) +{ + return *(u32*)addr; +} + +static inline u16 get_u16(u32 addr) +{ + return *(u16*)addr; +} + +static bool verify_firmware(void) +{ + u32 len = get_u32(BOOTLOADER_APP_START + HDR_LENGTH); + u16 flash_in_progress = get_u16(BOOTLOADER_APP_START + HDR_FLASH_IN_PROGRESS); + + // Just to be sure + len = MIN(len, flash_size_kb * 1024); + + crc_reset(); + u32 crc = crc_calculate_block((u32 *)BOOTLOADER_APP_START, len/4); + u32 want_crc = get_u32(BOOTLOADER_APP_START + len); + DEBUG("BOOT: fip=%04x crc=%08x/%08x len=%u\n", (uint) flash_in_progress, (uint) crc, (uint) want_crc, (uint) len); + if (flash_in_progress || crc != want_crc) { + DEBUG("BOOT: Bad firmware\n"); + return 0; + } + + return 1; +} + +static bool flash_block(void) +{ + if (current_block_number >= flash_size_kb * 1024 / BLOCK_SIZE) { + DEBUG("BOOT: Bad block nr\n"); + return false; + } + + if (current_block_number == 0) { + // The "flash in progress" word is programmed as 0xffff first and reset later + *(u16*)(current_block + HDR_FLASH_IN_PROGRESS) = 0xffff; + } + + u32 baseaddr = BOOTLOADER_APP_START + current_block_number * BLOCK_SIZE; + DEBUG("BOOT: Block %u -> %08x\n", current_block_number, (uint) baseaddr); + + flash_unlock(); + flash_erase_page(baseaddr); + for (uint i = 0; i < BLOCK_SIZE; i += 2) + flash_program_half_word(baseaddr + i, *(u16*)(current_block + i)); + flash_lock(); + + for (uint i = 0; i < BLOCK_SIZE; i++) { + if (*(byte *)(baseaddr + i) != current_block[i]) { + DEBUG("BOOT: Verification failed\n"); + return false; + } + } + + return true; +} + +static bool flash_end(void) +{ + flash_unlock(); + flash_program_half_word(BOOTLOADER_APP_START + 0x20, 0); + flash_lock(); + + return verify_firmware(); +} + +/* + * This is a modified version of rcc_clock_setup_in_hsi_out_48mhz(), + * which properly turns off the PLL before setting its parameters. + */ +static void my_rcc_clock_setup_in_hsi_out_48mhz(void) +{ + /* Enable internal high-speed oscillator. */ + rcc_osc_on(RCC_HSI); + rcc_wait_for_osc_ready(RCC_HSI); + + /* Select HSI as SYSCLK source. */ + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSICLK); + + // XXX: Disable PLL + rcc_osc_off(RCC_PLL); + + /* + * Set prescalers for AHB, ADC, ABP1, ABP2. + * Do this before touching the PLL (TODO: why?). + */ + rcc_set_hpre(RCC_CFGR_HPRE_SYSCLK_NODIV); /*Set.48MHz Max.72MHz */ + rcc_set_adcpre(RCC_CFGR_ADCPRE_PCLK2_DIV8); /*Set. 6MHz Max.14MHz */ + rcc_set_ppre1(RCC_CFGR_PPRE1_HCLK_DIV2); /*Set.24MHz Max.36MHz */ + rcc_set_ppre2(RCC_CFGR_PPRE2_HCLK_NODIV); /*Set.48MHz Max.72MHz */ + rcc_set_usbpre(RCC_CFGR_USBPRE_PLL_CLK_NODIV); /*Set.48MHz Max.48MHz */ + + /* + * Sysclk runs with 48MHz -> 1 waitstates. + * 0WS from 0-24MHz + * 1WS from 24-48MHz + * 2WS from 48-72MHz + */ + flash_set_ws(FLASH_ACR_LATENCY_1WS); + + /* + * Set the PLL multiplication factor to 12. + * 8MHz (internal) * 12 (multiplier) / 2 (PLLSRC_HSI_CLK_DIV2) = 48MHz + */ + rcc_set_pll_multiplication_factor(RCC_CFGR_PLLMUL_PLL_CLK_MUL12); + + /* Select HSI/2 as PLL source. */ + rcc_set_pll_source(RCC_CFGR_PLLSRC_HSI_CLK_DIV2); + + /* Enable PLL oscillator and wait for it to stabilize. */ + rcc_osc_on(RCC_PLL); + rcc_wait_for_osc_ready(RCC_PLL); + + /* Select PLL as SYSCLK source. */ + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_PLLCLK); + + /* Set the peripheral clock frequencies used */ + rcc_ahb_frequency = 48000000; + rcc_apb1_frequency = 24000000; + rcc_apb2_frequency = 48000000; +} + +static void clock_plain_hsi(void) +{ + // Select HSI as SYSCLK source + rcc_set_sysclk_source(RCC_CFGR_SW_SYSCLKSEL_HSICLK); + + // Disable PLL + rcc_osc_off(RCC_PLL); + + // Set prescalers for AHB, ADC, ABP1, ABP2, USB to defaults + rcc_set_hpre(RCC_CFGR_HPRE_SYSCLK_NODIV); + rcc_set_adcpre(RCC_CFGR_ADCPRE_PCLK2_DIV2); + rcc_set_ppre1(RCC_CFGR_PPRE1_HCLK_NODIV); + rcc_set_ppre2(RCC_CFGR_PPRE2_HCLK_NODIV); + rcc_set_usbpre(RCC_CFGR_USBPRE_PLL_VCO_CLK_DIV3); +} + +static void reset_peripherals(void) +{ + // Turn off clock to all peripherals and reset them + RCC_AHBENR = 0x00000014; + RCC_APB1ENR = 0; + RCC_APB2ENR = 0; + RCC_APB1RSTR = 0x22fec9ff; + RCC_APB2RSTR = 0x0038fffd; + RCC_APB1RSTR = 0; + RCC_APB2RSTR = 0; +} + +static void configure_hardware(void) +{ + rcc_periph_clock_enable(RCC_GPIOA); + rcc_periph_clock_enable(RCC_GPIOB); + rcc_periph_clock_enable(RCC_GPIOC); + rcc_periph_clock_enable(RCC_CRC); + +#ifdef DEBUG_USART +#if DEBUG_USART == USART1 + rcc_periph_clock_enable(RCC_USART1); + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); +#elif DEBUG_USART == USART2 + rcc_periph_clock_enable(RCC_USART2); + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO2); +#elif DEBUG_USART == USART3 + rcc_periph_clock_enable(RCC_USART3); + gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO10); +#else +#error "Unknown USART for debugging" +#endif + usart_set_baudrate(DEBUG_USART, 115200); + usart_set_databits(DEBUG_USART, 8); + usart_set_stopbits(DEBUG_USART, USART_STOPBITS_1); + usart_set_mode(DEBUG_USART, USART_MODE_TX); + usart_set_parity(DEBUG_USART, USART_PARITY_NONE); + usart_set_flow_control(DEBUG_USART, USART_FLOWCONTROL_NONE); + usart_enable(DEBUG_USART); +#endif + +#ifdef DEBUG_LED_BLUEPILL + // BluePill LED + gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO13); + debug_led(1); +#endif + + // Systick: set to overflow in 1 ms, will use only the overflow flag, no interrupts + systick_set_frequency(1000, CPU_CLOCK_MHZ * 1000000); + systick_clear(); + systick_counter_enable(); +} + +/*** Modbus callbacks ***/ + +bool modbus_check_discrete_input(u16 addr UNUSED) +{ + return false; +} + +bool modbus_get_discrete_input(u16 addr UNUSED) +{ + return false; +} + +bool modbus_check_coil(u16 addr UNUSED) +{ + return false; +} + +bool modbus_get_coil(u16 addr UNUSED) +{ + return false; +} + +void modbus_set_coil(u16 addr UNUSED, bool value UNUSED) +{ +} + +bool modbus_check_input_register(u16 addr) +{ + return addr >= BL_INPUT_MAGIC_HI && addr < BL_INPUT_MAX; +} + +u16 modbus_get_input_register(u16 addr) +{ + switch (addr) { + case BL_INPUT_MAGIC_HI: + return BL_MAGIC_HI; + case BL_INPUT_MAGIC_LO: + return BL_MAGIC_LO; + case BL_INPUT_LOADER_VERSION: + return BL_LOADER_VERSION; + case BL_INPUT_STATUS: + return status; + case BL_INPUT_VENDOR_ID: + return BOOTLOADER_VENDOR_ID; + case BL_INPUT_DEVICE_ID: + return BOOTLOADER_DEVICE_ID; + case BL_INPUT_BLOCK_SIZE: + return BLOCK_SIZE; + case BL_INPUT_FLASH_SIZE: + return flash_size_kb; + default: + return get_u16_le((byte *)usb_serial_number + 2*(addr - BL_INPUT_SERIAL_NUMBER)); + } +} + +bool modbus_check_holding_register(u16 addr) +{ + return addr >= BL_HOLD_COMMAND && addr < BL_HOLD_MAX + || addr >= BL_HOLD_BLOCK_DATA && addr < BL_HOLD_BLOCK_DATA + BLOCK_SIZE / 2; +} + +u16 modbus_get_holding_register(u16 addr UNUSED) +{ + // Reading of holding registers is not supported + return 0; +} + +static uint bl_command(uint value) +{ + switch (value) { + case BL_COMMAND_BOOT: + if (status == BL_STATUS_READY) + return BL_STATUS_BOOTING; + return BL_STATUS_ERROR; + case BL_COMMAND_FLASH_START: + return BL_STATUS_FLASHING; + case BL_COMMAND_FLASH_END: + if (flash_end()) + return BL_STATUS_READY; + else + return BL_STATUS_CORRUPTED; + case BL_COMMAND_FLASH_BLOCK: + if (flash_block()) + return BL_STATUS_FLASHING; + else + return BL_STATUS_ERROR; + default: + return BL_STATUS_ERROR; + } +} + +void modbus_set_holding_register(u16 addr, u16 value) +{ + switch (addr) { + case BL_HOLD_COMMAND: + status = bl_command(value); + DEBUG("BOOT: cmd=%u status=%u\n", value, status); + timeout = DEFAULT_TIMEOUT; + break; + case BL_HOLD_BLOCK_NUMBER: + current_block_number = value; + break; + default: + put_u16_le(current_block + 2*(addr - BL_HOLD_BLOCK_DATA), value); + } +} + +// These should be implemented by board-specific code +// void modbus_ready_hook(void); +// void modbus_frame_start_hook(void); +// const char * const modbus_id_strings[MODBUS_ID_MAX]; + +static void delay_ms(uint ms) +{ + for (uint j=0; j + */ + +enum modbus_function { + MODBUS_FUNC_READ_COILS = 0x01, + MODBUS_FUNC_READ_DISCRETE_INPUTS = 0x02, + MODBUS_FUNC_READ_HOLDING_REGISTERS = 0x03, + MODBUS_FUNC_READ_INPUT_REGISTERS = 0x04, + MODBUS_FUNC_WRITE_SINGLE_COIL = 0x05, + MODBUS_FUNC_WRITE_SINGLE_REGISTER = 0x06, + MODBUS_FUNC_READ_EXCEPTION_STATUS = 0x07, + MODBUS_FUNC_DIAGNOSTICS = 0x08, + MODBUS_FUNC_GET_COMM_EVENT_COUNTER = 0x0b, + MODBUS_FUNC_GET_COMM_EVENT_LOG = 0x0c, + MODBUS_FUNC_WRITE_MULTIPLE_COILS = 0x0f, + MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS = 0x10, + MODBUS_FUNC_REPORT_SLAVE_ID = 0x11, + MODBUS_FUNC_READ_FILE_RECORD = 0x14, + MODBUS_FUNC_WRITE_FILE_RECORD = 0x15, + MODBUS_FUNC_MASK_WRITE_REGISTER = 0x16, + MODBUS_FUNC_READ_WRITE_MULTIPLE_REGISTERS = 0x17, + MODBUS_FUNC_READ_FIFO_QUEUE = 0x18, + MODBUS_FUNC_ENCAPSULATED_INTERFACE_TRANSPORT = 0x2b, +}; + +enum modbus_error { + MODBUS_ERR_ILLEGAL_FUNCTION = 0x01, + MODBUS_ERR_ILLEGAL_DATA_ADDRESS = 0x02, + MODBUS_ERR_ILLEGAL_DATA_VALUE = 0x03, + MODBUS_ERR_SLAVE_DEVICE_FAILURE = 0x04, + MODBUS_ERR_ACKNOWLEDGE = 0x05, + MODBUS_ERR_SLAVE_DEVICE_BUSY = 0x06, + MODBUS_ERR_MEMORY_PARITY_ERROR = 0x08, + MODBUS_ERR_GATEWAY_PATH_UNAVAILABLE = 0x0a, + MODBUS_ERR_GATEWAY_TARGET_DEVICE_FAILED = 0x0b, +}; + +enum modbus_encapsulated_interface_transport_type { + MODBUS_EIT_CANOPEN = 0x0d, + MODBUS_EIT_READ_DEVICE_IDENT = 0x0e, +}; diff --git a/ucw-stm32lib/lib/modbus.c b/ucw-stm32lib/lib/modbus.c new file mode 100644 index 0000000..ab8156c --- /dev/null +++ b/ucw-stm32lib/lib/modbus.c @@ -0,0 +1,767 @@ +/* + * Generic MODBUS Library for STM32 + * + * (c) 2019--2023 Martin Mareš + */ + +#include "util.h" +#include "modbus.h" +#include "modbus-proto.h" + +#include +#include + +#include +#include +#include +#include +#include + +/*** Configuration ***/ + +// You should set the following parameters in config.h + +// USART (pins are expected to be configured by the caller) +// #define MODBUS_USART USART2 +// #define MODBUS_NVIC_USART_IRQ NVIC_USART2_IRQ +// #define MODBUS_USART_ISR usart2_isr + +// GPIO pin for transmitter enable (pins is expected to be configured by the caller) +// #define MODBUS_TXEN_GPIO_PORT GPIOA +// #define MODBUS_TXEN_GPIO_PIN GPIO1 + +// Timer +// #define MODBUS_TIMER TIM2 +// #define MODBUS_NVIC_TIMER_IRQ NVIC_TIM2_IRQ +// #define MODBUS_TIMER_ISR tim2_isr + +// Slave address we are responding at +// #define MODBUS_OUR_ADDRESS 42 + +// Baud rate +#ifndef MODBUS_BAUD_RATE +#define MODBUS_BAUD_RATE 19200 +#endif + +// CPU clock frequency +// #define CPU_CLOCK_MHZ 72 + +// Receive buffer size (standard specifies 256 bytes, you can make it shorter if necessary) +#ifndef MODBUS_RX_BUFSIZE +#define MODBUS_RX_BUFSIZE 256 +#endif + +// Transmit buffer size (standard specifies 256 bytes, you can make it shorter if necessary) +#ifndef MODBUS_TX_BUFSIZE +#define MODBUS_TX_BUFSIZE 256 +#endif + +// Receive timeout in microseconds +#ifndef MODBUS_RX_TIMEOUT +#if MODBUS_BAUD_RATE <= 19200 +// For low baud rates, the standard specifies timeout of 1.5 character times +// (1 character = start bit + 8 data bits + parity bit + stop bit = 11 bits) +#define MODBUS_RX_TIMEOUT (1000000*11*3/2/MODBUS_BAUD_RATE) +#else +// For high rates, the timeout is fixed to 750 μs +#define MODBUS_RX_TIMEOUT 750 +#endif +#endif + +// Inter-frame gap in microseconds +#ifndef MODBUS_RX_GAP +#if MODBUS_BAUD_RATE <= 19200 +// For low baud rates, the standard specifies 3.5 character times +#define MODBUS_RX_GAP (1000000*11*7/2/MODBUS_BAUD_RATE) +#else +// For high rates, the gap is fixed to 1750 μs +#define MODBUS_RX_GAP 1750 +#endif +#endif + +// Debugging +// #define MODBUS_DEBUG +// #define MODBUS_DEBUG_ISR + +#ifdef MODBUS_DEBUG +#define DEBUG debug_printf +#else +#define DEBUG(xxx, ...) do { } while (0) +#endif + +#ifdef MODBUS_DEBUG_ISR +#define DEBUG_ISR(c) debug_putc(c) +#else +#define DEBUG_ISR(c) do { } while (0) +#endif + +/*** State ***/ + +enum mb_state { + STATE_RX, + STATE_RX_DONE, + STATE_PROCESSING, + STATE_GAP, + STATE_TX, + STATE_TX_LAST, + STATE_TX_DONE, +}; + +static byte rx_buf[MODBUS_RX_BUFSIZE]; +static u16 rx_size; +static byte rx_bad; +static byte state; // STATE_xxx + +static byte *rx_frame; +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); + +/*** Low-level layer ***/ + +static void rx_init(void) +{ + DEBUG_ISR('<'); + state = STATE_RX; + rx_size = 0; + 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) +{ + DEBUG_ISR('>'); + state = STATE_RX_DONE; + usart_disable_rx_interrupt(MODBUS_USART); +} + +static void tx_gap_init(void) +{ + timer_set_period(MODBUS_TIMER, (MODBUS_RX_GAP > MODBUS_RX_TIMEOUT ? MODBUS_RX_GAP - MODBUS_RX_TIMEOUT : 1)); + timer_generate_event(MODBUS_TIMER, TIM_EGR_UG); + timer_enable_counter(MODBUS_TIMER); +} + +static void tx_init(void) +{ + DEBUG_ISR('['); + state = STATE_TX; + tx_pos = 0; + gpio_set(MODBUS_TXEN_GPIO_PORT, MODBUS_TXEN_GPIO_PIN); + usart_set_mode(MODBUS_USART, USART_MODE_TX); + usart_enable_tx_interrupt(MODBUS_USART); +} + +static void tx_done(void) +{ + state = STATE_TX_DONE; + // usart_disable_tx_interrupt(MODBUS_USART); // Already done by irq handler + gpio_clear(MODBUS_TXEN_GPIO_PORT, MODBUS_TXEN_GPIO_PIN); +} + +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); + timer_disable_preload(MODBUS_TIMER); + timer_one_shot_mode(MODBUS_TIMER); + timer_enable_irq(MODBUS_TIMER, TIM_DIER_UIE); + nvic_enable_irq(MODBUS_NVIC_TIMER_IRQ); + + gpio_clear(MODBUS_TXEN_GPIO_PORT, MODBUS_TXEN_GPIO_PIN); + + usart_set_baudrate(MODBUS_USART, MODBUS_BAUD_RATE); + usart_set_databits(MODBUS_USART, 9); + usart_set_stopbits(MODBUS_USART, USART_STOPBITS_1); + usart_set_parity(MODBUS_USART, USART_PARITY_EVEN); + usart_set_flow_control(MODBUS_USART, USART_FLOWCONTROL_NONE); + + rx_init(); + + nvic_enable_irq(MODBUS_NVIC_USART_IRQ); + usart_enable(MODBUS_USART); +} + +void MODBUS_USART_ISR(void) +{ + u32 status = USART_SR(MODBUS_USART); + + if (status & USART_SR_RXNE) { + uint ch = usart_recv(MODBUS_USART); + if (state == STATE_RX) { + if (status & (USART_SR_FE | USART_SR_ORE | USART_SR_NE)) { + DEBUG_ISR('!'); + rx_bad = 1; + } else if (rx_size < MODBUS_RX_BUFSIZE) { + DEBUG_ISR('.'); + if (!rx_size) + modbus_frame_start_hook(); + rx_buf[rx_size++] = ch; + } else { + // Frame too long + DEBUG_ISR('#'); + rx_bad = 2; + } + timer_set_period(MODBUS_TIMER, MODBUS_RX_TIMEOUT); + timer_generate_event(MODBUS_TIMER, TIM_EGR_UG); + timer_enable_counter(MODBUS_TIMER); + } + } + + if (state == STATE_TX) { + if (status & USART_SR_TXE) { + if (tx_pos < tx_size) { + usart_send(MODBUS_USART, tx_buf[tx_pos++]); + DEBUG_ISR(':'); + } 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(MODBUS_USART); + USART_CR1(MODBUS_USART) |= USART_CR1_TCIE; + state = STATE_TX_LAST; + DEBUG_ISR(']'); + } + } + } else if (state == STATE_TX_LAST) { + if (status & USART_SR_TC) { + // Transfer of the last byte is complete. Release the bus. + USART_CR1(MODBUS_USART) &= ~USART_CR1_TCIE; + tx_done(); + rx_init(); + } + } +} + +void MODBUS_TIMER_ISR(void) +{ + if (TIM_SR(MODBUS_TIMER) & TIM_SR_UIF) { + TIM_SR(MODBUS_TIMER) &= ~TIM_SR_UIF; + if (state == STATE_RX) + rx_done(); + else if (state == STATE_GAP) + tx_init(); + } +} + +void modbus_loop(void) +{ + if (state != STATE_RX_DONE) + return; + state = STATE_PROCESSING; + + if (!check_frame()) { + rx_init(); + 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 + tx_gap_init(); + process_frame(); + DEBUG("MODBUS: > status=%02x len=%u\n", tx_buf[1], tx_size); + CM_ATOMIC_BLOCK() { + if (TIM_CR1(MODBUS_TIMER) & TIM_CR1_CEN) { + // The timer is still running, so let it handle the start of transmission. + // Even if it expires just now, the interrupt is deferred. + state = STATE_GAP; + } + } + if (state == STATE_PROCESSING) { + // Interrupt already expired, so fire up transmission from here. + 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(); + } +} + +/** CRC ***/ + +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); +} + +/*** High-level layer ***/ + +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; + } + + 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? + DEBUG("MODBUS: Bad CRC\n"); + return false; + } + + rx_frame = rx_buf + 1; + rx_frame_end = rx_frame + rx_size - 2; + return true; +} + +static uint read_remains(void) +{ + return rx_frame_end - rx_frame; +} + +static byte read_byte(void) +{ + return *rx_frame++; +} + +static u16 read_u16(void) +{ + byte hi = *rx_frame++; + byte lo = *rx_frame++; + return (hi << 8) | lo; +} + +static void write_byte(byte v) +{ + tx_buf[tx_size++] = v; +} + +static void write_u16(u16 v) +{ + write_byte(v >> 8); + write_byte(v); +} + +static bool body_fits(uint body_len) +{ + // body_len excludes slave address, function code, and CRC + return (2 + body_len + 2 <= MODBUS_TX_BUFSIZE); +} + +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 func_read_bits(bool coils) +{ + if (read_remains() < 4) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + + uint bytes = (count+7) / 8; + if (!body_fits(1 + bytes)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!(coils ? modbus_check_coil : modbus_check_discrete_input)(start + i)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + write_byte(bytes); + for (u16 i = 0; i < bytes; i++) { + byte b = 0; + for (byte j = 0; j < 8 && 8*i + j < count; j++) { + uint addr = start + 8*i + j; + if ((coils ? modbus_get_coil : modbus_get_discrete_input)(addr)) + b |= 1 << j; + } + write_byte(b); + } +} + +static void func_read_registers(byte holding) +{ + if (read_remains() < 4) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + + uint bytes = 2*count; + if (!body_fits(1 + bytes)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!(holding ? modbus_check_holding_register : modbus_check_input_register)(start + i)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + // FIXME: Reporting of slave failures? + write_byte(bytes); + for (u16 i = 0; i < count; i++) + write_u16((holding ? modbus_get_holding_register : modbus_get_input_register)(start + i)); +} + +static void func_write_single_coil(void) +{ + if (read_remains() < 4) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 addr = read_u16(); + u16 value = read_u16(); + + if (!modbus_check_coil(addr)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + if (value != 0x0000 && value != 0xff00) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + modbus_set_coil(addr, value); + + write_u16(addr); + write_u16(value); +} + +static void func_write_single_register(void) +{ + if (read_remains() < 4) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 addr = read_u16(); + u16 value = read_u16(); + + if (!modbus_check_holding_register(addr)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + modbus_set_holding_register(addr, value); + + write_u16(addr); + write_u16(value); +} + +static void func_write_multiple_coils(void) +{ + if (read_remains() < 5) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + byte bytes = read_byte(); + + if (read_remains() < bytes || bytes != (count+7) / 8) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!modbus_check_coil(start + i)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + 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) +{ + if (read_remains() < 5) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + byte bytes = read_byte(); + + if (read_remains() < bytes || bytes != 2*count) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!modbus_check_holding_register(start + i)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + 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) +{ + if (read_remains() < 6) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 addr = read_u16(); + u16 and_mask = read_u16(); + u16 or_mask = read_u16(); + + if (!modbus_check_holding_register(addr)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + 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) +{ + if (read_remains() < 9) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + u16 read_start = read_u16(); + u16 read_count = read_u16(); + u16 write_start = read_u16(); + u16 write_count = read_u16(); + byte write_bytes = read_byte(); + + if (read_remains() < write_bytes || write_bytes != 2*write_count) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < read_count; i++) + if (!modbus_check_holding_register(read_start + i)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + for (u16 i = 0; i < write_count; i++) + if (!modbus_check_holding_register(write_start + i)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + + byte read_bytes = 2*write_count; + if (!body_fits(1 + read_bytes)) + return report_error(MODBUS_ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < write_count; i++) + modbus_set_holding_register(write_start + i, read_u16()); + + write_byte(read_bytes); + for (u16 i = 0; i < read_count; i++) + modbus_get_holding_register(read_start + i); +} + +static void func_encapsulated_interface_transport(void) +{ + if (read_remains() < 3 || + read_byte() != MODBUS_EIT_READ_DEVICE_IDENT) + return report_error(MODBUS_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(MODBUS_ERR_ILLEGAL_DATA_ADDRESS); + range_min = range_max = id; + break; + default: + return report_error(MODBUS_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 = MODBUS_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(); + + // Prepare reply frame + tx_buf[0] = MODBUS_OUR_ADDRESS; + tx_buf[1] = rx_buf[1]; + tx_size = 2; + pending_error = 0; + + switch (func) { + case MODBUS_FUNC_READ_COILS: + func_read_bits(true); + break; + case MODBUS_FUNC_READ_DISCRETE_INPUTS: + func_read_bits(false); + break; + case MODBUS_FUNC_READ_HOLDING_REGISTERS: + func_read_registers(true); + break; + case MODBUS_FUNC_READ_INPUT_REGISTERS: + func_read_registers(false); + break; + case MODBUS_FUNC_WRITE_SINGLE_COIL: + func_write_single_coil(); + break; + case MODBUS_FUNC_WRITE_SINGLE_REGISTER: + func_write_single_register(); + break; + case MODBUS_FUNC_WRITE_MULTIPLE_COILS: + func_write_multiple_coils(); + break; + case MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS: + func_write_multiple_registers(); + break; + case MODBUS_FUNC_MASK_WRITE_REGISTER: + func_mask_write_register(); + break; + case MODBUS_FUNC_READ_WRITE_MULTIPLE_REGISTERS: + func_read_write_multiple_registers(); + break; + case MODBUS_FUNC_ENCAPSULATED_INTERFACE_TRANSPORT: + func_encapsulated_interface_transport(); + break; + default: + report_error(MODBUS_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 = MODBUS_ERR_SLAVE_DEVICE_FAILURE; +} + +bool modbus_is_idle(void) +{ + return state == STATE_RX && !rx_size; +} diff --git a/ucw-stm32lib/lib/modbus.h b/ucw-stm32lib/lib/modbus.h new file mode 100644 index 0000000..c4a6d53 --- /dev/null +++ b/ucw-stm32lib/lib/modbus.h @@ -0,0 +1,49 @@ +/* + * Generic MODBUS Library for STM32 + * + * (c) 2019--2023 Martin Mareš + */ + +#ifndef _MODBUS_H +#define _MODBUS_H + +void modbus_init(void); +void modbus_loop(void); + +// If a call-back wants to signal a slave error in the reply +void modbus_slave_error(void); + +bool modbus_is_idle(void); + +// Callbacks + +bool modbus_check_discrete_input(u16 addr); +bool modbus_get_discrete_input(u16 addr); + +bool modbus_check_coil(u16 addr); +bool modbus_get_coil(u16 addr); +void modbus_set_coil(u16 addr, bool value); + +bool modbus_check_input_register(u16 addr); +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); + +void modbus_ready_hook(void); +void modbus_frame_start_hook(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, +}; + +extern const char * const modbus_id_strings[MODBUS_ID_MAX]; + +#endif diff --git a/ucw-stm32lib/lib/util-debug.c b/ucw-stm32lib/lib/util-debug.c new file mode 100644 index 0000000..d0914a6 --- /dev/null +++ b/ucw-stm32lib/lib/util-debug.c @@ -0,0 +1,223 @@ +/* + * Debugging Utilities for STM32 + * + * (c) 2018--2019 Martin Mareš + */ + +#include "util.h" + +#include +#include + +#include +#include + +/*** Configuration ***/ + +// You should set the following parameters in config.h + +// Use the semi-hosting interface for debugging messages +// #define DEBUG_SEMIHOSTING + +// Use this USART for debugging messages +// #define DEBUG_USART USART1 + +// Use this LED for debugging +#ifdef DEBUG_LED_BLUEPILL +#define DEBUG_LED_GPIO GPIOC +#define DEBUG_LED_PIN GPIO13 +#define DEBUG_LED_INVERSE +#endif + +/*** Implementation ***/ + +#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(DEBUG_USART, '\r'); + usart_send_blocking(DEBUG_USART, c); +#endif +} + +void debug_flush(void) +{ +#ifdef DEBUG_USART + while (!usart_get_flag(DEBUG_USART, USART_FLAG_TC)) + ; +#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 'c': + debug_putc(va_arg(args, int)); + break; + 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); +} + +void debug_led(bool light) +{ +#ifdef DEBUG_LED_GPIO +#ifdef DEBUG_LED_INVERSE + light = !light; +#endif + if (light) + gpio_set(DEBUG_LED_GPIO, DEBUG_LED_PIN); + else + gpio_clear(DEBUG_LED_GPIO, DEBUG_LED_PIN); +#endif +} + +void debug_led_toggle(void) +{ +#ifdef DEBUG_LED_GPIO + gpio_toggle(DEBUG_LED_GPIO, DEBUG_LED_PIN); +#endif +} diff --git a/ucw-stm32lib/lib/util.h b/ucw-stm32lib/lib/util.h new file mode 100644 index 0000000..18e3652 --- /dev/null +++ b/ucw-stm32lib/lib/util.h @@ -0,0 +1,103 @@ +/* + * General Utility Library for STM32 + * + * (c) 2018--2019 Martin Mareš + */ + +#include +#include +#include + +#include "config.h" + +// Types + +typedef unsigned int uint; +typedef uint8_t byte; +typedef uint16_t u16; +typedef int16_t s16; +typedef uint32_t u32; +typedef int32_t s32; + +// Macros + +#define MIN(x,y) ((x) < (y) ? (x) : (y)) +#define MAX(x,y) ((x) > (y) ? (x) : (y)) +#define CLAMP(x,min,max) ({ typeof(x) _t=x; (_t < min) ? min : (_t > max) ? max : _t; }) +#define ARRAY_SIZE(ary) (sizeof(ary)/sizeof((ary)[0])) + +#define UNUSED __attribute__((unused)) + +// Unaligned access to data + +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; +} + +// CPU instructions not covered by libopencm3 + +static inline void wait_for_interrupt(void) +{ + asm volatile ("wfi"); +} + +// A compiler memory barrier + +static inline void barrier(void) +{ + asm volatile ("" : : : "memory"); +} + +// util-debug.c + +void debug_printf(const char *fmt, ...) __attribute__((format(printf,1,2))); +void debug_puts(const char *s); +void debug_putc(int c); +void debug_flush(void); + +void debug_led(bool light); +void debug_led_toggle(void); diff --git a/ucw-stm32lib/mk/bluepill.mk b/ucw-stm32lib/mk/bluepill.mk new file mode 100644 index 0000000..de32aa5 --- /dev/null +++ b/ucw-stm32lib/mk/bluepill.mk @@ -0,0 +1,173 @@ +# Generic Makefile fragment for Blue Pill devices and LibOpenCM3 +# +# Parameters: +# +# STM32LIB path to root of the stm32lib +# OPENCM3_DIR path to root of libopencm3 +# BINARY binary to build (without extension) +# OBJS list of object files +# LIB_OBJS list of library object files +# WITH_BOOT_LOADER define if code origin should be shifted by 8K for boot loader +# WITH_DFU_FLASH flashing uses dfu-util +# WITH_SERIAL_FLASH flashing uses the built-in serial boot-loader +# MAX_SIZE complain if the built firmware exceeds this size +# + +vpath %.c $(STM32LIB)/lib +OBJS += $(LIB_OBJS) + +DEVICE?=stm32f103x8 + +.PHONY: all +all:: $(BINARY).elf + +.PHONY: flash +flash: $(BINARY).flash + +ifneq ($(V),1) +Q := @ +NULL := 2>/dev/null +endif + +include $(OPENCM3_DIR)/mk/genlink-config.mk + +ifdef WITH_BOOT_LOADER + +# We want to generate a linked script for a different ROM start address +UCW_LDSCRIPT=bootloader-$(DEVICE).ld + +bootloader-$(DEVICE).ld: generated.$(DEVICE).ld + @printf " GENLNK2 $@\n" + $(Q)sed '/^ rom /s/ORIGIN = 0x08000000/ORIGIN = 0x08002000/' <$< >$@ + +else + +UCW_LDSCRIPT=$(LDSCRIPT) + +endif + +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=gnu99 + +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 -Wno-parentheses +TGT_CFLAGS += -fno-common -ffunction-sections -fdata-sections +TGT_CFLAGS += -I. -I$(STM32LIB)/lib + +TGT_CPPFLAGS += -MD + +TGT_LDFLAGS += --static -nostartfiles +TGT_LDFLAGS += -T$(UCW_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 $< -> $@\n" + $(Q)$(OBJCOPY) -Obinary $< $@ +ifdef MAX_SIZE + $(Q)if [ $$(stat -c '%s' $@) -gt $(MAX_SIZE) ] ; then echo >&2 "Output too exceeds $(MAX_SIZE) bytes!" ; false ; fi +endif + +%.elf: $(OBJS) $(UCW_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 $@ -c $< + +.PHONY: clean +clean: + @printf " CLEAN\n" + $(Q)rm -f *.elf *.bin *.dfu *.o *.d *.map $(LDSCRIPT) $(UCW_LDSCRIPT) + +ifdef WITH_DFU_FLASH +HAVE_FLASH := 1 + +all:: $(BINARY).dfu + +%.flash: %.dfu + @printf " FLASH $<\n" + $(Q)dfu-util $(DFU_ARGS) -D $< + +# For the STM32duino-bootloader, we used: +#%.flash: %.bin +# @printf " FLASH $<\n" +# $(Q)dfu-util -a2 -D $(*).bin + +endif + +ifdef WITH_SERIAL_FLASH +HAVE_FLASH := 1 + +all:: $(BINARY).bin + +BOOT_SERIAL ?= /dev/ttyUSB0 + +%.flash: %.bin + @printf " FLASH $<\n" + $(Q)stm32flash $(BOOT_SERIAL) -i 'dtr,-dtr' -w $< -g 0 + +.PHONY: reset +reset: all + $(Q)stm32flash $(BOOT_SERIAL) -i 'dtr,-dtr' -g 0 + +endif + +ifdef WITH_MODBUS_FLASH +HAVE_FLASH := 1 + +all:: $(BINARY).dfu + +%.flash: %.dfu + @printf " FLASH $<\n" + $(Q)$(STM32LIB)/tools/modbus-flash $(MODBUS_FLASH_ARGS) --flash $< + +endif + +ifndef HAVE_FLASH + +all:: $(BINARY).bin + +%.flash: %.bin + @printf " FLASH $<\n" + $(Q)st-flash write $(*).bin 0x8000000 + +.PHONY: reset +reset: + st-flash reset + +endif + +%.dfu: %.bin $(STM32LIB)/tools/dfu-sign + @printf " SIGN $< -> $@\n" + $(Q)$(STM32LIB)/tools/dfu-sign $< $@ + +$(STM32LIB)/tools/dfu-sign: + make -C $(STM32LIB)/tools + +.SECONDEXPANSION: +.SECONDARY: + +-include $(OBJS:.o=.d) diff --git a/ucw-stm32lib/tools/Makefile b/ucw-stm32lib/tools/Makefile new file mode 100644 index 0000000..b4cc1f0 --- /dev/null +++ b/ucw-stm32lib/tools/Makefile @@ -0,0 +1,13 @@ +PC=pkg-config +UCW_CFLAGS := $(shell $(PC) --cflags libucw) +UCW_LDLIBS := $(shell $(PC) --libs libucw) + +CFLAGS=$(UCW_CFLAGS) -O2 -std=gnu99 -Wall -Wextra -Wno-parentheses -Wstrict-prototypes -Wmissing-prototypes +LDLIBS=$(UCW_LDLIBS) -lz + +all: dfu-sign + +dfu-sign: dfu-sign.c + +clean: + rm -f dfu-sign *.o diff --git a/ucw-stm32lib/tools/dfu-sign.c b/ucw-stm32lib/tools/dfu-sign.c new file mode 100644 index 0000000..5e0c514 --- /dev/null +++ b/ucw-stm32lib/tools/dfu-sign.c @@ -0,0 +1,245 @@ +/* + * Sign firmware for our DFU boot-loader + * + * (c) 2020--2023 Martin Mareš + */ + +#include +#include +#include +#include +#include +#include +#include + +static char *in_name; +static char *out_name; + +static uint id_vendor = 0xffff; +static uint id_device = 0xffff; +static uint id_fw_ver = 0xffff; + +static const struct opt_section options = { + OPT_ITEMS { + OPT_HELP("A simple tool for signing firmware for UCW STM32 DFU bootloader."), + OPT_HELP("Usage: dfu-sign [options] "), + OPT_STRING(OPT_POSITIONAL(1), NULL, in_name, OPT_REQUIRED, ""), + OPT_STRING(OPT_POSITIONAL(2), NULL, out_name, OPT_REQUIRED, ""), + OPT_HELP(""), + OPT_HELP("Options:"), + OPT_UINT(0, "vendor", id_vendor, OPT_REQUIRED_VALUE, "0xid\tvendor ID"), + OPT_UINT(0, "device", id_device, OPT_REQUIRED_VALUE, "0xid\tdevice ID"), + OPT_UINT(0, "fw-version", id_fw_ver, OPT_REQUIRED_VALUE, "0xver\tfirmware version"), + OPT_HELP_OPTION, + OPT_END + } +}; + +struct fw_header { + // The header lives in unused space between interrupt vectors + byte vectors[0x1c]; + u32 length; // Firmware length in bytes, divisible by 4 + // (excluding CRC appended at the end) + u16 flash_in_progress; // Temporarily non-zero during flashing + u16 rfu1; + u32 rfu2; + u32 rfu3; +}; + +struct dfu_trailer { + u16 fw_version; + u16 product_id; + u16 vendor_id; + u16 dfu_version; + byte dfu_sig[3]; + byte trailer_len; + u32 crc; // CRC of the whole file except this field +}; + +static byte *firmware; +static uint firmware_len; // Without CRC and trailer +static struct fw_header *header; +static struct dfu_trailer *trailer; + +/*** CRC used in DFU ***/ + +static const u32 crc32_table[256] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, + 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, + 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2, + 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, + 0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, + 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c, + 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, + 0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, + 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106, + 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, + 0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, + 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950, + 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, + 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, + 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa, + 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, + 0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, + 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84, + 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, + 0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, + 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e, + 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, + 0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, + 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28, + 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, + 0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, + 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242, + 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, + 0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, + 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc, + 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, + 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, + 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d}; + +static u32 dfu_update_crc(u32 crc, byte x) +{ + return crc32_table[(crc ^ x) & 0xff] ^ (crc >> 8); +} + +static u32 dfu_crc(byte *buf, uint len) +{ + u32 crc = 0xffffffff; + for (uint i = 0; i < len; i++) + crc = dfu_update_crc(crc, buf[i]); + return crc; +} + +/*** CRC implemented in STM32F1 hardware ***/ + +static const u32 stm_crc_table[256] = { + 0x00000000, 0x04c11db7, 0x09823b6e, 0x0d4326d9, 0x130476dc, 0x17c56b6b, 0x1a864db2, 0x1e475005, + 0x2608edb8, 0x22c9f00f, 0x2f8ad6d6, 0x2b4bcb61, 0x350c9b64, 0x31cd86d3, 0x3c8ea00a, 0x384fbdbd, + 0x4c11db70, 0x48d0c6c7, 0x4593e01e, 0x4152fda9, 0x5f15adac, 0x5bd4b01b, 0x569796c2, 0x52568b75, + 0x6a1936c8, 0x6ed82b7f, 0x639b0da6, 0x675a1011, 0x791d4014, 0x7ddc5da3, 0x709f7b7a, 0x745e66cd, + 0x9823b6e0, 0x9ce2ab57, 0x91a18d8e, 0x95609039, 0x8b27c03c, 0x8fe6dd8b, 0x82a5fb52, 0x8664e6e5, + 0xbe2b5b58, 0xbaea46ef, 0xb7a96036, 0xb3687d81, 0xad2f2d84, 0xa9ee3033, 0xa4ad16ea, 0xa06c0b5d, + 0xd4326d90, 0xd0f37027, 0xddb056fe, 0xd9714b49, 0xc7361b4c, 0xc3f706fb, 0xceb42022, 0xca753d95, + 0xf23a8028, 0xf6fb9d9f, 0xfbb8bb46, 0xff79a6f1, 0xe13ef6f4, 0xe5ffeb43, 0xe8bccd9a, 0xec7dd02d, + 0x34867077, 0x30476dc0, 0x3d044b19, 0x39c556ae, 0x278206ab, 0x23431b1c, 0x2e003dc5, 0x2ac12072, + 0x128e9dcf, 0x164f8078, 0x1b0ca6a1, 0x1fcdbb16, 0x018aeb13, 0x054bf6a4, 0x0808d07d, 0x0cc9cdca, + 0x7897ab07, 0x7c56b6b0, 0x71159069, 0x75d48dde, 0x6b93dddb, 0x6f52c06c, 0x6211e6b5, 0x66d0fb02, + 0x5e9f46bf, 0x5a5e5b08, 0x571d7dd1, 0x53dc6066, 0x4d9b3063, 0x495a2dd4, 0x44190b0d, 0x40d816ba, + 0xaca5c697, 0xa864db20, 0xa527fdf9, 0xa1e6e04e, 0xbfa1b04b, 0xbb60adfc, 0xb6238b25, 0xb2e29692, + 0x8aad2b2f, 0x8e6c3698, 0x832f1041, 0x87ee0df6, 0x99a95df3, 0x9d684044, 0x902b669d, 0x94ea7b2a, + 0xe0b41de7, 0xe4750050, 0xe9362689, 0xedf73b3e, 0xf3b06b3b, 0xf771768c, 0xfa325055, 0xfef34de2, + 0xc6bcf05f, 0xc27dede8, 0xcf3ecb31, 0xcbffd686, 0xd5b88683, 0xd1799b34, 0xdc3abded, 0xd8fba05a, + 0x690ce0ee, 0x6dcdfd59, 0x608edb80, 0x644fc637, 0x7a089632, 0x7ec98b85, 0x738aad5c, 0x774bb0eb, + 0x4f040d56, 0x4bc510e1, 0x46863638, 0x42472b8f, 0x5c007b8a, 0x58c1663d, 0x558240e4, 0x51435d53, + 0x251d3b9e, 0x21dc2629, 0x2c9f00f0, 0x285e1d47, 0x36194d42, 0x32d850f5, 0x3f9b762c, 0x3b5a6b9b, + 0x0315d626, 0x07d4cb91, 0x0a97ed48, 0x0e56f0ff, 0x1011a0fa, 0x14d0bd4d, 0x19939b94, 0x1d528623, + 0xf12f560e, 0xf5ee4bb9, 0xf8ad6d60, 0xfc6c70d7, 0xe22b20d2, 0xe6ea3d65, 0xeba91bbc, 0xef68060b, + 0xd727bbb6, 0xd3e6a601, 0xdea580d8, 0xda649d6f, 0xc423cd6a, 0xc0e2d0dd, 0xcda1f604, 0xc960ebb3, + 0xbd3e8d7e, 0xb9ff90c9, 0xb4bcb610, 0xb07daba7, 0xae3afba2, 0xaafbe615, 0xa7b8c0cc, 0xa379dd7b, + 0x9b3660c6, 0x9ff77d71, 0x92b45ba8, 0x9675461f, 0x8832161a, 0x8cf30bad, 0x81b02d74, 0x857130c3, + 0x5d8a9099, 0x594b8d2e, 0x5408abf7, 0x50c9b640, 0x4e8ee645, 0x4a4ffbf2, 0x470cdd2b, 0x43cdc09c, + 0x7b827d21, 0x7f436096, 0x7200464f, 0x76c15bf8, 0x68860bfd, 0x6c47164a, 0x61043093, 0x65c52d24, + 0x119b4be9, 0x155a565e, 0x18197087, 0x1cd86d30, 0x029f3d35, 0x065e2082, 0x0b1d065b, 0x0fdc1bec, + 0x3793a651, 0x3352bbe6, 0x3e119d3f, 0x3ad08088, 0x2497d08d, 0x2056cd3a, 0x2d15ebe3, 0x29d4f654, + 0xc5a92679, 0xc1683bce, 0xcc2b1d17, 0xc8ea00a0, 0xd6ad50a5, 0xd26c4d12, 0xdf2f6bcb, 0xdbee767c, + 0xe3a1cbc1, 0xe760d676, 0xea23f0af, 0xeee2ed18, 0xf0a5bd1d, 0xf464a0aa, 0xf9278673, 0xfde69bc4, + 0x89b8fd09, 0x8d79e0be, 0x803ac667, 0x84fbdbd0, 0x9abc8bd5, 0x9e7d9662, 0x933eb0bb, 0x97ffad0c, + 0xafb010b1, 0xab710d06, 0xa6322bdf, 0xa2f33668, 0xbcb4666d, 0xb8757bda, 0xb5365d03, 0xb1f740b4, +}; + +static u32 stm_update_crc(u32 crc, byte x) +{ + return (crc << 8) ^ stm_crc_table[0xff & ((crc >> 24) ^ x)]; +} + +static u32 stm32_crc(byte *buf, uint len) +{ + u32 crc = 0xffffffff; + for (uint i = 0; i < len; i += 4) { + crc = stm_update_crc(crc, buf[i+3]); + crc = stm_update_crc(crc, buf[i+2]); + crc = stm_update_crc(crc, buf[i+1]); + crc = stm_update_crc(crc, buf[i+0]); + } + return crc; +} + +static void read_fw(void) +{ + int fd = open(in_name, O_RDONLY); + if (fd < 0) + die("Cannot open %s: %m", in_name); + + uint orig_firmware_len = lseek(fd, 0, SEEK_END); + lseek(fd, 0, SEEK_SET); + firmware_len = orig_firmware_len; + while (firmware_len % 4) + firmware_len++; + + firmware = xmalloc_zero(firmware_len + 4 + sizeof(struct dfu_trailer)); + header = (struct fw_header *)firmware; + trailer = (struct dfu_trailer *)(firmware + firmware_len + 4); + + if (read(fd, firmware, orig_firmware_len) != (int) orig_firmware_len) + die("Error reading %s: %m", in_name); + + close(fd); +} + +static void build_header_and_crc(void) +{ + put_u32_le(&header->length, firmware_len); + header->rfu1 = 0; + header->rfu2 = 0; + header->rfu3 = 0; + + put_u32_le(firmware + firmware_len, stm32_crc(firmware, firmware_len)); +} + +static void build_trailer(void) +{ + struct dfu_trailer *t = trailer; + + t->trailer_len = sizeof(struct dfu_trailer); + memcpy(t->dfu_sig, "UFD", 3); + put_u16_le(&t->dfu_version, 0x0100); + put_u16_le(&t->vendor_id, id_vendor); + put_u16_le(&t->product_id, id_device); + put_u16_le(&t->fw_version, id_fw_ver); + put_u32_le(&t->crc, dfu_crc(firmware, firmware_len + 4 + t->trailer_len - 4)); +} + +static void write_out(void) +{ + int fd = open(out_name, O_WRONLY | O_CREAT | O_TRUNC, 0666); + if (fd < 0) + die("Cannot create %s: %m", out_name); + + int len = firmware_len + 4 + sizeof(struct dfu_trailer); + if (write(fd, firmware, len) != len) + die("Error writing %s: %m", out_name); + + close(fd); +} + +int main(int argc UNUSED, char **argv) +{ + opt_parse(&options, argv+1); + read_fw(); + build_header_and_crc(); + build_trailer(); + write_out(); + + return 0; +}