From: Martin Mares Date: Sun, 20 Feb 2022 15:26:03 +0000 (+0100) Subject: Indicators: Renamed to Rainbow X-Git-Url: http://mj.ucw.cz/gitweb/?a=commitdiff_plain;h=0677e7ad6779bb1d285b1d537df14446f62b4021;p=home-hw.git Indicators: Renamed to Rainbow --- diff --git a/USB-IDS b/USB-IDS index 591f21b..cd3428a 100644 --- a/USB-IDS +++ b/USB-IDS @@ -8,7 +8,7 @@ USB IDs used by our gadgets 4242:0006 DMX512 interface bootloader 4242:0007 Test gadget 4242:0008 Test gadget bootloader -4242:0009 Neopixel indicators -4242:000a Neopixel indicators bootloader +4242:0009 Neopixel rainbow +4242:000a Neopixel rainbow bootloader cafe:cafe KSP Space Alert thermometer cafe:caff KSP Space Alert accelerometer diff --git a/indicators/README b/indicators/README deleted file mode 100644 index 203357f..0000000 --- a/indicators/README +++ /dev/null @@ -1,30 +0,0 @@ -Assignment of peripherals and pins -================================== - -USART1 debugging -TIM4 Neopixel control - - - Blue Pill pinout - +--------------------+ - | VBATT 3.3V | -BluePill LED | PC13 GND | Neopixel ground - | PC14 5V | Neopixel power - | PC15 PB9 | - | PA0 PB8 | TIM4_CH3 - Neopixel data (needs pull-up) - | PA1 PB7 | - | PA2 PB6 | - | PA3 PB5 | - | PA4 PB4 | - | PA5 PB3 | - | PA6 PA15 | - | PA7 PA12 | - | PB0 PA11 | - | PB1 PA10 | RXD1 - debugging console - | PB10 PA9 | TXD1 - debugging console - | PB11 PA8 | - | RESET PB15 | - | 3.3 V PB14 | - | GND PB13 | - | GND PB12 | - +--------------------+ diff --git a/indicators/bootloader/Makefile b/indicators/bootloader/Makefile deleted file mode 100644 index 9ba9617..0000000 --- a/indicators/bootloader/Makefile +++ /dev/null @@ -1,7 +0,0 @@ -ROOT=../.. -BINARY=bootloader -OBJS= -LIB_OBJS=util-debug.o dfu-bootloader.o -MAX_SIZE=8192 - -include $(ROOT)/mk/bluepill.mk diff --git a/indicators/bootloader/config.h b/indicators/bootloader/config.h deleted file mode 100644 index bd6d9b2..0000000 --- a/indicators/bootloader/config.h +++ /dev/null @@ -1,24 +0,0 @@ -/* - * Neopixel Indicators Bootloader -- Configuration - * - * (c) 2022 Martin Mareš - */ - -// Processor clock - -#define CPU_CLOCK_MHZ 48 - -// Debugging port - -#define DEBUG_USART USART1 -#define DEBUG_LED_BLUEPILL - -// Bootloader settings - -#undef BOOTLOADER_DEBUG -#define BOOTLOADER_APP_START 0x08002000 -#define BOOTLOADER_MFG_ID 0x4242 -#define BOOTLOADER_PROD_ID 0x000a -#define BOOTLOADER_PROD_VERSION 0x0100 -#define BOOTLOADER_MFG_NAME "United Computer Wizards" -#define BOOTLOADER_PROD_NAME "Neopixel Indicators (boot-loader)" diff --git a/indicators/daemon/Makefile b/indicators/daemon/Makefile deleted file mode 100644 index 779acf2..0000000 --- a/indicators/daemon/Makefile +++ /dev/null @@ -1,24 +0,0 @@ -PC=pkg-config -UCW_CFLAGS := $(shell $(PC) --cflags libucw) -UCW_LIBS := $(shell $(PC) --libs libucw) -USB_CFLAGS := $(shell $(PC) --cflags libusb-1.0) -USB_LIBS := $(shell $(PC) --libs libusb-1.0) -MOSQUITTO_CFLAGS := $(shell $(PC) --cflags libmosquitto) -MOSQUITTO_LIBS := $(shell $(PC) --libs libmosquitto) - -CFLAGS=-O2 -Wall -Wextra -Wno-sign-compare -Wno-parentheses -Wstrict-prototypes -Wmissing-prototypes $(UCW_CFLAGS) $(USB_CFLAGS) $(MOSQUITTO_CFLAGS) -LDLIBS=$(UCW_LIBS) $(USB_LIBS) $(MOSQUITTO_LIBS) -lpthread -lm - -all: burrow-rainbowd - -burrow-rainbowd: burrow-rainbowd.o - -burrow-rainbowd.o: burrow-rainbowd.c ../firmware/interface.h - -install: burrow-rainbowd - install burrow-rainbowd /usr/local/sbin/ - -clean: - rm -f *.o burrow-rainbowd - -.PHONY: all install clean diff --git a/indicators/daemon/burrow-rainbowd.c b/indicators/daemon/burrow-rainbowd.c deleted file mode 100644 index 4da1b8a..0000000 --- a/indicators/daemon/burrow-rainbowd.c +++ /dev/null @@ -1,287 +0,0 @@ -/* - * Daemon for Neopixel Indicators over USB - * - * (c) 2022 Martin Mares - */ - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -typedef unsigned char byte; -typedef uint32_t u32; -typedef unsigned int uint; - -#include "../firmware/interface.h" - -static mtx_t led_mutex; -static cnd_t led_cond; -static bool led_refresh; - -struct led { - byte r, g, b; -}; - -static struct led leds[NPIX_NUM_LEDS]; - -/*** MQTT ***/ - -static struct mosquitto *mosq; -static bool mqtt_connected; - -static void mqtt_publish(const char *topic, const char *fmt, ...) -{ - va_list args; - va_start(args, fmt); - - if (mqtt_connected) { - char m[256]; - int l = vsnprintf(m, sizeof(m), fmt, args); - int err = mosquitto_publish(mosq, NULL, topic, l, m, 0, true); - if (err != MOSQ_ERR_SUCCESS) - msg(L_ERROR, "Mosquitto: Publish failed, error=%d", err); - } - - va_end(args); -} - -static void mqtt_conn_callback(struct mosquitto *mosq UNUSED, void *obj UNUSED, int status) -{ - if (!status) { - msg(L_DEBUG, "MQTT: Connection established"); - mqtt_connected = true; - mqtt_publish("status/rainbow", "ok"); - if (mosquitto_subscribe(mosq, NULL, "burrow/lights/rainbow/#", 1) != MOSQ_ERR_SUCCESS) - die("Mosquitto: subscribe failed"); - } else if (mqtt_connected) { - msg(L_DEBUG, "MQTT: Connection lost"); - mqtt_connected = false; - } -} - -static void mqtt_log_callback(struct mosquitto *mosq UNUSED, void *obj UNUSED, int level, const char *message) -{ - msg(L_DEBUG, "MQTT(%d): %s", level, message); -} - -static void mqtt_msg_callback(struct mosquitto *mosq UNUSED, void *obj UNUSED, const struct mosquitto_message *m) -{ - char val[256]; - if (m->payloadlen >= sizeof(val) - 1) { - msg(L_ERROR, "Invalid value for topic %s", m->topic); - return; - } - memcpy(val, m->payload, m->payloadlen); - val[m->payloadlen] = 0; - msg(L_DEBUG, "MQTT < %s %s", m->topic, val); - - static const char px[] = "burrow/lights/rainbow/"; - if (!str_has_prefix(m->topic, px)) - return; - - uint index; - if (str_to_uint(&index, m->topic + strlen(px), NULL, STN_DEC | STN_WHOLE) || index >= NPIX_NUM_LEDS) { - msg(L_ERROR, "Unsupported topic: %s", m->topic); - return; - } - - uint r, g, b; - if (sscanf(val, "%u%u%u", &r, &g, &b) != 3 || r >= 256 || g >= 256 || b >= 256) { - msg(L_ERROR, "Invalid value of %s: %s", m->topic, val); - return; - } - - mtx_lock(&led_mutex); - leds[index].r = r; - leds[index].g = g; - leds[index].b = b; - led_refresh = 1; - cnd_broadcast(&led_cond); - mtx_unlock(&led_mutex); -} - -static void mqtt_init(void) -{ - mosquitto_lib_init(); - - mosq = mosquitto_new("rainbowd", 1, NULL); - if (!mosq) - die("Mosquitto: Initialization failed"); - - mosquitto_connect_callback_set(mosq, mqtt_conn_callback); - mosquitto_log_callback_set(mosq, mqtt_log_callback); - mosquitto_message_callback_set(mosq, mqtt_msg_callback); - - if (mosquitto_will_set(mosq, "status/rainbow", 4, "dead", 0, true) != MOSQ_ERR_SUCCESS) - die("Mosquitto: Unable to set will"); - - if (mosquitto_tls_set(mosq, "/etc/burrow-mqtt/ca.crt", NULL, "/etc/burrow-mqtt/client.crt", "/etc/burrow-mqtt/client.key", NULL) != MOSQ_ERR_SUCCESS) - die("Mosquitto: Unable to set TLS parameters"); - - if (mosquitto_connect_async(mosq, "burrow-mqtt", 8883, 60) != MOSQ_ERR_SUCCESS) - die("Mosquitto: Unable to connect"); - - if (mosquitto_loop_start(mosq)) - die("Mosquitto: Cannot start service thread"); -} - -/*** USB ***/ - -static struct libusb_context *usb_ctxt; -static struct libusb_device_handle *devh; - -static void usb_error(const char *msg, ...) -{ - va_list args; - va_start(args, msg); - ucw_vmsg(L_ERROR, msg, args); - va_end(args); - - if (devh) { - libusb_close(devh); - devh = NULL; - } -} - -static void open_device(void) -{ - int err; - libusb_device **devlist; - ssize_t devn = libusb_get_device_list(usb_ctxt, &devlist); - if (devn < 0) - die("Cannot enumerate USB devices: error %d", (int) devn); - - for (ssize_t i=0; ilevels &= ~(1U << L_DEBUG); - - mtx_init(&led_mutex, mtx_plain); - cnd_init(&led_cond); - - mqtt_init(); - init_usb(); - - bool need_resend = true; - for (;;) { - if (!devh) { - msg(L_INFO, "Waiting for device to appear..."); - while (!devh) { - sleep(5); - open_device(); - } - need_resend = true; - } - - mtx_lock(&led_mutex); - while (!need_resend && !led_refresh) - cnd_wait(&led_cond, &led_mutex); - - int len = npix_build_packet(); - led_refresh = 0; - need_resend = 0; - mtx_unlock(&led_mutex); - - msg(L_DEBUG, "Sending NPIX packet"); - - int err, transferred; - if (err = libusb_bulk_transfer(devh, 0x01, npix_packet, len, &transferred, 1000)) - usb_error("USB transfer failed: error %d", err); - else if (transferred != len) - usb_error("USB short transfer: %d out of %d bytes", transferred, len); - } -} diff --git a/indicators/firmware/Makefile b/indicators/firmware/Makefile deleted file mode 100644 index 9964b54..0000000 --- a/indicators/firmware/Makefile +++ /dev/null @@ -1,10 +0,0 @@ -ROOT=../.. -BINARY=firmware -OBJS=main.o -LIB_OBJS=util-debug.o - -WITH_BOOT_LOADER=1 -WITH_DFU_FLASH=1 -DFU_ARGS=-d 4242:0009 - -include $(ROOT)/mk/bluepill.mk diff --git a/indicators/firmware/config.h b/indicators/firmware/config.h deleted file mode 100644 index 312c7be..0000000 --- a/indicators/firmware/config.h +++ /dev/null @@ -1,14 +0,0 @@ -/* - * Test Gadget -- Configuration - * - * (c) 2020 Martin Mareš - */ - -// Processor clock - -#define CPU_CLOCK_MHZ 72 - -// Debugging port - -#define DEBUG_USART USART1 -#define DEBUG_LED_BLUEPILL diff --git a/indicators/firmware/interface.h b/indicators/firmware/interface.h deleted file mode 100644 index b76d2b0..0000000 --- a/indicators/firmware/interface.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * Neopixel Indicators -- Interface Definitions - * - * (c) 2022 Martin Mareš - */ - -#define NPIX_USB_VENDOR 0x4242 -#define NPIX_USB_PRODUCT 0x0009 -#define NPIX_USB_VERSION 0x0100 - -#define NPIX_NUM_LEDS 12 - -/* - * Endpoints: - * - * 0x01 = bulk endpoint - * Accepts up to 12 3-byte records (R, G, B), each describing one LED. - * If less than 12 records are sent, the remaining LEDs are left unmodified. - */ diff --git a/indicators/firmware/main.c b/indicators/firmware/main.c deleted file mode 100644 index d133e95..0000000 --- a/indicators/firmware/main.c +++ /dev/null @@ -1,421 +0,0 @@ -/* - * Neopixel (WS2812B) Test - * - * (c) 2022 Martin Mareš - */ - -#include "util.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include "interface.h" - -/*** Hardware init ***/ - -static void clock_init(void) -{ - rcc_clock_setup_pll(&rcc_hse_configs[RCC_CLOCK_HSE8_72MHZ]); - - rcc_periph_clock_enable(RCC_GPIOA); - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_GPIOC); - rcc_periph_clock_enable(RCC_USART1); - rcc_periph_clock_enable(RCC_TIM4); - rcc_periph_clock_enable(RCC_DMA1); - - rcc_periph_reset_pulse(RST_GPIOA); - rcc_periph_reset_pulse(RST_GPIOB); - rcc_periph_reset_pulse(RST_GPIOC); - rcc_periph_reset_pulse(RST_USART1); - rcc_periph_reset_pulse(RST_TIM4); -} - -static void gpio_init(void) -{ - // PA9 = TXD1 for debugging console - // PA10 = RXD1 for debugging console - gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); - gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO10); - - // PC13 = BluePill LED - gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO13); - gpio_clear(GPIOC, GPIO13); - - // PB8 = data for Neopixel - gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN, GPIO8); -} - -static void usart_init(void) -{ - usart_set_baudrate(USART1, 115200); - usart_set_databits(USART1, 8); - usart_set_stopbits(USART1, USART_STOPBITS_1); - usart_set_mode(USART1, USART_MODE_TX); - usart_set_parity(USART1, USART_PARITY_NONE); - usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE); - - usart_enable(USART1); -} - -/*** System ticks ***/ - -static volatile u32 ms_ticks; - -void sys_tick_handler(void) -{ - ms_ticks++; -} - -static void tick_init(void) -{ - systick_set_frequency(1000, CPU_CLOCK_MHZ * 1000000); - systick_counter_enable(); - systick_interrupt_enable(); -} - -static void delay_ms(uint ms) -{ - u32 start_ticks = ms_ticks; - while (ms_ticks - start_ticks < ms) - ; -} - -/*** Neopixels ***/ - -#define NPIX_PERIOD 90 // timer runs on 72 MHz, so 90 periods = 1250 ns -#define NPIX_RESET 64 // the chip needs longer reset pulse than documented -#define NPIX_B0 30 -#define NPIX_B1 60 - -static byte led_rgb[NPIX_NUM_LEDS][3]; - -static byte neopixel_buf[NPIX_RESET + NPIX_NUM_LEDS*24 + 1]; -static bool neopixel_dma_running; -static bool neopixel_want_send; - -static void neopixel_run_dma(void) -{ - // When STM32 is programmed using ST-Link, the DMA sometimes keeps running. - dma_channel_reset(DMA1, 7); - - // This order of register writes is recommended in the manual. - dma_set_peripheral_address(DMA1, 7, (u32) &TIM_CCR3(TIM4)); - dma_set_memory_address(DMA1, 7, (u32) neopixel_buf); - dma_set_number_of_data(DMA1, 7, ARRAY_SIZE(neopixel_buf)); - dma_set_priority(DMA1, 7, DMA_CCR_PL_VERY_HIGH); - - dma_set_read_from_memory(DMA1, 7); - - dma_set_memory_size(DMA1, 7, DMA_CCR_MSIZE_8BIT); - dma_enable_memory_increment_mode(DMA1, 7); - - dma_set_peripheral_size(DMA1, 7, DMA_CCR_PSIZE_16BIT); - dma_disable_peripheral_increment_mode(DMA1, 7); - - dma_clear_interrupt_flags(DMA1, 7, DMA_TCIF); - dma_enable_channel(DMA1, 7); - neopixel_dma_running = 1; -} - -static void neopixel_recalc(void) -{ - byte *buf = neopixel_buf; - for (uint i = 0; i < NPIX_RESET; i++) - *buf++ = 0; - for (uint i = 0; i < NPIX_NUM_LEDS; i++) { - // The order is GRB, MSB first - for (uint m = 0x80; m; m >>= 1) - *buf++ = ((led_rgb[i][1] & m) ? NPIX_B1 : NPIX_B0); - for (uint m = 0x80; m; m >>= 1) - *buf++ = ((led_rgb[i][0] & m) ? NPIX_B1 : NPIX_B0); - for (uint m = 0x80; m; m >>= 1) - *buf++ = ((led_rgb[i][2] & m) ? NPIX_B1 : NPIX_B0); - } - *buf++ = NPIX_PERIOD; - - neopixel_run_dma(); - neopixel_want_send = 0; -} - -static void neopixel_init(void) -{ - // TIM4 is always running and producing DMA requests on each update - // (connected to DMA1 channel 7). When we have something to send, - // the DMA is enabled. - - timer_set_prescaler(TIM4, 0); - 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, NPIX_PERIOD - 1); - - timer_set_oc_mode(TIM4, TIM_OC3, TIM_OCM_PWM1); - timer_set_oc_value(TIM4, TIM_OC3, 0); - timer_set_oc_polarity_high(TIM4, TIM_OC3); - timer_enable_oc_output(TIM4, TIM_OC3); - - timer_set_dma_on_update_event(TIM4); - TIM_DIER(TIM4) |= TIM_DIER_UDE; - - led_rgb[0][1] = 0xaa; - - timer_enable_counter(TIM4); - neopixel_recalc(); -} - -static bool neopixel_ready(void) -{ - if (!neopixel_dma_running) - return 1; - - if (!dma_get_interrupt_flag(DMA1, 7, DMA_TCIF)) - return 0; - - dma_disable_channel(DMA1, 7); - neopixel_dma_running = 0; - return 1; -} - -/*** USB ***/ - -static usbd_device *usbd_dev; - -enum usb_string { - STR_MANUFACTURER = 1, - STR_PRODUCT, - STR_SERIAL, -}; - -static char usb_serial_number[13]; - -static const char *usb_strings[] = { - "United Computer Wizards", - "Neopixel Indicators", - usb_serial_number, -}; - -static const struct usb_device_descriptor device = { - .bLength = USB_DT_DEVICE_SIZE, - .bDescriptorType = USB_DT_DEVICE, - .bcdUSB = 0x0200, - .bDeviceClass = 0xFF, - .bDeviceSubClass = 0, - .bDeviceProtocol = 0, - .bMaxPacketSize0 = 64, - .idVendor = NPIX_USB_VENDOR, - .idProduct = NPIX_USB_PRODUCT, - .bcdDevice = NPIX_USB_VERSION, - .iManufacturer = STR_MANUFACTURER, - .iProduct = STR_PRODUCT, - .iSerialNumber = STR_SERIAL, - .bNumConfigurations = 1, -}; - -static const struct usb_endpoint_descriptor endpoints[] = {{ - // Bulk end-point for sending LED values - .bLength = USB_DT_ENDPOINT_SIZE, - .bDescriptorType = USB_DT_ENDPOINT, - .bEndpointAddress = 0x01, - .bmAttributes = USB_ENDPOINT_ATTR_BULK, - .wMaxPacketSize = sizeof(led_rgb), - .bInterval = 1, -}}; - -static const struct usb_interface_descriptor iface = { - .bLength = USB_DT_INTERFACE_SIZE, - .bDescriptorType = USB_DT_INTERFACE, - .bInterfaceNumber = 0, - .bAlternateSetting = 0, - .bNumEndpoints = 1, - .bInterfaceClass = 0xFF, - .bInterfaceSubClass = 0, - .bInterfaceProtocol = 0, - .iInterface = 0, - .endpoint = endpoints, -}; - -static 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 = 1024, - .bcdDFUVersion = 0x0100, -}; - -static const struct usb_interface_descriptor dfu_iface = { - .bLength = USB_DT_INTERFACE_SIZE, - .bDescriptorType = USB_DT_INTERFACE, - .bInterfaceNumber = 1, - .bAlternateSetting = 0, - .bNumEndpoints = 0, - .bInterfaceClass = 0xFE, - .bInterfaceSubClass = 1, - .bInterfaceProtocol = 1, - .iInterface = 0, - - .extra = &dfu_function, - .extralen = sizeof(dfu_function), -}; - -static const struct usb_interface ifaces[] = {{ - .num_altsetting = 1, - .altsetting = &iface, -}, { - .num_altsetting = 1, - .altsetting = &dfu_iface, -}}; - -static const struct usb_config_descriptor config = { - .bLength = USB_DT_CONFIGURATION_SIZE, - .bDescriptorType = USB_DT_CONFIGURATION, - .wTotalLength = 0, - .bNumInterfaces = 2, - .bConfigurationValue = 1, - .iConfiguration = 0, - .bmAttributes = 0x80, - .bMaxPower = 100, // multiplied by 2 mA - .interface = ifaces, -}; - -static byte usb_configured; -static uint8_t usbd_control_buffer[64]; - -static void dfu_detach_complete(usbd_device *dev UNUSED, struct usb_setup_data *req UNUSED) -{ - // Reset to bootloader, which implements the rest of DFU - debug_printf("Switching to DFU\n"); - debug_flush(); - scb_reset_core(); -} - -static enum usbd_request_return_codes dfu_control_cb(usbd_device *dev UNUSED, - struct usb_setup_data *req, - uint8_t **buf UNUSED, - uint16_t *len UNUSED, - void (**complete)(usbd_device *dev, struct usb_setup_data *req)) -{ - if (req->bmRequestType != 0x21 || req->bRequest != DFU_DETACH) - return USBD_REQ_NOTSUPP; - - *complete = dfu_detach_complete; - return USBD_REQ_HANDLED; -} - -static void ep01_cb(usbd_device *dev, uint8_t ep UNUSED) -{ - // We received a frame from the USB host - uint len = usbd_ep_read_packet(dev, 0x01, led_rgb, sizeof(led_rgb)); - debug_printf("USB: Host sent %u bytes\n", len); - neopixel_want_send = 1; -} - -static void set_config_cb(usbd_device *dev, uint16_t wValue UNUSED) -{ - usbd_register_control_callback( - dev, - USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE, - USB_REQ_TYPE_TYPE | USB_REQ_TYPE_RECIPIENT, - dfu_control_cb); - usbd_ep_setup(dev, 0x01, USB_ENDPOINT_ATTR_BULK, 64, ep01_cb); - usb_configured = 1; -} - -static void reset_cb(void) -{ - debug_printf("USB: Reset\n"); - usb_configured = 0; -} - -static volatile bool usb_event_pending; - -void usb_lp_can_rx0_isr(void) -{ - /* - * We handle USB in the main loop to avoid race conditions between - * USB interrupts and other code. However, we need an interrupt to - * up the main loop from sleep. - * - * We set up only the low-priority ISR, because high-priority ISR handles - * only double-buffered bulk transfers and isochronous transfers. - */ - nvic_disable_irq(NVIC_USB_LP_CAN_RX0_IRQ); - usb_event_pending = 1; -} - -static void usb_init(void) -{ - // Simulate USB disconnect - gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_OPENDRAIN, GPIO11 | GPIO12); - gpio_clear(GPIOA, GPIO11 | GPIO12); - delay_ms(100); - - usbd_dev = usbd_init( - &st_usbfs_v1_usb_driver, - &device, - &config, - usb_strings, - ARRAY_SIZE(usb_strings), - usbd_control_buffer, - sizeof(usbd_control_buffer) - ); - usbd_register_reset_callback(usbd_dev, reset_cb); - usbd_register_set_config_callback(usbd_dev, set_config_cb); - usb_event_pending = 1; -} - -/*** Main ***/ - -int main(void) -{ - clock_init(); - gpio_init(); - usart_init(); - tick_init(); - neopixel_init(); - usb_init(); - - debug_printf("Hello, world!\n"); - - u32 last_blink = 0; - u32 last_send = 0; - - for (;;) { - if (ms_ticks - last_blink >= 100) { - debug_led_toggle(); - last_blink = ms_ticks; - led_rgb[NPIX_NUM_LEDS - 1][1] ^= 0x33; - neopixel_want_send = 1; - } - - if (usb_event_pending) { - usbd_poll(usbd_dev); - usb_event_pending = 0; - nvic_clear_pending_irq(NVIC_USB_LP_CAN_RX0_IRQ); - nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ); - } - - if (neopixel_ready()) { - if (neopixel_want_send || ms_ticks - last_send >= 100) { - // Re-send every 100 ms - neopixel_recalc(); - last_send = ms_ticks; - } - } - - wait_for_interrupt(); - } - - return 0; -} diff --git a/indicators/test/Makefile b/indicators/test/Makefile deleted file mode 100644 index a31dce1..0000000 --- a/indicators/test/Makefile +++ /dev/null @@ -1,9 +0,0 @@ -UCWCF:=$(shell PKG_CONFIG_PATH=$(LIBUCW)/lib/pkgconfig pkg-config --cflags libucw) -UCWLF:=$(shell PKG_CONFIG_PATH=$(LIBUCW)/lib/pkgconfig pkg-config --libs libucw) - -CFLAGS=-std=gnu99 -O2 -Wall -Wextra -Wno-parentheses $(UCWCF) -LDLIBS=-lusb-1.0 $(UCWLF) - -all: test - -test: test.c diff --git a/indicators/test/test.c b/indicators/test/test.c deleted file mode 100644 index 55c37fb..0000000 --- a/indicators/test/test.c +++ /dev/null @@ -1,75 +0,0 @@ -#include - -#include -#include -#include -#include -#include -#include - -#include "../firmware/interface.h" - -struct libusb_context *usb_ctxt; -struct libusb_device_handle *devh; - -static libusb_device *find_device(void) -{ - libusb_device **devlist; - ssize_t devn = libusb_get_device_list(usb_ctxt, &devlist); - if (devn < 0) - { - fprintf(stderr, "Cannot enumerate USB devices: error %d\n", (int) devn); - exit(1); - } - - for (ssize_t i=0; i NPIX_NUM_LEDS*3 + 1) - die("Too many arguments!"); - - for (int i=1; i < argc; i++) - packet[len++] = atoi(argv[i]); - - int err; - if (err = libusb_init(&usb_ctxt)) - die("Cannot initialize libusb: error %d", err); - - libusb_device *dev = find_device(); - - if (err = libusb_open(dev, &devh)) - die("Cannot open device: error %d", err); - // libusb_reset_device(devh); - if (err = libusb_claim_interface(devh, 0)) - die("Cannot claim interface: error %d", err); - - int transferred; - if (err = libusb_bulk_transfer(devh, 0x01, packet, len, &transferred, 1000)) - die("Transfer failed: error %d\n", err); - if (transferred != len) - die("Short transfer: %d out of %d bytes", transferred, len); - - return 0; -} diff --git a/rainbow/README b/rainbow/README new file mode 100644 index 0000000..203357f --- /dev/null +++ b/rainbow/README @@ -0,0 +1,30 @@ +Assignment of peripherals and pins +================================== + +USART1 debugging +TIM4 Neopixel control + + + Blue Pill pinout + +--------------------+ + | VBATT 3.3V | +BluePill LED | PC13 GND | Neopixel ground + | PC14 5V | Neopixel power + | PC15 PB9 | + | PA0 PB8 | TIM4_CH3 - Neopixel data (needs pull-up) + | PA1 PB7 | + | PA2 PB6 | + | PA3 PB5 | + | PA4 PB4 | + | PA5 PB3 | + | PA6 PA15 | + | PA7 PA12 | + | PB0 PA11 | + | PB1 PA10 | RXD1 - debugging console + | PB10 PA9 | TXD1 - debugging console + | PB11 PA8 | + | RESET PB15 | + | 3.3 V PB14 | + | GND PB13 | + | GND PB12 | + +--------------------+ diff --git a/rainbow/bootloader/Makefile b/rainbow/bootloader/Makefile new file mode 100644 index 0000000..9ba9617 --- /dev/null +++ b/rainbow/bootloader/Makefile @@ -0,0 +1,7 @@ +ROOT=../.. +BINARY=bootloader +OBJS= +LIB_OBJS=util-debug.o dfu-bootloader.o +MAX_SIZE=8192 + +include $(ROOT)/mk/bluepill.mk diff --git a/rainbow/bootloader/config.h b/rainbow/bootloader/config.h new file mode 100644 index 0000000..bd6d9b2 --- /dev/null +++ b/rainbow/bootloader/config.h @@ -0,0 +1,24 @@ +/* + * Neopixel Indicators Bootloader -- Configuration + * + * (c) 2022 Martin Mareš + */ + +// Processor clock + +#define CPU_CLOCK_MHZ 48 + +// Debugging port + +#define DEBUG_USART USART1 +#define DEBUG_LED_BLUEPILL + +// Bootloader settings + +#undef BOOTLOADER_DEBUG +#define BOOTLOADER_APP_START 0x08002000 +#define BOOTLOADER_MFG_ID 0x4242 +#define BOOTLOADER_PROD_ID 0x000a +#define BOOTLOADER_PROD_VERSION 0x0100 +#define BOOTLOADER_MFG_NAME "United Computer Wizards" +#define BOOTLOADER_PROD_NAME "Neopixel Indicators (boot-loader)" diff --git a/rainbow/daemon/Makefile b/rainbow/daemon/Makefile new file mode 100644 index 0000000..779acf2 --- /dev/null +++ b/rainbow/daemon/Makefile @@ -0,0 +1,24 @@ +PC=pkg-config +UCW_CFLAGS := $(shell $(PC) --cflags libucw) +UCW_LIBS := $(shell $(PC) --libs libucw) +USB_CFLAGS := $(shell $(PC) --cflags libusb-1.0) +USB_LIBS := $(shell $(PC) --libs libusb-1.0) +MOSQUITTO_CFLAGS := $(shell $(PC) --cflags libmosquitto) +MOSQUITTO_LIBS := $(shell $(PC) --libs libmosquitto) + +CFLAGS=-O2 -Wall -Wextra -Wno-sign-compare -Wno-parentheses -Wstrict-prototypes -Wmissing-prototypes $(UCW_CFLAGS) $(USB_CFLAGS) $(MOSQUITTO_CFLAGS) +LDLIBS=$(UCW_LIBS) $(USB_LIBS) $(MOSQUITTO_LIBS) -lpthread -lm + +all: burrow-rainbowd + +burrow-rainbowd: burrow-rainbowd.o + +burrow-rainbowd.o: burrow-rainbowd.c ../firmware/interface.h + +install: burrow-rainbowd + install burrow-rainbowd /usr/local/sbin/ + +clean: + rm -f *.o burrow-rainbowd + +.PHONY: all install clean diff --git a/rainbow/daemon/burrow-rainbowd.c b/rainbow/daemon/burrow-rainbowd.c new file mode 100644 index 0000000..99d575f --- /dev/null +++ b/rainbow/daemon/burrow-rainbowd.c @@ -0,0 +1,287 @@ +/* + * Daemon for Neopixel Rainbow Indicators over USB + * + * (c) 2022 Martin Mares + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +typedef unsigned char byte; +typedef uint32_t u32; +typedef unsigned int uint; + +#include "../firmware/interface.h" + +static mtx_t led_mutex; +static cnd_t led_cond; +static bool led_refresh; + +struct led { + byte r, g, b; +}; + +static struct led leds[NPIX_NUM_LEDS]; + +/*** MQTT ***/ + +static struct mosquitto *mosq; +static bool mqtt_connected; + +static void mqtt_publish(const char *topic, const char *fmt, ...) +{ + va_list args; + va_start(args, fmt); + + if (mqtt_connected) { + char m[256]; + int l = vsnprintf(m, sizeof(m), fmt, args); + int err = mosquitto_publish(mosq, NULL, topic, l, m, 0, true); + if (err != MOSQ_ERR_SUCCESS) + msg(L_ERROR, "Mosquitto: Publish failed, error=%d", err); + } + + va_end(args); +} + +static void mqtt_conn_callback(struct mosquitto *mosq UNUSED, void *obj UNUSED, int status) +{ + if (!status) { + msg(L_DEBUG, "MQTT: Connection established"); + mqtt_connected = true; + mqtt_publish("status/rainbow", "ok"); + if (mosquitto_subscribe(mosq, NULL, "burrow/lights/rainbow/#", 1) != MOSQ_ERR_SUCCESS) + die("Mosquitto: subscribe failed"); + } else if (mqtt_connected) { + msg(L_DEBUG, "MQTT: Connection lost"); + mqtt_connected = false; + } +} + +static void mqtt_log_callback(struct mosquitto *mosq UNUSED, void *obj UNUSED, int level, const char *message) +{ + msg(L_DEBUG, "MQTT(%d): %s", level, message); +} + +static void mqtt_msg_callback(struct mosquitto *mosq UNUSED, void *obj UNUSED, const struct mosquitto_message *m) +{ + char val[256]; + if (m->payloadlen >= sizeof(val) - 1) { + msg(L_ERROR, "Invalid value for topic %s", m->topic); + return; + } + memcpy(val, m->payload, m->payloadlen); + val[m->payloadlen] = 0; + msg(L_DEBUG, "MQTT < %s %s", m->topic, val); + + static const char px[] = "burrow/lights/rainbow/"; + if (!str_has_prefix(m->topic, px)) + return; + + uint index; + if (str_to_uint(&index, m->topic + strlen(px), NULL, STN_DEC | STN_WHOLE) || index >= NPIX_NUM_LEDS) { + msg(L_ERROR, "Unsupported topic: %s", m->topic); + return; + } + + uint r, g, b; + if (sscanf(val, "%u%u%u", &r, &g, &b) != 3 || r >= 256 || g >= 256 || b >= 256) { + msg(L_ERROR, "Invalid value of %s: %s", m->topic, val); + return; + } + + mtx_lock(&led_mutex); + leds[index].r = r; + leds[index].g = g; + leds[index].b = b; + led_refresh = 1; + cnd_broadcast(&led_cond); + mtx_unlock(&led_mutex); +} + +static void mqtt_init(void) +{ + mosquitto_lib_init(); + + mosq = mosquitto_new("rainbowd", 1, NULL); + if (!mosq) + die("Mosquitto: Initialization failed"); + + mosquitto_connect_callback_set(mosq, mqtt_conn_callback); + mosquitto_log_callback_set(mosq, mqtt_log_callback); + mosquitto_message_callback_set(mosq, mqtt_msg_callback); + + if (mosquitto_will_set(mosq, "status/rainbow", 4, "dead", 0, true) != MOSQ_ERR_SUCCESS) + die("Mosquitto: Unable to set will"); + + if (mosquitto_tls_set(mosq, "/etc/burrow-mqtt/ca.crt", NULL, "/etc/burrow-mqtt/client.crt", "/etc/burrow-mqtt/client.key", NULL) != MOSQ_ERR_SUCCESS) + die("Mosquitto: Unable to set TLS parameters"); + + if (mosquitto_connect_async(mosq, "burrow-mqtt", 8883, 60) != MOSQ_ERR_SUCCESS) + die("Mosquitto: Unable to connect"); + + if (mosquitto_loop_start(mosq)) + die("Mosquitto: Cannot start service thread"); +} + +/*** USB ***/ + +static struct libusb_context *usb_ctxt; +static struct libusb_device_handle *devh; + +static void usb_error(const char *msg, ...) +{ + va_list args; + va_start(args, msg); + ucw_vmsg(L_ERROR, msg, args); + va_end(args); + + if (devh) { + libusb_close(devh); + devh = NULL; + } +} + +static void open_device(void) +{ + int err; + libusb_device **devlist; + ssize_t devn = libusb_get_device_list(usb_ctxt, &devlist); + if (devn < 0) + die("Cannot enumerate USB devices: error %d", (int) devn); + + for (ssize_t i=0; ilevels &= ~(1U << L_DEBUG); + + mtx_init(&led_mutex, mtx_plain); + cnd_init(&led_cond); + + mqtt_init(); + init_usb(); + + bool need_resend = true; + for (;;) { + if (!devh) { + msg(L_INFO, "Waiting for device to appear..."); + while (!devh) { + sleep(5); + open_device(); + } + need_resend = true; + } + + mtx_lock(&led_mutex); + while (!need_resend && !led_refresh) + cnd_wait(&led_cond, &led_mutex); + + int len = npix_build_packet(); + led_refresh = 0; + need_resend = 0; + mtx_unlock(&led_mutex); + + msg(L_DEBUG, "Sending NPIX packet"); + + int err, transferred; + if (err = libusb_bulk_transfer(devh, 0x01, npix_packet, len, &transferred, 1000)) + usb_error("USB transfer failed: error %d", err); + else if (transferred != len) + usb_error("USB short transfer: %d out of %d bytes", transferred, len); + } +} diff --git a/rainbow/firmware/Makefile b/rainbow/firmware/Makefile new file mode 100644 index 0000000..9964b54 --- /dev/null +++ b/rainbow/firmware/Makefile @@ -0,0 +1,10 @@ +ROOT=../.. +BINARY=firmware +OBJS=main.o +LIB_OBJS=util-debug.o + +WITH_BOOT_LOADER=1 +WITH_DFU_FLASH=1 +DFU_ARGS=-d 4242:0009 + +include $(ROOT)/mk/bluepill.mk diff --git a/rainbow/firmware/config.h b/rainbow/firmware/config.h new file mode 100644 index 0000000..a70a756 --- /dev/null +++ b/rainbow/firmware/config.h @@ -0,0 +1,14 @@ +/* + * Neopixel Rainbow -- Configuration + * + * (c) 2022 Martin Mareš + */ + +// Processor clock + +#define CPU_CLOCK_MHZ 72 + +// Debugging port + +#define DEBUG_USART USART1 +#define DEBUG_LED_BLUEPILL diff --git a/rainbow/firmware/interface.h b/rainbow/firmware/interface.h new file mode 100644 index 0000000..2a3a1d4 --- /dev/null +++ b/rainbow/firmware/interface.h @@ -0,0 +1,19 @@ +/* + * Neopixel Rainbow -- Interface Definitions + * + * (c) 2022 Martin Mareš + */ + +#define NPIX_USB_VENDOR 0x4242 +#define NPIX_USB_PRODUCT 0x0009 +#define NPIX_USB_VERSION 0x0100 + +#define NPIX_NUM_LEDS 12 + +/* + * Endpoints: + * + * 0x01 = bulk endpoint + * Accepts up to 12 3-byte records (R, G, B), each describing one LED. + * If less than 12 records are sent, the remaining LEDs are left unmodified. + */ diff --git a/rainbow/firmware/main.c b/rainbow/firmware/main.c new file mode 100644 index 0000000..5dee456 --- /dev/null +++ b/rainbow/firmware/main.c @@ -0,0 +1,421 @@ +/* + * Neopixel (WS2812B) Rainbow + * + * (c) 2022 Martin Mareš + */ + +#include "util.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "interface.h" + +/*** Hardware init ***/ + +static void clock_init(void) +{ + rcc_clock_setup_pll(&rcc_hse_configs[RCC_CLOCK_HSE8_72MHZ]); + + rcc_periph_clock_enable(RCC_GPIOA); + rcc_periph_clock_enable(RCC_GPIOB); + rcc_periph_clock_enable(RCC_GPIOC); + rcc_periph_clock_enable(RCC_USART1); + rcc_periph_clock_enable(RCC_TIM4); + rcc_periph_clock_enable(RCC_DMA1); + + rcc_periph_reset_pulse(RST_GPIOA); + rcc_periph_reset_pulse(RST_GPIOB); + rcc_periph_reset_pulse(RST_GPIOC); + rcc_periph_reset_pulse(RST_USART1); + rcc_periph_reset_pulse(RST_TIM4); +} + +static void gpio_init(void) +{ + // PA9 = TXD1 for debugging console + // PA10 = RXD1 for debugging console + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_PUSHPULL, GPIO9); + gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO10); + + // PC13 = BluePill LED + gpio_set_mode(GPIOC, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO13); + gpio_clear(GPIOC, GPIO13); + + // PB8 = data for Neopixel + gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_ALTFN_OPENDRAIN, GPIO8); +} + +static void usart_init(void) +{ + usart_set_baudrate(USART1, 115200); + usart_set_databits(USART1, 8); + usart_set_stopbits(USART1, USART_STOPBITS_1); + usart_set_mode(USART1, USART_MODE_TX); + usart_set_parity(USART1, USART_PARITY_NONE); + usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE); + + usart_enable(USART1); +} + +/*** System ticks ***/ + +static volatile u32 ms_ticks; + +void sys_tick_handler(void) +{ + ms_ticks++; +} + +static void tick_init(void) +{ + systick_set_frequency(1000, CPU_CLOCK_MHZ * 1000000); + systick_counter_enable(); + systick_interrupt_enable(); +} + +static void delay_ms(uint ms) +{ + u32 start_ticks = ms_ticks; + while (ms_ticks - start_ticks < ms) + ; +} + +/*** Neopixels ***/ + +#define NPIX_PERIOD 90 // timer runs on 72 MHz, so 90 periods = 1250 ns +#define NPIX_RESET 64 // the chip needs longer reset pulse than documented +#define NPIX_B0 30 +#define NPIX_B1 60 + +static byte led_rgb[NPIX_NUM_LEDS][3]; + +static byte neopixel_buf[NPIX_RESET + NPIX_NUM_LEDS*24 + 1]; +static bool neopixel_dma_running; +static bool neopixel_want_send; + +static void neopixel_run_dma(void) +{ + // When STM32 is programmed using ST-Link, the DMA sometimes keeps running. + dma_channel_reset(DMA1, 7); + + // This order of register writes is recommended in the manual. + dma_set_peripheral_address(DMA1, 7, (u32) &TIM_CCR3(TIM4)); + dma_set_memory_address(DMA1, 7, (u32) neopixel_buf); + dma_set_number_of_data(DMA1, 7, ARRAY_SIZE(neopixel_buf)); + dma_set_priority(DMA1, 7, DMA_CCR_PL_VERY_HIGH); + + dma_set_read_from_memory(DMA1, 7); + + dma_set_memory_size(DMA1, 7, DMA_CCR_MSIZE_8BIT); + dma_enable_memory_increment_mode(DMA1, 7); + + dma_set_peripheral_size(DMA1, 7, DMA_CCR_PSIZE_16BIT); + dma_disable_peripheral_increment_mode(DMA1, 7); + + dma_clear_interrupt_flags(DMA1, 7, DMA_TCIF); + dma_enable_channel(DMA1, 7); + neopixel_dma_running = 1; +} + +static void neopixel_recalc(void) +{ + byte *buf = neopixel_buf; + for (uint i = 0; i < NPIX_RESET; i++) + *buf++ = 0; + for (uint i = 0; i < NPIX_NUM_LEDS; i++) { + // The order is GRB, MSB first + for (uint m = 0x80; m; m >>= 1) + *buf++ = ((led_rgb[i][1] & m) ? NPIX_B1 : NPIX_B0); + for (uint m = 0x80; m; m >>= 1) + *buf++ = ((led_rgb[i][0] & m) ? NPIX_B1 : NPIX_B0); + for (uint m = 0x80; m; m >>= 1) + *buf++ = ((led_rgb[i][2] & m) ? NPIX_B1 : NPIX_B0); + } + *buf++ = NPIX_PERIOD; + + neopixel_run_dma(); + neopixel_want_send = 0; +} + +static void neopixel_init(void) +{ + // TIM4 is always running and producing DMA requests on each update + // (connected to DMA1 channel 7). When we have something to send, + // the DMA is enabled. + + timer_set_prescaler(TIM4, 0); + 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, NPIX_PERIOD - 1); + + timer_set_oc_mode(TIM4, TIM_OC3, TIM_OCM_PWM1); + timer_set_oc_value(TIM4, TIM_OC3, 0); + timer_set_oc_polarity_high(TIM4, TIM_OC3); + timer_enable_oc_output(TIM4, TIM_OC3); + + timer_set_dma_on_update_event(TIM4); + TIM_DIER(TIM4) |= TIM_DIER_UDE; + + led_rgb[0][1] = 0xaa; + + timer_enable_counter(TIM4); + neopixel_recalc(); +} + +static bool neopixel_ready(void) +{ + if (!neopixel_dma_running) + return 1; + + if (!dma_get_interrupt_flag(DMA1, 7, DMA_TCIF)) + return 0; + + dma_disable_channel(DMA1, 7); + neopixel_dma_running = 0; + return 1; +} + +/*** USB ***/ + +static usbd_device *usbd_dev; + +enum usb_string { + STR_MANUFACTURER = 1, + STR_PRODUCT, + STR_SERIAL, +}; + +static char usb_serial_number[13]; + +static const char *usb_strings[] = { + "United Computer Wizards", + "Neopixel Indicators", + usb_serial_number, +}; + +static const struct usb_device_descriptor device = { + .bLength = USB_DT_DEVICE_SIZE, + .bDescriptorType = USB_DT_DEVICE, + .bcdUSB = 0x0200, + .bDeviceClass = 0xFF, + .bDeviceSubClass = 0, + .bDeviceProtocol = 0, + .bMaxPacketSize0 = 64, + .idVendor = NPIX_USB_VENDOR, + .idProduct = NPIX_USB_PRODUCT, + .bcdDevice = NPIX_USB_VERSION, + .iManufacturer = STR_MANUFACTURER, + .iProduct = STR_PRODUCT, + .iSerialNumber = STR_SERIAL, + .bNumConfigurations = 1, +}; + +static const struct usb_endpoint_descriptor endpoints[] = {{ + // Bulk end-point for sending LED values + .bLength = USB_DT_ENDPOINT_SIZE, + .bDescriptorType = USB_DT_ENDPOINT, + .bEndpointAddress = 0x01, + .bmAttributes = USB_ENDPOINT_ATTR_BULK, + .wMaxPacketSize = sizeof(led_rgb), + .bInterval = 1, +}}; + +static const struct usb_interface_descriptor iface = { + .bLength = USB_DT_INTERFACE_SIZE, + .bDescriptorType = USB_DT_INTERFACE, + .bInterfaceNumber = 0, + .bAlternateSetting = 0, + .bNumEndpoints = 1, + .bInterfaceClass = 0xFF, + .bInterfaceSubClass = 0, + .bInterfaceProtocol = 0, + .iInterface = 0, + .endpoint = endpoints, +}; + +static 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 = 1024, + .bcdDFUVersion = 0x0100, +}; + +static const struct usb_interface_descriptor dfu_iface = { + .bLength = USB_DT_INTERFACE_SIZE, + .bDescriptorType = USB_DT_INTERFACE, + .bInterfaceNumber = 1, + .bAlternateSetting = 0, + .bNumEndpoints = 0, + .bInterfaceClass = 0xFE, + .bInterfaceSubClass = 1, + .bInterfaceProtocol = 1, + .iInterface = 0, + + .extra = &dfu_function, + .extralen = sizeof(dfu_function), +}; + +static const struct usb_interface ifaces[] = {{ + .num_altsetting = 1, + .altsetting = &iface, +}, { + .num_altsetting = 1, + .altsetting = &dfu_iface, +}}; + +static const struct usb_config_descriptor config = { + .bLength = USB_DT_CONFIGURATION_SIZE, + .bDescriptorType = USB_DT_CONFIGURATION, + .wTotalLength = 0, + .bNumInterfaces = 2, + .bConfigurationValue = 1, + .iConfiguration = 0, + .bmAttributes = 0x80, + .bMaxPower = 100, // multiplied by 2 mA + .interface = ifaces, +}; + +static byte usb_configured; +static uint8_t usbd_control_buffer[64]; + +static void dfu_detach_complete(usbd_device *dev UNUSED, struct usb_setup_data *req UNUSED) +{ + // Reset to bootloader, which implements the rest of DFU + debug_printf("Switching to DFU\n"); + debug_flush(); + scb_reset_core(); +} + +static enum usbd_request_return_codes dfu_control_cb(usbd_device *dev UNUSED, + struct usb_setup_data *req, + uint8_t **buf UNUSED, + uint16_t *len UNUSED, + void (**complete)(usbd_device *dev, struct usb_setup_data *req)) +{ + if (req->bmRequestType != 0x21 || req->bRequest != DFU_DETACH) + return USBD_REQ_NOTSUPP; + + *complete = dfu_detach_complete; + return USBD_REQ_HANDLED; +} + +static void ep01_cb(usbd_device *dev, uint8_t ep UNUSED) +{ + // We received a frame from the USB host + uint len = usbd_ep_read_packet(dev, 0x01, led_rgb, sizeof(led_rgb)); + debug_printf("USB: Host sent %u bytes\n", len); + neopixel_want_send = 1; +} + +static void set_config_cb(usbd_device *dev, uint16_t wValue UNUSED) +{ + usbd_register_control_callback( + dev, + USB_REQ_TYPE_CLASS | USB_REQ_TYPE_INTERFACE, + USB_REQ_TYPE_TYPE | USB_REQ_TYPE_RECIPIENT, + dfu_control_cb); + usbd_ep_setup(dev, 0x01, USB_ENDPOINT_ATTR_BULK, 64, ep01_cb); + usb_configured = 1; +} + +static void reset_cb(void) +{ + debug_printf("USB: Reset\n"); + usb_configured = 0; +} + +static volatile bool usb_event_pending; + +void usb_lp_can_rx0_isr(void) +{ + /* + * We handle USB in the main loop to avoid race conditions between + * USB interrupts and other code. However, we need an interrupt to + * up the main loop from sleep. + * + * We set up only the low-priority ISR, because high-priority ISR handles + * only double-buffered bulk transfers and isochronous transfers. + */ + nvic_disable_irq(NVIC_USB_LP_CAN_RX0_IRQ); + usb_event_pending = 1; +} + +static void usb_init(void) +{ + // Simulate USB disconnect + gpio_set_mode(GPIOA, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_OPENDRAIN, GPIO11 | GPIO12); + gpio_clear(GPIOA, GPIO11 | GPIO12); + delay_ms(100); + + usbd_dev = usbd_init( + &st_usbfs_v1_usb_driver, + &device, + &config, + usb_strings, + ARRAY_SIZE(usb_strings), + usbd_control_buffer, + sizeof(usbd_control_buffer) + ); + usbd_register_reset_callback(usbd_dev, reset_cb); + usbd_register_set_config_callback(usbd_dev, set_config_cb); + usb_event_pending = 1; +} + +/*** Main ***/ + +int main(void) +{ + clock_init(); + gpio_init(); + usart_init(); + tick_init(); + neopixel_init(); + usb_init(); + + debug_printf("The Bifrőst bridge spans its colors between Midgard and Asgard...\n"); + + u32 last_blink = 0; + u32 last_send = 0; + + for (;;) { + if (ms_ticks - last_blink >= 100) { + debug_led_toggle(); + last_blink = ms_ticks; + led_rgb[NPIX_NUM_LEDS - 1][1] ^= 0x33; + neopixel_want_send = 1; + } + + if (usb_event_pending) { + usbd_poll(usbd_dev); + usb_event_pending = 0; + nvic_clear_pending_irq(NVIC_USB_LP_CAN_RX0_IRQ); + nvic_enable_irq(NVIC_USB_LP_CAN_RX0_IRQ); + } + + if (neopixel_ready()) { + if (neopixel_want_send || ms_ticks - last_send >= 100) { + // Re-send every 100 ms + neopixel_recalc(); + last_send = ms_ticks; + } + } + + wait_for_interrupt(); + } + + return 0; +} diff --git a/rainbow/test/Makefile b/rainbow/test/Makefile new file mode 100644 index 0000000..a31dce1 --- /dev/null +++ b/rainbow/test/Makefile @@ -0,0 +1,9 @@ +UCWCF:=$(shell PKG_CONFIG_PATH=$(LIBUCW)/lib/pkgconfig pkg-config --cflags libucw) +UCWLF:=$(shell PKG_CONFIG_PATH=$(LIBUCW)/lib/pkgconfig pkg-config --libs libucw) + +CFLAGS=-std=gnu99 -O2 -Wall -Wextra -Wno-parentheses $(UCWCF) +LDLIBS=-lusb-1.0 $(UCWLF) + +all: test + +test: test.c diff --git a/rainbow/test/test.c b/rainbow/test/test.c new file mode 100644 index 0000000..e079ec5 --- /dev/null +++ b/rainbow/test/test.c @@ -0,0 +1,77 @@ +/* Testing Neopixel Rainbow */ + +#include + +#include +#include +#include +#include +#include +#include + +#include "../firmware/interface.h" + +struct libusb_context *usb_ctxt; +struct libusb_device_handle *devh; + +static libusb_device *find_device(void) +{ + libusb_device **devlist; + ssize_t devn = libusb_get_device_list(usb_ctxt, &devlist); + if (devn < 0) + { + fprintf(stderr, "Cannot enumerate USB devices: error %d\n", (int) devn); + exit(1); + } + + for (ssize_t i=0; i NPIX_NUM_LEDS*3 + 1) + die("Too many arguments!"); + + for (int i=1; i < argc; i++) + packet[len++] = atoi(argv[i]); + + int err; + if (err = libusb_init(&usb_ctxt)) + die("Cannot initialize libusb: error %d", err); + + libusb_device *dev = find_device(); + + if (err = libusb_open(dev, &devh)) + die("Cannot open device: error %d", err); + // libusb_reset_device(devh); + if (err = libusb_claim_interface(devh, 0)) + die("Cannot claim interface: error %d", err); + + int transferred; + if (err = libusb_bulk_transfer(devh, 0x01, packet, len, &transferred, 1000)) + die("Transfer failed: error %d\n", err); + if (transferred != len) + die("Short transfer: %d out of %d bytes", transferred, len); + + return 0; +}