diff --git a/applications/send_datagram/main.cxx b/applications/send_datagram/main.cxx index 1815d32fa..c8c626bff 100644 --- a/applications/send_datagram/main.cxx +++ b/applications/send_datagram/main.cxx @@ -241,6 +241,7 @@ int appl_main(int argc, char *argv[]) } if (wait_for_response) { g_datagram_can.registry()->insert(&g_node, payload[0], &printer); + g_datagram_can.registry()->insert(&g_node, payload[0] ^ 1, &printer); } Buffer *b; mainBufferPool->alloc(&b); @@ -256,11 +257,11 @@ int appl_main(int argc, char *argv[]) fprintf(stderr, "Datagram send result: %04x\n", client->result()); if (!(client->result() & DatagramClient::OK_REPLY_PENDING)) { LOG(INFO, "Target node indicates no response pending."); - } else if (wait_for_response) { - printer.wait(); } if (wait_for_response) { + printer.wait(); g_datagram_can.registry()->erase(&g_node, payload[0], &printer); + g_datagram_can.registry()->erase(&g_node, payload[0] ^ 1, &printer); } } diff --git a/applications/usb_can/main.cxx b/applications/usb_can/main.cxx index 9d968c952..9f519e0f0 100644 --- a/applications/usb_can/main.cxx +++ b/applications/usb_can/main.cxx @@ -45,9 +45,10 @@ #include "utils/GridConnectHub.hxx" #include "utils/Hub.hxx" #include "utils/HubDevice.hxx" +#include "utils/HubDeviceSelect.hxx" #include "utils/blinker.h" -Executor<1> g_executor("g_executor", 0, 1024); +Executor<1> g_executor(NO_THREAD{}); Service g_service(&g_executor); CanHubFlow can_hub0(&g_service); @@ -62,7 +63,8 @@ OVERRIDE_CONST(main_thread_stack_size, 2500); OVERRIDE_CONST(main_thread_stack_size, 900); #endif -OVERRIDE_CONST(gridconnect_bridge_max_incoming_packets, 10); +OVERRIDE_CONST(gridconnect_bridge_max_incoming_packets, 5); +OVERRIDE_CONST(gridconnect_port_max_incoming_packets, 2); /** Entry point to application. * @param argc number of command line arguments @@ -73,19 +75,17 @@ int appl_main(int argc, char* argv[]) { int serial_fd = ::open("/dev/serUSB0", O_RDWR); // or /dev/ser0 HASSERT(serial_fd >= 0); - create_gc_port_for_can_hub(&can_hub0, serial_fd); + ::fcntl(serial_fd, F_SETFL, O_NONBLOCK); + + create_gc_port_for_can_hub(&can_hub0, serial_fd, nullptr, true); int can_fd = ::open("/dev/can0", O_RDWR); HASSERT(can_fd >= 0); + ::fcntl(can_fd, F_SETFL, O_NONBLOCK); - FdHubPort can_hub_port( + HubDeviceSelect can_hub_port( &can_hub0, can_fd, EmptyNotifiable::DefaultInstance()); - while(1) { - sleep(1); - resetblink(1); - sleep(1); - resetblink(0); - } + g_executor.thread_body(); return 0; } diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/HwInit.cxx b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/HwInit.cxx new file mode 100644 index 000000000..9531c1edb --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/HwInit.cxx @@ -0,0 +1,281 @@ +/** \copyright + * Copyright (c) 2012, Stuart W Baker + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \file HwInit.cxx + * This file represents the hardware initialization for the TI Tiva MCU. + * + * @author Stuart W. Baker + * @date 5 January 2013 + */ + +#include +#include + +#include "freertos_drivers/st/stm32f_hal_conf.hxx" +#include "stm32f0xx_hal_rcc.h" +#include "stm32f0xx_hal_flash.h" +#include "stm32f0xx_hal_gpio.h" +#include "stm32f0xx_hal_gpio_ex.h" +#include "stm32f0xx_hal_dma.h" +#include "stm32f0xx_hal_tim.h" + +#include "os/OS.hxx" +#include "Stm32Uart.hxx" +#include "Stm32Can.hxx" +#include "Stm32UsbCdc.hxx" +#include "Stm32EEPROMEmulation.hxx" +#include "hardware.hxx" + +/** override stdin */ +const char *STDIN_DEVICE = "/dev/ser0"; + +/** override stdout */ +const char *STDOUT_DEVICE = "/dev/ser0"; + +/** override stderr */ +const char *STDERR_DEVICE = "/dev/ser0"; + +/** UART 0 serial driver instance */ +// static Stm32Uart uart0("/dev/ser0", USART1, USART1_IRQn); + +/** CAN 0 CAN driver instance */ +static Stm32Can can0("/dev/can0"); + +/** EEPROM emulation driver. The file size might be made bigger. */ +static Stm32EEPROMEmulation eeprom0("/dev/eeprom", 512); + +const size_t EEPROMEmulation::SECTOR_SIZE = 2048; + +/** USB-Serial instance */ +static Stm32UsbCdc serUSB0("/dev/serUSB0"); + +extern "C" { + +/** Blink LED */ +uint32_t blinker_pattern = 0; +static uint32_t rest_pattern = 0; + +void hw_set_to_safe(void) +{ +} + +void resetblink(uint32_t pattern) +{ + BLINKER_RAW_Pin::set(pattern ? true : false); + blinker_pattern = pattern; + /* make a timer event trigger immediately */ +} + +void setblink(uint32_t pattern) +{ + resetblink(pattern); +} + +void timer14_interrupt_handler(void) +{ + // + // Clear the timer interrupt. + // + TIM14->SR = ~TIM_IT_UPDATE; + + // Set output LED. + BLINKER_RAW_Pin::set(rest_pattern & 1); + + // Shift and maybe reset pattern. + rest_pattern >>= 1; + if (!rest_pattern) + { + rest_pattern = blinker_pattern; + } +} + +void diewith(uint32_t pattern) +{ + // vPortClearInterruptMask(0x20); + asm("cpsie i\n"); + + resetblink(pattern); + while (1) + ; +} + +/** Setup the system clock */ +static void clock_setup(void) +{ + /* reset clock configuration to default state */ + RCC->CR = RCC_CR_HSITRIM_4 | RCC_CR_HSION; + while (!(RCC->CR & RCC_CR_HSIRDY)) + ; + +#define USE_EXTERNAL_8_MHz_CLOCK_SOURCE 0 +/* configure PLL: 8 MHz * 6 = 48 MHz */ +#if USE_EXTERNAL_8_MHz_CLOCK_SOURCE + RCC->CR |= RCC_CR_HSEON; + while (!(RCC->CR & RCC_CR_HSERDY)) + ; + RCC->CFGR = RCC_CFGR_PLLMUL6 | RCC_CFGR_PLLSRC_HSE_PREDIV | RCC_CFGR_SW_HSE; + while (!((RCC->CFGR & RCC_CFGR_SWS) == RCC_CFGR_SWS_HSE)) + ; +#else + RCC->CFGR = RCC_CFGR_PLLMUL6 | RCC_CFGR_PLLSRC_HSI_PREDIV | RCC_CFGR_SW_HSI; + while (!((RCC->CFGR & RCC_CFGR_SWS) == RCC_CFGR_SWS_HSI)) + ; +#endif + /* enable PLL */ + RCC->CR |= RCC_CR_PLLON; + while (!(RCC->CR & RCC_CR_PLLRDY)) + ; + + /* set PLL as system clock */ + RCC->CFGR = (RCC->CFGR & (~RCC_CFGR_SW)) | RCC_CFGR_SW_PLL; + while (!((RCC->CFGR & RCC_CFGR_SWS) == RCC_CFGR_SWS_PLL)) + ; + + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0}; + /* Select HSI48 as USB clock source */ + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USB; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_HSI48; + HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct); + + /*Configure the clock recovery system (CRS)********************************/ + + /*Enable CRS Clock*/ + __HAL_RCC_CRS_CLK_ENABLE(); + + RCC_CRSInitTypeDef RCC_CRSInitStruct = {0}; + + /* Default Synchro Signal division factor (not divided) */ + RCC_CRSInitStruct.Prescaler = RCC_CRS_SYNC_DIV1; + + /* Set the SYNCSRC[1:0] bits according to CRS_Source value */ + RCC_CRSInitStruct.Source = RCC_CRS_SYNC_SOURCE_USB; + + /* HSI48 is synchronized with USB SOF at 1KHz rate */ + RCC_CRSInitStruct.ReloadValue = + __HAL_RCC_CRS_RELOADVALUE_CALCULATE(48000000, 1000); + RCC_CRSInitStruct.ErrorLimitValue = RCC_CRS_ERRORLIMIT_DEFAULT; + + /* Set the TRIM[5:0] to the default value*/ + RCC_CRSInitStruct.HSI48CalibrationValue = 0x20; + + /* Start automatic synchronization */ + HAL_RCCEx_CRSConfig(&RCC_CRSInitStruct); +} + +/** Initialize the processor hardware. + */ +void hw_preinit(void) +{ + /* Globally disables interrupts until the FreeRTOS scheduler is up. */ + asm("cpsid i\n"); + + /* these FLASH settings enable opertion at 48 MHz */ + __HAL_FLASH_PREFETCH_BUFFER_ENABLE(); + __HAL_FLASH_SET_LATENCY(FLASH_LATENCY_1); + + /* setup the system clock */ + clock_setup(); + + /* enable peripheral clocks */ + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_USART1_CLK_ENABLE(); + __HAL_RCC_CAN1_CLK_ENABLE(); + __HAL_RCC_TIM14_CLK_ENABLE(); + __HAL_RCC_USB_CLK_ENABLE(); + + /* setup pinmux */ + GPIO_InitTypeDef gpio_init; + memset(&gpio_init, 0, sizeof(gpio_init)); + + /* USART1 pinmux on PA9 and PA10 */ + gpio_init.Mode = GPIO_MODE_AF_PP; + gpio_init.Pull = GPIO_PULLUP; + gpio_init.Speed = GPIO_SPEED_FREQ_HIGH; + gpio_init.Alternate = GPIO_AF1_USART1; + gpio_init.Pin = GPIO_PIN_9; + HAL_GPIO_Init(GPIOA, &gpio_init); + gpio_init.Pin = GPIO_PIN_10; + HAL_GPIO_Init(GPIOA, &gpio_init); + + /* CAN pinmux !!OPEN-DRAIN!! on PB8 and PB9 */ + gpio_init.Mode = GPIO_MODE_AF_OD; + gpio_init.Speed = GPIO_SPEED_FREQ_HIGH; + gpio_init.Pull = GPIO_PULLUP; + gpio_init.Alternate = GPIO_AF4_CAN; + gpio_init.Pin = GPIO_PIN_8; + HAL_GPIO_Init(GPIOB, &gpio_init); + gpio_init.Pin = GPIO_PIN_9; + HAL_GPIO_Init(GPIOB, &gpio_init); + + // USB Pins + // Configure USB DM and DP pins. This is optional, and maintained only for + // user guidance. + gpio_init.Pin = (GPIO_PIN_11 | GPIO_PIN_12); + gpio_init.Mode = GPIO_MODE_INPUT; + gpio_init.Pull = GPIO_NOPULL; + gpio_init.Speed = GPIO_SPEED_FREQ_HIGH; + HAL_GPIO_Init(GPIOA, &gpio_init); + + GpioInit::hw_init(); + + /* Initializes the blinker timer. */ + TIM_HandleTypeDef TimHandle; + memset(&TimHandle, 0, sizeof(TimHandle)); + TimHandle.Instance = TIM14; + TimHandle.Init.Period = configCPU_CLOCK_HZ / 10000 / 8; + TimHandle.Init.Prescaler = 10000; + TimHandle.Init.ClockDivision = 0; + TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; + TimHandle.Init.RepetitionCounter = 0; + if (HAL_TIM_Base_Init(&TimHandle) != HAL_OK) + { + /* Initialization Error */ + HASSERT(0); + } + if (HAL_TIM_Base_Start_IT(&TimHandle) != HAL_OK) + { + /* Starting Error */ + HASSERT(0); + } + SetInterruptPriority(TIM14_IRQn, 0); + NVIC_EnableIRQ(TIM14_IRQn); +} + +void hw_postinit(void) +{ + serUSB0.hw_postinit(); +} + +extern void dcd_int_handler(int); + +void usb_interrupt_handler(void) +{ + dcd_int_handler(0); +} + +} // extern "C" diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/Makefile b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/Makefile new file mode 100644 index 000000000..969e41601 --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/Makefile @@ -0,0 +1,72 @@ +APP_PATH ?= $(realpath ../..) +-include $(APP_PATH)/config.mk +-include local_config.mk + +SUBDIRS= + +OPENMRNPATH ?= $(shell \ +sh -c "if [ \"X`printenv OPENMRNPATH`\" != \"X\" ]; then printenv OPENMRNPATH; \ + elif [ -d /opt/openmrn/src ]; then echo /opt/openmrn; \ + elif [ -d ~/openmrn/src ]; then echo ~/openmrn; \ + elif [ -d ../../../src ]; then echo ../../..; \ + else echo OPENMRNPATH not found; fi" \ +) + +-include $(OPENMRNPATH)/etc/config.mk +include $(OPENMRNPATH)/etc/stm32cubef0.mk + +CFLAGSEXTRA += -DSTM32F072xB +CXXFLAGSEXTRA += -DSTM32F072xB +SYSLIBRARIESEXTRA += -lfreertos_drivers_stm32cubef071xb_2xb -lfreertos_drivers_tinyusb_stm32f072xb +include find-emulator.mk + +export TARGET := freertos.armv6m + +include $(OPENMRNPATH)/etc/prog.mk + +ifndef DEFAULT_ADDRESS +DEFAULT_ADDRESS=0xff +endif + +include $(OPENMRNPATH)/etc/node_id.mk + +all: $(EXECUTABLE).bin + +# How to use: make multibin ADDRESS=0x20 ADDRHIGH=0x45 NUM=3 +# starting address, high bits (user range), count +multibin: + for i in $$(seq 1 $(NUM)) ; do $(MAKE) $(EXECUTABLE).bin ADDRESS=$$(printf 0x%02x $$(($(ADDRESS)+$$i))) ; cp $(EXECUTABLE).bin $(EXECUTABLE).$(MCU_SHORT).$$(printf %02x%02x $(ADDRHIGH) $$(($(ADDRESS)+$$i-1))).bin ; done + +startup.o: CFLAGS+= -DHAL_RCC_GetSysClockFreq=OBSOLETEClock + +ifeq ($(call find_missing_deps,OPENOCDPATH OPENOCDSCRIPTSPATH),) + +flash: $(EXECUTABLE)$(EXTENTION) $(EXECUTABLE).bin $(EXECUTABLE).lst + @if ps ax -o comm | grep -q openocd ; then echo openocd already running. quit existing first. ; exit 1 ; fi + $(GDB) $< -ex "target remote | $(OPENOCDPATH)/openocd -c \"gdb_port pipe\" --search $(OPENOCDSCRIPTSPATH) $(OPENOCDARGS)" -ex "monitor reset halt" -ex "load" -ex "monitor reset init" -ex "monitor reset run" -ex "detach" -ex "quit" + +lflash: $(EXECUTABLE).bin $(EXECUTABLE).lst + @if ps ax -o comm | grep -q openocd ; then echo openocd already running. quit existing first. ; exit 1 ; fi + $(GDB) $< -ex "target remote | $(OPENOCDPATH)/openocd -c \"gdb_port pipe\" --search $(OPENOCDSCRIPTSPATH) $(OPENOCDARGS)" -ex "monitor reset halt" -ex "monitor program $< 0x08000000 verify reset exit" -ex "monitor reset run" -ex "detach" -ex "quit" + +RELEASEDIR=./releases/$(shell date +%Y-%m-%d-%H-%M-%S) + +release: $(EXECUTABLE)$(EXTENTION) $(EXECUTABLE).bin $(EXECUTABLE).lst + mkdir -p $(RELEASEDIR) + cp -fax $(EXECUTABLE){$(EXTENTION),.bin,.lst,.map} $(RELEASEDIR) + ln -sf $(RELEASEDIR) latest-release + +release-flash: + $(GDB) latest-release/*elf -ex "target remote | $(OPENOCDPATH)/openocd -c \"gdb_port pipe\" --search $(OPENOCDSCRIPTSPATH) $(OPENOCDARGS)" -ex "monitor reset halt" -ex "load" -ex "monitor reset init" -ex "monitor reset run" -ex "detach" -ex "quit" + +gdb: + @if ps ax -o comm | grep -q openocd ; then echo openocd already running. quit existing first. ; exit 1 ; fi + $(GDB) $(EXECUTABLE)$(EXTENTION) -ex "target remote | $(OPENOCDPATH)/openocd -c \"gdb_port pipe\" --search $(OPENOCDSCRIPTSPATH) $(OPENOCDARGS)" -ex "continue" # -ex "monitor reset halt" + +else + +flash gdb: + echo OPENOCD not found ; exit 1 + +endif + diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/find-emulator.mk b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/find-emulator.mk new file mode 120000 index 000000000..5f885c0fb --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/find-emulator.mk @@ -0,0 +1 @@ +../../../../boards/st-stm32f0x1_x2_x8-generic/find-emulator.mk \ No newline at end of file diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/hardware.hxx b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/hardware.hxx new file mode 100644 index 000000000..b99fd9333 --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/hardware.hxx @@ -0,0 +1,14 @@ + +#include "Stm32Gpio.hxx" +#include "utils/GpioInitializer.hxx" + +GPIO_PIN(LED_GREEN_RAW, LedPin, F, 0); +using LED_GREEN_Pin = ::InvertedGpio; +GPIO_PIN(LED_OTHER_RAW, LedPin, F, 1); +using LED_OTHER_Pin = ::InvertedGpio; + +typedef GpioInitializer< // + LED_GREEN_RAW_Pin, LED_OTHER_RAW_Pin> + GpioInit; + +typedef LED_GREEN_Pin BLINKER_RAW_Pin; diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/memory_map.ld b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/memory_map.ld new file mode 120000 index 000000000..bbfbdd717 --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/memory_map.ld @@ -0,0 +1 @@ +../../../../boards/st-stm32f072b-discovery/memory_map.ld \ No newline at end of file diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/startup.c b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/startup.c new file mode 120000 index 000000000..f931754c5 --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/startup.c @@ -0,0 +1 @@ +../../../../boards/st-stm32f0x1_x2_x8-generic/startup.c \ No newline at end of file diff --git a/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/target.ld b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/target.ld new file mode 120000 index 000000000..0d43368b5 --- /dev/null +++ b/applications/usb_can/targets/freertos.armv6m.stm32f072cb-gol/target.ld @@ -0,0 +1 @@ +../../../../boards/st-stm32f0x1_x2_x8-generic/target.ld \ No newline at end of file diff --git a/arduino/OpenMRNLite.cpp b/arduino/OpenMRNLite.cpp index 497ad4892..d38f6b283 100644 --- a/arduino/OpenMRNLite.cpp +++ b/arduino/OpenMRNLite.cpp @@ -34,6 +34,8 @@ #include +OVERRIDE_CONST(gridconnect_bridge_max_incoming_packets, 5); + namespace openmrn_arduino { OpenMRN::OpenMRN(openlcb::NodeID node_id) diff --git a/arduino/OpenMRNLite.h b/arduino/OpenMRNLite.h index b7c22bf29..87bcdbf29 100644 --- a/arduino/OpenMRNLite.h +++ b/arduino/OpenMRNLite.h @@ -39,6 +39,7 @@ #include #include "CDIXMLGenerator.hxx" +#include "executor/Notifiable.hxx" #include "freertos_drivers/arduino/Can.hxx" #include "freertos_drivers/arduino/WifiDefs.hxx" #include "openlcb/SimpleStack.hxx" @@ -178,14 +179,26 @@ template class SerialBridge : public Executable /// Handles data coming in from the serial port and sends it to OpenMRN. void loop_for_read() { + if (!bn_.is_done()) + { + // Blocked because data we've just read has not yet been processed. + return; + } int av = port_->available(); if (av <= 0) { return; } + // We don't read too many bytes into one buffer. 64 is exactly one USB + // packet's length. + if (av > 64) + { + av = 64; + } auto *b = txtHub_.alloc(); b->data()->skipMember_ = &writePort_; b->data()->resize(av); + b->set_done(bn_.reset(EmptyNotifiable::DefaultInstance())); port_->readBytes((char*)b->data()->data(), b->data()->size()); txtHub_.send(b); } @@ -247,6 +260,14 @@ template class SerialBridge : public Executable size_t writeOfs_; /// Hub for the textual data. HubFlow txtHub_{service_}; + /// This notifiable will know whether the txt packet we read from the + /// serial has been processed by the hub. This is a pushback mechanism for + /// us not to run out of memory when there is too many packets coming from + /// the host or socket. + /// + /// This notifiable is active while there is a message in flight to the txt + /// hub. + BarrierNotifiable bn_; }; /// Bridge class that connects a native CAN controller to the OpenMRN core diff --git a/boards/st-stm32f0x1_x2_x8-generic/find-emulator.mk b/boards/st-stm32f0x1_x2_x8-generic/find-emulator.mk new file mode 100644 index 000000000..68a06c8a8 --- /dev/null +++ b/boards/st-stm32f0x1_x2_x8-generic/find-emulator.mk @@ -0,0 +1,21 @@ +# Searches the connected USB devices for a known emulator for the +# STM32F0. Exports the OPENOCDARGS variable when found. +# All emulators here are configured to use 2-wire SWD mode. + +USBLIST=$(shell lsusb) + +ifneq ($(findstring 0483:374b,$(USBLIST)),) +$(info emulator ST-Link V2.1 or nucleo) +OPENOCDARGS = -f board/st_nucleo_f0.cfg +else ifneq ($(findstring 0451:bef3,$(USBLIST)),) +$(info emulator TI CC3220SF launchpad CMSIS-DAP) +OPENOCDARGS = -f interface/xds110.cfg -c 'transport select swd' -c 'adapter_khz 950' -f target/stm32f0x.cfg -c 'reset_config srst_only' -c 'adapter_khz 950' +else ifneq ($(findstring 15ba:002a,$(USBLIST)),) +$(info emulator Olimex arm-usb-tiny-H) +OPENOCDARGS = -f interface/ftdi/olimex-arm-usb-tiny-h.cfg -f interface/ftdi/swd-resistor-hack.cfg -f target/stm32f0x.cfg +else ifneq ($(findstring 15ba:002b,$(USBLIST)),) +$(info emulator Olimex arm-usb-ocd-H) +OPENOCDARGS = -f interface/ftdi/olimex-arm-usb-ocd-h.cfg -f interface/ftdi/swd-resistor-hack.cfg -f target/stm32f0x.cfg +else +OPENOCDARGS = " -emulator-not-found-add-your-usb-device-to-boards_st-stm32f0x_find-emulator.mk " +endif diff --git a/etc/path.mk b/etc/path.mk index a8462bd38..4a27f28a2 100644 --- a/etc/path.mk +++ b/etc/path.mk @@ -718,6 +718,18 @@ SXMLCPATH:=$(TRYPATH) endif endif #SXMLCPATH +################ TinyUSB ################## +ifndef TINYUSBPATH +SEARCHPATH := \ + /opt/tinyusb/default \ + /opt/tinyusb/tinyusb \ + +TRYPATH:=$(call findfirst,src/tusb.c,$(SEARCHPATH)) +ifneq ($(TRYPATH),) +TINYUSBPATH:=$(TRYPATH) +endif +endif #TINYUSBPATH + endif # ifndef OPENMRN_EXPLICIT_DEPS_ONLY endif # if $(OS) != Windows_NT diff --git a/etc/tinyusb.mk b/etc/tinyusb.mk new file mode 100644 index 000000000..60ad2b978 --- /dev/null +++ b/etc/tinyusb.mk @@ -0,0 +1,14 @@ +include $(OPENMRNPATH)/etc/path.mk + +ifdef TINYUSBPATH +INCLUDES += -I$(TINYUSBPATH)/src \ + -I$(OPENMRNPATH)/src/freertos_drivers/tinyusb + +TUSB_VPATH = $(TINYUSBPATH)/src \ + $(TINYUSBPATH)/src/class/cdc \ + $(TINYUSBPATH)/src/common \ + $(TINYUSBPATH)/src/device \ + +endif + +DEPS += TINYUSBPATH diff --git a/src/freertos_drivers/sources b/src/freertos_drivers/sources index 1d101a5f7..a116413c1 100644 --- a/src/freertos_drivers/sources +++ b/src/freertos_drivers/sources @@ -87,7 +87,8 @@ SUBDIRS += drivers_lpc2368 endif ifeq ($(TARGET),freertos.armv6m) -SUBDIRS += drivers_lpc11cxx stm32cubef071xb_2xb stm32cubef091xc +SUBDIRS += drivers_lpc11cxx stm32cubef071xb_2xb stm32cubef091xc \ + tinyusb_stm32f072xb # Avoids exception handling from operator new. CXXSRCS += c++_operators.cxx # Implementations for the __atomic_* "builtins" for GCC. diff --git a/src/freertos_drivers/st/Stm32UsbCdc.cxx b/src/freertos_drivers/st/Stm32UsbCdc.cxx new file mode 100644 index 000000000..0e8c5ef14 --- /dev/null +++ b/src/freertos_drivers/st/Stm32UsbCdc.cxx @@ -0,0 +1,36 @@ +/** \copyright + * Copyright (c) 2023, Balazs Racz + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \file Stm32UsbCdc.cxx + * Device driver for the STM32 devices using the TinyUsb stack. + * + * @author Balazs Racz + * @date 13 Nov 2023 + */ + +#include "freertos_drivers/st/Stm32UsbCdc.hxx" + +#include "freertos_drivers/tinyusb/TinyUsbCdcImpl.hxx" diff --git a/src/freertos_drivers/st/Stm32UsbCdc.hxx b/src/freertos_drivers/st/Stm32UsbCdc.hxx new file mode 100644 index 000000000..ae42d6fd4 --- /dev/null +++ b/src/freertos_drivers/st/Stm32UsbCdc.hxx @@ -0,0 +1,57 @@ +/** \copyright + * Copyright (c) 2023, Balazs Racz + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \file Stm32UsbCdc.hxx + * Device driver for the STM32 devices using the TinyUsb stack. + * + * @author Balazs Racz + * @date 13 Nov 2023 + */ + +#ifndef _FREERTOS_DRIVERS_ST_STM32CDCUSB_HXX_ +#define _FREERTOS_DRIVERS_ST_STM32CDCUSB_HXX_ + +#include "freertos_drivers/tinyusb/TinyUsbCdc.hxx" + +// Usage: +// +// Define an instance in HwInit.cxx. +// `Stm32UsbCdc serialUsb("/dev/serialusb");` +// in `hw_postinit()`, call `serialUsb.hw_postinit();` +// +// Make sure that the USB clock is set up in `hw_preinit()`. Use the Clock +// Recovery System if there is no crystal. Set up the USB pin map in +// hw_preinit. +class Stm32UsbCdc : public TinyUsbCdc +{ +public: + Stm32UsbCdc(const char *name) + : TinyUsbCdc(name) + { + } +}; + +#endif // _FREERTOS_DRIVERS_ST_STM32CDCUSB_HXX_ diff --git a/src/freertos_drivers/ti/TivaGPIO.hxx b/src/freertos_drivers/ti/TivaGPIO.hxx index 6ca080cac..9fc92da95 100644 --- a/src/freertos_drivers/ti/TivaGPIO.hxx +++ b/src/freertos_drivers/ti/TivaGPIO.hxx @@ -384,6 +384,41 @@ struct GpioInputNP : public GpioInputPin { }; +/// Common class for GPIO input/output pins. +template struct GpioInputOutputPin : public GpioShared +{ +public: + using Defs::GPIO_PERIPH; + using Defs::GPIO_BASE; + using Defs::GPIO_PIN; + /// Initializes the hardware pin. + static void hw_init() + { + MAP_SysCtlPeripheralEnable(GPIO_PERIPH); + MAP_GPIOPinTypeGPIOInput(GPIO_BASE, GPIO_PIN); + MAP_GPIOPadConfigSet(GPIO_BASE, GPIO_PIN, GPIO_STRENGTH_2MA, + GPIO_PIN_TYPE_STD); + } + /// Sets the hardware pin to a safe state. + static void hw_set_to_safe() + { + hw_init(); + } + /// Sets the direction of the I/O pin. + /// @param direction direction to set pin to + static void set_direction(Gpio::Direction direction) + { + MAP_GPIODirModeSet(GPIO_BASE, GPIO_PIN, + direction == Gpio::Direction::DINPUT ? + GPIO_DIR_MODE_IN : GPIO_DIR_MODE_OUT); + } + /// @return true if the pin is set to an output. + static bool is_output() + { + return (MAP_GPIODirModeGet(GPIO_BASE, GPIO_PIN) == GPIO_DIR_MODE_OUT); + } +}; + /// GPIO Input pin in ADC configuration (analog). /// /// This pin cannot be read or written directly (will fail compilation). diff --git a/src/freertos_drivers/tinyusb/TinyUsbCdc.hxx b/src/freertos_drivers/tinyusb/TinyUsbCdc.hxx new file mode 100644 index 000000000..e17e2c7e8 --- /dev/null +++ b/src/freertos_drivers/tinyusb/TinyUsbCdc.hxx @@ -0,0 +1,108 @@ +/** \copyright + * Copyright (c) 2023, Balazs Racz + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \file TinyUsbCdc.hxx + * Base class for implementing a CDC Device driver using the TinyUsb stack. + * + * @author Balazs Racz + * @date 13 Nov 2023 + */ + +#ifndef _FREERTOS_DRIVERS_TINYUSB_TINYUSBCDC_HXX_ +#define _FREERTOS_DRIVERS_TINYUSB_TINYUSBCDC_HXX_ + +#include "Devtab.hxx" +#include "os/OS.hxx" +#include "utils/Singleton.hxx" + +class TinyUsbCdc : public Node, public Singleton +{ +public: + TinyUsbCdc(const char *name) + : Node(name) + { + } + ~TinyUsbCdc(); + + // Call this function once from hw_postinit. + void hw_postinit(); + + // Called from static C callback functions. + inline void rx_available(); + inline void tx_complete(); + +private: + void enable() override + { + } + void disable() override + { + } + void flush_buffers() override + { + } + + /** Device select method. Default impementation returns true. + * @param file reference to the file + * @param mode FREAD for read active, FWRITE for write active, 0 for + * exceptions + * @return true if active, false if inactive + */ + bool select(File *file, int mode) override; + + /** Read from a file or device. + * @param file file reference for this device + * @param buf location to place read data + * @param count number of bytes to read + * @return number of bytes read upon success, -1 upon failure with errno + * containing the cause + */ + ssize_t read(File *file, void *buf, size_t count) override; + + /** Write to a file or device. + * @param file file reference for this device + * @param buf location to find write data + * @param count number of bytes to write + * @return number of bytes written upon success, -1 upon failure with errno + * containing the cause + */ + ssize_t write(File *file, const void *buf, size_t count) override; + + /// Thread for running the tiny usb device stack. + class UsbDeviceThread : public OSThread + { + public: + void *entry() override; + } usbdThread_; + + /// Handles the select for incoming data (read). + Device::SelectInfo selectInfoRead_; + + /// Handles the select for outgoing data (write). + Device::SelectInfo selectInfoWrite_; +}; + +#endif // _FREERTOS_DRIVERS_TINYUSB_TINYUSBCDC_HXX_ diff --git a/src/freertos_drivers/tinyusb/TinyUsbCdcImpl.hxx b/src/freertos_drivers/tinyusb/TinyUsbCdcImpl.hxx new file mode 100644 index 000000000..030f0d2fe --- /dev/null +++ b/src/freertos_drivers/tinyusb/TinyUsbCdcImpl.hxx @@ -0,0 +1,457 @@ +/** \copyright + * Copyright (c) 2023, Balazs Racz + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * \file TinyUsbCdcImpl.hxx + * Base class for implementing a CDC Device driver using the TinyUsb stack. + * + * @author Balazs Racz + * @date 13 Nov 2023 + */ + +// This file does not compile standalone, instead it has to be included in the +// board-speciofic Cdc driver implementation cxx file. +#ifndef _FREERTOS_DRIVERS_TINYUSB_TINYUSBCDCIMPL_HXX_ +#define _FREERTOS_DRIVERS_TINYUSB_TINYUSBCDCIMPL_HXX_ + +#include "freertos_drivers/tinyusb/TinyUsbCdc.hxx" + +#include "freertos_drivers/common/DeviceBuffer.hxx" +#include "os/OS.hxx" +#include + +#include "tusb.h" + +#ifndef USBD_STACK_SIZE +#define USBD_STACK_SIZE 768 +#endif + +#ifndef USBD_TASK_PRIO +#define USBD_TASK_PRIO 3 +#endif + +TinyUsbCdc::~TinyUsbCdc() +{ +} + +void TinyUsbCdc::hw_postinit() +{ + usbdThread_.start("tinyusb_device", USBD_TASK_PRIO, USBD_STACK_SIZE); +} + +void *TinyUsbCdc::UsbDeviceThread::entry() +{ + // init device stack on configured roothub port + // This should be called after scheduler/kernel is started. + // Otherwise it could cause kernel issue since USB IRQ handler does use RTOS + // queue API. + tud_init(BOARD_TUD_RHPORT); + + // RTOS forever loop + while (1) + { + // put this thread to waiting state until there is new events + tud_task(); + + // following code only run if tud_task() process at least 1 event + tud_cdc_write_flush(); + } +} + +ssize_t TinyUsbCdc::read(File *file, void *buf, size_t count) +{ + unsigned char *data = (unsigned char *)buf; + ssize_t result = 0; + + while (count) + { + portENTER_CRITICAL(); + /* We limit the amount of bytes we read with each iteration in order + * to limit the amount of time that interrupts are disabled and + * preserve our real-time performance. + */ + size_t bytes_read = tud_cdc_read(data, count < 64 ? count : 64); + portEXIT_CRITICAL(); + + if (bytes_read == 0) + { + /* no more data to receive */ + if ((file->flags & O_NONBLOCK) || result > 0) + { + break; + } + else + { + /* wait for data to come in, this call will release the + * critical section lock. + */ + DeviceBufferBase::block_until_condition(file, true); + } + } + + count -= bytes_read; + result += bytes_read; + data += bytes_read; + } + + if (!result && (file->flags & O_NONBLOCK)) + { + return -EAGAIN; + } + + return result; +} + +ssize_t TinyUsbCdc::write(File *file, const void *buf, size_t count) +{ + const unsigned char *data = (const unsigned char *)buf; + ssize_t result = 0; + + while (count) + { + portENTER_CRITICAL(); + /* We limit the amount of bytes we write with each iteration in order + * to limit the amount of time that interrupts are disabled and + * preserve our real-time performance. + */ + size_t bytes_written = tud_cdc_write(data, count < 64 ? count : 64); + portEXIT_CRITICAL(); + + if (bytes_written == 0) + { + /* no more space to send data */ + if ((file->flags & O_NONBLOCK) || result > 0) + { + break; + } + else + { + /* wait for space to be available, this call will release the + * critical section lock. + */ + DeviceBufferBase::block_until_condition(file, false); + } + } + + count -= bytes_written; + result += bytes_written; + data += bytes_written; + } + + if (!result && (file->flags & O_NONBLOCK)) + { + return -EAGAIN; + } + + if (result) + { + tud_cdc_write_flush(); + } + + return result; +} + +/** Device select method. Default impementation returns true. + * @param file reference to the file + * @param mode FREAD for read active, FWRITE for write active, 0 for + * exceptions + * @return true if active, false if inactive + */ +bool TinyUsbCdc::select(File *file, int mode) +{ + bool retval = false; + switch (mode) + { + case FREAD: + portENTER_CRITICAL(); + if (tud_cdc_available() > 0) + { + retval = true; + } + else + { + Device::select_insert(&selectInfoRead_); + } + portEXIT_CRITICAL(); + break; + case FWRITE: + portENTER_CRITICAL(); + if (tud_cdc_write_available() > 0) + { + retval = true; + } + else + { + Device::select_insert(&selectInfoWrite_); + } + portEXIT_CRITICAL(); + break; + default: + case 0: + /* we don't support any exceptions */ + break; + } + return retval; +} + +inline void TinyUsbCdc::rx_available() +{ + Device::select_wakeup(&selectInfoRead_); +} + +inline void TinyUsbCdc::tx_complete() +{ + Device::select_wakeup(&selectInfoWrite_); +} + +extern "C" { + +// Invoked when CDC interface received data from host +void tud_cdc_rx_cb(uint8_t itf) +{ + (void)itf; + Singleton::instance()->rx_available(); +} + +// Invoked when a TX is complete and therefore space becomes available in TX +// buffer +void tud_cdc_tx_complete_cb(uint8_t itf) +{ + (void)itf; + Singleton::instance()->tx_complete(); +} + +// ===================== USB DESCRIPTORS ============================= + +/* A combination of interfaces must have a unique product id, since PC will save + * device driver after the first plug. Same VID/PID with different interface e.g + * MSC (first), then CDC (later) will possibly cause system error on PC. + * + * Auto ProductID layout's Bitmap: + * [MSB] HID | MSC | CDC [LSB] + */ +#define _PID_MAP(itf, n) ((CFG_TUD_##itf) << (n)) +#define USB_PID \ + (0x4000 | _PID_MAP(CDC, 0) | _PID_MAP(MSC, 1) | _PID_MAP(HID, 2) | \ + _PID_MAP(MIDI, 3) | _PID_MAP(VENDOR, 4)) + +#define USB_VID 0xCAFE +#define USB_BCD 0x0200 + +// Invoked when received GET DEVICE DESCRIPTOR +// Application return pointer to descriptor +uint8_t const *tud_descriptor_device_cb(void) +{ + static tusb_desc_device_t const desc_device = { + .bLength = sizeof(tusb_desc_device_t), + .bDescriptorType = TUSB_DESC_DEVICE, + .bcdUSB = USB_BCD, + + // Use Interface Association Descriptor (IAD) for CDC + // As required by USB Specs IAD's subclass must be common class (2) and + // protocol must be IAD (1) + .bDeviceClass = TUSB_CLASS_MISC, + .bDeviceSubClass = MISC_SUBCLASS_COMMON, + .bDeviceProtocol = MISC_PROTOCOL_IAD, + + .bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE, + + .idVendor = USB_VID, + .idProduct = USB_PID, + .bcdDevice = 0x0100, + + .iManufacturer = 0x01, + .iProduct = 0x02, + .iSerialNumber = 0x03, + + .bNumConfigurations = 0x01}; + + return (uint8_t const *)&desc_device; +} + +enum +{ + ITF_NUM_CDC = 0, + ITF_NUM_CDC_DATA, + ITF_NUM_TOTAL +}; + +#define EPNUM_CDC_NOTIF 0x81 +#define EPNUM_CDC_OUT 0x02 +#define EPNUM_CDC_IN 0x82 + +#define CONFIG_TOTAL_LEN (TUD_CONFIG_DESC_LEN + TUD_CDC_DESC_LEN) + +// full speed configuration +uint8_t const desc_fs_configuration[] = { + // Config number, interface count, string index, total length, attribute, + // power in mA + TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 100), + + // Interface number, string index, EP notification address and size, EP data + // address (out, in) and size. + TUD_CDC_DESCRIPTOR( + ITF_NUM_CDC, 4, EPNUM_CDC_NOTIF, 8, EPNUM_CDC_OUT, EPNUM_CDC_IN, 64), +}; + +#if TUD_OPT_HIGH_SPEED +// Per USB specs: high speed capable device must report device_qualifier and +// other_speed_configuration + +// high speed configuration +uint8_t const desc_hs_configuration[] = { + // Config number, interface count, string index, total length, attribute, + // power in mA + TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, CONFIG_TOTAL_LEN, 0x00, 100), + + // Interface number, string index, EP notification address and size, EP data + // address (out, in) and size. + TUD_CDC_DESCRIPTOR( + ITF_NUM_CDC, 4, EPNUM_CDC_NOTIF, 8, EPNUM_CDC_OUT, EPNUM_CDC_IN, 512), +}; + +// other speed configuration +uint8_t desc_other_speed_config[CONFIG_TOTAL_LEN]; + +// device qualifier is mostly similar to device descriptor since we don't change +// configuration based on speed +tusb_desc_device_qualifier_t const desc_device_qualifier = { + .bLength = sizeof(tusb_desc_device_qualifier_t), + .bDescriptorType = TUSB_DESC_DEVICE_QUALIFIER, + .bcdUSB = USB_BCD, + + .bDeviceClass = TUSB_CLASS_MISC, + .bDeviceSubClass = MISC_SUBCLASS_COMMON, + .bDeviceProtocol = MISC_PROTOCOL_IAD, + + .bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE, + .bNumConfigurations = 0x01, + .bReserved = 0x00}; + +// Invoked when received GET DEVICE QUALIFIER DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long +// enough for transfer to complete. device_qualifier descriptor describes +// information about a high-speed capable device that would change if the device +// were operating at the other speed. If not highspeed capable stall this +// request. +uint8_t const *tud_descriptor_device_qualifier_cb(void) +{ + return (uint8_t const *)&desc_device_qualifier; +} + +// Invoked when received GET OTHER SEED CONFIGURATION DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long +// enough for transfer to complete Configuration descriptor in the other speed +// e.g if high speed then this is for full speed and vice versa +uint8_t const *tud_descriptor_other_speed_configuration_cb(uint8_t index) +{ + (void)index; // for multiple configurations + + // if link speed is high return fullspeed config, and vice versa + // Note: the descriptor type is OHER_SPEED_CONFIG instead of CONFIG + memcpy(desc_other_speed_config, + (tud_speed_get() == TUSB_SPEED_HIGH) ? desc_fs_configuration + : desc_hs_configuration, + CONFIG_TOTAL_LEN); + + desc_other_speed_config[1] = TUSB_DESC_OTHER_SPEED_CONFIG; + + return desc_other_speed_config; +} + +#endif // highspeed + +// Invoked when received GET CONFIGURATION DESCRIPTOR +// Application return pointer to descriptor +// Descriptor contents must exist long enough for transfer to complete +uint8_t const *tud_descriptor_configuration_cb(uint8_t index) +{ + (void)index; // for multiple configurations + +#if TUD_OPT_HIGH_SPEED + // Although we are highspeed, host may be fullspeed. + return (tud_speed_get() == TUSB_SPEED_HIGH) ? desc_hs_configuration + : desc_fs_configuration; +#else + return desc_fs_configuration; +#endif +} + +// array of pointer to string descriptors +char const *string_desc_arr[] = { + (const char[]) {0x09, 0x04}, // 0: is supported language is English (0x0409) + "TinyUSB", // 1: Manufacturer + "TinyUSB Device", // 2: Product + "123456789012", // 3: Serials, should use chip ID + "TinyUSB CDC", // 4: CDC Interface +}; + +static uint16_t _desc_str[32]; + +// Invoked when received GET STRING DESCRIPTOR request +// Application return pointer to descriptor, whose contents must exist long +// enough for transfer to complete +uint16_t const *tud_descriptor_string_cb(uint8_t index, uint16_t langid) +{ + (void)langid; + + uint8_t chr_count; + + if (index == 0) + { + memcpy(&_desc_str[1], string_desc_arr[0], 2); + chr_count = 1; + } + else + { + // Note: the 0xEE index string is a Microsoft OS 1.0 Descriptors. + // https://docs.microsoft.com/en-us/windows-hardware/drivers/usbcon/microsoft-defined-usb-descriptors + + if (!(index < sizeof(string_desc_arr) / sizeof(string_desc_arr[0]))) + return NULL; + + const char *str = string_desc_arr[index]; + + // Cap at max char + chr_count = (uint8_t)strlen(str); + if (chr_count > 31) + chr_count = 31; + + // Convert ASCII string into UTF-16 + for (uint8_t i = 0; i < chr_count; i++) + { + _desc_str[1 + i] = str[i]; + } + } + + // first byte is length (including header), second byte is string type + _desc_str[0] = (uint16_t)((TUSB_DESC_STRING << 8) | (2 * chr_count + 2)); + + return _desc_str; +} + +} // extern "C" + +#endif // _FREERTOS_DRIVERS_TINYUSB_TINYUSBCDCIMPL_HXX_ diff --git a/src/freertos_drivers/tinyusb/tusb_config.h b/src/freertos_drivers/tinyusb/tusb_config.h new file mode 100644 index 000000000..c0ae10b5c --- /dev/null +++ b/src/freertos_drivers/tinyusb/tusb_config.h @@ -0,0 +1,113 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2022 Nathaniel Brough + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + * + */ + +#ifndef _TUSB_CONFIG_H_ +#define _TUSB_CONFIG_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +//--------------------------------------------------------------------+ +// Board Specific Configuration +//--------------------------------------------------------------------+ + +// RHPort number used for device can be defined by board.mk, default to port 0 +#ifndef BOARD_TUD_RHPORT +#define BOARD_TUD_RHPORT 0 +#endif + +// RHPort max operational speed can defined by board.mk +#ifndef BOARD_TUD_MAX_SPEED +#define BOARD_TUD_MAX_SPEED OPT_MODE_DEFAULT_SPEED +#endif + +//-------------------------------------------------------------------- +// Common Configuration +//-------------------------------------------------------------------- + +// defined by compiler flags for flexibility +#ifndef CFG_TUSB_MCU +#error CFG_TUSB_MCU must be defined +#endif + +#ifndef CFG_TUSB_OS +#define CFG_TUSB_OS OPT_OS_FREERTOS +#endif + +#ifndef CFG_TUSB_DEBUG +#define CFG_TUSB_DEBUG 0 +#endif + +// Enable Device stack +#define CFG_TUD_ENABLED 1 + +// Default is max speed that hardware controller could support with on-chip PHY +#define CFG_TUD_MAX_SPEED BOARD_TUD_MAX_SPEED + +/* USB DMA on some MCUs can only access a specific SRAM region with restriction + * on alignment. Tinyusb use follows macros to declare transferring memory so + * that they can be put into those specific section. e.g + * - CFG_TUSB_MEM SECTION : __attribute__ (( section(".usb_ram") )) + * - CFG_TUSB_MEM_ALIGN : __attribute__ ((aligned(4))) + */ +#ifndef CFG_TUSB_MEM_SECTION +#define CFG_TUSB_MEM_SECTION +#endif + +#ifndef CFG_TUSB_MEM_ALIGN +#define CFG_TUSB_MEM_ALIGN __attribute__((aligned(4))) +#endif + +//-------------------------------------------------------------------- +// DEVICE CONFIGURATION +//-------------------------------------------------------------------- + +#ifndef CFG_TUD_ENDPOINT0_SIZE +#define CFG_TUD_ENDPOINT0_SIZE 64 +#endif + +//------------- CLASS -------------// +#define CFG_TUD_CDC 1 +#define CFG_TUD_MSC 0 +#define CFG_TUD_HID 0 +#define CFG_TUD_MIDI 0 +#define CFG_TUD_VENDOR 0 + +// CDC FIFO size of TX and RX +#define CFG_TUD_CDC_RX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) +#define CFG_TUD_CDC_TX_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) + +// CDC Endpoint transfer buffer size, more is faster +#define CFG_TUD_CDC_EP_BUFSIZE (TUD_OPT_HIGH_SPEED ? 512 : 64) + +// MSC Buffer size of Device Mass storage +#define CFG_TUD_MSC_EP_BUFSIZE 512 + +#ifdef __cplusplus +} +#endif + +#endif /* _TUSB_CONFIG_H_ */ diff --git a/src/utils/HubDeviceSelect.cxx b/src/utils/HubDeviceSelect.cxx index 78aad7f4a..d9ec68251 100644 --- a/src/utils/HubDeviceSelect.cxx +++ b/src/utils/HubDeviceSelect.cxx @@ -1,2 +1,10 @@ #include "utils/HubDeviceSelect.hxx" + +#include "nmranet_config.h" + +/// @return the number of packets to limit read input if we are throttling. +int hubdevice_incoming_packet_limit() +{ + return config_gridconnect_port_max_incoming_packets(); +} diff --git a/src/utils/HubDeviceSelect.hxx b/src/utils/HubDeviceSelect.hxx index ea232f47e..f913c6b4d 100644 --- a/src/utils/HubDeviceSelect.hxx +++ b/src/utils/HubDeviceSelect.hxx @@ -46,6 +46,7 @@ #include "executor/StateFlow.hxx" #include "utils/Hub.hxx" +#include "utils/LimitedPool.hxx" /// Generic template for the buffer traits. HubDeviceSelect will not compile on /// this default template because it lacks the necessary definitions. For each @@ -76,6 +77,12 @@ template <> struct SelectBufferInfo { return false; } + + /// @return true if the input needs to be throttled. + static bool limit_input() + { + return true; + } }; /// Partial template specialization of buffer traits for struct-typed hubs. @@ -102,6 +109,12 @@ struct SelectBufferInfo>>> { return true; } + + /// @return true if the input needs to be throttled. + static bool limit_input() + { + return true; + } }; /// Partial template specialization of buffer traits for CAN frame-typed @@ -130,8 +143,18 @@ struct SelectBufferInfo> { { return true; } + + /// @return true if the input needs to be throttled. + static bool limit_input() + { + // We should never throttle a CAN-bus reader. + return false; + } }; +/// @return the number of packets to limit read input if we are throttling. +int hubdevice_incoming_packet_limit(); + /// State flow implementing select-aware fd reads. template class HubDeviceSelectReadFlow : public StateFlowBase { @@ -145,11 +168,23 @@ public: HubDeviceSelectReadFlow(FdHubPortService *device, typename HFlow::port_type *dst, typename HFlow::port_type *skip_member) : StateFlowBase(device) + , shouldThrottle_(SelectBufferInfo::limit_input()) , b_(nullptr) , dst_(dst) , skipMember_(skip_member) { this->start_flow(STATE(allocate_buffer)); + set_limit_input(shouldThrottle_); + } + + void set_limit_input(bool should_throttle) + { + shouldThrottle_ = should_throttle; + int limit = hubdevice_incoming_packet_limit(); + if (shouldThrottle_ && limit < 1000000 && !inputPool_) + { + inputPool_.reset(new LimitedPool(sizeof(buffer_type), limit)); + } } /// Unregisters the current flow from the hub. @@ -173,7 +208,9 @@ public: /// Allocates a new buffer for incoming data. @return next state. Action allocate_buffer() { - return this->allocate_and_call(dst_, STATE(try_read)); + // If inputPool is nullptr, this will use the destination flow's pool, + // which is typically mainBufferPool. + return this->allocate_and_call(dst_, STATE(try_read), inputPool_.get()); } /// Attempts to read into the current buffer from the target @@ -231,10 +268,14 @@ private: /// true iff pending parent->barrier_.notify() bool barrierOwned_{true}; + /// True if we are throttling via a limited pool. + bool shouldThrottle_; /// Helper object for read/write FD asynchronously. StateFlowSelectHelper selectHelper_{this}; /// Buffer that we are currently filling. buffer_type *b_; + /// Throttling input helper. + std::unique_ptr inputPool_; /// Where do we forward the messages we created. typename HFlow::port_type *dst_; /// What should be the source port designation. diff --git a/targets/freertos.armv6m/freertos_drivers/tinyusb_stm32f072xb/Makefile b/targets/freertos.armv6m/freertos_drivers/tinyusb_stm32f072xb/Makefile new file mode 100644 index 000000000..72e52af15 --- /dev/null +++ b/targets/freertos.armv6m/freertos_drivers/tinyusb_stm32f072xb/Makefile @@ -0,0 +1,2 @@ +OPENMRNPATH ?= $(realpath ../../../..) +include $(OPENMRNPATH)/etc/lib.mk diff --git a/targets/freertos.armv6m/freertos_drivers/tinyusb_stm32f072xb/sources b/targets/freertos.armv6m/freertos_drivers/tinyusb_stm32f072xb/sources new file mode 100644 index 000000000..f0e799742 --- /dev/null +++ b/targets/freertos.armv6m/freertos_drivers/tinyusb_stm32f072xb/sources @@ -0,0 +1,25 @@ +include $(OPENMRNPATH)/etc/stm32cubef0.mk +include $(OPENMRNPATH)/etc/tinyusb.mk + +VPATH = $(OPENMRNPATH)/src/freertos_drivers/st \ + $(TUSB_VPATH) \ + $(TINYUSBPATH)/src/portable/st/stm32_fsdev \ + + +CFLAGS += -DSTM32F072xB -DCFG_TUSB_MCU=OPT_MCU_STM32F0 +CXXFLAGS += -DSTM32F072xB -DCFG_TUSB_MCU=OPT_MCU_STM32F0 + +CSRCS += usbd.c \ + usbd_control.c \ + tusb_fifo.c \ + cdc_device.c \ + tusb.c \ + dcd_stm32_fsdev.c \ + + +CXXSRCS += Stm32UsbCdc.cxx + +# This is needed because we are compiling with a C standard, and GCC's builtin +# assembler can only be reached using the internal format. + +dcd_stm32_fsdev.o: CFLAGS+=-Dasm=__asm__