diff --git a/src/Makefile b/src/Makefile index cf2f5a944..bbbe994cb 100755 --- a/src/Makefile +++ b/src/Makefile @@ -46,6 +46,7 @@ BUILD=$(TOP_DIR)/build FW_DIR=$(BUILD)/bin OMV_DIR=omv UVC_DIR=uvc +CM4_DIR=cm4 BOOTLDR_DIR=bootloader CUBEAI_DIR=stm32cubeai CMSIS_DIR=hal/cmsis diff --git a/src/cm4/Makefile b/src/cm4/Makefile new file mode 100755 index 000000000..458d1afe6 --- /dev/null +++ b/src/cm4/Makefile @@ -0,0 +1,59 @@ +# This file is part of the OpenMV project. +# +# Copyright (c) 2013-2021 Ibrahim Abdelkader +# Copyright (c) 2013-2021 Kwabena W. Agyeman +# +# This work is licensed under the MIT license, see the file LICENSE for details. +# +# CM4 firmware Makefile +SRC_C = $(wildcard src/*.c) +SRC_C += $(addprefix $(HAL_DIR)/src/, \ + stm32h7xx_hal.c \ + stm32h7xx_hal_cortex.c \ + stm32h7xx_hal_gpio.c \ + stm32h7xx_hal_pwr.c \ + stm32h7xx_hal_pwr_ex.c \ + stm32h7xx_hal_rcc.c \ + stm32h7xx_hal_rcc_ex.c \ + stm32h7xx_hal_rtc.c \ + stm32h7xx_hal_rtc_ex.c \ +) +#SRCS += $(addprefix $(HAL_DIR)/src/, $(notdir $(wildcard ../$(HAL_DIR)/src/*.c))) + +SRC_C += $(addprefix $(CMSIS_DIR)/src/, \ + $(SYSTEM).c \ +) + +SRC_S += $(addprefix $(CMSIS_DIR)/src/, \ + $(STARTUP).s \ +) + +OBJS = $(addprefix $(BUILD)/, $(SRC_S:.s=.o)) +OBJS += $(addprefix $(BUILD)/, $(SRC_C:.c=.o)) +OBJ_DIRS = $(sort $(dir $(OBJS))) + +all: | $(OBJ_DIRS) $(OBJS) +$(OBJ_DIRS): + $(MKDIR) -p $@ + +$(BUILD)/$(HAL_DIR)/src/%.o : $(TOP_DIR)/$(HAL_DIR)/src/%.c + $(ECHO) "CC $<" + $(CC) $(CFLAGS) -c -o $@ $< + +$(BUILD)/$(CMSIS_DIR)/src/%.o : $(TOP_DIR)/$(CMSIS_DIR)/src/%.c + $(ECHO) "CC $<" + $(CC) $(CFLAGS) -c -o $@ $< + +$(BUILD)/$(CMSIS_DIR)/src/%.o : $(TOP_DIR)/$(CMSIS_DIR)/src/%.s + $(ECHO) "AS $<" + $(AS) $(AFLAGS) $< -o $@ + +$(BUILD)/%.o : %.c + $(ECHO) "CC $<" + $(CC) $(CFLAGS) -c -o $@ $< + +$(BUILD)/%.o : %.s + $(ECHO) "AS $<" + $(AS) $(AFLAGS) $< -o $@ + +-include $(OBJS:%.o=%.d) diff --git a/src/cm4/src/main.c b/src/cm4/src/main.c new file mode 100644 index 000000000..29b67b80e --- /dev/null +++ b/src/cm4/src/main.c @@ -0,0 +1,14 @@ +#include STM32_HAL_H + +int main(void) +{ + __disable_irq(); + + // Add Cortex-M4 code here. + HAL_PWREx_ClearPendingEvent(); + HAL_PWREx_EnterSTOPMode(PWR_MAINREGULATOR_ON, PWR_STOPENTRY_WFE, PWR_D2_DOMAIN); + + while (1) { + __WFI(); + } +} diff --git a/src/cm4/src/stm32h7xx_it.c b/src/cm4/src/stm32h7xx_it.c new file mode 100644 index 000000000..c5bcf44b3 --- /dev/null +++ b/src/cm4/src/stm32h7xx_it.c @@ -0,0 +1,139 @@ +/** + ****************************************************************************** + * @file PWR/PWR_STANDBY_RTC/CM4/Src/stm32h7xx_it.c + * @author MCD Application Team + * @brief Main Interrupt Service Routines. + * This file provides template for all exceptions handler and + * peripherals interrupt service routine. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2019 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ +#include STM32_HAL_H +/** + * @brief This function handles NMI exception. + * @param None + * @retval None + */ +void NMI_Handler(void) +{ +} + +/** + * @brief This function handles Hard Fault exception. + * @param None + * @retval None + */ +void HardFault_Handler(void) +{ + /* Go to infinite loop when Hard Fault exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles Memory Manage exception. + * @param None + * @retval None + */ +void MemManage_Handler(void) +{ + /* Go to infinite loop when Memory Manage exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles Bus Fault exception. + * @param None + * @retval None + */ +void BusFault_Handler(void) +{ + /* Go to infinite loop when Bus Fault exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles Usage Fault exception. + * @param None + * @retval None + */ +void UsageFault_Handler(void) +{ + /* Go to infinite loop when Usage Fault exception occurs */ + while (1) + { + } +} + +/** + * @brief This function handles SVCall exception. + * @param None + * @retval None + */ +void SVC_Handler(void) +{ +} + +/** + * @brief This function handles Debug Monitor exception. + * @param None + * @retval None + */ +void DebugMon_Handler(void) +{ +} + +/** + * @brief This function handles PendSVC exception. + * @param None + * @retval None + */ +void PendSV_Handler(void) +{ +} + +/** + * @brief This function handles SysTick Handler. + * @param None + * @retval None + */ +void SysTick_Handler(void) +{ + HAL_IncTick(); +} + + +/** + * @brief This function handles HSEM2 wake-up interrupt request. + * @param None + * @retval None + */ +void HSEM2_IRQHandler(void) +{ + +} + +/** + * @} + */ + +/** + * @} + */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/cm4/stm32fxxx.ld.S b/src/cm4/stm32fxxx.ld.S new file mode 100644 index 000000000..52e722e83 --- /dev/null +++ b/src/cm4/stm32fxxx.ld.S @@ -0,0 +1,102 @@ +/* + * This file is part of the OpenMV project. + * + * Copyright (c) 2013-2021 Ibrahim Abdelkader + * Copyright (c) 2013-2021 Kwabena W. Agyeman + * + * This work is licensed under the MIT license, see the file LICENSE for details. + * + * Linker script for STM32 Devices. + */ + +/* Entry Point */ +ENTRY(Reset_Handler) + +#include "omv_boardconfig.h" + +/* Specify the memory areas */ +MEMORY +{ + RAM (xrw) : ORIGIN = OMV_CM4_RAM_ORIGIN, LENGTH = OMV_CM4_RAM_LENGTH + FLASH (rx) : ORIGIN = OMV_CM4_FLASH_ORIGIN, LENGTH = OMV_CM4_FLASH_LENGTH +} + +_heap_size = (1 * 1024); /* heap size */ +_stack_size = (4 * 1024); /* stack size */ + +/* Define output sections */ +SECTIONS +{ + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + KEEP(*(.isr_vector))// ISR table + . = ALIGN(4); + *(.text) // .text sections (code) + . = ALIGN(4); + *(.text*) // .text* sections (code) + . = ALIGN(4); + *(.rodata) // .rodata sections (constants, strings, etc.) + . = ALIGN(4); + *(.rodata*) // .rodata* sections (constants, strings, etc.) + . = ALIGN(4); + _etext = .; // define a global symbols at end of code + } >FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections */ + .data : + { + . = ALIGN(4); + _sdata = .; // Create a global symbol at data start + _ram_start = .; + *(.data) // .data sections + + . = ALIGN(4); + *(.data*) // .data* sections + + . = ALIGN(4); + _edata = .; // define a global symbol at data end + } >RAM AT> FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss (NOLOAD) : + { + . = ALIGN(4); + _sbss = .; // Used by the startup to initialize the .bss secion + . = ALIGN(4); + *(.bss*) + . = ALIGN(4); + *(COMMON) + . = ALIGN(4); + _ebss = .; // define a global symbol at bss end + } >RAM + + ._heap (NOLOAD) : + { + . = ALIGN(4); + _heap_start = .; + . = . + _heap_size; + + . = ALIGN(4); + _heap_end = .; + + } >RAM + + /* Make sure there is enough ram for the stack */ + ._stack (NOLOAD) : + { + . = ALIGN(8); + _sstack = .; + . = . + _stack_size; + + . = ALIGN(8); + _estack = .; + } >RAM + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/hal/cmsis/src/st/system_stm32fxxx.c b/src/hal/cmsis/src/st/system_stm32fxxx.c index 4acea1ff5..798b1de63 100644 --- a/src/hal/cmsis/src/st/system_stm32fxxx.c +++ b/src/hal/cmsis/src/st/system_stm32fxxx.c @@ -97,8 +97,13 @@ void SystemInit(void) #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ #endif - /* Reset the RCC clock configuration to the default reset state ------------*/ + /*SEVONPEND enabled so that an interrupt coming from the CPU(n) interrupt + signal is detectable by the CPU after a WFI/WFE instruction. */ + //SCB->SCR |= SCB_SCR_SEVONPEND_Msk; + + #if !defined(CORE_CM4) + /* Reset the RCC clock configuration to the default reset state ------------*/ /* Set HSION bit */ RCC->CR |= CONFIG_RCC_CR_1ST; @@ -159,13 +164,6 @@ void SystemInit(void) *((__IO uint32_t*)0x51008108) = 0x00000001; #endif // defined(MCU_SERIES_H7) - /* Configure the Vector Table location add offset address ------------------*/ - #ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ - #else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ - #endif - /* dpgeorge: enable 8-byte stack alignment for IRQ handlers, in accord with EABI */ SCB->CCR |= SCB_CCR_STKALIGN_Msk; @@ -178,6 +176,15 @@ void SystemInit(void) DBGMCU->CR |= DBGMCU_CR_DBG_STANDBYD1; #endif #endif + + #endif // !defined(CORE_CM4) + + /* Configure the Vector Table location add offset address ------------------*/ + #ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ + #else + SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ + #endif } void SystemClock_Config(void) @@ -366,4 +373,9 @@ void SystemClock_Config(void) // Enable the USB voltage level detector HAL_PWREx_EnableUSBVoltageDetector(); #endif + + #if defined(CORE_CM7) && defined(M4_APP_ADDR) + HAL_SYSCFG_CM4BootAddConfig(SYSCFG_BOOT_ADDR0, M4_APP_ADDR); + HAL_RCCEx_EnableBootCore(RCC_BOOT_C2); + #endif } diff --git a/src/omv/boards/PORTENTA/omv_boardconfig.h b/src/omv/boards/PORTENTA/omv_boardconfig.h index 9e01e07a0..c3ff10f07 100644 --- a/src/omv/boards/PORTENTA/omv_boardconfig.h +++ b/src/omv/boards/PORTENTA/omv_boardconfig.h @@ -38,8 +38,8 @@ #define OMV_OV7725_BANDING (0x7F) // Bootloader LED GPIO port/pin -#define OMV_BOOTLDR_LED_PIN (GPIO_PIN_1) -#define OMV_BOOTLDR_LED_PORT (GPIOC) +#define OMV_BOOTLDR_LED_PIN (GPIO_PIN_6) +#define OMV_BOOTLDR_LED_PORT (GPIOK) // RAW buffer size #define OMV_RAW_BUF_SIZE (4*1024*1024) @@ -152,7 +152,7 @@ #define OMV_FB_SIZE (4M) // FB memory: header + VGA/GS image #define OMV_FB_ALLOC_SIZE (3M) // minimum fb alloc size #define OMV_STACK_SIZE (64K) -#define OMV_HEAP_SIZE (196K) +#define OMV_HEAP_SIZE (180K) #define OMV_SDRAM_SIZE (8 * 1024 * 1024) // This needs to be here for UVC firmware. #define OMV_LINE_BUF_SIZE (11 * 1024) // Image line buffer round(2592 * 2BPP * 2 buffers). @@ -168,8 +168,8 @@ #define OMV_DTCM_ORIGIN 0x20000000 // Note accessible by CPU and MDMA only. #define OMV_DTCM_LENGTH 128K #define OMV_SRAM1_ORIGIN 0x30000000 -#define OMV_SRAM1_LENGTH 272K // SRAM1 + SRAM2 + 1/2 SRAM3 -#define OMV_SRAM3_ORIGIN 0x30044000 +#define OMV_SRAM1_LENGTH 256K // SRAM1 + SRAM2 +#define OMV_SRAM3_ORIGIN 0x30040000 // Second half of SRAM3 reserved for M4. #define OMV_SRAM3_LENGTH 16K #define OMV_SRAM4_ORIGIN 0x38000000 #define OMV_SRAM4_LENGTH 64K @@ -179,6 +179,10 @@ #define OMV_DRAM_LENGTH 8M #define OMV_FLASH_EXT_ORIGIN 0x90000000 #define OMV_FLASH_EXT_LENGTH 16M +#define OMV_CM4_RAM_ORIGIN 0x30044000 // Cortex-M4 memory. +#define OMV_CM4_RAM_LENGTH 16K +#define OMV_CM4_FLASH_ORIGIN 0x08020000 +#define OMV_CM4_FLASH_LENGTH 128K // Domain 1 DMA buffers region. #define OMV_DMA_MEMORY_D1 AXI_SRAM diff --git a/src/omv/boards/PORTENTA/omv_boardconfig.mk b/src/omv/boards/PORTENTA/omv_boardconfig.mk index f787ee9b1..bda2657cc 100755 --- a/src/omv/boards/PORTENTA/omv_boardconfig.mk +++ b/src/omv/boards/PORTENTA/omv_boardconfig.mk @@ -8,11 +8,14 @@ HAL_INC='' CFLAGS_MCU=MCU_SERIES_H7 VECT_TAB_OFFSET=0x40000 MAIN_APP_ADDR=0x08040000 +M4_VECT_TAB_OFFSET=0x20000 +M4_APP_ADDR=0x08020000 OMV_HSE_VALUE=25000000 DFU_DEVICE=0x2341:0x035b OMV_BOARD_EXTRA_CFLAGS = -DCORE_CM7 OMV_ENABLE_BL=0 OMV_ENABLE_UVC=1 +OMV_ENABLE_CM4=0 MICROPY_PY_SENSOR = 1 MICROPY_PY_ULAB = 1 MICROPY_PY_WINC1500 = 0 diff --git a/src/omv/ports/stm32/omv_portconfig.mk b/src/omv/ports/stm32/omv_portconfig.mk index 00f261a41..bb5b4f53f 100644 --- a/src/omv/ports/stm32/omv_portconfig.mk +++ b/src/omv/ports/stm32/omv_portconfig.mk @@ -66,6 +66,21 @@ UVC_LDFLAGS = -mcpu=$(CPU) -mabi=aapcs-linux -mthumb -mfpu=$(FPU) -mfloat-abi=ha -nostdlib -Wl,--gc-sections -Wl,-T$(BUILD)/$(UVC_DIR)/stm32fxxx.lds endif +ifeq ($(OMV_ENABLE_CM4), 1) +CFLAGS += -DM4_APP_ADDR=$(M4_APP_ADDR) +CM4_CFLAGS += -std=gnu99 -Wall -Werror -Warray-bounds -mthumb -nostartfiles -fdata-sections -ffunction-sections +CM4_CFLAGS += -D$(MCU) -D$(CFLAGS_MCU) -D$(ARM_MATH) -DARM_NN_TRUNCATE -DCORE_CM4\ + -fsingle-precision-constant -Wdouble-promotion -mcpu=cortex-m4 -mtune=cortex-m4 -mfpu=$(FPU) -mfloat-abi=hard +CM4_CFLAGS += -D__FPU_PRESENT=1 -D__VFP_FP__ -DHSE_VALUE=$(OMV_HSE_VALUE)\ + -D$(TARGET) -DVECT_TAB_OFFSET=$(M4_VECT_TAB_OFFSET) -DMAIN_APP_ADDR=$(M4_APP_ADDR) -DSTM32_HAL_H=$(HAL_INC) +CM4_CFLAGS += $(HAL_CFLAGS) +CM4_CFLAGS += -I$(OMV_BOARD_CONFIG_DIR) +CM4_CFLAGS += -I$(TOP_DIR)/$(CM4_DIR)/include/ +# Linker Flags +CM4_LDFLAGS = -mcpu=cortex-m4 -mabi=aapcs-linux -mthumb -mfpu=$(FPU) -mfloat-abi=hard\ + -nostdlib -Wl,--gc-sections -Wl,-T$(BUILD)/$(CM4_DIR)/stm32fxxx.lds +endif + CFLAGS += $(HAL_CFLAGS) $(MPY_CFLAGS) $(OMV_CFLAGS) # Linker Flags @@ -498,6 +513,17 @@ UVC_OBJ += $(wildcard $(BUILD)/$(MLX90640_DIR)/src/*.o) UVC_OBJ += $(wildcard $(BUILD)/$(MLX90641_DIR)/src/*.o) endif +ifeq ($(OMV_ENABLE_CM4), 1) +CM4 = cm4 +# CM4 object files +CM4_OBJ += $(wildcard $(BUILD)/$(CM4_DIR)/src/*.o) +CM4_OBJ += $(wildcard $(BUILD)/$(CM4_DIR)/$(HAL_DIR)/src/*.o) +CM4_OBJ += $(addprefix $(BUILD)/$(CM4_DIR)/$(CMSIS_DIR)/src/, \ + $(STARTUP).o \ + $(SYSTEM).o \ +) +endif + ################################################### #Export Variables export Q @@ -519,6 +545,8 @@ export STARTUP export SYSTEM export FROZEN_MANIFEST export PORT +export HAL_DIR +export CMSIS_DIR ################################################### all: $(OPENMV) @@ -550,9 +578,13 @@ ifeq ($(OMV_ENABLE_UVC), 1) UVC_OBJS: FIRMWARE_OBJS $(MAKE) -C $(UVC_DIR) BUILD=$(BUILD)/$(UVC_DIR) CFLAGS="$(UVC_CFLAGS) -MMD" endif +ifeq ($(OMV_ENABLE_CM4), 1) +CM4_OBJS: FIRMWARE_OBJS + $(MAKE) -C $(CM4_DIR) BUILD=$(BUILD)/$(CM4_DIR) CFLAGS="$(CM4_CFLAGS) -MMD" +endif ifeq ($(OMV_ENABLE_BL), 1) BOOTLOADER_OBJS: FIRMWARE_OBJS - $(MAKE) -C $(BOOTLDR_DIR) BUILD=$(BUILD)/$(BOOTLDR_DIR) CFLAGS="$(BL_CFLAGS) -MMD" + $(MAKE) -C $(BOOTLDR_DIR) BUILD=$(BUILD)/$(BOOTLDR_DIR) CFLAGS="$(BL_CFLAGS) -MMD" endif # This target generates the main/app firmware image located at 0x08010000 @@ -580,8 +612,17 @@ $(UVC): FIRMWARE_OBJS UVC_OBJS $(PYTHON) $(MKDFU) -D $(DFU_DEVICE) -b $(MAIN_APP_ADDR):$(FW_DIR)/$(UVC).bin $(FW_DIR)/$(UVC).dfu endif +ifeq ($(OMV_ENABLE_CM4), 1) +# This target generates CM4 firmware for dual core micros. +$(CM4): FIRMWARE_OBJS CM4_OBJS + $(CPP) -P -E -I$(OMV_BOARD_CONFIG_DIR) $(CM4_DIR)/stm32fxxx.ld.S > $(BUILD)/$(CM4_DIR)/stm32fxxx.lds + $(CC) $(CM4_LDFLAGS) $(CM4_OBJ) -o $(FW_DIR)/$(CM4).elf -lgcc + $(OBJCOPY) -Obinary $(FW_DIR)/$(CM4).elf $(FW_DIR)/$(CM4).bin + $(PYTHON) $(MKDFU) -D $(DFU_DEVICE) -b $(M4_APP_ADDR):$(FW_DIR)/$(CM4).bin $(FW_DIR)/$(CM4).dfu +endif + # This target generates the uvc, bootloader and firmware images. -$(OPENMV): $(BOOTLOADER) $(UVC) $(FIRMWARE) +$(OPENMV): $(BOOTLOADER) $(UVC) $(CM4) $(FIRMWARE) ifeq ($(OMV_ENABLE_BL), 1) $(SIZE) $(FW_DIR)/$(BOOTLOADER).elf endif