From ad8d89cd3eb89ed28932d719e27f03edf6eb918f Mon Sep 17 00:00:00 2001 From: Silvano Seva Date: Thu, 1 Oct 2020 09:42:24 +0200 Subject: [PATCH] Testing uC/OS-III on STM32F4 MCU --- Makefile.arm | 192 ++++++++++++++++++ openrtx/include/interfaces/gpio.h | 110 ++++++++++ platform/mcu/STM32F4xx/boot/startup.c | 15 +- rtos/uC-CPU/Cfg/cpu_cfg.h | 10 +- .../ARMv7-M/{os_cpu_a.S => os_cpu_a.s} | 0 tests/platform/stm32f4_uC.c | 133 ++++++++++++ 6 files changed, 448 insertions(+), 12 deletions(-) create mode 100644 Makefile.arm create mode 100644 openrtx/include/interfaces/gpio.h rename rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/{os_cpu_a.S => os_cpu_a.s} (100%) create mode 100644 tests/platform/stm32f4_uC.c diff --git a/Makefile.arm b/Makefile.arm new file mode 100644 index 00000000..cf944bb5 --- /dev/null +++ b/Makefile.arm @@ -0,0 +1,192 @@ +## +## OpenDMR - Open Source Firmware for DMR Radios +## + +## +## List here your source files (both .s, .c and .cpp) +## +SRC := tests/platform/stm32f4_uC.c + +## +## Drivers' source files and include directories +## +DRIVERS_INC := -Iplatform/mcu/STM32F4xx -Iplatform/mcu/STM32F4xx/drivers/usb +DRIVERS_SRC := \ +platform/mcu/STM32F4xx/drivers/usb/usb_bsp.c \ +platform/mcu/STM32F4xx/drivers/usb/usb_core.c \ +platform/mcu/STM32F4xx/drivers/usb/usb_dcd.c \ +platform/mcu/STM32F4xx/drivers/usb/usb_dcd_int.c \ +platform/mcu/STM32F4xx/drivers/usb/usbd_desc.c \ +platform/mcu/STM32F4xx/drivers/usb/usbd_core.c \ +platform/mcu/STM32F4xx/drivers/usb/usbd_ioreq.c \ +platform/mcu/STM32F4xx/drivers/usb/usbd_req.c \ +platform/mcu/STM32F4xx/drivers/usb/usbd_usr.c \ +platform/mcu/STM32F4xx/drivers/gpio.c \ +platform/mcu/STM32F4xx/drivers/usb_vcom.c \ +platform/mcu/STM32F4xx/drivers/delays.c + +## +## List here additional static libraries with relative path +## +LIBS := + +## +## List here additional include directories (in the form -Iinclude_dir) +## +INCLUDE_DIRS := -Iopenrtx/include/interfaces + +## +## List here additional defines +## +DEFINES := $(DEFINES) + +## +## Define used to select target processor +## +TARGET := -DSTM32F40_41xxx + +## +## System clock frequency, in hertz. Must be defined and set to correct value +## in order to make drivers working correctly +## +CLK_FREQ := -DHSE_VALUE=8000000 + + +## +## Optimization level +## +OPTLEVEL := -O0 +#OPTLEVEL:= -O2 +#OPTLEVEL:= -O3 +#OPTLEVEL:= -Os + +## +## Device-specific source files and include directories, e.g. startup code +## +DEVICE_INC := \ +-Iplatform/mcu/CMSIS/Include -Iplatform/mcu/CMSIS/Device/ST/STM32F4xx/Include + +DEVICE_SRC := \ +platform/mcu/STM32F4xx/boot/startup.c \ +platform/mcu/STM32F4xx/boot/libc_integration.cpp \ +platform/mcu/CMSIS/Device/ST/STM32F4xx/Source/system_stm32f4xx.c + +## +## Operating system's source files and include directories +## +OS_INC := \ +-Irtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M -Irtos/uC-OS3/Source -Irtos/uC-OS3/Cfg \ +-Irtos/uC-CPU/ARM-Cortex-M/ARMv7-M -Irtos/uC-CPU -Irtos/uC-CPU/Cfg \ +-Irtos/uC-LIB -Irtos/uC-LIB/Cfg + +OS_SRC := \ +rtos/uC-OS3/Source/__dbg_uCOS-III.c \ +rtos/uC-OS3/Source/os_cfg_app.c \ +rtos/uC-OS3/Source/os_core.c \ +rtos/uC-OS3/Source/os_dbg.c \ +rtos/uC-OS3/Source/os_flag.c \ +rtos/uC-OS3/Source/os_mem.c \ +rtos/uC-OS3/Source/os_msg.c \ +rtos/uC-OS3/Source/os_mutex.c \ +rtos/uC-OS3/Source/os_prio.c \ +rtos/uC-OS3/Source/os_q.c \ +rtos/uC-OS3/Source/os_sem.c \ +rtos/uC-OS3/Source/os_stat.c \ +rtos/uC-OS3/Source/os_task.c \ +rtos/uC-OS3/Source/os_tick.c \ +rtos/uC-OS3/Source/os_time.c \ +rtos/uC-OS3/Source/os_tmr.c \ +rtos/uC-OS3/Source/os_var.c \ +rtos/uC-OS3/Cfg/os_app_hooks.c \ +rtos/uC-CPU/cpu_core.c \ +rtos/uC-LIB/lib_ascii.c \ +rtos/uC-LIB/lib_math.c \ +rtos/uC-LIB/lib_mem.c \ +rtos/uC-LIB/lib_str.c \ + \ +rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/os_cpu_c.c \ +rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/os_cpu_a.S \ +rtos/uC-CPU/ARM-Cortex-M/ARMv7-M/cpu_c.c \ +rtos/uC-CPU/ARM-Cortex-M/ARMv7-M/cpu_a.s + +# rtos/uC-LIB/Ports/ARM-Cortex-M4/lib_mem_a.s \ +## +## Exceptions support. Uncomment to disable them and save code size +## +#EXCEPT := -fno-exceptions -fno-rtti -D__NO_EXCEPTIONS + +############################################################################## +## You should not need to modify anything below ## +############################################################################## + +ALL_INC := -Iinclude $(OS_INC) $(DEVICE_INC) $(DRIVERS_INC) $(INCLUDE_DIRS) +ALL_SRC := $(SRC) $(OS_SRC) $(DEVICE_SRC) $(DRIVERS_SRC) +CONFIGS := $(TARGET) $(CLK_FREQ) $(OPTLEVEL) -DDONT_USE_CMSIS_INIT + +ifeq ("$(VERBOSE)","1") +Q := +ECHO := @true +else +Q := @ +ECHO := @echo +endif + +## Replaces both "foo.cpp"-->"foo.o" and "foo.c"-->"foo.o" +OBJ := $(addsuffix .o, $(basename $(ALL_SRC))) + +CXXFLAGS := $(ALL_INC) -mcpu=cortex-m4 -mthumb -mfloat-abi=hard \ + -mfpu=fpv4-sp-d16 $(CONFIGS) $(DEFINES) $(EXCEPT) -c -g -std=c++11 +CFLAGS := $(ALL_INC) -mcpu=cortex-m4 -mthumb -mfloat-abi=hard \ + -mfpu=fpv4-sp-d16 $(CONFIGS) $(DEFINES) $(EXCEPT) -c -g +AFLAGS := -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 +LFLAGS := -mcpu=cortex-m4 -mthumb -mfloat-abi=hard -mfpu=fpv4-sp-d16 \ + -Wl,--gc-sections -Wl,-Map,main.map $(OPTLEVEL) -nostdlib \ + -Wl,-T./platform/mcu/STM32F4xx/linker_script.ld +DFLAGS := -MMD -MP + +LINK_LIBS := $(LIBS) -Wl,--start-group -lc -lgcc -lm -Wl,--end-group + +CC := arm-none-eabi-gcc +CXX := arm-none-eabi-g++ +AS := arm-none-eabi-as +CP := arm-none-eabi-objcopy +SZ := arm-none-eabi-size + +all: main.bin + +main.bin: main.elf + $(ECHO) "[CP ] main.hex" + $(Q)$(CP) -O ihex main.elf main.hex + $(ECHO) "[CP ] main.bin" + $(Q)$(CP) -O binary main.elf main.bin + $(Q)$(SZ) main.elf + +main.elf: $(OBJ) #all-recursive + $(ECHO) "[LD ] main.elf" + $(Q)$(CXX) $(LFLAGS) -o main.elf $(OBJ) $(LINK_LIBS) + +%.o: %.s + $(ECHO) "[AS ] $<" + $(Q)$(AS) $(AFLAGS) $< -o $@ + +%.o : %.c + $(ECHO) "[CC ] $<" + $(Q)$(CC) $(DFLAGS) $(CFLAGS) $< -o $@ + +%.o : %.cpp + $(ECHO) "[CXX ] $<" + $(Q)$(CXX) $(DFLAGS) $(CXXFLAGS) $< -o $@ + +flash: main_wrapped.bin + $(ECHO) "[DFU ] $<" + $(Q)./scripts/md380_dfu.py upgrade $< + +main_wrapped.bin: main.bin + $(ECHO) "[WRAP] $<" + $(Q)./scripts/md380_fw.py --wrap $< $@ + +clean: + -rm -f $(OBJ) main.elf main.hex main.bin main.map main_wrapped.bin $(OBJ:.o=.d) + +#pull in dependecy info for existing .o files +-include $(OBJ:.o=.d) diff --git a/openrtx/include/interfaces/gpio.h b/openrtx/include/interfaces/gpio.h new file mode 100644 index 00000000..07c558bd --- /dev/null +++ b/openrtx/include/interfaces/gpio.h @@ -0,0 +1,110 @@ +/*************************************************************************** + * Copyright (C) 2020 by Silvano Seva IU2KWO * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, see * + ***************************************************************************/ + +#ifndef GPIO_H +#define GPIO_H + +#include +#include "stm32f4xx.h" + +/** + * GPIO functional modes. + * For more details see microcontroller's reference manual. + */ +enum Mode +{ + INPUT = 0, ///Floating Input + INPUT_PULL_UP = 1, ///Pullup Input + INPUT_PULL_DOWN = 2, ///Pulldown Input + INPUT_ANALOG = 3, ///Analog Input + OUTPUT = 4, ///Push Pull Output + OPEN_DRAIN = 5, ///Open Drain Output + ALTERNATE = 6, ///Alternate function + ALTERNATE_OD = 7, ///Alternate Open Drain +}; + +/** + * Maximum GPIO switching speed. + * For more details see microcontroller's reference manual and datasheet. + */ +enum Speed +{ + LOW = 0x0, /// 2MHz for STM32 + MEDIUM = 0x1, /// 25MHz for STM32 + FAST = 0x2, /// 50MHz for STM32 + HIGH = 0x3 /// 100MHz for STM32 +}; + +/** + * Configure GPIO pin functional mode. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + * @param mode: GPIO functional mode to be set. + */ +void gpio_setMode(GPIO_TypeDef *port, uint8_t pin, enum Mode mode); + +/** + * Map alternate function to GPIO pin. The pin has to be configured in alternate + * mode by calling 'gpio_setMode'. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + * @param afNum: alternate function number, retrieved from mapping table in + * microcontroller's datasheet. + */ +void gpio_setAlternateFunction(GPIO_TypeDef *port, uint8_t pin, uint8_t afNum); + +/** + * Configure GPIO pin maximum output speed. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + * @param spd: GPIO output speed to be set. + */ +void gpio_setOutputSpeed(GPIO_TypeDef *port, uint8_t pin, enum Speed spd); + +/** + * Set GPIO pin to high logic level. + * NOTE: this operation is performed atomically. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + */ +void gpio_setPin(GPIO_TypeDef *port, uint8_t pin); + +/** + * Set GPIO pin to low logic level. + * NOTE: this operation is performed atomically. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + */ +void gpio_clearPin(GPIO_TypeDef *port, uint8_t pin); + +/** + * Toggle logic level of a GPIO pin, with respect to its state before calling + * this function. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + */ +void gpio_togglePin(GPIO_TypeDef *port, uint8_t pin); + +/** + * Read GPIO pin's logic level. + * @param port: GPIO port, it has to be equal to GPIOA_BASE, GPIOB_BASE, ... + * @param pin: GPIO pin number, between 0 and 15. + * @return 1 if pin is at high logic level, 0 if pin is at low logic level. + */ +uint8_t gpio_readPin(const GPIO_TypeDef *port, uint8_t pin); + +#endif /* GPIO_H */ diff --git a/platform/mcu/STM32F4xx/boot/startup.c b/platform/mcu/STM32F4xx/boot/startup.c index 8565214a..c03cdffa 100644 --- a/platform/mcu/STM32F4xx/boot/startup.c +++ b/platform/mcu/STM32F4xx/boot/startup.c @@ -1,5 +1,6 @@ /*************************************************************************** - * Copyright (C) 2020 by Silvano Seva IU2KWO * + * Copyright (C) 2020 by Federico Izzo IU2NUO, Niccolò Izzo IU2KIN and * + * Silvano Seva IU2KWO * * * * This program is free software; you can redistribute it and/or modify * * it under the terms of the GNU General Public License as published by * @@ -27,6 +28,7 @@ int main(int argc, char *argv[]); void Reset_Handler() __attribute__((__interrupt__, noreturn)); void Reset_Handler() { + // Disable interrupts at startup, as the RTOS requires this. __disable_irq(); // Call CMSIS init function, it's safe to do it here. @@ -51,8 +53,6 @@ void Reset_Handler() memcpy(data, etext, edata-data); memset(bss_start, 0, bss_end-bss_start); - __enable_irq(); - // General system configurations: enabling all GPIO ports. RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN @@ -104,8 +104,9 @@ void __attribute__((weak)) BusFault_Handler(); void __attribute__((weak)) UsageFault_Handler(); void __attribute__((weak)) SVC_Handler(); void __attribute__((weak)) DebugMon_Handler(); -void __attribute__((weak)) PendSV_Handler(); -void __attribute__((weak)) SysTick_Handler(); + +void __attribute__((weak)) OS_CPU_PendSVHandler(); // uC/OS-III naming convention +void __attribute__((weak)) OS_CPU_SysTickHandler(); void __attribute__((weak)) WWDG_IRQHandler(); void __attribute__((weak)) PVD_IRQHandler(); @@ -212,8 +213,8 @@ void (* const __Vectors[])() __attribute__ ((section(".isr_vector"))) = SVC_Handler, DebugMon_Handler, 0, - PendSV_Handler, - SysTick_Handler, + OS_CPU_PendSVHandler, + OS_CPU_SysTickHandler, WWDG_IRQHandler, PVD_IRQHandler, diff --git a/rtos/uC-CPU/Cfg/cpu_cfg.h b/rtos/uC-CPU/Cfg/cpu_cfg.h index a267d8d3..4c2625fa 100644 --- a/rtos/uC-CPU/Cfg/cpu_cfg.h +++ b/rtos/uC-CPU/Cfg/cpu_cfg.h @@ -175,9 +175,9 @@ ********************************************************************************************************* */ -#if 0 -#define CPU_CFG_ENDIAN_TYPE CPU_ENDIAN_TYPE_BIG /* Defines CPU data word-memory order (see Note #2). */ -#endif +// #if 0 +#define CPU_CFG_ENDIAN_TYPE CPU_ENDIAN_TYPE_LITTLE /* Defines CPU data word-memory order (see Note #2). */ +// #endif /* @@ -239,9 +239,9 @@ * implementing only bits[7:6]. ********************************************************************************************************* */ -#if 0 +// #if 0 #define CPU_CFG_NVIC_PRIO_BITS 4u -#endif +// #endif /* diff --git a/rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/os_cpu_a.S b/rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/os_cpu_a.s similarity index 100% rename from rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/os_cpu_a.S rename to rtos/uC-OS3/Ports/ARM-Cortex-M/ARMv7-M/os_cpu_a.s diff --git a/tests/platform/stm32f4_uC.c b/tests/platform/stm32f4_uC.c new file mode 100644 index 00000000..c97a8920 --- /dev/null +++ b/tests/platform/stm32f4_uC.c @@ -0,0 +1,133 @@ +/*************************************************************************** + * Copyright (C) 2020 by Federico Izzo IU2NUO, Niccolò Izzo IU2KIN and * + * Silvano Seva IU2KWO * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 3 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, see * + ***************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "stm32f4xx.h" +#include "gpio.h" +#include "delays.h" +#include + +static OS_TCB startTCB; +static CPU_STK_SIZE startStk[APP_CFG_TASK_START_STK_SIZE]; +static void startTask(void *arg); + +static OS_TCB t1TCB; +static CPU_STK_SIZE t1Stk[128]; +static void t1(void *arg); + +static OS_TCB t2TCB; +static CPU_STK_SIZE t2Stk[128]; +static void t2(void *arg); + +int main(void) +{ + OS_ERR err; + + OSInit(&err); + + OSTaskCreate((OS_TCB *)&startTCB, + (CPU_CHAR *)" ", + (OS_TASK_PTR ) startTask, + (void *) 0, + (OS_PRIO ) APP_CFG_TASK_START_PRIO, + (CPU_STK *)&startStk[0], + (CPU_STK )startStk[APP_CFG_TASK_START_STK_SIZE / 10u], + (CPU_STK_SIZE) APP_CFG_TASK_START_STK_SIZE, + (OS_MSG_QTY ) 0, + (OS_TICK ) 0, + (void *) 0, + (OS_OPT )(OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + + OSStart(&err); + + for(;;) ; + return 0; +} + +static void startTask(void* arg) +{ + + (void) arg; + OS_ERR err; + + gpio_setMode(GPIOD, 13, OUTPUT); + gpio_setMode(GPIOD, 14, OUTPUT); + + CPU_Init(); + OS_CPU_SysTickInitFreq(SystemCoreClock); + + OSTaskCreate((OS_TCB *)&t1TCB, + (CPU_CHAR *)" ", + (OS_TASK_PTR ) t1, + (void *) 0, + (OS_PRIO ) 5, + (CPU_STK *)&t1Stk[0], + (CPU_STK ) 0, + (CPU_STK_SIZE) 128, + (OS_MSG_QTY ) 0, + (OS_TICK ) 0, + (void *) 0, + (OS_OPT )(OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + + OSTaskCreate((OS_TCB *)&t2TCB, + (CPU_CHAR *)" ", + (OS_TASK_PTR ) t2, + (void *) 0, + (OS_PRIO ) 5, + (CPU_STK *)&t2Stk[0], + (CPU_STK ) 0, + (CPU_STK_SIZE) 128, + (OS_MSG_QTY ) 0, + (OS_TICK ) 0, + (void *) 0, + (OS_OPT )(OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR), + (OS_ERR *)&err); + + while(1) ; +} + +static void t1(void *arg) +{ + (void) arg; + OS_ERR os_err; + + while(1) + { + gpio_togglePin(GPIOD, 13); + OSTimeDlyHMSM(0u, 0u, 1u, 0u, OS_OPT_TIME_HMSM_STRICT, &os_err); + } +} + +static void t2(void *arg) +{ + (void) arg; + OS_ERR os_err; + + while(1) + { + gpio_togglePin(GPIOD, 14); + OSTimeDlyHMSM(0u, 0u, 2u, 0u, OS_OPT_TIME_HMSM_STRICT, &os_err); + } +}