diff --git a/Makefile.firmware b/Makefile.firmware index fe196446c..8c20ec486 100644 --- a/Makefile.firmware +++ b/Makefile.firmware @@ -294,7 +294,6 @@ OBJ_MP += $(addprefix $(BUILD_MP)/,\ stmhal/gchelper.o \ stmhal/pendsv.o \ stmhal/startup_stm32.o \ - stmhal/system_stm32.o \ stmhal/systick.o \ \ stmhal/usbd_cdc_interface.o \ @@ -313,11 +312,16 @@ OBJ_MP += $(addprefix $(BUILD_MP)/,\ # OBJ micropython/ OBJ_FW += $(addprefix $(BUILD_FW)/, \ - firmware/main.o \ - trezorhal/stm32_it.o \ - trezorhal/stm32f4xx_hal_sram.o \ - trezorhal/stm32f4xx_ll_fsmc.o \ - ) + firmware/main.o \ + trezorhal/flash.o \ + trezorhal/i2c.o \ + trezorhal/rng.o \ + trezorhal/usb.o \ + trezorhal/stm32_it.o \ + trezorhal/stm32_system.o \ + trezorhal/hal/stm32f4xx_hal_sram.o \ + trezorhal/hal/stm32f4xx_ll_fsmc.o \ + ) # make a list of all the .py files that need compiling and freezing FROZEN_MPY_PY_FILES := $(shell find -L $(FROZEN_MPY_DIR) -type f -name '*.py' | $(SED) -e 's=^$(FROZEN_MPY_DIR)/==') @@ -336,6 +340,7 @@ CROSS_COMPILE = arm-none-eabi- INC += -I. INC += -Imicropython/firmware INC += -Imicropython/trezorhal +INC += -Imicropython/trezorhal/hal INC += -Ivendor/micropython INC += -Ivendor/micropython/stmhal INC += -Ivendor/micropython/stmhal/cmsis @@ -363,7 +368,7 @@ CFLAGS += -DMICROPY_MODULE_FROZEN_MPY LIBS = $(shell $(CC) $(CFLAGS) -print-libgcc-file-name) -LDFLAGS = -nostdlib -T micropython/trezorhal/trezorv2.ld -Map=$@.map --cref +LDFLAGS = -nostdlib -T micropython/trezorhal/memory.ld -Map=$@.map --cref # remove uncalled code from the final image CFLAGS += -fdata-sections -ffunction-sections diff --git a/micropython/firmware/main.c b/micropython/firmware/main.c index 2b817c228..2d51b32eb 100644 --- a/micropython/firmware/main.c +++ b/micropython/firmware/main.c @@ -2,12 +2,6 @@ #include #include -#include "usbd_core.h" -#include "usbd_desc.h" -#include "usbd_cdc_msc_hid.h" -#include "usbd_cdc_interface.h" -#include "usbd_hid_interface.h" - #include "py/nlr.h" #include "py/compile.h" #include "py/runtime.h" @@ -16,21 +10,12 @@ #include "py/gc.h" #include "lib/utils/pyexec.h" +#include "gccollect.h" #include "pendsv.h" void SystemClock_Config(void); - -extern uint32_t _etext; -extern uint32_t _sidata; -extern uint32_t _ram_start; -extern uint32_t _sdata; -extern uint32_t _edata; -extern uint32_t _sbss; -extern uint32_t _ebss; -extern uint32_t _heap_start; -extern uint32_t _heap_end; -extern uint32_t _estack; -extern uint32_t _ram_end; +void USBD_CDC_TxAlways(const uint8_t * buf, uint32_t len); +int USBD_CDC_Rx(uint8_t * buf, uint32_t len, uint32_t timeout); void flash_init(void); void usb_init(void); @@ -110,102 +95,6 @@ void MP_WEAK __assert_func(const char *file, int line, const char *func, const c } #endif -// Flash - -void flash_init(void) { - // Enable the flash IRQ, which is used to also call our storage IRQ handler - // It needs to go at a higher priority than all those components that rely on - // the flash storage (eg higher than USB MSC). - HAL_NVIC_SetPriority(FLASH_IRQn, 2, 0); - HAL_NVIC_EnableIRQ(FLASH_IRQn); -} - -// USB - -USBD_HandleTypeDef hUSBDDevice; - -void usb_init(void) { - const uint16_t vid = 0x1209; - const uint16_t pid = 0x53C1; - - USBD_HID_ModeInfoTypeDef hid_info = { - .subclass = 0, - .protocol = 0, - .max_packet_len = 64, - .polling_interval = 1, - .report_desc = (const uint8_t*)"\x06\x00\xff\x09\x01\xa1\x01\x09\x20\x15\x00\x26\xff\x00\x75\x08\x95\x40\x81\x02\x09\x21\x15\x00\x26\xff\x00\x75\x08\x95\x40\x91\x02\xc0", - .report_desc_len = 34, - }; - - USBD_SetVIDPIDRelease(vid, pid, 0x0200, 0); - if (USBD_SelectMode(USBD_MODE_CDC_HID, &hid_info) != 0) { - for (;;) { - __fatal_error("USB init failed"); - } - } - USBD_Init(&hUSBDDevice, (USBD_DescriptorsTypeDef*)&USBD_Descriptors, 0); // 0 == full speed - USBD_RegisterClass(&hUSBDDevice, &USBD_CDC_MSC_HID); - USBD_CDC_RegisterInterface(&hUSBDDevice, (USBD_CDC_ItfTypeDef*)&USBD_CDC_fops); - USBD_HID_RegisterInterface(&hUSBDDevice, (USBD_HID_ItfTypeDef*)&USBD_HID_fops); - USBD_Start(&hUSBDDevice); -} - -// I2C - -I2C_HandleTypeDef *i2c_handle = 0; - -void i2c_init(I2C_HandleTypeDef *i2c) { - - // Enable I2C clock - __HAL_RCC_I2C1_CLK_ENABLE(); - - // Init SCL and SDA GPIO lines (PB6 & PB7) - GPIO_InitTypeDef GPIO_InitStructure = { - .Pin = GPIO_PIN_6 | GPIO_PIN_7, - .Mode = GPIO_MODE_AF_OD, - .Pull = GPIO_NOPULL, - .Speed = GPIO_SPEED_FREQ_VERY_HIGH, - .Alternate = GPIO_AF4_I2C1, - }; - HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); - - // Init I2C handle - if (HAL_I2C_Init(i2c) != HAL_OK) { - for (;;) { - __fatal_error("i2c_init failed"); - } - } - - // Enable IRQs - i2c_handle = i2c; - HAL_NVIC_EnableIRQ(I2C1_EV_IRQn); - HAL_NVIC_EnableIRQ(I2C1_ER_IRQn); -} - -// RNG - -STATIC RNG_HandleTypeDef rng_handle = { - .State = HAL_RNG_STATE_RESET, - .Instance = RNG, -}; - -void rng_init(RNG_HandleTypeDef *rng) { - - // Enable RNG clock - __HAL_RCC_RNG_CLK_ENABLE(); - - // Init RNG handle - HAL_RNG_Init(rng); -} - -uint32_t rng_get(void) { - if (rng_handle.State == HAL_RNG_STATE_RESET) { - rng_init(&rng_handle); - } - - return HAL_RNG_GetRandomNumber(&rng_handle); -} - // I/O mp_lexer_t *mp_lexer_new_from_file(const char *filename) { diff --git a/micropython/firmware/mpconfigboard.h b/micropython/firmware/mpconfigboard.h deleted file mode 100644 index ae5478983..000000000 --- a/micropython/firmware/mpconfigboard.h +++ /dev/null @@ -1,49 +0,0 @@ -#define MICROPY_HW_BOARD_NAME "TREZORv2" -#define MICROPY_HW_MCU_NAME "STM32F405VG" -#define MICROPY_PY_SYS_PLATFORM "trezor" - -#define MICROPY_HW_HAS_SWITCH (0) -#define MICROPY_HW_HAS_FLASH (0) -#define MICROPY_HW_HAS_SDCARD (1) -#define MICROPY_HW_HAS_MMA7660 (0) -#define MICROPY_HW_HAS_LIS3DSH (0) -#define MICROPY_HW_HAS_LCD (0) -#define MICROPY_HW_ENABLE_RNG (1) -#define MICROPY_HW_ENABLE_RTC (0) -#define MICROPY_HW_ENABLE_TIMER (1) -#define MICROPY_HW_ENABLE_SERVO (0) -#define MICROPY_HW_ENABLE_DAC (0) -#define MICROPY_HW_ENABLE_CAN (0) - -// HSE is 8MHz -#define MICROPY_HW_CLK_PLLM (8) -#define MICROPY_HW_CLK_PLLN (336) -#define MICROPY_HW_CLK_PLLP (RCC_PLLP_DIV2) -#define MICROPY_HW_CLK_PLLQ (7) -#define MICROPY_HW_CLK_LAST_FREQ (1) - -// I2C busses -#define MICROPY_HW_I2C1_NAME "I2C" -#define MICROPY_HW_I2C1_SCL (pin_B6) -#define MICROPY_HW_I2C1_SDA (pin_B7) - -// UART config -#define MICROPY_HW_UART1_NAME "UART" -#define MICROPY_HW_UART1_TX (pin_A2) -#define MICROPY_HW_UART1_RX (pin_A3) - -// The board has 2 LEDs -#define MICROPY_HW_LED1 (pin_C6) -#define MICROPY_HW_LED2 (pin_B13) -#define MICROPY_HW_LED2_PWM { TIM1, 1, TIM_CHANNEL_1, GPIO_AF1_TIM1 } -#define MICROPY_HW_LED_OTYPE (GPIO_MODE_OUTPUT_PP) -#define MICROPY_HW_LED_ON(pin) (mp_hal_pin_high(pin)) -#define MICROPY_HW_LED_OFF(pin) (mp_hal_pin_low(pin)) - -// SD card detect switch -#define MICROPY_HW_SDCARD_DETECT_PIN (pin_C13) -#define MICROPY_HW_SDCARD_DETECT_PULL (GPIO_PULLUP) -#define MICROPY_HW_SDCARD_DETECT_PRESENT (GPIO_PIN_RESET) - -// USB config -#define MICROPY_HW_USB_VBUS_DETECT_PIN (pin_A9) diff --git a/micropython/firmware/mpconfigport.h b/micropython/firmware/mpconfigport.h index 1a910deb7..7274d70b5 100644 --- a/micropython/firmware/mpconfigport.h +++ b/micropython/firmware/mpconfigport.h @@ -29,8 +29,6 @@ #include #include -#include "mpconfigboard.h" - #define MICROPY_DEBUG_PRINTERS (1) // Memory allocation policies diff --git a/micropython/trezorhal/flash.c b/micropython/trezorhal/flash.c new file mode 100644 index 000000000..c2692c084 --- /dev/null +++ b/micropython/trezorhal/flash.c @@ -0,0 +1,11 @@ +#include STM32_HAL_H + +void __fatal_error(const char *msg); + +void flash_init(void) { + // Enable the flash IRQ, which is used to also call our storage IRQ handler + // It needs to go at a higher priority than all those components that rely on + // the flash storage (eg higher than USB MSC). + HAL_NVIC_SetPriority(FLASH_IRQn, 2, 0); + HAL_NVIC_EnableIRQ(FLASH_IRQn); +} diff --git a/micropython/trezorhal/stm32f4xx_hal_conf.h b/micropython/trezorhal/hal/stm32f4xx_hal_conf.h similarity index 100% rename from micropython/trezorhal/stm32f4xx_hal_conf.h rename to micropython/trezorhal/hal/stm32f4xx_hal_conf.h diff --git a/micropython/trezorhal/stm32f4xx_hal_sram.c b/micropython/trezorhal/hal/stm32f4xx_hal_sram.c similarity index 100% rename from micropython/trezorhal/stm32f4xx_hal_sram.c rename to micropython/trezorhal/hal/stm32f4xx_hal_sram.c diff --git a/micropython/trezorhal/stm32f4xx_hal_sram.h b/micropython/trezorhal/hal/stm32f4xx_hal_sram.h similarity index 100% rename from micropython/trezorhal/stm32f4xx_hal_sram.h rename to micropython/trezorhal/hal/stm32f4xx_hal_sram.h diff --git a/micropython/trezorhal/stm32f4xx_ll_fsmc.c b/micropython/trezorhal/hal/stm32f4xx_ll_fsmc.c similarity index 100% rename from micropython/trezorhal/stm32f4xx_ll_fsmc.c rename to micropython/trezorhal/hal/stm32f4xx_ll_fsmc.c diff --git a/micropython/trezorhal/stm32f4xx_ll_fsmc.h b/micropython/trezorhal/hal/stm32f4xx_ll_fsmc.h similarity index 100% rename from micropython/trezorhal/stm32f4xx_ll_fsmc.h rename to micropython/trezorhal/hal/stm32f4xx_ll_fsmc.h diff --git a/micropython/trezorhal/i2c.c b/micropython/trezorhal/i2c.c new file mode 100644 index 000000000..d091ac500 --- /dev/null +++ b/micropython/trezorhal/i2c.c @@ -0,0 +1,32 @@ +#include STM32_HAL_H + +I2C_HandleTypeDef *i2c_handle = 0; + +void __fatal_error(const char *msg); + +void i2c_init(I2C_HandleTypeDef *i2c) { + + // Enable I2C clock + __HAL_RCC_I2C1_CLK_ENABLE(); + + // Init SCL and SDA GPIO lines (PB6 & PB7) + GPIO_InitTypeDef GPIO_InitStructure = { + .Pin = GPIO_PIN_6 | GPIO_PIN_7, + .Mode = GPIO_MODE_AF_OD, + .Pull = GPIO_NOPULL, + .Speed = GPIO_SPEED_FREQ_VERY_HIGH, + .Alternate = GPIO_AF4_I2C1, + }; + HAL_GPIO_Init(GPIOB, &GPIO_InitStructure); + + // Init I2C handle + if (HAL_I2C_Init(i2c) != HAL_OK) { + __fatal_error("i2c_init failed"); + return; + } + + // Enable IRQs + i2c_handle = i2c; + HAL_NVIC_EnableIRQ(I2C1_EV_IRQn); + HAL_NVIC_EnableIRQ(I2C1_ER_IRQn); +} diff --git a/micropython/trezorhal/trezorv2.ld b/micropython/trezorhal/memory.ld similarity index 100% rename from micropython/trezorhal/trezorv2.ld rename to micropython/trezorhal/memory.ld diff --git a/micropython/trezorhal/rng.c b/micropython/trezorhal/rng.c new file mode 100644 index 000000000..ecc188deb --- /dev/null +++ b/micropython/trezorhal/rng.c @@ -0,0 +1,23 @@ +#include STM32_HAL_H + +static RNG_HandleTypeDef rng_handle = { + .State = HAL_RNG_STATE_RESET, + .Instance = RNG, +}; + +void rng_init(RNG_HandleTypeDef *rng) { + + // Enable RNG clock + __HAL_RCC_RNG_CLK_ENABLE(); + + // Init RNG handle + HAL_RNG_Init(rng); +} + +uint32_t rng_get(void) { + if (rng_handle.State == HAL_RNG_STATE_RESET) { + rng_init(&rng_handle); + } + + return HAL_RNG_GetRandomNumber(&rng_handle); +} diff --git a/micropython/trezorhal/stm32_it.c b/micropython/trezorhal/stm32_it.c index fb2ae5d23..5c2fd110b 100644 --- a/micropython/trezorhal/stm32_it.c +++ b/micropython/trezorhal/stm32_it.c @@ -739,6 +739,7 @@ void FLASH_IRQHandler(void) { extern I2C_HandleTypeDef *i2c_handle; void I2C1_EV_IRQHandler(void) { + printf("ev\n"); IRQ_ENTER(I2C1_EV_IRQn); if (i2c_handle) { HAL_I2C_EV_IRQHandler(i2c_handle); @@ -747,6 +748,7 @@ void I2C1_EV_IRQHandler(void) { } void I2C1_ER_IRQHandler(void) { + printf("er\n"); IRQ_ENTER(I2C1_ER_IRQn); if (i2c_handle) { HAL_I2C_ER_IRQHandler(i2c_handle); diff --git a/micropython/trezorhal/stm32_system.c b/micropython/trezorhal/stm32_system.c new file mode 100644 index 000000000..cb925d71f --- /dev/null +++ b/micropython/trezorhal/stm32_system.c @@ -0,0 +1,501 @@ +/* + * This file is part of the Micro Python project, http://micropython.org/ + * + * Taken from ST Cube library and modified. See below for original header. + * + * The MIT License (MIT) + * + * Copyright (c) 2013, 2014 Damien P. George + * + * 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. + */ + +/** + ****************************************************************************** + * @file system_stm32.c + * @author MCD Application Team + * @version V1.0.1 + * @date 26-February-2014 + * @brief CMSIS Cortex-M4/M7 Device Peripheral Access Layer System Source File. + * + * This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * + ****************************************************************************** + * @attention + * + *

© COPYRIGHT 2014 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32fxxx_system + * @{ + */ + +/** @addtogroup STM32Fxxx_System_Private_Includes + * @{ + */ + +#include STM32_HAL_H + +#define MICROPY_HW_CLK_PLLM (8) +#define MICROPY_HW_CLK_PLLN (336) +#define MICROPY_HW_CLK_PLLP (RCC_PLLP_DIV2) +#define MICROPY_HW_CLK_PLLQ (7) +#define MICROPY_HW_CLK_LAST_FREQ (1) + +void __fatal_error(const char *msg); + +/** + * @} + */ + +/** @addtogroup STM32Fxxx_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32Fxxx_System_Private_Defines + * @{ + */ + +#if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) + +#define CONFIG_RCC_CR_1ST (RCC_CR_HSION) +#define CONFIG_RCC_CR_2ND (RCC_CR_HSEON || RCC_CR_CSSON || RCC_CR_PLLON) +#define CONFIG_RCC_PLLCFGR (0x24003010) + +#if defined(MCU_SERIES_F7) +const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4}; +#endif + +#elif defined(MCU_SERIES_L4) + +#define CONFIG_RCC_CR_1ST (RCC_CR_MSION) +#define CONFIG_RCC_CR_2ND (RCC_CR_HSEON || RCC_CR_CSSON || RCC_CR_HSION || RCC_CR_PLLON) +#define CONFIG_RCC_PLLCFGR (0x00001000) +/* + * FIXME Do not know why I have to define these arrays here! they should be defined in the + * hal_rcc-file!! + * + */ +const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4}; +const uint32_t MSIRangeTable[12] = {100000, 200000, 400000, 800000, 1000000, 2000000, \ + 4000000, 8000000, 16000000, 24000000, 32000000, 48000000}; +#else +#error Unknown processor +#endif + +/************************* Miscellaneous Configuration ************************/ + +/*!< Uncomment the following line if you need to relocate your vector Table in + Internal SRAM. */ +/* #define VECT_TAB_SRAM */ +#define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +/******************************************************************************/ + +/** + * @} + */ + +/** @addtogroup STM32Fxxx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32Fxxx_System_Private_Variables + * @{ + */ + /* This variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. + */ + uint32_t SystemCoreClock = 16000000; + +/** + * @} + */ + +/** @addtogroup STM32Fxxx_System_Private_FunctionPrototypes + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32Fxxx_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the FPU setting, vector table location and External memory + * configuration. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #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 ------------*/ + + /* Set HSION bit */ + RCC->CR |= CONFIG_RCC_CR_1ST; + + /* Reset CFGR register */ + RCC->CFGR = 0x00000000; + + /* Reset HSEON, CSSON and PLLON bits */ + RCC->CR &= ~ CONFIG_RCC_CR_2ND; + + /* Reset PLLCFGR register */ + RCC->PLLCFGR = CONFIG_RCC_PLLCFGR; + + /* Reset HSEBYP bit */ + RCC->CR &= (uint32_t)0xFFFBFFFF; + + /* Disable all interrupts */ + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) + RCC->CIR = 0x00000000; + #elif defined(MCU_SERIES_L4) + RCC->CIER = 0x00000000; + #endif + + /* Configure the Vector Table location add offset address ------------------*/ +#ifdef VECT_TAB_SRAM + SCB->VTOR = SRAM1_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; +} + + +/** + * @brief System Clock Configuration + * + * The system Clock is configured for F4/F7 as follows: + * System Clock source = PLL (HSE) + * SYSCLK(Hz) = 168000000 + * HCLK(Hz) = 168000000 + * AHB Prescaler = 1 + * APB1 Prescaler = 4 + * APB2 Prescaler = 2 + * HSE Frequency(Hz) = HSE_VALUE + * PLL_M = HSE_VALUE/1000000 + * PLL_N = 336 + * PLL_P = 2 + * PLL_Q = 7 + * VDD(V) = 3.3 + * Main regulator output voltage = Scale1 mode + * Flash Latency(WS) = 5 + * + * The system Clock is configured for L4 as follows: + * System Clock source = PLL (MSI) + * SYSCLK(Hz) = 80000000 + * HCLK(Hz) = 80000000 + * AHB Prescaler = 1 + * APB1 Prescaler = 1 + * APB2 Prescaler = 1 + * MSI Frequency(Hz) = MSI_VALUE (4000000) + * LSE Frequency(Hz) = 32768 + * PLL_M = 1 + * PLL_N = 40 + * PLL_P = 7 + * PLL_Q = 2 + * PLL_R = 2 <= This is the source for SysClk, not as on F4/7 PLL_P + * Flash Latency(WS) = 4 + * @param None + * @retval None + * + * PLL is configured as follows: + * + * VCO_IN + * F4/F7 = HSE / M + * L4 = MSI / M + * VCO_OUT + * F4/F7 = HSE / M * N + * L4 = MSI / M * N + * PLLCLK + * F4/F7 = HSE / M * N / P + * L4 = MSI / M * N / R + * PLL48CK + * F4/F7 = HSE / M * N / Q + * L4 = MSI / M * N / Q USB Clock is obtained over PLLSAI1 + * + * SYSCLK = PLLCLK + * HCLK = SYSCLK / AHB_PRESC + * PCLKx = HCLK / APBx_PRESC + * + * Constraints on parameters: + * + * VCO_IN between 1MHz and 2MHz (2MHz recommended) + * VCO_OUT between 192MHz and 432MHz + * HSE = 8MHz + * M = 2 .. 63 (inclusive) + * N = 192 ... 432 (inclusive) + * P = 2, 4, 6, 8 + * Q = 2 .. 15 (inclusive) + * + * AHB_PRESC=1,2,4,8,16,64,128,256,512 + * APBx_PRESC=1,2,4,8,16 + * + * Output clocks: + * + * CPU SYSCLK max 168MHz + * USB,RNG,SDIO PLL48CK must be 48MHz for USB + * AHB HCLK max 168MHz + * APB1 PCLK1 max 42MHz + * APB2 PCLK2 max 84MHz + * + * Timers run from APBx if APBx_PRESC=1, else 2x APBx + */ +void SystemClock_Config(void) +{ + RCC_ClkInitTypeDef RCC_ClkInitStruct; + RCC_OscInitTypeDef RCC_OscInitStruct; + + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) + /* Enable Power Control clock */ + __PWR_CLK_ENABLE(); + + /* The voltage scaling allows optimizing the power consumption when the device is + clocked below the maximum system frequency, to update the voltage scaling value + regarding system frequency refer to product datasheet. */ + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); + #elif defined(MCU_SERIES_L4) + /* Enable the LSE Oscillator */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSE; + RCC_OscInitStruct.LSEState = RCC_LSE_ON; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { + __fatal_error("HAL_RCC_OscConfig"); + } + #endif + + /* Enable HSE Oscillator and activate PLL with HSE as source */ + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + #elif defined(MCU_SERIES_L4) + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSE|RCC_OSCILLATORTYPE_MSI; + RCC_OscInitStruct.LSEState = RCC_LSE_ON; + RCC_OscInitStruct.MSIState = RCC_MSI_ON; + RCC_OscInitStruct.MSICalibrationValue = RCC_MSICALIBRATION_DEFAULT; + RCC_OscInitStruct.MSIClockRange = RCC_MSIRANGE_6; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_MSI; + #endif + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 + clocks dividers */ + RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2); + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + +#if defined(MICROPY_HW_CLK_LAST_FREQ) && MICROPY_HW_CLK_LAST_FREQ + #if defined(MCU_SERIES_F7) + #define FREQ_BKP BKP31R + #elif defined(MCU_SERIES_L4) + #error Unsupported Processor + #else + #define FREQ_BKP BKP19R + #endif + uint32_t m = RTC->FREQ_BKP; + uint32_t n; + uint32_t p; + uint32_t q; + + // 222111HH HHQQQQPP nNNNNNNN NNMMMMMM + uint32_t h = (m >> 22) & 0xf; + uint32_t b1 = (m >> 26) & 0x7; + uint32_t b2 = (m >> 29) & 0x7; + q = (m >> 18) & 0xf; + p = (((m >> 16) & 0x03)+1)*2; + n = (m >> 6) & 0x3ff; + m &= 0x3f; + if ((q < 2) || (q > 15) || (p > 8) || (p < 2) || (n < 192) || (n >= 433) || (m < 2)) { + m = MICROPY_HW_CLK_PLLM; + n = MICROPY_HW_CLK_PLLN; + p = MICROPY_HW_CLK_PLLP; + q = MICROPY_HW_CLK_PLLQ; + h = RCC_SYSCLK_DIV1; + b1 = RCC_HCLK_DIV4; + b2 = RCC_HCLK_DIV2; + } else { + h <<= 4; + b1 <<= 10; + b2 <<= 10; + } + RCC_OscInitStruct.PLL.PLLM = m; //MICROPY_HW_CLK_PLLM; + RCC_OscInitStruct.PLL.PLLN = n; //MICROPY_HW_CLK_PLLN; + RCC_OscInitStruct.PLL.PLLP = p; //MICROPY_HW_CLK_PLLP; + RCC_OscInitStruct.PLL.PLLQ = q; //MICROPY_HW_CLK_PLLQ; + + RCC_ClkInitStruct.AHBCLKDivider = h; //RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = b1; //RCC_HCLK_DIV4; + RCC_ClkInitStruct.APB2CLKDivider = b2; //RCC_HCLK_DIV2; +#else // defined(MICROPY_HW_CLK_LAST_FREQ) && MICROPY_HW_CLK_LAST_FREQ + RCC_OscInitStruct.PLL.PLLM = MICROPY_HW_CLK_PLLM; + RCC_OscInitStruct.PLL.PLLN = MICROPY_HW_CLK_PLLN; + RCC_OscInitStruct.PLL.PLLP = MICROPY_HW_CLK_PLLP; + RCC_OscInitStruct.PLL.PLLQ = MICROPY_HW_CLK_PLLQ; + #if defined(MCU_SERIES_L4) + RCC_OscInitStruct.PLL.PLLR = MICROPY_HW_CLK_PLLR; + #endif + + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + #if defined(MCU_SERIES_F4) || defined(MCU_SERIES_F7) + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; + #elif defined(MCU_SERIES_L4) + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + #endif +#endif + if(HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + __fatal_error("HAL_RCC_OscConfig"); + } + +#if defined(MCU_SERIES_F7) + /* Activate the OverDrive to reach the 200 MHz Frequency */ + if (HAL_PWREx_EnableOverDrive() != HAL_OK) + { + __fatal_error("HAL_PWREx_EnableOverDrive"); + } +#endif + +#if !defined(MICROPY_HW_FLASH_LATENCY) +#define MICROPY_HW_FLASH_LATENCY FLASH_LATENCY_5 +#endif + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, MICROPY_HW_FLASH_LATENCY) != HAL_OK) + { + __fatal_error("HAL_RCC_ClockConfig"); + } + +#if defined(MCU_SERIES_F7) + // The DFU bootloader changes the clocksource register from its default power + // on reset value, so we set it back here, so the clocksources are the same + // whether we were started from DFU or from a power on reset. + + RCC->DCKCFGR2 = 0; +#endif +#if defined(MCU_SERIES_L4) + // Enable MSI-Hardware auto calibration mode with LSE + HAL_RCCEx_EnableMSIPLLMode(); + + RCC_PeriphCLKInitTypeDef PeriphClkInitStruct; + PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SAI1|RCC_PERIPHCLK_I2C1 + |RCC_PERIPHCLK_USB |RCC_PERIPHCLK_ADC + |RCC_PERIPHCLK_RNG |RCC_PERIPHCLK_RTC; + PeriphClkInitStruct.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1; + /* PLLSAI is used to clock USB, ADC, I2C1 and RNG. The frequency is + HSE(8MHz)/PLLM(2)*PLLSAI1N(24)/PLLSAIQ(2) = 48MHz. See the STM32CubeMx + application or the reference manual. */ + PeriphClkInitStruct.Sai1ClockSelection = RCC_SAI1CLKSOURCE_PLLSAI1; + PeriphClkInitStruct.AdcClockSelection = RCC_ADCCLKSOURCE_PLLSAI1; + PeriphClkInitStruct.UsbClockSelection = RCC_USBCLKSOURCE_PLLSAI1; + PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSE; + PeriphClkInitStruct.RngClockSelection = RCC_RNGCLKSOURCE_PLLSAI1; + PeriphClkInitStruct.PLLSAI1.PLLSAI1N = 24; + PeriphClkInitStruct.PLLSAI1.PLLSAI1P = RCC_PLLP_DIV7; + PeriphClkInitStruct.PLLSAI1.PLLSAI1Q = RCC_PLLQ_DIV2; + PeriphClkInitStruct.PLLSAI1.PLLSAI1R = RCC_PLLR_DIV2; + PeriphClkInitStruct.PLLSAI1.PLLSAI1ClockOut = RCC_PLLSAI1_SAI1CLK + |RCC_PLLSAI1_48M2CLK + |RCC_PLLSAI1_ADC1CLK; + + if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK) + { + __fatal_error("HAL_RCCEx_PeriphCLKConfig"); + } + + __PWR_CLK_ENABLE(); + + HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE1); + + HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); + + HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK); +#endif +} + +void HAL_MspInit(void) { +#if defined(MCU_SERIES_F7) + /* Enable I-Cache */ + SCB_EnableICache(); + + /* Enable D-Cache */ + SCB_EnableDCache(); +#endif +} diff --git a/micropython/trezorhal/usb.c b/micropython/trezorhal/usb.c new file mode 100644 index 000000000..1f891e8e5 --- /dev/null +++ b/micropython/trezorhal/usb.c @@ -0,0 +1,36 @@ +#include STM32_HAL_H + +#include "usbd_core.h" +#include "usbd_desc.h" +#include "usbd_cdc_msc_hid.h" +#include "usbd_cdc_interface.h" +#include "usbd_hid_interface.h" + +USBD_HandleTypeDef hUSBDDevice; + +void __fatal_error(const char *msg); + +void usb_init(void) { + const uint16_t vid = 0x1209; + const uint16_t pid = 0x53C1; + + USBD_HID_ModeInfoTypeDef hid_info = { + .subclass = 0, + .protocol = 0, + .max_packet_len = 64, + .polling_interval = 1, + .report_desc = (const uint8_t*)"\x06\x00\xff\x09\x01\xa1\x01\x09\x20\x15\x00\x26\xff\x00\x75\x08\x95\x40\x81\x02\x09\x21\x15\x00\x26\xff\x00\x75\x08\x95\x40\x91\x02\xc0", + .report_desc_len = 34, + }; + + USBD_SetVIDPIDRelease(vid, pid, 0x0200, 0); + if (USBD_SelectMode(USBD_MODE_CDC_HID, &hid_info) != 0) { + __fatal_error("USB init failed"); + return; + } + USBD_Init(&hUSBDDevice, (USBD_DescriptorsTypeDef*)&USBD_Descriptors, 0); // 0 == full speed + USBD_RegisterClass(&hUSBDDevice, &USBD_CDC_MSC_HID); + USBD_CDC_RegisterInterface(&hUSBDDevice, (USBD_CDC_ItfTypeDef*)&USBD_CDC_fops); + USBD_HID_RegisterInterface(&hUSBDDevice, (USBD_HID_ItfTypeDef*)&USBD_HID_fops); + USBD_Start(&hUSBDDevice); +}