diff --git a/Mcu/e230/Inc/blutil.h b/Mcu/e230/Inc/blutil.h index 6be9f779..70c35a38 100644 --- a/Mcu/e230/Inc/blutil.h +++ b/Mcu/e230/Inc/blutil.h @@ -25,27 +25,27 @@ static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - gpio_mode_set(input_port, GPIO_MODE_INPUT, pull_up_down, pin); + gpio_mode_set(input_port, GPIO_MODE_INPUT, pull_up_down, pin); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - gpio_mode_set(input_port, GPIO_MODE_OUTPUT, output_mode, pin); + gpio_mode_set(input_port, GPIO_MODE_OUTPUT, output_mode, pin); } static inline void gpio_set(uint32_t pin) { - gpio_bit_set(input_port, pin); + gpio_bit_set(input_port, pin); } static inline void gpio_clear(uint32_t pin) { - gpio_bit_reset(input_port, pin); + gpio_bit_reset(input_port, pin); } static inline bool gpio_read(uint32_t pin) { - return (gpio_input_port_get(input_port) & pin) != 0; + return (gpio_input_port_get(input_port) & pin) != 0; } #define BL_TIMER TIMER16 @@ -55,11 +55,11 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - rcu_periph_clock_enable(RCU_TIMER16); - TIMER_CAR(BL_TIMER) = 0xFFFF; - TIMER_PSC(BL_TIMER) = 71; - timer_auto_reload_shadow_enable(BL_TIMER); - timer_enable(BL_TIMER); + rcu_periph_clock_enable(RCU_TIMER16); + TIMER_CAR(BL_TIMER) = 0xFFFF; + TIMER_PSC(BL_TIMER) = 71; + timer_auto_reload_shadow_enable(BL_TIMER); + timer_enable(BL_TIMER); } /* @@ -67,12 +67,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - timer_disable(BL_TIMER); + timer_disable(BL_TIMER); } static inline uint16_t bl_timer_us(void) { - return timer_counter_read(BL_TIMER); + return timer_counter_read(BL_TIMER); } /* @@ -80,8 +80,8 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - rcu_periph_clock_enable(RCU_GPIOA); - rcu_periph_clock_enable(RCU_GPIOB); + rcu_periph_clock_enable(RCU_GPIOA); + rcu_periph_clock_enable(RCU_GPIOB); } static inline void bl_gpio_init(void) @@ -93,7 +93,7 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCU_RSTSCK & RCU_RSTSCK_SWRSTF) != 0; + return (RCU_RSTSCK & RCU_RSTSCK_SWRSTF) != 0; } /* @@ -101,22 +101,22 @@ static inline bool bl_was_software_reset(void) */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; - // setup vector table - SCB->VTOR = app_address; + // setup vector table + SCB->VTOR = app_address; - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } void SysTick_Handler(void) @@ -138,51 +138,51 @@ void SysTick_Handler(void) static void system_clock_72m_irc8m(void) { - uint32_t timeout = 0U; - uint32_t stab_flag = 0U; + uint32_t timeout = 0U; + uint32_t stab_flag = 0U; - /* enable IRC8M */ - RCU_CTL0 |= RCU_CTL0_IRC8MEN; + /* enable IRC8M */ + RCU_CTL0 |= RCU_CTL0_IRC8MEN; - /* wait until IRC8M is stable or the startup time is longer than - * IRC8M_STARTUP_TIMEOUT */ - do { - timeout++; - stab_flag = (RCU_CTL0 & RCU_CTL0_IRC8MSTB); - } while ((0U == stab_flag) && (IRC8M_STARTUP_TIMEOUT != timeout)); + /* wait until IRC8M is stable or the startup time is longer than + * IRC8M_STARTUP_TIMEOUT */ + do { + timeout++; + stab_flag = (RCU_CTL0 & RCU_CTL0_IRC8MSTB); + } while ((0U == stab_flag) && (IRC8M_STARTUP_TIMEOUT != timeout)); - /* if fail */ - if (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { - while (1) { - } + /* if fail */ + if (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { + while (1) { } + } - FMC_WS = (FMC_WS & (~FMC_WS_WSCNT)) | WS_WSCNT_2; + FMC_WS = (FMC_WS & (~FMC_WS_WSCNT)) | WS_WSCNT_2; - /* AHB = SYSCLK */ - RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; - /* APB2 = AHB */ - RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; - /* APB1 = AHB */ - RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; - /* PLL = (IRC8M/2) * 18 = 72 MHz */ - RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF); - RCU_CFG0 |= (RCU_PLLSRC_IRC8M_DIV2 | RCU_PLL_MUL18); + /* AHB = SYSCLK */ + RCU_CFG0 |= RCU_AHB_CKSYS_DIV1; + /* APB2 = AHB */ + RCU_CFG0 |= RCU_APB2_CKAHB_DIV1; + /* APB1 = AHB */ + RCU_CFG0 |= RCU_APB1_CKAHB_DIV1; + /* PLL = (IRC8M/2) * 18 = 72 MHz */ + RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF); + RCU_CFG0 |= (RCU_PLLSRC_IRC8M_DIV2 | RCU_PLL_MUL18); - /* enable PLL */ - RCU_CTL0 |= RCU_CTL0_PLLEN; + /* enable PLL */ + RCU_CTL0 |= RCU_CTL0_PLLEN; - /* wait until PLL is stable */ - while (0U == (RCU_CTL0 & RCU_CTL0_PLLSTB)) { - } + /* wait until PLL is stable */ + while (0U == (RCU_CTL0 & RCU_CTL0_PLLSTB)) { + } - /* select PLL as system clock */ - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CFG0 |= RCU_CKSYSSRC_PLL; + /* select PLL as system clock */ + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CFG0 |= RCU_CKSYSSRC_PLL; - /* wait until PLL is selected as system clock */ - while (0U == (RCU_CFG0 & RCU_SCSS_PLL)) { - } + /* wait until PLL is selected as system clock */ + while (0U == (RCU_CFG0 & RCU_SCSS_PLL)) { + } } /*! @@ -193,7 +193,7 @@ static void system_clock_72m_irc8m(void) */ static void system_clock_config(void) { - system_clock_72m_irc8m(); + system_clock_72m_irc8m(); } /*! @@ -204,24 +204,24 @@ static void system_clock_config(void) */ void SystemInit(void) { - /* enable IRC8M */ - RCU_CTL0 |= RCU_CTL0_IRC8MEN; - while (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { - } - - RCU_MODIFY(0x80); - RCU_CFG0 &= ~RCU_CFG0_SCS; - RCU_CTL0 &= ~(RCU_CTL0_HXTALEN | RCU_CTL0_CKMEN | RCU_CTL0_PLLEN | RCU_CTL0_HXTALBPS); - /* reset RCU */ - RCU_CFG0 &= ~(RCU_CFG0_SCS | RCU_CFG0_AHBPSC | RCU_CFG0_APB1PSC | RCU_CFG0_APB2PSC | RCU_CFG0_ADCPSC | RCU_CFG0_CKOUTSEL | RCU_CFG0_CKOUTDIV | RCU_CFG0_PLLDV); - RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF | RCU_CFG0_PLLMF4 | RCU_CFG0_PLLDV); - RCU_CFG1 &= ~(RCU_CFG1_PREDV); - RCU_CFG2 &= ~(RCU_CFG2_USART0SEL | RCU_CFG2_ADCSEL); - RCU_CFG2 &= ~RCU_CFG2_IRC28MDIV; - RCU_CFG2 &= ~RCU_CFG2_ADCPSC2; - RCU_CTL1 &= ~RCU_CTL1_IRC28MEN; - RCU_INT = 0x00000000U; - - /* configure system clock */ - system_clock_config(); + /* enable IRC8M */ + RCU_CTL0 |= RCU_CTL0_IRC8MEN; + while (0U == (RCU_CTL0 & RCU_CTL0_IRC8MSTB)) { + } + + RCU_MODIFY(0x80); + RCU_CFG0 &= ~RCU_CFG0_SCS; + RCU_CTL0 &= ~(RCU_CTL0_HXTALEN | RCU_CTL0_CKMEN | RCU_CTL0_PLLEN | RCU_CTL0_HXTALBPS); + /* reset RCU */ + RCU_CFG0 &= ~(RCU_CFG0_SCS | RCU_CFG0_AHBPSC | RCU_CFG0_APB1PSC | RCU_CFG0_APB2PSC | RCU_CFG0_ADCPSC | RCU_CFG0_CKOUTSEL | RCU_CFG0_CKOUTDIV | RCU_CFG0_PLLDV); + RCU_CFG0 &= ~(RCU_CFG0_PLLSEL | RCU_CFG0_PLLMF | RCU_CFG0_PLLMF4 | RCU_CFG0_PLLDV); + RCU_CFG1 &= ~(RCU_CFG1_PREDV); + RCU_CFG2 &= ~(RCU_CFG2_USART0SEL | RCU_CFG2_ADCSEL); + RCU_CFG2 &= ~RCU_CFG2_IRC28MDIV; + RCU_CFG2 &= ~RCU_CFG2_ADCPSC2; + RCU_CTL1 &= ~RCU_CTL1_IRC28MEN; + RCU_INT = 0x00000000U; + + /* configure system clock */ + system_clock_config(); } diff --git a/Mcu/e230/Src/eeprom.c b/Mcu/e230/Src/eeprom.c index e6aeb4ba..1aad5e43 100644 --- a/Mcu/e230/Src/eeprom.c +++ b/Mcu/e230/Src/eeprom.c @@ -8,35 +8,35 @@ bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x3) != 0 || (length & 0x3) != 0) { - return false; - } - const uint32_t data_length = length / 4; - - // unlock flash - - fmc_unlock(); - - // erase page if address even divisable by 1024 - if ((add % page_size) == 0) { - fmc_page_erase(add); - } - - volatile uint32_t index = 0; - while (index < data_length) { - uint32_t word; - memcpy(&word, (void*)(data+(index*4)), sizeof(word)); - fmc_word_program(add + (index * 4), word); - fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGERR); - index++; - } - fmc_lock(); - - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + if ((add & 0x3) != 0 || (length & 0x3) != 0) { + return false; + } + const uint32_t data_length = length / 4; + + // unlock flash + + fmc_unlock(); + + // erase page if address even divisable by 1024 + if ((add % page_size) == 0) { + fmc_page_erase(add); + } + + volatile uint32_t index = 0; + while (index < data_length) { + uint32_t word; + memcpy(&word, (void*)(data+(index*4)), sizeof(word)); + fmc_word_program(add + (index * 4), word); + fmc_flag_clear(FMC_FLAG_END | FMC_FLAG_WPERR | FMC_FLAG_PGERR); + index++; + } + fmc_lock(); + + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/f031/Inc/blutil.h b/Mcu/f031/Inc/blutil.h index 6712b335..21bb8210 100644 --- a/Mcu/f031/Inc/blutil.h +++ b/Mcu/f031/Inc/blutil.h @@ -30,29 +30,29 @@ uint32_t SystemCoreClock = 8000000U; static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); - LL_GPIO_SetPinPull(input_port, pin, pull_up_down); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); + LL_GPIO_SetPinPull(input_port, pin, pull_up_down); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); - LL_GPIO_SetPinOutputType(input_port, pin, output_mode); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); + LL_GPIO_SetPinOutputType(input_port, pin, output_mode); } static inline void gpio_set(uint32_t pin) { - LL_GPIO_SetOutputPin(input_port, pin); + LL_GPIO_SetOutputPin(input_port, pin); } static inline void gpio_clear(uint32_t pin) { - LL_GPIO_ResetOutputPin(input_port, pin); + LL_GPIO_ResetOutputPin(input_port, pin); } static inline bool gpio_read(uint32_t pin) { - return LL_GPIO_IsInputPinSet(input_port, pin); + return LL_GPIO_IsInputPinSet(input_port, pin); } #define BL_TIMER TIM2 @@ -62,23 +62,23 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = {0}; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - - TIM_InitStruct.Prescaler = 47; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFFFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(BL_TIMER, &TIM_InitStruct); - LL_TIM_DisableARRPreload(BL_TIMER); - LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(BL_TIMER); - - LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); - LL_TIM_EnableCounter(BL_TIMER); + LL_TIM_InitTypeDef TIM_InitStruct = {0}; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + + TIM_InitStruct.Prescaler = 47; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFFFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(BL_TIMER, &TIM_InitStruct); + LL_TIM_DisableARRPreload(BL_TIMER); + LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(BL_TIMER); + + LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); + LL_TIM_EnableCounter(BL_TIMER); } /* @@ -86,12 +86,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - LL_TIM_DeInit(BL_TIMER); + LL_TIM_DeInit(BL_TIMER); } static inline uint16_t bl_timer_us(void) { - return LL_TIM_GetCounter(BL_TIMER); + return LL_TIM_GetCounter(BL_TIMER); } /* @@ -99,50 +99,50 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - FLASH->ACR |= FLASH_ACR_PRFTBE; // prefetch buffer enable + FLASH->ACR |= FLASH_ACR_PRFTBE; // prefetch buffer enable - LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); - LL_RCC_HSI_Enable(); + LL_RCC_HSI_Enable(); - /* Wait till HSI is ready */ - while (LL_RCC_HSI_IsReady() != 1) ; - LL_RCC_HSI_SetCalibTrimming(16); - LL_RCC_HSI14_Enable(); + /* Wait till HSI is ready */ + while (LL_RCC_HSI_IsReady() != 1) ; + LL_RCC_HSI_SetCalibTrimming(16); + LL_RCC_HSI14_Enable(); - /* Wait till HSI14 is ready */ - while (LL_RCC_HSI14_IsReady() != 1) ; - LL_RCC_HSI14_SetCalibTrimming(16); - LL_RCC_LSI_Enable(); + /* Wait till HSI14 is ready */ + while (LL_RCC_HSI14_IsReady() != 1) ; + LL_RCC_HSI14_SetCalibTrimming(16); + LL_RCC_LSI_Enable(); - /* Wait till LSI is ready */ - while (LL_RCC_LSI_IsReady() != 1) ; - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); - LL_RCC_PLL_Enable(); + /* Wait till LSI is ready */ + while (LL_RCC_LSI_IsReady() != 1) ; + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); + LL_RCC_PLL_Enable(); - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) ; - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) ; + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; - LL_Init1msTick(48000000); - LL_SetSystemCoreClock(48000000); + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; + LL_Init1msTick(48000000); + LL_SetSystemCoreClock(48000000); } static inline void bl_gpio_init(void) { - LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - - /**/ - GPIO_InitStruct.Pin = input_pin; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(input_port, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + + /**/ + GPIO_InitStruct.Pin = input_pin; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + LL_GPIO_Init(input_port, &GPIO_InitStruct); } /* @@ -150,7 +150,7 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; + return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; } /* @@ -165,17 +165,17 @@ void SystemInit() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } diff --git a/Mcu/f031/Src/eeprom.c b/Mcu/f031/Src/eeprom.c index 8c4859c5..7789da27 100644 --- a/Mcu/f031/Src/eeprom.c +++ b/Mcu/f031/Src/eeprom.c @@ -17,52 +17,52 @@ static const uint32_t FLASH_FKEY2 = 0xCDEF89AB; bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x1) != 0 || (length & 0x1) != 0) { - return false; - } - const uint32_t data_length = length / 2; + if ((add & 0x1) != 0 || (length & 0x1) != 0) { + return false; + } + const uint32_t data_length = length / 2; - // unlock flash - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out*/ - } - if ((FLASH->CR & FLASH_CR_LOCK) != 0) { - FLASH->KEYR = FLASH_FKEY1; - FLASH->KEYR = FLASH_FKEY2; - } + // unlock flash + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out*/ + } + if ((FLASH->CR & FLASH_CR_LOCK) != 0) { + FLASH->KEYR = FLASH_FKEY1; + FLASH->KEYR = FLASH_FKEY2; + } - // erase page if address even divisable by 1024 - if ((add % page_size) == 0) { - FLASH->CR = FLASH_CR_PER; - FLASH->AR = add; - FLASH->CR |= FLASH_CR_STRT; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out */ - } - FLASH->SR = FLASH_SR_EOP; - FLASH->CR &= ~FLASH_CR_PER; + // erase page if address even divisable by 1024 + if ((add % page_size) == 0) { + FLASH->CR = FLASH_CR_PER; + FLASH->AR = add; + FLASH->CR |= FLASH_CR_STRT; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out */ } + FLASH->SR = FLASH_SR_EOP; + FLASH->CR &= ~FLASH_CR_PER; + } - volatile uint32_t write_cnt = 0, index = 0; - while (index < data_length) { - uint16_t word16; - memcpy(&word16, &data[index*2], sizeof(word16)); - FLASH->CR = FLASH_CR_PG; /* (1) */ - *(__IO uint16_t*)(add + write_cnt) = word16; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ - } - FLASH->SR = FLASH_SR_EOP; - FLASH->CR = 0; - write_cnt += 2; - index++; + volatile uint32_t write_cnt = 0, index = 0; + while (index < data_length) { + uint16_t word16; + memcpy(&word16, &data[index*2], sizeof(word16)); + FLASH->CR = FLASH_CR_PG; /* (1) */ + *(__IO uint16_t*)(add + write_cnt) = word16; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ } - SET_BIT(FLASH->CR, FLASH_CR_LOCK); + FLASH->SR = FLASH_SR_EOP; + FLASH->CR = 0; + write_cnt += 2; + index++; + } + SET_BIT(FLASH->CR, FLASH_CR_LOCK); - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (const uint8_t *)add, out_buff_len); + memcpy(data, (const uint8_t *)add, out_buff_len); } diff --git a/Mcu/f051/Inc/blutil.h b/Mcu/f051/Inc/blutil.h index 0c7490cb..854d5afa 100644 --- a/Mcu/f051/Inc/blutil.h +++ b/Mcu/f051/Inc/blutil.h @@ -29,29 +29,29 @@ uint32_t SystemCoreClock = 8000000U; static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); - LL_GPIO_SetPinPull(input_port, pin, pull_up_down); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); + LL_GPIO_SetPinPull(input_port, pin, pull_up_down); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); - LL_GPIO_SetPinOutputType(input_port, pin, output_mode); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); + LL_GPIO_SetPinOutputType(input_port, pin, output_mode); } static inline void gpio_set(uint32_t pin) { - LL_GPIO_SetOutputPin(input_port, pin); + LL_GPIO_SetOutputPin(input_port, pin); } static inline void gpio_clear(uint32_t pin) { - LL_GPIO_ResetOutputPin(input_port, pin); + LL_GPIO_ResetOutputPin(input_port, pin); } static inline bool gpio_read(uint32_t pin) { - return LL_GPIO_IsInputPinSet(input_port, pin); + return LL_GPIO_IsInputPinSet(input_port, pin); } #define BL_TIMER TIM2 @@ -61,23 +61,23 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = {0}; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - - TIM_InitStruct.Prescaler = 47; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFFFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(BL_TIMER, &TIM_InitStruct); - LL_TIM_DisableARRPreload(BL_TIMER); - LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(BL_TIMER); - - LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); - LL_TIM_EnableCounter(BL_TIMER); + LL_TIM_InitTypeDef TIM_InitStruct = {0}; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + + TIM_InitStruct.Prescaler = 47; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFFFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(BL_TIMER, &TIM_InitStruct); + LL_TIM_DisableARRPreload(BL_TIMER); + LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(BL_TIMER); + + LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); + LL_TIM_EnableCounter(BL_TIMER); } /* @@ -85,12 +85,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - LL_TIM_DeInit(BL_TIMER); + LL_TIM_DeInit(BL_TIMER); } static inline uint16_t bl_timer_us(void) { - return LL_TIM_GetCounter(BL_TIMER); + return LL_TIM_GetCounter(BL_TIMER); } /* @@ -98,50 +98,50 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - FLASH->ACR |= FLASH_ACR_PRFTBE; // prefetch buffer enable + FLASH->ACR |= FLASH_ACR_PRFTBE; // prefetch buffer enable - LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_1); - LL_RCC_HSI_Enable(); + LL_RCC_HSI_Enable(); - /* Wait till HSI is ready */ - while (LL_RCC_HSI_IsReady() != 1) ; - LL_RCC_HSI_SetCalibTrimming(16); - LL_RCC_HSI14_Enable(); + /* Wait till HSI is ready */ + while (LL_RCC_HSI_IsReady() != 1) ; + LL_RCC_HSI_SetCalibTrimming(16); + LL_RCC_HSI14_Enable(); - /* Wait till HSI14 is ready */ - while (LL_RCC_HSI14_IsReady() != 1) ; - LL_RCC_HSI14_SetCalibTrimming(16); - LL_RCC_LSI_Enable(); + /* Wait till HSI14 is ready */ + while (LL_RCC_HSI14_IsReady() != 1) ; + LL_RCC_HSI14_SetCalibTrimming(16); + LL_RCC_LSI_Enable(); - /* Wait till LSI is ready */ - while (LL_RCC_LSI_IsReady() != 1) ; - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); - LL_RCC_PLL_Enable(); + /* Wait till LSI is ready */ + while (LL_RCC_LSI_IsReady() != 1) ; + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12); + LL_RCC_PLL_Enable(); - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) ; - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) ; + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; - LL_Init1msTick(48000000); - LL_SetSystemCoreClock(48000000); + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; + LL_Init1msTick(48000000); + LL_SetSystemCoreClock(48000000); } static inline void bl_gpio_init(void) { - LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); - LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); - - /**/ - GPIO_InitStruct.Pin = input_pin; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; - LL_GPIO_Init(input_port, &GPIO_InitStruct); + LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB); + LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA); + + /**/ + GPIO_InitStruct.Pin = input_pin; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_UP; + LL_GPIO_Init(input_port, &GPIO_InitStruct); } /* @@ -149,7 +149,7 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; + return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; } /* @@ -164,17 +164,17 @@ void SystemInit() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } diff --git a/Mcu/f051/Src/eeprom.c b/Mcu/f051/Src/eeprom.c index 5448b572..b9107e58 100644 --- a/Mcu/f051/Src/eeprom.c +++ b/Mcu/f051/Src/eeprom.c @@ -17,52 +17,52 @@ static const uint32_t FLASH_FKEY2 = 0xCDEF89AB; bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x1) != 0 || (length & 0x1) != 0) { - return false; - } - const uint32_t data_length = length / 2; + if ((add & 0x1) != 0 || (length & 0x1) != 0) { + return false; + } + const uint32_t data_length = length / 2; - // unlock flash - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out*/ - } - if ((FLASH->CR & FLASH_CR_LOCK) != 0) { - FLASH->KEYR = FLASH_FKEY1; - FLASH->KEYR = FLASH_FKEY2; - } + // unlock flash + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out*/ + } + if ((FLASH->CR & FLASH_CR_LOCK) != 0) { + FLASH->KEYR = FLASH_FKEY1; + FLASH->KEYR = FLASH_FKEY2; + } - // erase page if address even divisable by 1024 - if ((add % page_size) == 0) { - FLASH->CR = FLASH_CR_PER; - FLASH->AR = add; - FLASH->CR |= FLASH_CR_STRT; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { - /* add time-out */ - } - FLASH->SR = FLASH_SR_EOP; - FLASH->CR = 0; + // erase page if address even divisable by 1024 + if ((add % page_size) == 0) { + FLASH->CR = FLASH_CR_PER; + FLASH->AR = add; + FLASH->CR |= FLASH_CR_STRT; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { + /* add time-out */ } + FLASH->SR = FLASH_SR_EOP; + FLASH->CR = 0; + } - volatile uint32_t write_cnt = 0, index = 0; - while (index < data_length) { - uint16_t word16; - memcpy(&word16, &data[index*2], sizeof(word16)); - FLASH->CR = FLASH_CR_PG; /* (1) */ - *(__IO uint16_t*)(add + write_cnt) = word16; - while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ - } - FLASH->SR = FLASH_SR_EOP; - FLASH->CR = 0; - write_cnt += 2; - index++; + volatile uint32_t write_cnt = 0, index = 0; + while (index < data_length) { + uint16_t word16; + memcpy(&word16, &data[index*2], sizeof(word16)); + FLASH->CR = FLASH_CR_PG; /* (1) */ + *(__IO uint16_t*)(add + write_cnt) = word16; + while ((FLASH->SR & FLASH_SR_BSY) != 0) { /* add time-out */ } - SET_BIT(FLASH->CR, FLASH_CR_LOCK); + FLASH->SR = FLASH_SR_EOP; + FLASH->CR = 0; + write_cnt += 2; + index++; + } + SET_BIT(FLASH->CR, FLASH_CR_LOCK); - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (const uint8_t *)add, out_buff_len); + memcpy(data, (const uint8_t *)add, out_buff_len); } diff --git a/Mcu/f415/Inc/blutil.h b/Mcu/f415/Inc/blutil.h index 4d082c64..764261ee 100644 --- a/Mcu/f415/Inc/blutil.h +++ b/Mcu/f415/Inc/blutil.h @@ -23,42 +23,42 @@ #ifdef PORT_LETTER static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - if (pin < GPIO_PIN(8)) { - const uint32_t pin4 = (pin*pin*pin*pin); - input_port->cfglr = (input_port->cfglr & ~(pin4*0xfU)) | (pin4*pull_up_down); - } else { - pin >>= 8U; - const uint32_t pin4 = (pin*pin*pin*pin); - input_port->cfghr = (input_port->cfglr & ~(pin4*0xfU)) | (pin4*pull_up_down); - } + if (pin < GPIO_PIN(8)) { + const uint32_t pin4 = (pin*pin*pin*pin); + input_port->cfglr = (input_port->cfglr & ~(pin4*0xfU)) | (pin4*pull_up_down); + } else { + pin >>= 8U; + const uint32_t pin4 = (pin*pin*pin*pin); + input_port->cfghr = (input_port->cfglr & ~(pin4*0xfU)) | (pin4*pull_up_down); + } } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - const uint32_t output_normal_strength = 2U; - if (pin < GPIO_PIN(8)) { - const uint32_t pin4 = (pin*pin*pin*pin); - input_port->cfglr = (input_port->cfglr & ~(pin4*0xfU)) | (pin4*(output_normal_strength | output_mode)); - } else { - pin >>= 8U; - const uint32_t pin4 = (pin*pin*pin*pin); - input_port->cfghr = (input_port->cfghr & ~(pin4*0xfU)) | (pin4*(output_normal_strength | output_mode)); - } + const uint32_t output_normal_strength = 2U; + if (pin < GPIO_PIN(8)) { + const uint32_t pin4 = (pin*pin*pin*pin); + input_port->cfglr = (input_port->cfglr & ~(pin4*0xfU)) | (pin4*(output_normal_strength | output_mode)); + } else { + pin >>= 8U; + const uint32_t pin4 = (pin*pin*pin*pin); + input_port->cfghr = (input_port->cfghr & ~(pin4*0xfU)) | (pin4*(output_normal_strength | output_mode)); + } } static inline void gpio_set(uint32_t pin) { - input_port->scr = pin; + input_port->scr = pin; } static inline void gpio_clear(uint32_t pin) { - input_port->clr = pin; + input_port->clr = pin; } static inline bool gpio_read(uint32_t pin) { - return (input_port->idt & pin) != 0; + return (input_port->idt & pin) != 0; } #endif // PORT_LETTER @@ -69,13 +69,13 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); - BL_TIMER->div = 143; - BL_TIMER->pr = 0xFFFF; - BL_TIMER->swevt_bit.ovfswtr = TRUE; - - BL_TIMER->ctrl1_bit.tmren = TRUE; + BL_TIMER->div = 143; + BL_TIMER->pr = 0xFFFF; + BL_TIMER->swevt_bit.ovfswtr = TRUE; + + BL_TIMER->ctrl1_bit.tmren = TRUE; } #ifdef PORT_LETTER @@ -86,22 +86,24 @@ unsigned int system_core_clock = HICK_VALUE; /*!< system clock frequen */ void system_core_clock_update(void) { - uint32_t temp = 0, div_value = 0; - - static const uint8_t sys_ahb_div_table[16] = { 0, 0, 0, 0, 0, 0, 0, 0, - 1, 2, 3, 4, 6, 7, 8, 9 }; - - if (((CRM->misc2_bit.hick_to_sclk) != RESET) && ((CRM->misc1_bit.hickdiv) != RESET)) - system_core_clock = HICK_VALUE * 6; - else - system_core_clock = HICK_VALUE; - - /* compute sclk, ahbclk frequency */ - /* get ahb division */ - temp = CRM->cfg_bit.ahbdiv; - div_value = sys_ahb_div_table[temp]; - /* ahbclk frequency */ - system_core_clock = system_core_clock >> div_value; + uint32_t temp = 0, div_value = 0; + + static const uint8_t sys_ahb_div_table[16] = { 0, 0, 0, 0, 0, 0, 0, 0, + 1, 2, 3, 4, 6, 7, 8, 9 + }; + + if (((CRM->misc2_bit.hick_to_sclk) != RESET) && ((CRM->misc1_bit.hickdiv) != RESET)) { + system_core_clock = HICK_VALUE * 6; + } else { + system_core_clock = HICK_VALUE; + } + + /* compute sclk, ahbclk frequency */ + /* get ahb division */ + temp = CRM->cfg_bit.ahbdiv; + div_value = sys_ahb_div_table[temp]; + /* ahbclk frequency */ + system_core_clock = system_core_clock >> div_value; } #endif // PORT_LETTER @@ -110,75 +112,72 @@ void system_core_clock_update(void) */ static inline void bl_clock_config(void) { - /* config flash psr register */ - flash_psr_set(FLASH_WAIT_CYCLE_4); + /* config flash psr register */ + flash_psr_set(FLASH_WAIT_CYCLE_4); - /* reset crm */ - crm_reset(); + /* reset crm */ + crm_reset(); - crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); + crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); - /* wait till hick is ready */ - while(crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) - { - } + /* wait till hick is ready */ + while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) { + } - /* config pll clock resource */ - crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_36); + /* config pll clock resource */ + crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_36); - /* enable pll */ - crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); + /* enable pll */ + crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); - /* wait till pll is ready */ - while(crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) - { - } + /* wait till pll is ready */ + while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) { + } - /* config ahbclk */ - crm_ahb_div_set(CRM_AHB_DIV_1); + /* config ahbclk */ + crm_ahb_div_set(CRM_AHB_DIV_1); - /* config apb2clk */ - crm_apb2_div_set(CRM_APB2_DIV_2); + /* config apb2clk */ + crm_apb2_div_set(CRM_APB2_DIV_2); - /* config apb1clk */ - crm_apb1_div_set(CRM_APB1_DIV_2); + /* config apb1clk */ + crm_apb1_div_set(CRM_APB1_DIV_2); - /* enable auto step mode */ - crm_auto_step_mode_enable(TRUE); + /* enable auto step mode */ + crm_auto_step_mode_enable(TRUE); - /* select pll as system clock source */ - crm_sysclk_switch(CRM_SCLK_PLL); + /* select pll as system clock source */ + crm_sysclk_switch(CRM_SCLK_PLL); - /* wait till pll is used as system clock source */ - while(crm_sysclk_switch_status_get() != CRM_SCLK_PLL) - { - } + /* wait till pll is used as system clock source */ + while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) { + } - /* disable auto step mode */ - crm_auto_step_mode_enable(FALSE); + /* disable auto step mode */ + crm_auto_step_mode_enable(FALSE); - /* update system_core_clock global variable */ - system_core_clock_update(); + /* update system_core_clock global variable */ + system_core_clock_update(); } #ifdef PORT_LETTER static inline void bl_gpio_init(void) { - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK , TRUE); - gpio_pin_remap_config(SWJTAG_GMUX_010, TRUE); // pb4 GPIO - - gpio_init_type gpio_init_struct; - gpio_default_para_init(&gpio_init_struct); - - gpio_init_struct.gpio_pins = input_pin; - gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_pull = GPIO_PULL_NONE; - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - - gpio_init(input_port, &gpio_init_struct); + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); + gpio_pin_remap_config(SWJTAG_GMUX_010, TRUE); // pb4 GPIO + + gpio_init_type gpio_init_struct; + gpio_default_para_init(&gpio_init_struct); + + gpio_init_struct.gpio_pins = input_pin; + gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + + gpio_init(input_port, &gpio_init_struct); } #endif // PORT_LETTER @@ -187,12 +186,12 @@ static inline void bl_gpio_init(void) */ static inline void bl_timer_disable(void) { - BL_TIMER->ctrl1_bit.tmren = FALSE; + BL_TIMER->ctrl1_bit.tmren = FALSE; } static inline uint16_t bl_timer_us(void) { - return BL_TIMER->cval; + return BL_TIMER->cval; } /* @@ -200,7 +199,7 @@ static inline uint16_t bl_timer_us(void) */ static inline bool bl_was_software_reset(void) { - return crm_flag_get(CRM_SW_RESET_FLAG) == SET; + return crm_flag_get(CRM_SW_RESET_FLAG) == SET; } #ifdef PORT_LETTER @@ -216,18 +215,18 @@ void SystemInit() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } #endif // PORT_LETTER diff --git a/Mcu/f415/Inc/functions.h b/Mcu/f415/Inc/functions.h index 77d984c8..042e9241 100644 --- a/Mcu/f415/Inc/functions.h +++ b/Mcu/f415/Inc/functions.h @@ -12,7 +12,7 @@ #include "main.h" void gpio_mode_QUICK(gpio_type* gpio_periph, uint32_t mode, - uint32_t pull_up_down, uint32_t pin); + uint32_t pull_up_down, uint32_t pin); void gpio_mode_set(uint32_t mode, uint32_t pull_up_down, uint32_t pin); int getAbsDif(int number1, int number2); void delayMicros(uint32_t micros); diff --git a/Mcu/f415/Inc/system_at32f415.h b/Mcu/f415/Inc/system_at32f415.h index 4c439802..dbd2cf78 100644 --- a/Mcu/f415/Inc/system_at32f415.h +++ b/Mcu/f415/Inc/system_at32f415.h @@ -56,7 +56,7 @@ extern "C" { */ extern unsigned int - system_core_clock; /*!< system clock frequency (core clock) */ +system_core_clock; /*!< system clock frequency (core clock) */ /** * @} diff --git a/Mcu/f415/Src/eeprom.c b/Mcu/f415/Src/eeprom.c index e6b0dfcf..2407a356 100644 --- a/Mcu/f415/Src/eeprom.c +++ b/Mcu/f415/Src/eeprom.c @@ -12,49 +12,49 @@ */ static inline uint32_t sector_size() { - const uint16_t *F_SIZE = (const uint16_t *)0x1FFFF7E0; - if (*F_SIZE <= 128) { - // 1k sectors for 128k flash or less - return 1024; - } - // 256k flash is 2k sectors - return 2048; + const uint16_t *F_SIZE = (const uint16_t *)0x1FFFF7E0; + if (*F_SIZE <= 128) { + // 1k sectors for 128k flash or less + return 1024; + } + // 256k flash is 2k sectors + return 2048; } bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x3) != 0 || (length & 0x3) != 0) { - return false; - } - /* - we need the data to be 32 bit aligned - */ - const uint32_t word_length = length / 4; - - // unlock flash - flash_unlock(); - - // erase page if address even divisable by sector size - if ((add % sector_size()) == 0) { - flash_sector_erase(add); - } - - uint32_t index = 0; - while (index < word_length) { - uint32_t word; - memcpy(&word, &data[index*4], sizeof(word)); - flash_word_program(add + (index * 4), word); - flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); - index++; - } - flash_lock(); - - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + if ((add & 0x3) != 0 || (length & 0x3) != 0) { + return false; + } + /* + we need the data to be 32 bit aligned + */ + const uint32_t word_length = length / 4; + + // unlock flash + flash_unlock(); + + // erase page if address even divisable by sector size + if ((add % sector_size()) == 0) { + flash_sector_erase(add); + } + + uint32_t index = 0; + while (index < word_length) { + uint32_t word; + memcpy(&word, &data[index*4], sizeof(word)); + flash_word_program(add + (index * 4), word); + flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); + index++; + } + flash_lock(); + + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/f421/Inc/blutil.h b/Mcu/f421/Inc/blutil.h index aea42f84..e492469f 100644 --- a/Mcu/f421/Inc/blutil.h +++ b/Mcu/f421/Inc/blutil.h @@ -18,31 +18,31 @@ static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - const uint32_t pinsq = (pin*pin); - input_port->cfgr = (input_port->cfgr & ~(pinsq * 0x3U)) | (pinsq * GPIO_MODE_INPUT); - input_port->pull = (input_port->pull & ~(pinsq * 0x3U)) | (pinsq * pull_up_down); + const uint32_t pinsq = (pin*pin); + input_port->cfgr = (input_port->cfgr & ~(pinsq * 0x3U)) | (pinsq * GPIO_MODE_INPUT); + input_port->pull = (input_port->pull & ~(pinsq * 0x3U)) | (pinsq * pull_up_down); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - const uint32_t pinsq = (pin*pin); - input_port->cfgr = (input_port->cfgr & ~(pinsq * 0x3U)) | (pinsq * GPIO_MODE_OUTPUT); - input_port->omode = (input_port->omode & ~pin) | (output_mode << pin); + const uint32_t pinsq = (pin*pin); + input_port->cfgr = (input_port->cfgr & ~(pinsq * 0x3U)) | (pinsq * GPIO_MODE_OUTPUT); + input_port->omode = (input_port->omode & ~pin) | (output_mode << pin); } static inline void gpio_set(uint32_t pin) { - input_port->scr = pin; + input_port->scr = pin; } static inline void gpio_clear(uint32_t pin) { - input_port->clr = pin; + input_port->clr = pin; } static inline bool gpio_read(uint32_t pin) { - return (input_port->idt & pin) != 0; + return (input_port->idt & pin) != 0; } #define BL_TIMER TMR3 @@ -52,14 +52,14 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); - - BL_TIMER->div = 119; - BL_TIMER->pr = 0xFFFF; - BL_TIMER->ctrl1_bit.tmren = TRUE; - BL_TIMER->ctrl1_bit.clkdiv = TMR_CLOCK_DIV1; - BL_TIMER->ctrl1_bit.cnt_dir = TMR_COUNT_UP; - BL_TIMER->swevt_bit.ovfswtr = TRUE; + crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE); + + BL_TIMER->div = 119; + BL_TIMER->pr = 0xFFFF; + BL_TIMER->ctrl1_bit.tmren = TRUE; + BL_TIMER->ctrl1_bit.clkdiv = TMR_CLOCK_DIV1; + BL_TIMER->ctrl1_bit.cnt_dir = TMR_COUNT_UP; + BL_TIMER->swevt_bit.ovfswtr = TRUE; } /* @@ -67,12 +67,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - BL_TIMER->ctrl1_bit.tmren = FALSE; + BL_TIMER->ctrl1_bit.tmren = FALSE; } static inline uint16_t bl_timer_us(void) { - return BL_TIMER->cval; + return BL_TIMER->cval; } /* @@ -80,43 +80,40 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - flash_psr_set(FLASH_WAIT_CYCLE_3); - crm_reset(); - crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); - while(crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) - { - } - crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_30); - crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); - while(crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) - { - } - crm_ahb_div_set(CRM_AHB_DIV_1); - crm_apb2_div_set(CRM_APB2_DIV_1); - crm_apb1_div_set(CRM_APB1_DIV_1); - crm_auto_step_mode_enable(TRUE); - crm_sysclk_switch(CRM_SCLK_PLL); - while(crm_sysclk_switch_status_get() != CRM_SCLK_PLL) - { - } - crm_auto_step_mode_enable(FALSE); + flash_psr_set(FLASH_WAIT_CYCLE_3); + crm_reset(); + crm_clock_source_enable(CRM_CLOCK_SOURCE_HICK, TRUE); + while (crm_flag_get(CRM_HICK_STABLE_FLAG) != SET) { + } + crm_pll_config(CRM_PLL_SOURCE_HICK, CRM_PLL_MULT_30); + crm_clock_source_enable(CRM_CLOCK_SOURCE_PLL, TRUE); + while (crm_flag_get(CRM_PLL_STABLE_FLAG) != SET) { + } + crm_ahb_div_set(CRM_AHB_DIV_1); + crm_apb2_div_set(CRM_APB2_DIV_1); + crm_apb1_div_set(CRM_APB1_DIV_1); + crm_auto_step_mode_enable(TRUE); + crm_sysclk_switch(CRM_SCLK_PLL); + while (crm_sysclk_switch_status_get() != CRM_SCLK_PLL) { + } + crm_auto_step_mode_enable(FALSE); } static inline void bl_gpio_init(void) { - crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - - gpio_init_type gpio_init_struct; - gpio_default_para_init(&gpio_init_struct); - - gpio_init_struct.gpio_pins = input_pin; - gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_pull = GPIO_PULL_NONE; - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - - gpio_init(input_port, &gpio_init_struct); + crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + + gpio_init_type gpio_init_struct; + gpio_default_para_init(&gpio_init_struct); + + gpio_init_struct.gpio_pins = input_pin; + gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + + gpio_init(input_port, &gpio_init_struct); } /* @@ -124,7 +121,7 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return crm_flag_get(CRM_SW_RESET_FLAG) == SET; + return crm_flag_get(CRM_SW_RESET_FLAG) == SET; } /* @@ -139,17 +136,17 @@ void SystemInit() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } diff --git a/Mcu/f421/Inc/system_at32f421.h b/Mcu/f421/Inc/system_at32f421.h index 38c3be61..cbb23719 100644 --- a/Mcu/f421/Inc/system_at32f421.h +++ b/Mcu/f421/Inc/system_at32f421.h @@ -56,7 +56,7 @@ extern "C" { */ extern unsigned int - system_core_clock; /*!< system clock frequency (core clock) */ +system_core_clock; /*!< system clock frequency (core clock) */ /** * @} diff --git a/Mcu/f421/Src/eeprom.c b/Mcu/f421/Src/eeprom.c index b468dd17..262c9898 100644 --- a/Mcu/f421/Src/eeprom.c +++ b/Mcu/f421/Src/eeprom.c @@ -8,34 +8,34 @@ bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x3) != 0 || (length & 0x3) != 0) { - return false; - } - const uint32_t data_length = length / 4; - - // unlock flash - flash_unlock(); - - // erase page if address even divisable by 1024 - if ((add % page_size) == 0) { - flash_sector_erase(add); - } - - uint32_t index = 0; - while (index < data_length) { - uint32_t word; - memcpy(&word, &data[index*4], sizeof(word)); - flash_word_program(add + (index * 4), word); - flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); - index++; - } - flash_lock(); - - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + if ((add & 0x3) != 0 || (length & 0x3) != 0) { + return false; + } + const uint32_t data_length = length / 4; + + // unlock flash + flash_unlock(); + + // erase page if address even divisable by 1024 + if ((add % page_size) == 0) { + flash_sector_erase(add); + } + + uint32_t index = 0; + while (index < data_length) { + uint32_t word; + memcpy(&word, &data[index*4], sizeof(word)); + flash_word_program(add + (index * 4), word); + flash_flag_clear(FLASH_PROGRAM_ERROR | FLASH_EPP_ERROR | FLASH_OPERATE_DONE); + index++; + } + flash_lock(); + + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/g071/Inc/blutil.h b/Mcu/g071/Inc/blutil.h index 46870884..0f273e8f 100644 --- a/Mcu/g071/Inc/blutil.h +++ b/Mcu/g071/Inc/blutil.h @@ -33,29 +33,29 @@ uint32_t SystemCoreClock = 8000000U; static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); - LL_GPIO_SetPinPull(input_port, pin, pull_up_down); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); + LL_GPIO_SetPinPull(input_port, pin, pull_up_down); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); - LL_GPIO_SetPinOutputType(input_port, pin, output_mode); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); + LL_GPIO_SetPinOutputType(input_port, pin, output_mode); } static inline void gpio_set(uint32_t pin) { - LL_GPIO_SetOutputPin(input_port, pin); + LL_GPIO_SetOutputPin(input_port, pin); } static inline void gpio_clear(uint32_t pin) { - LL_GPIO_ResetOutputPin(input_port, pin); + LL_GPIO_ResetOutputPin(input_port, pin); } static inline bool gpio_read(uint32_t pin) { - return LL_GPIO_IsInputPinSet(input_port, pin); + return LL_GPIO_IsInputPinSet(input_port, pin); } #define BL_TIMER TIM2 @@ -65,23 +65,23 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = {0}; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - - TIM_InitStruct.Prescaler = 63; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFFFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(BL_TIMER, &TIM_InitStruct); - LL_TIM_DisableARRPreload(BL_TIMER); - LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(BL_TIMER); - - LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); - LL_TIM_EnableCounter(BL_TIMER); + LL_TIM_InitTypeDef TIM_InitStruct = {0}; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + + TIM_InitStruct.Prescaler = 63; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFFFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(BL_TIMER, &TIM_InitStruct); + LL_TIM_DisableARRPreload(BL_TIMER); + LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(BL_TIMER); + + LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); + LL_TIM_EnableCounter(BL_TIMER); } /* @@ -89,12 +89,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - LL_TIM_DeInit(BL_TIMER); + LL_TIM_DeInit(BL_TIMER); } static inline uint16_t bl_timer_us(void) { - return LL_TIM_GetCounter(BL_TIMER); + return LL_TIM_GetCounter(BL_TIMER); } /* @@ -102,52 +102,49 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_2); - - /* HSI configuration and activation */ - LL_RCC_HSI_Enable(); - while(LL_RCC_HSI_IsReady() != 1) - { - } - - /* Main PLL configuration and activation */ - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_1, 8, LL_RCC_PLLR_DIV_2); - LL_RCC_PLL_Enable(); - LL_RCC_PLL_EnableDomain_SYS(); - while(LL_RCC_PLL_IsReady() != 1) - { - } - - /* Set AHB prescaler*/ - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - - /* Sysclk activation on the main PLL */ - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) - { - } - - /* Set APB1 prescaler*/ - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_Init1msTick(64000000); - LL_SetSystemCoreClock(64000000); - /* Update CMSIS variable (which can be updated also through SystemCoreClockUpdate function) */ - LL_SetSystemCoreClock(64000000); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_2); + + /* HSI configuration and activation */ + LL_RCC_HSI_Enable(); + while (LL_RCC_HSI_IsReady() != 1) { + } + + /* Main PLL configuration and activation */ + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_1, 8, LL_RCC_PLLR_DIV_2); + LL_RCC_PLL_Enable(); + LL_RCC_PLL_EnableDomain_SYS(); + while (LL_RCC_PLL_IsReady() != 1) { + } + + /* Set AHB prescaler*/ + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + + /* Sysclk activation on the main PLL */ + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) { + } + + /* Set APB1 prescaler*/ + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_Init1msTick(64000000); + LL_SetSystemCoreClock(64000000); + /* Update CMSIS variable (which can be updated also through SystemCoreClockUpdate function) */ + LL_SetSystemCoreClock(64000000); } static inline void bl_gpio_init(void) { - LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - /* GPIO Ports Clock Enable */ - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); - LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); + /* GPIO Ports Clock Enable */ + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA); + LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOB); - /**/ - GPIO_InitStruct.Pin = input_pin; - GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(input_port, &GPIO_InitStruct); + /**/ + GPIO_InitStruct.Pin = input_pin; + GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + LL_GPIO_Init(input_port, &GPIO_InitStruct); } /* @@ -155,7 +152,7 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; + return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; } /* @@ -170,20 +167,20 @@ void SystemInit() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup vector table - SCB->VTOR = app_address; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup vector table + SCB->VTOR = app_address; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } diff --git a/Mcu/g071/Src/eeprom.c b/Mcu/g071/Src/eeprom.c index a8bbc749..b322d9cd 100644 --- a/Mcu/g071/Src/eeprom.c +++ b/Mcu/g071/Src/eeprom.c @@ -17,63 +17,63 @@ static const uint32_t FLASH_FKEY2 = 0xCDEF89AB; bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x7) != 0 || (length & 0x7)) { - // address and length must be on 8 byte boundary - return false; - } - // we need to flash on 32 bit boundaries - const uint32_t data_length = length / 4; - volatile FLASH_TypeDef *flash = FLASH; - - // clear errors - flash->SR |= FLASH_SR_OPERR | FLASH_SR_PROGERR | FLASH_SR_WRPERR | FLASH_SR_PGAERR | - FLASH_SR_SIZERR | FLASH_SR_PGSERR | FLASH_SR_MISERR | FLASH_SR_FASTERR | - FLASH_SR_RDERR | FLASH_SR_OPTVERR; - - // unlock flash + if ((add & 0x7) != 0 || (length & 0x7)) { + // address and length must be on 8 byte boundary + return false; + } + // we need to flash on 32 bit boundaries + const uint32_t data_length = length / 4; + volatile FLASH_TypeDef *flash = FLASH; + + // clear errors + flash->SR |= FLASH_SR_OPERR | FLASH_SR_PROGERR | FLASH_SR_WRPERR | FLASH_SR_PGAERR | + FLASH_SR_SIZERR | FLASH_SR_PGSERR | FLASH_SR_MISERR | FLASH_SR_FASTERR | + FLASH_SR_RDERR | FLASH_SR_OPTVERR; + + // unlock flash + while ((flash->SR & FLASH_SR_BSY1) != 0) ; + + if ((flash->CR & FLASH_CR_LOCK) != 0) { + flash->KEYR = FLASH_FKEY1; + flash->KEYR = FLASH_FKEY2; + } + + // erase page if address is divisable by page size + if ((add % page_size) == 0) { + flash->CR = FLASH_CR_PER; + flash->CR |= (add/page_size) << 3; + flash->CR |= FLASH_CR_STRT; while ((flash->SR & FLASH_SR_BSY1) != 0) ; + } - if ((flash->CR & FLASH_CR_LOCK) != 0) { - flash->KEYR = FLASH_FKEY1; - flash->KEYR = FLASH_FKEY2; - } + uint32_t index = 0; + volatile uint32_t *fdata = (volatile uint32_t *)add; - // erase page if address is divisable by page size - if ((add % page_size) == 0){ - flash->CR = FLASH_CR_PER; - flash->CR |= (add/page_size) << 3; - flash->CR |= FLASH_CR_STRT; - while ((flash->SR & FLASH_SR_BSY1) != 0) ; - } + while (index < data_length) { + // flash two words at a time + uint32_t words[2]; + memcpy((void*)&words[0], &data[index*4], sizeof(words)); - uint32_t index = 0; - volatile uint32_t *fdata = (volatile uint32_t *)add; + flash->CR = FLASH_CR_PG; - while (index < data_length) { - // flash two words at a time - uint32_t words[2]; - memcpy((void*)&words[0], &data[index*4], sizeof(words)); + fdata[index] = words[0]; + fdata[index+1] = words[1]; - flash->CR = FLASH_CR_PG; - - fdata[index] = words[0]; - fdata[index+1] = words[1]; - - while ((flash->SR & FLASH_SR_BSY1) != 0) ; + while ((flash->SR & FLASH_SR_BSY1) != 0) ; - flash->SR |= FLASH_SR_EOP; - flash->CR = 0; - index += 2; - } + flash->SR |= FLASH_SR_EOP; + flash->CR = 0; + index += 2; + } - // lock flash again - SET_BIT(flash->CR, FLASH_CR_LOCK); + // lock flash again + SET_BIT(flash->CR, FLASH_CR_LOCK); - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/g431/Inc/blutil.h b/Mcu/g431/Inc/blutil.h index 6db4fa0b..47cbf7cd 100644 --- a/Mcu/g431/Inc/blutil.h +++ b/Mcu/g431/Inc/blutil.h @@ -29,29 +29,29 @@ static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); - LL_GPIO_SetPinPull(input_port, pin, pull_up_down); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); + LL_GPIO_SetPinPull(input_port, pin, pull_up_down); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); - LL_GPIO_SetPinOutputType(input_port, pin, output_mode); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); + LL_GPIO_SetPinOutputType(input_port, pin, output_mode); } static inline void gpio_set(uint32_t pin) { - LL_GPIO_SetOutputPin(input_port, pin); + LL_GPIO_SetOutputPin(input_port, pin); } static inline void gpio_clear(uint32_t pin) { - LL_GPIO_ResetOutputPin(input_port, pin); + LL_GPIO_ResetOutputPin(input_port, pin); } static inline bool gpio_read(uint32_t pin) { - return LL_GPIO_IsInputPinSet(input_port, pin); + return LL_GPIO_IsInputPinSet(input_port, pin); } #define BL_TIMER TIM2 @@ -61,23 +61,23 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = {0}; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - - TIM_InitStruct.Prescaler = (160-1); // HSI PLL clock is 160Mz, want 1MHz timer - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFFFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(BL_TIMER, &TIM_InitStruct); - LL_TIM_DisableARRPreload(BL_TIMER); - LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(BL_TIMER); - - LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); - LL_TIM_EnableCounter(BL_TIMER); + LL_TIM_InitTypeDef TIM_InitStruct = {0}; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + + TIM_InitStruct.Prescaler = (160-1); // HSI PLL clock is 160Mz, want 1MHz timer + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFFFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(BL_TIMER, &TIM_InitStruct); + LL_TIM_DisableARRPreload(BL_TIMER); + LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(BL_TIMER); + + LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); + LL_TIM_EnableCounter(BL_TIMER); } /* @@ -85,12 +85,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - LL_TIM_DeInit(BL_TIMER); + LL_TIM_DeInit(BL_TIMER); } static inline uint16_t bl_timer_us(void) { - return LL_TIM_GetCounter(BL_TIMER); + return LL_TIM_GetCounter(BL_TIMER); } /* @@ -98,44 +98,44 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_4); - while (LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_4) ; - LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); - while (LL_PWR_IsActiveFlag_VOS() != 0) ; + LL_FLASH_SetLatency(LL_FLASH_LATENCY_4); + while (LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_4) ; + LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); + while (LL_PWR_IsActiveFlag_VOS() != 0) ; - LL_RCC_HSI_Enable(); + LL_RCC_HSI_Enable(); - /* Wait till HSI is ready */ - while (LL_RCC_HSI_IsReady() != 1) ; + /* Wait till HSI is ready */ + while (LL_RCC_HSI_IsReady() != 1) ; - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_2, 40, LL_RCC_PLLR_DIV_2); - LL_RCC_PLL_EnableDomain_SYS(); - LL_RCC_PLL_Enable(); + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLLM_DIV_2, 40, LL_RCC_PLLR_DIV_2); + LL_RCC_PLL_EnableDomain_SYS(); + LL_RCC_PLL_Enable(); - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) ; - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) ; + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); } static inline void bl_gpio_init(void) { - LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); - GPIO_InitStruct.Pin = input_pin; - GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Pin = input_pin; + GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - LL_GPIO_Init(input_port, &GPIO_InitStruct); + LL_GPIO_Init(input_port, &GPIO_InitStruct); } /* @@ -143,12 +143,12 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; + return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; } void Error_Handler() { - while (1) {} + while (1) {} } /* @@ -156,22 +156,22 @@ void Error_Handler() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup vector table - SCB->VTOR = app_address; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup vector table + SCB->VTOR = app_address; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } /* diff --git a/Mcu/g431/Src/eeprom.c b/Mcu/g431/Src/eeprom.c index 7be94da0..a79ec2c5 100644 --- a/Mcu/g431/Src/eeprom.c +++ b/Mcu/g431/Src/eeprom.c @@ -18,67 +18,68 @@ static const uint32_t FLASH_FKEY2 =0xCDEF89AB; bool save_flash_nolib(const uint8_t *data, uint32_t length, uint32_t add) { - if ((add & 0x7) != 0 || (length & 0x7)) { - // address and length must be on 8 byte boundary - return false; - } - // we need to flash on 32 bit boundaries - uint32_t data_length = length / 4; - volatile FLASH_TypeDef *flash = FLASH; - - // clear errors - flash->SR |= FLASH_SR_OPERR | FLASH_SR_PROGERR | FLASH_SR_WRPERR | FLASH_SR_PGAERR | - FLASH_SR_SIZERR | FLASH_SR_PGSERR | FLASH_SR_MISERR | FLASH_SR_FASTERR | - FLASH_SR_RDERR | FLASH_SR_OPTVERR; - - // unlock flash + if ((add & 0x7) != 0 || (length & 0x7)) { + // address and length must be on 8 byte boundary + return false; + } + // we need to flash on 32 bit boundaries + uint32_t data_length = length / 4; + volatile FLASH_TypeDef *flash = FLASH; + + // clear errors + flash->SR |= FLASH_SR_OPERR | FLASH_SR_PROGERR | FLASH_SR_WRPERR | FLASH_SR_PGAERR | + FLASH_SR_SIZERR | FLASH_SR_PGSERR | FLASH_SR_MISERR | FLASH_SR_FASTERR | + FLASH_SR_RDERR | FLASH_SR_OPTVERR; + + // unlock flash + while ((flash->SR & FLASH_SR_BSY) != 0) ; + + if ((flash->CR & FLASH_CR_LOCK) != 0) { + flash->KEYR = FLASH_FKEY1; + flash->KEYR = FLASH_FKEY2; + } + + // erase page if address is divisable by page size + if ((add % page_size) == 0) { + flash->CR = FLASH_CR_PER; + flash->CR |= (add/page_size) << 3; + flash->CR |= FLASH_CR_STRT; while ((flash->SR & FLASH_SR_BSY) != 0) ; + } - if ((flash->CR & FLASH_CR_LOCK) != 0) { - flash->KEYR = FLASH_FKEY1; - flash->KEYR = FLASH_FKEY2; - } + uint32_t index = 0; + volatile uint32_t *fdata = (volatile uint32_t *)add; - // erase page if address is divisable by page size - if ((add % page_size) == 0){ - flash->CR = FLASH_CR_PER; - flash->CR |= (add/page_size) << 3; - flash->CR |= FLASH_CR_STRT; - while ((flash->SR & FLASH_SR_BSY) != 0) ; - } + while (index < data_length) { + // flash two words at a time + uint32_t words[2]; + memcpy((void*)&words[0], &data[index*4], sizeof(words)); - uint32_t index = 0; - volatile uint32_t *fdata = (volatile uint32_t *)add; + flash->CR = FLASH_CR_PG; - while (index < data_length) { - // flash two words at a time - uint32_t words[2]; - memcpy((void*)&words[0], &data[index*4], sizeof(words)); + fdata[index] = words[0]; + fdata[index+1] = words[1]; - flash->CR = FLASH_CR_PG; - - fdata[index] = words[0]; - fdata[index+1] = words[1]; - - while ((flash->SR & FLASH_SR_BSY) != 0) ; + while ((flash->SR & FLASH_SR_BSY) != 0) ; - flash->SR |= FLASH_SR_EOP; - flash->CR = 0; - index += 2; - } + flash->SR |= FLASH_SR_EOP; + flash->CR = 0; + index += 2; + } - // lock flash again - SET_BIT(flash->CR, FLASH_CR_LOCK); + // lock flash again + SET_BIT(flash->CR, FLASH_CR_LOCK); - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } -void read_flash_bin(uint8_t* data , uint32_t add , int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); +void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) +{ + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/g431/Src/stm32g4xx_it.c b/Mcu/g431/Src/stm32g4xx_it.c index 9f9754f4..c9f23e07 100644 --- a/Mcu/g431/Src/stm32g4xx_it.c +++ b/Mcu/g431/Src/stm32g4xx_it.c @@ -4,32 +4,32 @@ void NMI_Handler(void) { - while (1) { - } + while (1) { + } } void HardFault_Handler(void) { - while (1) { - } + while (1) { + } } void MemManage_Handler(void) { - while (1) { - } + while (1) { + } } void BusFault_Handler(void) { - while (1) { - } + while (1) { + } } void UsageFault_Handler(void) { - while (1) { - } + while (1) { + } } void SVC_Handler(void) { } diff --git a/Mcu/l431/Inc/blutil.h b/Mcu/l431/Inc/blutil.h index 06bf455a..e030a46d 100644 --- a/Mcu/l431/Inc/blutil.h +++ b/Mcu/l431/Inc/blutil.h @@ -27,29 +27,29 @@ #ifdef PORT_LETTER static inline void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); - LL_GPIO_SetPinPull(input_port, pin, pull_up_down); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_INPUT); + LL_GPIO_SetPinPull(input_port, pin, pull_up_down); } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); - LL_GPIO_SetPinOutputType(input_port, pin, output_mode); + LL_GPIO_SetPinMode(input_port, pin, LL_GPIO_MODE_OUTPUT); + LL_GPIO_SetPinOutputType(input_port, pin, output_mode); } static inline void gpio_set(uint32_t pin) { - LL_GPIO_SetOutputPin(input_port, pin); + LL_GPIO_SetOutputPin(input_port, pin); } static inline void gpio_clear(uint32_t pin) { - LL_GPIO_ResetOutputPin(input_port, pin); + LL_GPIO_ResetOutputPin(input_port, pin); } static inline bool gpio_read(uint32_t pin) { - return LL_GPIO_IsInputPinSet(input_port, pin); + return LL_GPIO_IsInputPinSet(input_port, pin); } #endif // PORT_LETTER @@ -60,23 +60,23 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - LL_TIM_InitTypeDef TIM_InitStruct = {0}; - - /* Peripheral clock enable */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); - - TIM_InitStruct.Prescaler = 79; - TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; - TIM_InitStruct.Autoreload = 0xFFFFFFFF; - TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; - LL_TIM_Init(BL_TIMER, &TIM_InitStruct); - LL_TIM_DisableARRPreload(BL_TIMER); - LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); - LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); - LL_TIM_DisableMasterSlaveMode(BL_TIMER); - - LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); - LL_TIM_EnableCounter(BL_TIMER); + LL_TIM_InitTypeDef TIM_InitStruct = {0}; + + /* Peripheral clock enable */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2); + + TIM_InitStruct.Prescaler = 79; + TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP; + TIM_InitStruct.Autoreload = 0xFFFFFFFF; + TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1; + LL_TIM_Init(BL_TIMER, &TIM_InitStruct); + LL_TIM_DisableARRPreload(BL_TIMER); + LL_TIM_SetClockSource(BL_TIMER, LL_TIM_CLOCKSOURCE_INTERNAL); + LL_TIM_SetTriggerOutput(BL_TIMER, LL_TIM_TRGO_RESET); + LL_TIM_DisableMasterSlaveMode(BL_TIMER); + + LL_TIM_SetCounterMode(BL_TIMER, LL_TIM_COUNTERMODE_UP); + LL_TIM_EnableCounter(BL_TIMER); } /* @@ -84,12 +84,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - LL_TIM_DeInit(BL_TIMER); + LL_TIM_DeInit(BL_TIMER); } static inline uint16_t bl_timer_us(void) { - return LL_TIM_GetCounter(BL_TIMER); + return LL_TIM_GetCounter(BL_TIMER); } /* @@ -97,46 +97,46 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - LL_FLASH_SetLatency(LL_FLASH_LATENCY_4); - while (LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_4) ; - LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); - - while (LL_PWR_IsActiveFlag_VOS() != 0) ; - LL_RCC_MSI_Enable(); - LL_RCC_LSI_Enable(); - - /* Wait till MSI and LSI are ready */ - while (LL_RCC_LSI_IsReady() != 1) ; - while (LL_RCC_MSI_IsReady() != 1) ; - - LL_RCC_SetRTCClockSource(LL_RCC_RTC_CLKSOURCE_LSI); - LL_RCC_EnableRTC(); - - LL_RCC_MSI_EnableRangeSelection(); - LL_RCC_MSI_SetRange(LL_RCC_MSIRANGE_6); - LL_RCC_MSI_SetCalibTrimming(0); - LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_MSI, LL_RCC_PLLM_DIV_1, 40, LL_RCC_PLLR_DIV_2); - LL_RCC_PLL_EnableDomain_SYS(); - LL_RCC_PLL_Enable(); - - /* Wait till PLL is ready */ - while (LL_RCC_PLL_IsReady() != 1) ; - LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); - - /* Wait till System clock is ready */ - while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; - - LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); - LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); - LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); + LL_FLASH_SetLatency(LL_FLASH_LATENCY_4); + while (LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_4) ; + LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1); + + while (LL_PWR_IsActiveFlag_VOS() != 0) ; + LL_RCC_MSI_Enable(); + LL_RCC_LSI_Enable(); + + /* Wait till MSI and LSI are ready */ + while (LL_RCC_LSI_IsReady() != 1) ; + while (LL_RCC_MSI_IsReady() != 1) ; + + LL_RCC_SetRTCClockSource(LL_RCC_RTC_CLKSOURCE_LSI); + LL_RCC_EnableRTC(); + + LL_RCC_MSI_EnableRangeSelection(); + LL_RCC_MSI_SetRange(LL_RCC_MSIRANGE_6); + LL_RCC_MSI_SetCalibTrimming(0); + LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_MSI, LL_RCC_PLLM_DIV_1, 40, LL_RCC_PLLR_DIV_2); + LL_RCC_PLL_EnableDomain_SYS(); + LL_RCC_PLL_Enable(); + + /* Wait till PLL is ready */ + while (LL_RCC_PLL_IsReady() != 1) ; + LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL); + + /* Wait till System clock is ready */ + while (LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL) ; + + LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1); + LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1); + LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1); } #ifdef PORT_LETTER static inline void bl_gpio_init(void) { - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); - LL_GPIO_ResetOutputPin(input_port, input_pin); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOB); + LL_GPIO_ResetOutputPin(input_port, input_pin); } /* @@ -144,12 +144,12 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; + return (RCC->CSR & RCC_CSR_SFTRSTF) != 0; } void Error_Handler() { - while (1) {} + while (1) {} } #ifdef FIRMWARE_RELATIVE_START @@ -158,22 +158,22 @@ void Error_Handler() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; - const uint32_t *app_data = (const uint32_t *)app_address; - const uint32_t stack_top = app_data[0]; - const uint32_t JumpAddress = app_data[1]; - - // setup vector table - SCB->VTOR = app_address; - - // setup sp, msp and jump - asm volatile( - "mov sp, %0 \n" - "msr msp, %0 \n" - "bx %1 \n" - : : "r"(stack_top), "r"(JumpAddress) :); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = MCU_FLASH_START + FIRMWARE_RELATIVE_START; + const uint32_t *app_data = (const uint32_t *)app_address; + const uint32_t stack_top = app_data[0]; + const uint32_t JumpAddress = app_data[1]; + + // setup vector table + SCB->VTOR = app_address; + + // setup sp, msp and jump + asm volatile( + "mov sp, %0 \n" + "msr msp, %0 \n" + "bx %1 \n" + : : "r"(stack_top), "r"(JumpAddress) :); } #endif // FIRMWARE_RELATIVE_START diff --git a/Mcu/l431/Src/eeprom.c b/Mcu/l431/Src/eeprom.c index a1d7fa22..33a358d0 100644 --- a/Mcu/l431/Src/eeprom.c +++ b/Mcu/l431/Src/eeprom.c @@ -17,67 +17,68 @@ static const uint32_t FLASH_FKEY2 =0xCDEF89AB; bool save_flash_nolib(const uint8_t *data, uint32_t length, uint32_t add) { - if ((add & 0x7) != 0 || (length & 0x7)) { - // address and length must be on 8 byte boundary - return false; - } - // we need to flash on 32 bit boundaries - uint32_t data_length = length / 4; - volatile FLASH_TypeDef *flash = FLASH; - - // clear errors - flash->SR |= FLASH_SR_OPERR | FLASH_SR_PROGERR | FLASH_SR_WRPERR | FLASH_SR_PGAERR | - FLASH_SR_SIZERR | FLASH_SR_PGSERR | FLASH_SR_MISERR | FLASH_SR_FASTERR | - FLASH_SR_RDERR | FLASH_SR_OPTVERR; - - // unlock flash + if ((add & 0x7) != 0 || (length & 0x7)) { + // address and length must be on 8 byte boundary + return false; + } + // we need to flash on 32 bit boundaries + uint32_t data_length = length / 4; + volatile FLASH_TypeDef *flash = FLASH; + + // clear errors + flash->SR |= FLASH_SR_OPERR | FLASH_SR_PROGERR | FLASH_SR_WRPERR | FLASH_SR_PGAERR | + FLASH_SR_SIZERR | FLASH_SR_PGSERR | FLASH_SR_MISERR | FLASH_SR_FASTERR | + FLASH_SR_RDERR | FLASH_SR_OPTVERR; + + // unlock flash + while ((flash->SR & FLASH_SR_BSY) != 0) ; + + if ((flash->CR & FLASH_CR_LOCK) != 0) { + flash->KEYR = FLASH_FKEY1; + flash->KEYR = FLASH_FKEY2; + } + + // erase page if address is divisable by page size + if ((add % page_size) == 0) { + flash->CR = FLASH_CR_PER; + flash->CR |= (add/page_size) << 3; + flash->CR |= FLASH_CR_STRT; while ((flash->SR & FLASH_SR_BSY) != 0) ; + } - if ((flash->CR & FLASH_CR_LOCK) != 0) { - flash->KEYR = FLASH_FKEY1; - flash->KEYR = FLASH_FKEY2; - } + uint32_t index = 0; + volatile uint32_t *fdata = (volatile uint32_t *)add; - // erase page if address is divisable by page size - if ((add % page_size) == 0){ - flash->CR = FLASH_CR_PER; - flash->CR |= (add/page_size) << 3; - flash->CR |= FLASH_CR_STRT; - while ((flash->SR & FLASH_SR_BSY) != 0) ; - } + while (index < data_length) { + // flash two words at a time + uint32_t words[2]; + memcpy((void*)&words[0], &data[index*4], sizeof(words)); - uint32_t index = 0; - volatile uint32_t *fdata = (volatile uint32_t *)add; + flash->CR = FLASH_CR_PG; - while (index < data_length) { - // flash two words at a time - uint32_t words[2]; - memcpy((void*)&words[0], &data[index*4], sizeof(words)); + fdata[index] = words[0]; + fdata[index+1] = words[1]; - flash->CR = FLASH_CR_PG; - - fdata[index] = words[0]; - fdata[index+1] = words[1]; - - while ((flash->SR & FLASH_SR_BSY) != 0) ; + while ((flash->SR & FLASH_SR_BSY) != 0) ; - flash->SR |= FLASH_SR_EOP; - flash->CR = 0; - index += 2; - } + flash->SR |= FLASH_SR_EOP; + flash->CR = 0; + index += 2; + } - // lock flash again - SET_BIT(flash->CR, FLASH_CR_LOCK); + // lock flash again + SET_BIT(flash->CR, FLASH_CR_LOCK); - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } -void read_flash_bin(uint8_t* data , uint32_t add , int out_buff_len) { - memcpy(data, (void*)add, out_buff_len); +void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) +{ + memcpy(data, (void*)add, out_buff_len); } diff --git a/Mcu/v203/Inc/blutil.h b/Mcu/v203/Inc/blutil.h index 1ed772e7..49f44eda 100644 --- a/Mcu/v203/Inc/blutil.h +++ b/Mcu/v203/Inc/blutil.h @@ -33,57 +33,57 @@ static NOINLINE void gpio_mode_set_input(uint32_t pin, uint32_t pull_up_down) { - volatile uint32_t *cfgr; - if (pin >= GPIO_PIN(8)) { - pin >>= 8; - cfgr = &input_port->CFGHR; - } else { - cfgr = &input_port->CFGLR; - } - const uint32_t mul = pin*pin*pin*pin; - const uint32_t CFG = (*cfgr) & ~(0xf * mul); - switch (pull_up_down) { - case GPIO_PULL_NONE: - *cfgr = CFG | (0x4*mul); - break; - case GPIO_PULL_UP: - input_port->OUTDR |= pin; - *cfgr = CFG | (0x8*mul); - break; - case GPIO_PULL_DOWN: - input_port->OUTDR &= ~pin; - *cfgr = CFG | (0x8*mul); - break; - } + volatile uint32_t *cfgr; + if (pin >= GPIO_PIN(8)) { + pin >>= 8; + cfgr = &input_port->CFGHR; + } else { + cfgr = &input_port->CFGLR; + } + const uint32_t mul = pin*pin*pin*pin; + const uint32_t CFG = (*cfgr) & ~(0xf * mul); + switch (pull_up_down) { + case GPIO_PULL_NONE: + *cfgr = CFG | (0x4*mul); + break; + case GPIO_PULL_UP: + input_port->OUTDR |= pin; + *cfgr = CFG | (0x8*mul); + break; + case GPIO_PULL_DOWN: + input_port->OUTDR &= ~pin; + *cfgr = CFG | (0x8*mul); + break; + } } static inline void gpio_mode_set_output(uint32_t pin, uint32_t output_mode) { - volatile uint32_t *cfgr; - if (pin >= GPIO_PIN(8)) { - pin >>= 8; - cfgr = &input_port->CFGHR; - } else { - cfgr = &input_port->CFGLR; - } - const uint32_t mul = pin*pin*pin*pin; - const uint32_t CFG = (*cfgr) & ~(0xf * mul); - (*cfgr) = CFG | (output_mode*mul); + volatile uint32_t *cfgr; + if (pin >= GPIO_PIN(8)) { + pin >>= 8; + cfgr = &input_port->CFGHR; + } else { + cfgr = &input_port->CFGLR; + } + const uint32_t mul = pin*pin*pin*pin; + const uint32_t CFG = (*cfgr) & ~(0xf * mul); + (*cfgr) = CFG | (output_mode*mul); } static inline void gpio_set(uint32_t pin) { - input_port->BSHR = pin; + input_port->BSHR = pin; } static inline void gpio_clear(uint32_t pin) { - input_port->BCR = pin; + input_port->BCR = pin; } static inline bool gpio_read(uint32_t pin) { - return (input_port->INDR & pin) != 0; + return (input_port->INDR & pin) != 0; } #define BL_TIMER TIM1 @@ -93,22 +93,22 @@ static inline bool gpio_read(uint32_t pin) */ static inline void bl_timer_init(void) { - RCC->APB2PCENR |= RCC_APB2Periph_TIM1; - - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Prescaler = 47; - TIM_TimeBaseStructure.TIM_Period = 0xFFFF; - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(BL_TIMER, &TIM_TimeBaseStructure); - TIM_SetCounter(BL_TIMER, 0); - - // enable preload - BL_TIMER->CTLR1 |= TIM_ARPE; - - // enable timer - BL_TIMER->CTLR1 |= TIM_CEN; + RCC->APB2PCENR |= RCC_APB2Periph_TIM1; + + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Prescaler = 47; + TIM_TimeBaseStructure.TIM_Period = 0xFFFF; + TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(BL_TIMER, &TIM_TimeBaseStructure); + TIM_SetCounter(BL_TIMER, 0); + + // enable preload + BL_TIMER->CTLR1 |= TIM_ARPE; + + // enable timer + BL_TIMER->CTLR1 |= TIM_CEN; } /* @@ -116,12 +116,12 @@ static inline void bl_timer_init(void) */ static inline void bl_timer_disable(void) { - RCC->APB2PCENR &= ~RCC_APB2Periph_TIM1; + RCC->APB2PCENR &= ~RCC_APB2Periph_TIM1; } static inline uint16_t bl_timer_us(void) { - return BL_TIMER->CNT; + return BL_TIMER->CNT; } /* @@ -129,20 +129,20 @@ static inline uint16_t bl_timer_us(void) */ static inline void bl_clock_config(void) { - SystemInit(); + SystemInit(); } static inline void bl_gpio_init(void) { - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE ); - RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE ); - - GPIO_InitTypeDef GPIO_InitStruct = {0}; - GPIO_StructInit(&GPIO_InitStruct); - GPIO_InitStruct.GPIO_Pin = input_pin; - GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING; - GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; - GPIO_Init(input_port, &GPIO_InitStruct); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE ); + RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE ); + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + GPIO_StructInit(&GPIO_InitStruct); + GPIO_InitStruct.GPIO_Pin = input_pin; + GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN_FLOATING; + GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_Init(input_port, &GPIO_InitStruct); } /* @@ -150,12 +150,12 @@ static inline void bl_gpio_init(void) */ static inline bool bl_was_software_reset(void) { - return (RCC->RSTSCKR & RCC_SFTRSTF) != 0; + return (RCC->RSTSCKR & RCC_SFTRSTF) != 0; } void Error_Handler() { - while (1) {} + while (1) {} } /* @@ -163,17 +163,17 @@ void Error_Handler() */ static inline void jump_to_application(void) { - __disable_irq(); - bl_timer_disable(); - const uint32_t app_address = FIRMWARE_RELATIVE_START; - const uint32_t stack_top = RAM_BASE + RAM_SIZE; - - // set the stack pointer and jump to the application - asm volatile( - "mv sp, %0 \n" - "jr %1 \n" - : // No output operands - : "r"(stack_top), "r"(app_address) - : // No clobbered registers - ); + __disable_irq(); + bl_timer_disable(); + const uint32_t app_address = FIRMWARE_RELATIVE_START; + const uint32_t stack_top = RAM_BASE + RAM_SIZE; + + // set the stack pointer and jump to the application + asm volatile( + "mv sp, %0 \n" + "jr %1 \n" + : // No output operands + : "r"(stack_top), "r"(app_address) + : // No clobbered registers + ); } diff --git a/Mcu/v203/Inc/ch32v20x_conf.h b/Mcu/v203/Inc/ch32v20x_conf.h index d1eeb38a..5adefccd 100644 --- a/Mcu/v203/Inc/ch32v20x_conf.h +++ b/Mcu/v203/Inc/ch32v20x_conf.h @@ -6,7 +6,7 @@ * Description : Library configuration file. ********************************************************************************* * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd. -* Attention: This software (modified or not) and binary are used for +* Attention: This software (modified or not) and binary are used for * microcontroller manufactured by Nanjing Qinheng Microelectronics. *******************************************************************************/ #ifndef __CH32V20x_CONF_H @@ -37,6 +37,6 @@ #endif /* __CH32V20x_CONF_H */ - - - + + + diff --git a/Mcu/v203/Inc/ch32v20x_it.h b/Mcu/v203/Inc/ch32v20x_it.h index f68b2034..def8f948 100644 --- a/Mcu/v203/Inc/ch32v20x_it.h +++ b/Mcu/v203/Inc/ch32v20x_it.h @@ -6,7 +6,7 @@ * Description : This file contains the headers of the interrupt handlers. ********************************************************************************* * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd. -* Attention: This software (modified or not) and binary are used for +* Attention: This software (modified or not) and binary are used for * microcontroller manufactured by Nanjing Qinheng Microelectronics. *******************************************************************************/ #ifndef __CH32V20x_IT_H diff --git a/Mcu/v203/Inc/system_ch32v20x.h b/Mcu/v203/Inc/system_ch32v20x.h index 9c8a3f71..65d2e2c7 100644 --- a/Mcu/v203/Inc/system_ch32v20x.h +++ b/Mcu/v203/Inc/system_ch32v20x.h @@ -6,19 +6,19 @@ * Description : CH32V20x Device Peripheral Access Layer System Header File. ********************************************************************************* * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd. -* Attention: This software (modified or not) and binary are used for +* Attention: This software (modified or not) and binary are used for * microcontroller manufactured by Nanjing Qinheng Microelectronics. *******************************************************************************/ -#ifndef __SYSTEM_ch32v20x_H +#ifndef __SYSTEM_ch32v20x_H #define __SYSTEM_ch32v20x_H #ifdef __cplusplus - extern "C" { -#endif +extern "C" { +#endif extern uint32_t SystemCoreClock; /* System Clock Frequency (Core Clock) */ -/* System_Exported_Functions */ +/* System_Exported_Functions */ extern void SystemInit(void); extern void SystemCoreClockUpdate(void); diff --git a/Mcu/v203/Src/ch32v20x_it.c b/Mcu/v203/Src/ch32v20x_it.c index 524edc7d..9f33e7f4 100644 --- a/Mcu/v203/Src/ch32v20x_it.c +++ b/Mcu/v203/Src/ch32v20x_it.c @@ -6,7 +6,7 @@ * Description : Main Interrupt Service Routines. ********************************************************************************* * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd. -* Attention: This software (modified or not) and binary are used for +* Attention: This software (modified or not) and binary are used for * microcontroller manufactured by Nanjing Qinheng Microelectronics. *******************************************************************************/ #include "ch32v20x_it.h" @@ -16,15 +16,13 @@ void HardFault_Handler(void) __attribute__((interrupt)); void NMI_Handler(void) { - while (1) - { + while (1) { } } void HardFault_Handler(void) { - while (1) - { + while (1) { } } @@ -32,5 +30,5 @@ void HardFault_Handler(void) //for tenkhz void SysTick_Handler(void) { - SysTick->SR = 0; + SysTick->SR = 0; } diff --git a/Mcu/v203/Src/eeprom.c b/Mcu/v203/Src/eeprom.c index 9d2c6da8..2c0e62fc 100644 --- a/Mcu/v203/Src/eeprom.c +++ b/Mcu/v203/Src/eeprom.c @@ -12,46 +12,46 @@ static void wait_not_busy(void) { - while ((FLASH->STATR & FLASH_STATR_BSY) != 0) {} + while ((FLASH->STATR & FLASH_STATR_BSY) != 0) {} } bool save_flash_nolib(const uint8_t* data, uint32_t length, uint32_t add) { - if ((add & 0x03) != 0 || (length & 0x03) != 0 || (length+(add&0xFF)) > page_size) { - return false; - } + if ((add & 0x03) != 0 || (length & 0x03) != 0 || (length+(add&0xFF)) > page_size) { + return false; + } - wait_not_busy(); + wait_not_busy(); - FLASH_Unlock_Fast(); + FLASH_Unlock_Fast(); - uint32_t flash_buffer[page_size/4]; - const uint8_t page_offset = add & 0xFFU; - const uint32_t page_base = add & ~0xFFU; + uint32_t flash_buffer[page_size/4]; + const uint8_t page_offset = add & 0xFFU; + const uint32_t page_base = add & ~0xFFU; - // get existing data - memcpy(flash_buffer, (void*)page_base, page_size); + // get existing data + memcpy(flash_buffer, (void*)page_base, page_size); - // overwrite with new data - memcpy(&flash_buffer[page_offset/4], data, length); + // overwrite with new data + memcpy(&flash_buffer[page_offset/4], data, length); - // fast erase is 256 bytes at a time, normal Flash_Erasepage - // is 4k at a time - FLASH_ErasePage_Fast(page_base); + // fast erase is 256 bytes at a time, normal Flash_Erasepage + // is 4k at a time + FLASH_ErasePage_Fast(page_base); - wait_not_busy(); + wait_not_busy(); - FLASH_ProgramPage_Fast(page_base, flash_buffer); + FLASH_ProgramPage_Fast(page_base, flash_buffer); - wait_not_busy(); + wait_not_busy(); - FLASH_Lock_Fast(); + FLASH_Lock_Fast(); - // ensure data is correct - return memcmp(data, (const void *)add, length) == 0; + // ensure data is correct + return memcmp(data, (const void *)add, length) == 0; } void read_flash_bin(uint8_t* data, uint32_t add, int out_buff_len) { - memcpy(data, (const uint8_t *)add, out_buff_len); + memcpy(data, (const uint8_t *)add, out_buff_len); } diff --git a/Mcu/v203/Src/system_ch32v20x.c b/Mcu/v203/Src/system_ch32v20x.c index 5d736a1c..25f28b4d 100644 --- a/Mcu/v203/Src/system_ch32v20x.c +++ b/Mcu/v203/Src/system_ch32v20x.c @@ -8,15 +8,15 @@ * For HSE = 8Mhz (other CH32V203x) ********************************************************************************* * Copyright (c) 2021 Nanjing Qinheng Microelectronics Co., Ltd. -* Attention: This software (modified or not) and binary are used for +* Attention: This software (modified or not) and binary are used for * microcontroller manufactured by Nanjing Qinheng Microelectronics. *******************************************************************************/ -#include "ch32v20x.h" +#include "ch32v20x.h" -/* -* Uncomment the line corresponding to the desired System clock (SYSCLK) frequency (after +/* +* Uncomment the line corresponding to the desired System clock (SYSCLK) frequency (after * reset the HSI is used as SYSCLK source). -* If none of the define below is enabled, the HSI is used as System clock source. +* If none of the define below is enabled, the HSI is used as System clock source. */ //#define SYSCLK_FREQ_HSE HSE_VALUE //#define SYSCLK_FREQ_48MHz_HSE 48000000 @@ -115,7 +115,7 @@ void SystemInit (void) RCC->CTLR &= (uint32_t)0xFEF6FFFF; RCC->CTLR &= (uint32_t)0xFFFBFFFF; RCC->CFGR0 &= (uint32_t)0xFF00FFFF; - RCC->INTR = 0x009F0000; + RCC->INTR = 0x009F0000; SetSysClock(); } @@ -133,63 +133,57 @@ void SystemCoreClockUpdate (void) tmp = RCC->CFGR0 & RCC_SWS; - switch (tmp) - { - case 0x00: - SystemCoreClock = HSI_VALUE; - break; - case 0x04: - SystemCoreClock = HSE_VALUE; - break; - case 0x08: - pllmull = RCC->CFGR0 & RCC_PLLMULL; - pllsource = RCC->CFGR0 & RCC_PLLSRC; - pllmull = ( pllmull >> 18) + 2; - - if(pllmull == 17) pllmull = 18; - - if (pllsource == 0x00) - { - if(EXTEN->EXTEN_CTR & EXTEN_PLL_HSI_PRE){ - SystemCoreClock = HSI_VALUE * pllmull; - } - else{ - SystemCoreClock = (HSI_VALUE >> 1) * pllmull; - } + switch (tmp) { + case 0x00: + SystemCoreClock = HSI_VALUE; + break; + case 0x04: + SystemCoreClock = HSE_VALUE; + break; + case 0x08: + pllmull = RCC->CFGR0 & RCC_PLLMULL; + pllsource = RCC->CFGR0 & RCC_PLLSRC; + pllmull = ( pllmull >> 18) + 2; + + if (pllmull == 17) { + pllmull = 18; + } + + if (pllsource == 0x00) { + if (EXTEN->EXTEN_CTR & EXTEN_PLL_HSI_PRE) { + SystemCoreClock = HSI_VALUE * pllmull; + } else { + SystemCoreClock = (HSI_VALUE >> 1) * pllmull; } - else - { + } else { #if defined (CH32V20x_D8W) - if((RCC->CFGR0 & (3<<22)) == (3<<22)) - { - SystemCoreClock = ((HSE_VALUE>>1)) * pllmull; - } - else + if ((RCC->CFGR0 & (3<<22)) == (3<<22)) { + SystemCoreClock = ((HSE_VALUE>>1)) * pllmull; + } else #endif - if ((RCC->CFGR0 & RCC_PLLXTPRE) != (uint32_t)RESET) - { + if ((RCC->CFGR0 & RCC_PLLXTPRE) != (uint32_t)RESET) { #if defined (CH32V20x_D8) || defined (CH32V20x_D8W) SystemCoreClock = ((HSE_VALUE>>2) >> 1) * pllmull; #else SystemCoreClock = (HSE_VALUE >> 1) * pllmull; #endif - } - else - { + } else { #if defined (CH32V20x_D8) || defined (CH32V20x_D8W) - SystemCoreClock = (HSE_VALUE>>2) * pllmull; + SystemCoreClock = (HSE_VALUE>>2) * pllmull; #else SystemCoreClock = HSE_VALUE * pllmull; #endif } - } + } - if(Pll_6_5 == 1) SystemCoreClock = (SystemCoreClock / 2); + if (Pll_6_5 == 1) { + SystemCoreClock = (SystemCoreClock / 2); + } - break; - default: - SystemCoreClock = HSI_VALUE; - break; + break; + default: + SystemCoreClock = HSI_VALUE; + break; } tmp = AHBPrescTable[((RCC->CFGR0 & RCC_HPRE) >> 4)]; @@ -207,37 +201,37 @@ static void SetSysClock(void) { //GPIO_IPD_Unused(); #ifdef SYSCLK_FREQ_HSE - SetSysClockToHSE(); + SetSysClockToHSE(); #elif defined SYSCLK_FREQ_48MHz_HSE - SetSysClockTo48_HSE(); + SetSysClockTo48_HSE(); #elif defined SYSCLK_FREQ_56MHz_HSE - SetSysClockTo56_HSE(); + SetSysClockTo56_HSE(); #elif defined SYSCLK_FREQ_72MHz_HSE - SetSysClockTo72_HSE(); + SetSysClockTo72_HSE(); #elif defined SYSCLK_FREQ_96MHz_HSE - SetSysClockTo96_HSE(); + SetSysClockTo96_HSE(); #elif defined SYSCLK_FREQ_120MHz_HSE - SetSysClockTo120_HSE(); + SetSysClockTo120_HSE(); #elif defined SYSCLK_FREQ_144MHz_HSE - SetSysClockTo144_HSE(); + SetSysClockTo144_HSE(); #elif defined SYSCLK_FREQ_48MHz_HSI - SetSysClockTo48_HSI(); + SetSysClockTo48_HSI(); #elif defined SYSCLK_FREQ_56MHz_HSI - SetSysClockTo56_HSI(); + SetSysClockTo56_HSI(); #elif defined SYSCLK_FREQ_72MHz_HSI - SetSysClockTo72_HSI(); + SetSysClockTo72_HSI(); #elif defined SYSCLK_FREQ_96MHz_HSI - SetSysClockTo96_HSI(); + SetSysClockTo96_HSI(); #elif defined SYSCLK_FREQ_120MHz_HSI - SetSysClockTo120_HSI(); + SetSysClockTo120_HSI(); #elif defined SYSCLK_FREQ_144MHz_HSI - SetSysClockTo144_HSI(); + SetSysClockTo144_HSI(); #endif - - /* If none of the define above is enabled, the HSI is used as System clock - * source (default after reset) - */ + + /* If none of the define above is enabled, the HSI is used as System clock + * source (default after reset) + */ } @@ -253,54 +247,46 @@ static void SetSysClock(void) static void SetSysClockToHSE(void) { __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - - + + RCC->CTLR |= ((uint32_t)RCC_HSEON); - + /* Wait till HSE is ready and if Time out is reached exit */ - do - { + do { HSEStatus = RCC->CTLR & RCC_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + StartUpCounter++; + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if ((RCC->CTLR & RCC_HSERDY) != RESET) - { + if ((RCC->CTLR & RCC_HSERDY) != RESET) { HSEStatus = (uint32_t)0x01; - } - else - { + } else { HSEStatus = (uint32_t)0x00; - } + } - if (HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; /* PCLK2 = HCLK */ RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; /* PCLK1 = HCLK */ RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV1; - + /* Select HSE as system clock source * CH32V20x_D6 (HSE=8MHZ) * CH32V20x_D8 (HSE=32MHZ) * CH32V20x_D8W (HSE=32MHZ) */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_HSE; + RCC->CFGR0 |= (uint32_t)RCC_SW_HSE; /* Wait till HSE is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x04) - { + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x04) { } + } else { + /* If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ } - else - { - /* If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ - } } #elif defined SYSCLK_FREQ_48MHz_HSE @@ -315,31 +301,26 @@ static void SetSysClockToHSE(void) static void SetSysClockTo48_HSE(void) { __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - - + + RCC->CTLR |= ((uint32_t)RCC_HSEON); /* Wait till HSE is ready and if Time out is reached exit */ - do - { + do { HSEStatus = RCC->CTLR & RCC_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + StartUpCounter++; + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if ((RCC->CTLR & RCC_HSERDY) != RESET) - { + if ((RCC->CTLR & RCC_HSERDY) != RESET) { HSEStatus = (uint32_t)0x01; - } - else - { + } else { HSEStatus = (uint32_t)0x00; - } + } - if (HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; /* PCLK1 = HCLK */ RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; @@ -349,29 +330,25 @@ static void SetSysClockTo48_HSE(void) */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL6); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL6); /* Enable PLL */ RCC->CTLR |= RCC_PLLON; /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { + while ((RCC->CTLR & RCC_PLLRDY) == 0) { } /* Select PLL as system clock source */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { } + } else { + /* + * If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ } - else - { - /* - * If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ - } } #elif defined SYSCLK_FREQ_56MHz_HSE @@ -386,34 +363,29 @@ static void SetSysClockTo48_HSE(void) static void SetSysClockTo56_HSE(void) { __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - + RCC->CTLR |= ((uint32_t)RCC_HSEON); /* Wait till HSE is ready and if Time out is reached exit */ - do - { + do { HSEStatus = RCC->CTLR & RCC_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + StartUpCounter++; + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if ((RCC->CTLR & RCC_HSERDY) != RESET) - { + if ((RCC->CTLR & RCC_HSERDY) != RESET) { HSEStatus = (uint32_t)0x01; - } - else - { + } else { HSEStatus = (uint32_t)0x00; - } + } - if (HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; /* PCLK2 = HCLK */ RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; /* PCLK1 = HCLK */ RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - + /* CH32V20x_D6-PLL configuration: PLLCLK = HSE * 7 = 56 MHz (HSE=8MHZ) * CH32V20x_D8-PLL configuration: PLLCLK = HSE/4 * 7 = 56 MHz (HSE=32MHZ) * CH32V20x_D8W-PLL configuration: PLLCLK = HSE/4 * 7 = 56 MHz (HSE=32MHZ) @@ -425,25 +397,21 @@ static void SetSysClockTo56_HSE(void) /* Enable PLL */ RCC->CTLR |= RCC_PLLON; /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { + while ((RCC->CTLR & RCC_PLLRDY) == 0) { } /* Select PLL as system clock source */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { } + } else { + /* + * If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ } - else - { - /* - * If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ - } } #elif defined SYSCLK_FREQ_72MHz_HSE @@ -458,63 +426,54 @@ static void SetSysClockTo56_HSE(void) static void SetSysClockTo72_HSE(void) { __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - + RCC->CTLR |= ((uint32_t)RCC_HSEON); - + /* Wait till HSE is ready and if Time out is reached exit */ - do - { + do { HSEStatus = RCC->CTLR & RCC_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + StartUpCounter++; + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if ((RCC->CTLR & RCC_HSERDY) != RESET) - { + if ((RCC->CTLR & RCC_HSERDY) != RESET) { HSEStatus = (uint32_t)0x01; - } - else - { + } else { HSEStatus = (uint32_t)0x00; - } + } - if (HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; /* PCLK1 = HCLK */ RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - + /* CH32V20x_D6-PLL configuration: PLLCLK = HSE * 9 = 72 MHz (HSE=8MHZ) * CH32V20x_D8-PLL configuration: PLLCLK = HSE/4 * 9 = 72 MHz (HSE=32MHZ) * CH32V20x_D8W-PLL configuration: PLLCLK = HSE/4 * 9 = 72 MHz (HSE=32MHZ) */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | - RCC_PLLMULL)); + RCC_PLLMULL)); RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL9); /* Enable PLL */ RCC->CTLR |= RCC_PLLON; /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } /* Select PLL as system clock source */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { } - } - else - { - /* - * If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ + } else { + /* + * If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ } } @@ -535,23 +494,18 @@ static void SetSysClockTo96_HSE(void) RCC->CTLR |= ((uint32_t)RCC_HSEON); /* Wait till HSE is ready and if Time out is reached exit */ - do - { + do { HSEStatus = RCC->CTLR & RCC_HSERDY; StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if ((RCC->CTLR & RCC_HSERDY) != RESET) - { + if ((RCC->CTLR & RCC_HSERDY) != RESET) { HSEStatus = (uint32_t)0x01; - } - else - { + } else { HSEStatus = (uint32_t)0x00; } - if (HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { /* HCLK = SYSCLK */ RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; /* PCLK2 = HCLK */ @@ -564,30 +518,26 @@ static void SetSysClockTo96_HSE(void) * CH32V20x_D8W-PLL configuration: PLLCLK = HSE/4 * 12 = 96 MHz (HSE=32MHZ) */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | - RCC_PLLMULL)); + RCC_PLLMULL)); RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL12); /* Enable PLL */ RCC->CTLR |= RCC_PLLON; /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { + while ((RCC->CTLR & RCC_PLLRDY) == 0) { } /* Select PLL as system clock source */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { } - } - else - { - /* - * If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ + } else { + /* + * If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ } } @@ -603,71 +553,62 @@ static void SetSysClockTo96_HSE(void) */ static void SetSysClockTo120_HSE(void) { - __IO uint32_t StartUpCounter = 0, HSEStatus = 0; + __IO uint32_t StartUpCounter = 0, HSEStatus = 0; - RCC->CTLR |= ((uint32_t)RCC_HSEON); + RCC->CTLR |= ((uint32_t)RCC_HSEON); - /* Wait till HSE is ready and if Time out is reached exit */ - do - { - HSEStatus = RCC->CTLR & RCC_HSERDY; - StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + /* Wait till HSE is ready and if Time out is reached exit */ + do { + HSEStatus = RCC->CTLR & RCC_HSERDY; + StartUpCounter++; + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if((RCC->CTLR & RCC_HSERDY) != RESET) - { - HSEStatus = (uint32_t)0x01; - } - else - { - HSEStatus = (uint32_t)0x00; - } + if ((RCC->CTLR & RCC_HSERDY) != RESET) { + HSEStatus = (uint32_t)0x01; + } else { + HSEStatus = (uint32_t)0x00; + } - if(HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { #if defined (CH32V20x_D8W) - RCC->CFGR0 |= (uint32_t)(3<<22); - /* HCLK = SYSCLK/2 */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV2; + RCC->CFGR0 |= (uint32_t)(3<<22); + /* HCLK = SYSCLK/2 */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV2; #else - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; #endif - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - - /* CH32V20x_D6-PLL configuration: PLLCLK = HSE * 15 = 120 MHz (HSE=8MHZ) - * CH32V20x_D8-PLL configuration: PLLCLK = HSE/4 * 15 = 120 MHz (HSE=32MHZ) - * CH32V20x_D8W-PLL configuration: PLLCLK = HSE/2 * 15 = 240 MHz (HSE=32MHZ) - */ - RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_PLLSRC | RCC_PLLXTPRE | - RCC_PLLMULL)); - - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL15); - - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; + + /* CH32V20x_D6-PLL configuration: PLLCLK = HSE * 15 = 120 MHz (HSE=8MHZ) + * CH32V20x_D8-PLL configuration: PLLCLK = HSE/4 * 15 = 120 MHz (HSE=32MHZ) + * CH32V20x_D8W-PLL configuration: PLLCLK = HSE/2 * 15 = 240 MHz (HSE=32MHZ) + */ + RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_PLLSRC | RCC_PLLXTPRE | + RCC_PLLMULL)); + + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL15); + + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { } - else - { - /* - * If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { } + } else { + /* + * If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ + } } #elif defined SYSCLK_FREQ_144MHz_HSE @@ -685,23 +626,18 @@ static void SetSysClockTo144_HSE(void) RCC->CTLR |= ((uint32_t)RCC_HSEON); /* Wait till HSE is ready and if Time out is reached exit */ - do - { + do { HSEStatus = RCC->CTLR & RCC_HSERDY; StartUpCounter++; - } while((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); + } while ((HSEStatus == 0) && (StartUpCounter != HSE_STARTUP_TIMEOUT)); - if ((RCC->CTLR & RCC_HSERDY) != RESET) - { + if ((RCC->CTLR & RCC_HSERDY) != RESET) { HSEStatus = (uint32_t)0x01; - } - else - { + } else { HSEStatus = (uint32_t)0x00; } - if (HSEStatus == (uint32_t)0x01) - { + if (HSEStatus == (uint32_t)0x01) { /* HCLK = SYSCLK */ RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; /* PCLK2 = HCLK */ @@ -714,30 +650,26 @@ static void SetSysClockTo144_HSE(void) * CH32V20x_D8W-PLL configuration: PLLCLK = HSE/4 * 18 = 144 MHz (HSE=32MHZ) */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | - RCC_PLLMULL)); + RCC_PLLMULL)); RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSE | RCC_PLLXTPRE_HSE | RCC_PLLMULL18); /* Enable PLL */ RCC->CTLR |= RCC_PLLON; /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { + while ((RCC->CTLR & RCC_PLLRDY) == 0) { } /* Select PLL as system clock source */ RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { } - } - else - { - /* - * If HSE fails to start-up, the application will have wrong clock - * configuration. User can add here some code to deal with this error - */ + } else { + /* + * If HSE fails to start-up, the application will have wrong clock + * configuration. User can add here some code to deal with this error + */ } } @@ -752,33 +684,31 @@ static void SetSysClockTo144_HSE(void) */ static void SetSysClockTo48_HSI(void) { - EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; + EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - /* PLL configuration: PLLCLK = HSI * 6 = 48 MHz */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); + /* PLL configuration: PLLCLK = HSI * 6 = 48 MHz */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL6); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL6); - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { + } } #elif defined SYSCLK_FREQ_56MHz_HSI @@ -792,33 +722,31 @@ static void SetSysClockTo48_HSI(void) */ static void SetSysClockTo56_HSI(void) { - EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; + EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - /* PLL configuration: PLLCLK = HSI * 7 = 48 MHz */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); + /* PLL configuration: PLLCLK = HSI * 7 = 48 MHz */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL7); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL7); - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { + } } #elif defined SYSCLK_FREQ_72MHz_HSI @@ -832,33 +760,31 @@ static void SetSysClockTo56_HSI(void) */ static void SetSysClockTo72_HSI(void) { - EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; + EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - /* PLL configuration: PLLCLK = HSI * 9 = 72 MHz */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); + /* PLL configuration: PLLCLK = HSI * 9 = 72 MHz */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL9); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL9); - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { + } } @@ -873,33 +799,31 @@ static void SetSysClockTo72_HSI(void) */ static void SetSysClockTo96_HSI(void) { - EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; + EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV4; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV4; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV4; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV4; - /* PLL configuration: PLLCLK = HSI * 12 = 96 MHz */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); + /* PLL configuration: PLLCLK = HSI * 12 = 96 MHz */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL12); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL12); - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { + } } @@ -914,34 +838,32 @@ static void SetSysClockTo96_HSI(void) */ static void SetSysClockTo120_HSI(void) { - EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; + EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - /* PLL configuration: PLLCLK = HSI * 15 = 120 MHz */ - RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_PLLSRC | RCC_PLLXTPRE | - RCC_PLLMULL)); + /* PLL configuration: PLLCLK = HSI * 15 = 120 MHz */ + RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_PLLSRC | RCC_PLLXTPRE | + RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL15); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL15); - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t) ~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { + } } #elif defined SYSCLK_FREQ_144MHz_HSI @@ -954,33 +876,31 @@ static void SetSysClockTo120_HSI(void) */ static void SetSysClockTo144_HSI(void) { - EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; + EXTEN->EXTEN_CTR |= EXTEN_PLL_HSI_PRE; - /* HCLK = SYSCLK */ - RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; - /* PCLK2 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; - /* PCLK1 = HCLK */ - RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; + /* HCLK = SYSCLK */ + RCC->CFGR0 |= (uint32_t)RCC_HPRE_DIV1; + /* PCLK2 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE2_DIV1; + /* PCLK1 = HCLK */ + RCC->CFGR0 |= (uint32_t)RCC_PPRE1_DIV2; - /* PLL configuration: PLLCLK = HSI * 18 = 144 MHz */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); + /* PLL configuration: PLLCLK = HSI * 18 = 144 MHz */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_PLLSRC | RCC_PLLXTPRE | RCC_PLLMULL)); - RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL18); + RCC->CFGR0 |= (uint32_t)(RCC_PLLSRC_HSI_Div2 | RCC_PLLMULL18); - /* Enable PLL */ - RCC->CTLR |= RCC_PLLON; - /* Wait till PLL is ready */ - while((RCC->CTLR & RCC_PLLRDY) == 0) - { - } - /* Select PLL as system clock source */ - RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); - RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; - /* Wait till PLL is used as system clock source */ - while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) - { - } + /* Enable PLL */ + RCC->CTLR |= RCC_PLLON; + /* Wait till PLL is ready */ + while ((RCC->CTLR & RCC_PLLRDY) == 0) { + } + /* Select PLL as system clock source */ + RCC->CFGR0 &= (uint32_t)((uint32_t)~(RCC_SW)); + RCC->CFGR0 |= (uint32_t)RCC_SW_PLL; + /* Wait till PLL is used as system clock source */ + while ((RCC->CFGR0 & (uint32_t)RCC_SWS) != (uint32_t)0x08) { + } } diff --git a/bl_update/main.c b/bl_update/main.c index c4e398bd..5c10cb0e 100644 --- a/bl_update/main.c +++ b/bl_update/main.c @@ -37,20 +37,20 @@ static uint32_t input_pin; application signature, filled in by set_app_signature.py */ const struct { - uint32_t magic1; - uint32_t magic2; - uint32_t fwlen; // total fw length in bytes - uint32_t crc1; // crc32 up to start of app_signature - uint32_t crc2; // crc32 from end of app_signature to end of fw - char mcu[16]; - uint32_t unused[2]; + uint32_t magic1; + uint32_t magic2; + uint32_t fwlen; // total fw length in bytes + uint32_t crc1; // crc32 up to start of app_signature + uint32_t crc2; // crc32 from end of app_signature to end of fw + char mcu[16]; + uint32_t unused[2]; } app_signature __attribute__((section(".app_signature"))) = { - .magic1 = APP_SIGNATURE_MAGIC1, - .magic2 = APP_SIGNATURE_MAGIC2, - .fwlen = 0, - .crc1 = 0, - .crc2 = 0, - .mcu = AM32_MCU, + .magic1 = APP_SIGNATURE_MAGIC1, + .magic2 = APP_SIGNATURE_MAGIC2, + .fwlen = 0, + .crc1 = 0, + .crc2 = 0, + .mcu = AM32_MCU, }; #endif @@ -68,51 +68,51 @@ const struct { static void delayMicroseconds(uint32_t micros) { - while (micros > 0) { - uint16_t us = micros>10000?10000:micros; - const uint16_t us_start = bl_timer_us(); - while ((uint16_t)(bl_timer_us() - us_start) < us) ; - micros -= us; - } + while (micros > 0) { + uint16_t us = micros>10000?10000:micros; + const uint16_t us_start = bl_timer_us(); + while ((uint16_t)(bl_timer_us() - us_start) < us) ; + micros -= us; + } } static void flash_bootloader(void) { - uint32_t length = sizeof(bl_image); - uint32_t address = MCU_FLASH_START; - const uint8_t *bl = &bl_image[0]; - - while (length > 0) { - uint32_t chunk = 256; - if (chunk > length) { - chunk = length; - } - // loop until the flash succeeds. We expect it to pass - // first time, so this is paranoia - while (!save_flash_nolib(bl, chunk, address)) { - } - length -= chunk; - address += chunk; - bl += chunk; + uint32_t length = sizeof(bl_image); + uint32_t address = MCU_FLASH_START; + const uint8_t *bl = &bl_image[0]; + + while (length > 0) { + uint32_t chunk = 256; + if (chunk > length) { + chunk = length; + } + // loop until the flash succeeds. We expect it to pass + // first time, so this is paranoia + while (!save_flash_nolib(bl, chunk, address)) { } + length -= chunk; + address += chunk; + bl += chunk; + } } int main(void) { - bl_clock_config(); - bl_timer_init(); + bl_clock_config(); + bl_timer_init(); - // don't risk erasing the bootloader if it already matches - if (memcmp((const void*)MCU_FLASH_START, bl_image, sizeof(bl_image)) != 0) { - // give 1.5s for debugger to attach - delayMicroseconds(1500000); + // don't risk erasing the bootloader if it already matches + if (memcmp((const void*)MCU_FLASH_START, bl_image, sizeof(bl_image)) != 0) { + // give 1.5s for debugger to attach + delayMicroseconds(1500000); - // do the flash - flash_bootloader(); - } + // do the flash + flash_bootloader(); + } - // and reset - NVIC_SystemReset(); + // and reset + NVIC_SystemReset(); - return 0; + return 0; } diff --git a/bootloader/DroneCAN/DroneCAN.c b/bootloader/DroneCAN/DroneCAN.c index 7e7100cf..a6af3b31 100644 --- a/bootloader/DroneCAN/DroneCAN.c +++ b/bootloader/DroneCAN/DroneCAN.c @@ -52,11 +52,11 @@ static uint8_t canard_memory_pool[CANARD_POOL_SIZE]; keep the state for firmware update */ static struct { - char path[256]; - uint8_t node_id; - uint8_t transfer_id; - uint32_t last_read_ms; - uint32_t offset; + char path[256]; + uint8_t node_id; + uint8_t transfer_id; + uint32_t last_read_ms; + uint32_t offset; } fwupdate; static bool have_raw_command; @@ -71,17 +71,17 @@ static bool have_raw_command; static struct uavcan_protocol_NodeStatus node_status; enum boot_code { - CHECK_FW_OK = 0, - FAIL_REASON_NO_APP_SIG = 10, - FAIL_REASON_BAD_LENGTH_APP = 11, - FAIL_REASON_BAD_BOARD_ID = 12, - FAIL_REASON_BAD_CRC = 13, - FAIL_REASON_IN_UPDATE = 14, - FAIL_REASON_WATCHDOG = 15, - FAIL_REASON_BAD_LENGTH_DESCRIPTOR = 16, - FAIL_REASON_BAD_FIRMWARE_SIGNATURE = 17, - FAIL_REASON_VERIFICATION = 18, - FAIL_REASON_NO_SIGNAL = 19, + CHECK_FW_OK = 0, + FAIL_REASON_NO_APP_SIG = 10, + FAIL_REASON_BAD_LENGTH_APP = 11, + FAIL_REASON_BAD_BOARD_ID = 12, + FAIL_REASON_BAD_CRC = 13, + FAIL_REASON_IN_UPDATE = 14, + FAIL_REASON_WATCHDOG = 15, + FAIL_REASON_BAD_LENGTH_DESCRIPTOR = 16, + FAIL_REASON_BAD_FIRMWARE_SIGNATURE = 17, + FAIL_REASON_VERIFICATION = 18, + FAIL_REASON_NO_SIGNAL = 19, }; #define APP_SIGNATURE_MAGIC1 0x68f058e6 @@ -91,13 +91,13 @@ enum boot_code { application signature */ struct app_signature { - uint32_t magic1; - uint32_t magic2; - uint32_t fwlen; // total fw length in bytes - uint32_t crc1; // crc32 up to start of app_signature - uint32_t crc2; // crc32 from end of app_signature to end of fw - char mcu[16]; - uint32_t unused[2]; + uint32_t magic1; + uint32_t magic2; + uint32_t fwlen; // total fw length in bytes + uint32_t crc1; // crc32 up to start of app_signature + uint32_t crc2; // crc32 from end of app_signature to end of fw + char mcu[16]; + uint32_t unused[2]; }; /* @@ -105,11 +105,11 @@ struct app_signature { */ static uint16_t get_random16(void) { - static uint32_t m_z = 1234; - static uint32_t m_w = 76542; - m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16); - m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16); - return ((m_z << 16) + m_w) & 0xFFFF; + static uint32_t m_z = 1234; + static uint32_t m_w = 76542; + m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16); + m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16); + return ((m_z << 16) + m_w) & 0xFFFF; } /* @@ -120,14 +120,14 @@ static uint16_t get_random16(void) */ static uint64_t micros64(void) { - static uint64_t base_us; - static uint16_t last_cnt; - uint16_t cnt = bl_timer_us(); - if (cnt < last_cnt) { - base_us += 0x10000; - } - last_cnt = cnt; - return base_us + cnt; + static uint64_t base_us; + static uint16_t last_cnt; + uint16_t cnt = bl_timer_us(); + if (cnt < last_cnt) { + base_us += 0x10000; + } + last_cnt = cnt; + return base_us + cnt; } /* @@ -135,53 +135,53 @@ static uint64_t micros64(void) */ static uint32_t millis32(void) { - return micros64() / 1000ULL; + return micros64() / 1000ULL; } /* default settings, based on public/assets/eeprom_default.bin in AM32 configurator */ static const uint8_t default_settings[] = { - 0x01, 0x02, BOOTLOADER_VERSION, 0x01, 0x23, 0x4e, 0x45, 0x4f, 0x45, 0x53, 0x43, 0x20, 0x66, 0x30, 0x35, 0x31, - 0x20, 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x02, 0x18, 0x64, 0x37, 0x0e, 0x00, 0x00, 0x05, 0x00, - 0x80, 0x80, 0x80, 0x32, 0x00, 0x32, 0x00, 0x00, 0x0f, 0x0a, 0x0a, 0x8d, 0x66, 0x06, 0x00, 0x00 + 0x01, 0x02, BOOTLOADER_VERSION, 0x01, 0x23, 0x4e, 0x45, 0x4f, 0x45, 0x53, 0x43, 0x20, 0x66, 0x30, 0x35, 0x31, + 0x20, 0x00, 0x00, 0x00, 0x01, 0x01, 0x01, 0x02, 0x18, 0x64, 0x37, 0x0e, 0x00, 0x00, 0x05, 0x00, + 0x80, 0x80, 0x80, 0x32, 0x00, 0x32, 0x00, 0x00, 0x0f, 0x0a, 0x0a, 0x8d, 0x66, 0x06, 0x00, 0x00 }; // crc32 implementation, slow method for small flash cost static uint32_t crc32(const uint8_t *buf, uint32_t size) { - uint32_t crc = 0; - while (size--) { - const uint8_t byte = *buf++; - crc ^= byte; - for (uint8_t i=0; i<8; i++) { - const uint32_t mask = -(crc & 1); - crc >>= 1; - crc ^= (0xEDB88320 & mask); - } - } - return crc; + uint32_t crc = 0; + while (size--) { + const uint8_t byte = *buf++; + crc ^= byte; + for (uint8_t i=0; i<8; i++) { + const uint32_t mask = -(crc & 1); + crc >>= 1; + crc ^= (0xEDB88320 & mask); + } + } + return crc; } // print to CAN LogMessage for debugging static void can_print(const char *s) { - struct uavcan_protocol_debug_LogMessage pkt; - memset(&pkt, 0, sizeof(pkt)); - - uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE]; - pkt.text.len = strlen(s); - memcpy(pkt.text.data, s, pkt.text.len); - - uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); - static uint8_t logmsg_transfer_id; - - canardBroadcast(&canard, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, - UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, - &logmsg_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, len); + struct uavcan_protocol_debug_LogMessage pkt; + memset(&pkt, 0, sizeof(pkt)); + + uint8_t buffer[UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE]; + pkt.text.len = strlen(s); + memcpy(pkt.text.data, s, pkt.text.len); + + uint32_t len = uavcan_protocol_debug_LogMessage_encode(&pkt, buffer); + static uint8_t logmsg_transfer_id; + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE, + UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID, + &logmsg_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, len); } /* @@ -189,34 +189,34 @@ static void can_print(const char *s) */ static void handle_param_ExecuteOpcode(CanardInstance* ins, CanardRxTransfer* transfer) { - struct uavcan_protocol_param_ExecuteOpcodeRequest req; - if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { - return; - } - struct uavcan_protocol_param_ExecuteOpcodeResponse pkt; - memset(&pkt, 0, sizeof(pkt)); - - pkt.ok = false; - - if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) { - can_print("resetting to defaults"); - save_flash_nolib(default_settings, sizeof(default_settings), EEPROM_START_ADD); - pkt.ok = true; - } - - - uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE]; - uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); - - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, - UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + struct uavcan_protocol_param_ExecuteOpcodeRequest req; + if (uavcan_protocol_param_ExecuteOpcodeRequest_decode(transfer, &req)) { + return; + } + struct uavcan_protocol_param_ExecuteOpcodeResponse pkt; + memset(&pkt, 0, sizeof(pkt)); + + pkt.ok = false; + + if (req.opcode == UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_REQUEST_OPCODE_ERASE) { + can_print("resetting to defaults"); + save_flash_nolib(default_settings, sizeof(default_settings), EEPROM_START_ADD); + pkt.ok = true; + } + + + uint8_t buffer[UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_RESPONSE_MAX_SIZE]; + uint16_t total_size = uavcan_protocol_param_ExecuteOpcodeResponse_encode(&pkt, buffer); + + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE, + UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } /* @@ -224,9 +224,9 @@ static void handle_param_ExecuteOpcode(CanardInstance* ins, CanardRxTransfer* tr */ static void handle_RestartNode(CanardInstance* ins, CanardRxTransfer* transfer) { - set_rtc_backup_register(0, 0); - // reboot the ESC - NVIC_SystemReset(); + set_rtc_backup_register(0, 0); + // reboot the ESC + NVIC_SystemReset(); } /* @@ -234,40 +234,40 @@ static void handle_RestartNode(CanardInstance* ins, CanardRxTransfer* transfer) */ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) { - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_GetNodeInfoResponse pkt; + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_GetNodeInfoResponse pkt; - memset(&pkt, 0, sizeof(pkt)); + memset(&pkt, 0, sizeof(pkt)); - node_status.uptime_sec = micros64() / 1000000ULL; - pkt.status = node_status; + node_status.uptime_sec = micros64() / 1000000ULL; + pkt.status = node_status; - // fill in your major and minor firmware version - pkt.software_version.major = BOOTLOADER_VERSION; - pkt.software_version.minor = 0; - pkt.software_version.optional_field_flags = 0; - pkt.software_version.vcs_commit = 0; // should put git hash in here + // fill in your major and minor firmware version + pkt.software_version.major = BOOTLOADER_VERSION; + pkt.software_version.minor = 0; + pkt.software_version.optional_field_flags = 0; + pkt.software_version.vcs_commit = 0; // should put git hash in here - // should fill in hardware version - pkt.hardware_version.major = 2; - pkt.hardware_version.minor = 3; + // should fill in hardware version + pkt.hardware_version.major = 2; + pkt.hardware_version.minor = 3; - sys_can_getUniqueID(pkt.hardware_version.unique_id); + sys_can_getUniqueID(pkt.hardware_version.unique_id); - strncpy((char*)pkt.name.data, "AM32_BOOTLOADER_" AM32_MCU, sizeof(pkt.name.data)); - pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); + strncpy((char*)pkt.name.data, "AM32_BOOTLOADER_" AM32_MCU, sizeof(pkt.name.data)); + pkt.name.len = strnlen((char*)pkt.name.data, sizeof(pkt.name.data)); - uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); + uint16_t total_size = uavcan_protocol_GetNodeInfoResponse_encode(&pkt, buffer); - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, - UAVCAN_PROTOCOL_GETNODEINFO_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_GETNODEINFO_SIGNATURE, + UAVCAN_PROTOCOL_GETNODEINFO_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); } /* @@ -276,45 +276,45 @@ static void handle_GetNodeInfo(CanardInstance *ins, CanardRxTransfer *transfer) */ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* transfer) { - /* - decode the request - */ - struct uavcan_protocol_file_BeginFirmwareUpdateRequest req; - if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { - return; - } - - /* - check for a repeated BeginFirmwareUpdateRequest - */ - if (fwupdate.node_id == transfer->source_node_id && - memcmp(fwupdate.path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len) == 0) { - /* ignore duplicate request */ - return; - } - - fwupdate.offset = 0; - fwupdate.node_id = transfer->source_node_id; - strncpy(fwupdate.path, (char*)req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); - - uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; - struct uavcan_protocol_file_BeginFirmwareUpdateResponse reply; - memset(&reply, 0, sizeof(reply)); - reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; - - uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); - - canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, - UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &buffer[0], - total_size); - - can_print("Started firmware update"); + /* + decode the request + */ + struct uavcan_protocol_file_BeginFirmwareUpdateRequest req; + if (uavcan_protocol_file_BeginFirmwareUpdateRequest_decode(transfer, &req)) { + return; + } + + /* + check for a repeated BeginFirmwareUpdateRequest + */ + if (fwupdate.node_id == transfer->source_node_id && + memcmp(fwupdate.path, req.image_file_remote_path.path.data, req.image_file_remote_path.path.len) == 0) { + /* ignore duplicate request */ + return; + } + + fwupdate.offset = 0; + fwupdate.node_id = transfer->source_node_id; + strncpy(fwupdate.path, (char*)req.image_file_remote_path.path.data, req.image_file_remote_path.path.len); + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_MAX_SIZE]; + struct uavcan_protocol_file_BeginFirmwareUpdateResponse reply; + memset(&reply, 0, sizeof(reply)); + reply.error = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_RESPONSE_ERROR_OK; + + uint32_t total_size = uavcan_protocol_file_BeginFirmwareUpdateResponse_encode(&reply, buffer); + + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE, + UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &buffer[0], + total_size); + + can_print("Started firmware update"); } /* @@ -323,33 +323,33 @@ static void handle_begin_firmware_update(CanardInstance* ins, CanardRxTransfer* */ static void send_firmware_read(void) { - uint32_t now = millis32(); - if (fwupdate.last_read_ms != 0 && now - fwupdate.last_read_ms < 750) { - // the server may still be responding - return; - } - fwupdate.last_read_ms = now; - - uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; - - struct uavcan_protocol_file_ReadRequest pkt; - memset(&pkt, 0, sizeof(pkt)); - - pkt.path.path.len = strlen((const char *)fwupdate.path); - pkt.offset = fwupdate.offset; - memcpy(pkt.path.path.data, fwupdate.path, pkt.path.path.len); - - uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer); - - canardRequestOrRespond(&canard, - fwupdate.node_id, - UAVCAN_PROTOCOL_FILE_READ_SIGNATURE, - UAVCAN_PROTOCOL_FILE_READ_ID, - &fwupdate.transfer_id, - CANARD_TRANSFER_PRIORITY_HIGH, - CanardRequest, - &buffer[0], - total_size); + uint32_t now = millis32(); + if (fwupdate.last_read_ms != 0 && now - fwupdate.last_read_ms < 750) { + // the server may still be responding + return; + } + fwupdate.last_read_ms = now; + + uint8_t buffer[UAVCAN_PROTOCOL_FILE_READ_REQUEST_MAX_SIZE]; + + struct uavcan_protocol_file_ReadRequest pkt; + memset(&pkt, 0, sizeof(pkt)); + + pkt.path.path.len = strlen((const char *)fwupdate.path); + pkt.offset = fwupdate.offset; + memcpy(pkt.path.path.data, fwupdate.path, pkt.path.path.len); + + uint16_t total_size = uavcan_protocol_file_ReadRequest_encode(&pkt, buffer); + + canardRequestOrRespond(&canard, + fwupdate.node_id, + UAVCAN_PROTOCOL_FILE_READ_SIGNATURE, + UAVCAN_PROTOCOL_FILE_READ_ID, + &fwupdate.transfer_id, + CANARD_TRANSFER_PRIORITY_HIGH, + CanardRequest, + &buffer[0], + total_size); } /* @@ -357,57 +357,57 @@ static void send_firmware_read(void) */ static void handle_file_read_response(CanardInstance* ins, CanardRxTransfer* transfer) { - if ((transfer->transfer_id+1)%32 != fwupdate.transfer_id || - transfer->source_node_id != fwupdate.node_id) { - /* not for us */ - can_print("Firmware update: not for us"); - return; - } - struct uavcan_protocol_file_ReadResponse pkt; - if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) { - /* bad packet */ - can_print("Firmware update: bad packet"); - return; - } - if (pkt.error.value != UAVCAN_PROTOCOL_FILE_ERROR_OK) { - if (fwupdate.offset == 0) { - // MissionPlanner does this sometimes, ignore - return; - } - /* read failed */ - fwupdate.node_id = 0; - can_print("Firmware update read failure"); - return; - } - - uint32_t len = pkt.data.len; - len = (len+7U) & ~7U; - save_flash_nolib(pkt.data.data, len, (uint32_t)MAIN_FW_START_ADDR + fwupdate.offset); - - fwupdate.offset += pkt.data.len; - - if (pkt.data.len < 256) { - /* firmware updare done */ - can_print("Firmwate update complete"); - fwupdate.node_id = 0; - set_rtc_backup_register(0, 0); - NVIC_SystemReset(); - return; - } + if ((transfer->transfer_id+1)%32 != fwupdate.transfer_id || + transfer->source_node_id != fwupdate.node_id) { + /* not for us */ + can_print("Firmware update: not for us"); + return; + } + struct uavcan_protocol_file_ReadResponse pkt; + if (uavcan_protocol_file_ReadResponse_decode(transfer, &pkt)) { + /* bad packet */ + can_print("Firmware update: bad packet"); + return; + } + if (pkt.error.value != UAVCAN_PROTOCOL_FILE_ERROR_OK) { + if (fwupdate.offset == 0) { + // MissionPlanner does this sometimes, ignore + return; + } + /* read failed */ + fwupdate.node_id = 0; + can_print("Firmware update read failure"); + return; + } + + uint32_t len = pkt.data.len; + len = (len+7U) & ~7U; + save_flash_nolib(pkt.data.data, len, (uint32_t)MAIN_FW_START_ADDR + fwupdate.offset); + + fwupdate.offset += pkt.data.len; + + if (pkt.data.len < 256) { + /* firmware updare done */ + can_print("Firmwate update complete"); + fwupdate.node_id = 0; + set_rtc_backup_register(0, 0); + NVIC_SystemReset(); + return; + } - /* trigger a new read */ - fwupdate.last_read_ms = 0; - send_firmware_read(); + /* trigger a new read */ + fwupdate.last_read_ms = 0; + send_firmware_read(); - DroneCAN_processTxQueue(); + DroneCAN_processTxQueue(); } /* data for dynamic node allocation process */ static struct { - uint32_t send_next_node_id_allocation_request_at_ms; - uint32_t node_id_allocation_unique_id_offset; + uint32_t send_next_node_id_allocation_request_at_ms; + uint32_t node_id_allocation_unique_id_offset; } DNA; /* @@ -415,50 +415,50 @@ static struct { */ static void handle_DNA_Allocation(CanardInstance *ins, CanardRxTransfer *transfer) { - if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { - // already allocated - return; - } - - // Rule C - updating the randomized time interval - DNA.send_next_node_id_allocation_request_at_ms = - millis32() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + - (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + if (canardGetLocalNodeID(&canard) != CANARD_BROADCAST_NODE_ID) { + // already allocated + return; + } - if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { - DNA.node_id_allocation_unique_id_offset = 0; - return; - } - - // Copying the unique ID from the message - struct uavcan_protocol_dynamic_node_id_Allocation msg; + // Rule C - updating the randomized time interval + DNA.send_next_node_id_allocation_request_at_ms = + millis32() + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); - if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { - /* bad packet */ - return; - } + if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) { + DNA.node_id_allocation_unique_id_offset = 0; + return; + } - // Obtaining the local unique ID - uint8_t my_unique_id[sizeof(msg.unique_id.data)]; - sys_can_getUniqueID(my_unique_id); + // Copying the unique ID from the message + struct uavcan_protocol_dynamic_node_id_Allocation msg; - // Matching the received UID against the local one - if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { - DNA.node_id_allocation_unique_id_offset = 0; - // No match, return - return; - } + if (uavcan_protocol_dynamic_node_id_Allocation_decode(transfer, &msg)) { + /* bad packet */ + return; + } - if (msg.unique_id.len < sizeof(msg.unique_id.data)) { - // The allocator has confirmed part of unique ID, switching to - // the next stage and updating the timeout. - DNA.node_id_allocation_unique_id_offset = msg.unique_id.len; - DNA.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + // Obtaining the local unique ID + uint8_t my_unique_id[sizeof(msg.unique_id.data)]; + sys_can_getUniqueID(my_unique_id); - } else { - // Allocation complete - copying the allocated node ID from the message - canardSetLocalNodeID(ins, msg.node_id); - } + // Matching the received UID against the local one + if (memcmp(msg.unique_id.data, my_unique_id, msg.unique_id.len) != 0) { + DNA.node_id_allocation_unique_id_offset = 0; + // No match, return + return; + } + + if (msg.unique_id.len < sizeof(msg.unique_id.data)) { + // The allocator has confirmed part of unique ID, switching to + // the next stage and updating the timeout. + DNA.node_id_allocation_unique_id_offset = msg.unique_id.len; + DNA.send_next_node_id_allocation_request_at_ms -= UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS; + + } else { + // Allocation complete - copying the allocated node ID from the message + canardSetLocalNodeID(ins, msg.node_id); + } } /* @@ -466,45 +466,45 @@ static void handle_DNA_Allocation(CanardInstance *ins, CanardRxTransfer *transfe */ static void request_DNA() { - const uint32_t now = millis32(); - static uint8_t node_id_allocation_transfer_id = 0; + const uint32_t now = millis32(); + static uint8_t node_id_allocation_transfer_id = 0; - DNA.send_next_node_id_allocation_request_at_ms = - now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + - (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); + DNA.send_next_node_id_allocation_request_at_ms = + now + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_MS + + (get_random16() % UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_MAX_FOLLOWUP_DELAY_MS); - // Structure of the request is documented in the DSDL definition - // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation - uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; - allocation_request[0] = (uint8_t)(PREFERRED_NODE_ID << 1U); + // Structure of the request is documented in the DSDL definition + // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation + uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; + allocation_request[0] = (uint8_t)(PREFERRED_NODE_ID << 1U); - if (DNA.node_id_allocation_unique_id_offset == 0) { - allocation_request[0] |= 1; // First part of unique ID - } + if (DNA.node_id_allocation_unique_id_offset == 0) { + allocation_request[0] |= 1; // First part of unique ID + } - uint8_t my_unique_id[16]; - sys_can_getUniqueID(my_unique_id); + uint8_t my_unique_id[16]; + sys_can_getUniqueID(my_unique_id); - static const uint8_t MaxLenOfUniqueIDInRequest = 6; - uint8_t uid_size = (uint8_t)(16 - DNA.node_id_allocation_unique_id_offset); - - if (uid_size > MaxLenOfUniqueIDInRequest) { - uid_size = MaxLenOfUniqueIDInRequest; - } + static const uint8_t MaxLenOfUniqueIDInRequest = 6; + uint8_t uid_size = (uint8_t)(16 - DNA.node_id_allocation_unique_id_offset); - memmove(&allocation_request[1], &my_unique_id[DNA.node_id_allocation_unique_id_offset], uid_size); + if (uid_size > MaxLenOfUniqueIDInRequest) { + uid_size = MaxLenOfUniqueIDInRequest; + } - // Broadcasting the request - canardBroadcast(&canard, - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, - UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, - &node_id_allocation_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - &allocation_request[0], - (uint16_t) (uid_size + 1)); + memmove(&allocation_request[1], &my_unique_id[DNA.node_id_allocation_unique_id_offset], uid_size); - // Preparing for timeout; if response is received, this value will be updated from the callback. - DNA.node_id_allocation_unique_id_offset = 0; + // Broadcasting the request + canardBroadcast(&canard, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE, + UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID, + &node_id_allocation_transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + &allocation_request[0], + (uint16_t) (uid_size + 1)); + + // Preparing for timeout; if response is received, this value will be updated from the callback. + DNA.node_id_allocation_unique_id_offset = 0; } /* @@ -512,44 +512,44 @@ static void request_DNA() */ static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer) { - // switch on data type ID to pass to the right handler function - if (transfer->transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - handle_GetNodeInfo(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_RESTARTNODE_ID: { - handle_RestartNode(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { - handle_begin_firmware_update(ins, transfer); - break; - } - case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { - handle_param_ExecuteOpcode(ins, transfer); - break; - } - } - } - if (transfer->transfer_type == CanardTransferTypeResponse) { - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_FILE_READ_ID: - handle_file_read_response(ins, transfer); - break; - } - } - if (transfer->transfer_type == CanardTransferTypeBroadcast) { - // check if we want to handle a specific broadcast message - switch (transfer->data_type_id) { - case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { - handle_DNA_Allocation(ins, transfer); - break; - } - } - } + // switch on data type ID to pass to the right handler function + if (transfer->transfer_type == CanardTransferTypeRequest) { + // check if we want to handle a specific service request + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + handle_GetNodeInfo(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_RESTARTNODE_ID: { + handle_RestartNode(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { + handle_begin_firmware_update(ins, transfer); + break; + } + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { + handle_param_ExecuteOpcode(ins, transfer); + break; + } + } + } + if (transfer->transfer_type == CanardTransferTypeResponse) { + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_FILE_READ_ID: + handle_file_read_response(ins, transfer); + break; + } + } + if (transfer->transfer_type == CanardTransferTypeBroadcast) { + // check if we want to handle a specific broadcast message + switch (transfer->data_type_id) { + case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { + handle_DNA_Allocation(ins, transfer); + break; + } + } + } } @@ -568,50 +568,50 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, CanardTransferType transfer_type, uint8_t source_node_id) { - if (transfer_type == CanardTransferTypeRequest) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_GETNODEINFO_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_RESTARTNODE_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; - return true; - } - case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; - return true; - } - } - } - if (transfer_type == CanardTransferTypeResponse) { - // check if we want to handle a specific service request - switch (data_type_id) { - case UAVCAN_PROTOCOL_FILE_READ_ID: - *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; - return true; - } - } - if (transfer_type == CanardTransferTypeBroadcast) { - // see if we want to handle a specific broadcast packet - switch (data_type_id) { - case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { - *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; - return true; - } - case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: { - // we are receiving RawCommand, we should boot to the main firmware - have_raw_command = true; - } - } - } - // we don't want any other messages - return false; + if (transfer_type == CanardTransferTypeRequest) { + // check if we want to handle a specific service request + switch (data_type_id) { + case UAVCAN_PROTOCOL_GETNODEINFO_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_GETNODEINFO_REQUEST_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_RESTARTNODE_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_RESTARTNODE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_BEGINFIRMWAREUPDATE_SIGNATURE; + return true; + } + case UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_PARAM_EXECUTEOPCODE_SIGNATURE; + return true; + } + } + } + if (transfer_type == CanardTransferTypeResponse) { + // check if we want to handle a specific service request + switch (data_type_id) { + case UAVCAN_PROTOCOL_FILE_READ_ID: + *out_data_type_signature = UAVCAN_PROTOCOL_FILE_READ_SIGNATURE; + return true; + } + } + if (transfer_type == CanardTransferTypeBroadcast) { + // see if we want to handle a specific broadcast packet + switch (data_type_id) { + case UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_ID: { + *out_data_type_signature = UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SIGNATURE; + return true; + } + case UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID: { + // we are receiving RawCommand, we should boot to the main firmware + have_raw_command = true; + } + } + } + // we don't want any other messages + return false; } /* @@ -620,36 +620,36 @@ static bool shouldAcceptTransfer(const CanardInstance *ins, */ static void send_NodeStatus(void) { - uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; - - node_status.uptime_sec = micros64() / 1000000ULL; - node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; - node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE; - node_status.sub_mode = 0; - - /* - when doing a firmware update put the size in kbytes in VSSC so - the user can see how far it has reached - */ - if (fwupdate.node_id != 0) { - node_status.vendor_specific_status_code = fwupdate.offset / 1024; - node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE; - } - - uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); - - // we need a static variable for the transfer ID. This is - // incremeneted on each transfer, allowing for detection of packet - // loss - static uint8_t transfer_id; - - canardBroadcast(&canard, - UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, - UAVCAN_PROTOCOL_NODESTATUS_ID, - &transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - buffer, - len); + uint8_t buffer[UAVCAN_PROTOCOL_GETNODEINFO_RESPONSE_MAX_SIZE]; + + node_status.uptime_sec = micros64() / 1000000ULL; + node_status.health = UAVCAN_PROTOCOL_NODESTATUS_HEALTH_OK; + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_MAINTENANCE; + node_status.sub_mode = 0; + + /* + when doing a firmware update put the size in kbytes in VSSC so + the user can see how far it has reached + */ + if (fwupdate.node_id != 0) { + node_status.vendor_specific_status_code = fwupdate.offset / 1024; + node_status.mode = UAVCAN_PROTOCOL_NODESTATUS_MODE_SOFTWARE_UPDATE; + } + + uint32_t len = uavcan_protocol_NodeStatus_encode(&node_status, buffer); + + // we need a static variable for the transfer ID. This is + // incremeneted on each transfer, allowing for detection of packet + // loss + static uint8_t transfer_id; + + canardBroadcast(&canard, + UAVCAN_PROTOCOL_NODESTATUS_SIGNATURE, + UAVCAN_PROTOCOL_NODESTATUS_ID, + &transfer_id, + CANARD_TRANSFER_PRIORITY_LOW, + buffer, + len); } /* @@ -657,15 +657,15 @@ static void send_NodeStatus(void) */ static void process1HzTasks(uint64_t timestamp_usec) { - /* - Purge transfers that are no longer transmitted. This can free up some memory - */ - canardCleanupStaleTransfers(&canard, timestamp_usec); - - /* - Transmit the node status message - */ - send_NodeStatus(); + /* + Purge transfers that are no longer transmitted. This can free up some memory + */ + canardCleanupStaleTransfers(&canard, timestamp_usec); + + /* + Transmit the node status message + */ + send_NodeStatus(); } /* @@ -673,10 +673,10 @@ static void process1HzTasks(uint64_t timestamp_usec) */ void DroneCAN_receiveFrame(void) { - CanardCANFrame rx_frame = {0}; - while (sys_can_receive(&rx_frame) > 0) { - canardHandleRxFrame(&canard, &rx_frame, micros64()); - } + CanardCANFrame rx_frame = {0}; + while (sys_can_receive(&rx_frame) > 0) { + canardHandleRxFrame(&canard, &rx_frame, micros64()); + } } /* @@ -684,68 +684,68 @@ void DroneCAN_receiveFrame(void) */ void DroneCAN_processTxQueue(void) { - for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { - const int16_t tx_res = sys_can_transmit(txf); - if (tx_res == 0) { - // no space, stop trying - break; - } - // success or error, remove frame - canardPopTxQueue(&canard); - } + for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) { + const int16_t tx_res = sys_can_transmit(txf); + if (tx_res == 0) { + // no space, stop trying + break; + } + // success or error, remove frame + canardPopTxQueue(&canard); + } } //#pragma GCC optimize("O0") static void DroneCAN_Startup(void) { - canardInit(&canard, - canard_memory_pool, // Raw memory chunk used for dynamic allocation - sizeof(canard_memory_pool), - onTransferReceived, // Callback, see CanardOnTransferReception - shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer - NULL); - - /* - if doing fw update get node ID from main firmware via RTC backup - register - */ - uint8_t node_id = 0; - const uint32_t rtc0 = get_rtc_backup_register(0); - if ((rtc0 & 0xFFFFU) == RTC_BKUP0_FWUPDATE) { - node_id = rtc0 >> 24; - uint8_t src_node = (rtc0 >> 16) & 0xFF; - uint32_t path[2]; - path[0] = get_rtc_backup_register(1); - path[1] = get_rtc_backup_register(2); - if (path[0] != 0 && src_node <= 127) { - // we have update path, can start immediately - fwupdate.node_id = src_node; - memset(fwupdate.path, 0, sizeof(fwupdate.path)); - memcpy(fwupdate.path, path, sizeof(path)); - } - } - - if (node_id == 0) { - // check for valid node ID in settings - const uint8_t *eeprom = (const uint8_t *)EEPROM_START_ADD; - if (eeprom[0] == 1 && eeprom[176] <= 127) { - node_id = eeprom[176]; - } - } - - canardSetLocalNodeID(&canard, node_id); - - // initialise low level CAN peripheral hardware - sys_can_init(); + canardInit(&canard, + canard_memory_pool, // Raw memory chunk used for dynamic allocation + sizeof(canard_memory_pool), + onTransferReceived, // Callback, see CanardOnTransferReception + shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer + NULL); + + /* + if doing fw update get node ID from main firmware via RTC backup + register + */ + uint8_t node_id = 0; + const uint32_t rtc0 = get_rtc_backup_register(0); + if ((rtc0 & 0xFFFFU) == RTC_BKUP0_FWUPDATE) { + node_id = rtc0 >> 24; + uint8_t src_node = (rtc0 >> 16) & 0xFF; + uint32_t path[2]; + path[0] = get_rtc_backup_register(1); + path[1] = get_rtc_backup_register(2); + if (path[0] != 0 && src_node <= 127) { + // we have update path, can start immediately + fwupdate.node_id = src_node; + memset(fwupdate.path, 0, sizeof(fwupdate.path)); + memcpy(fwupdate.path, path, sizeof(path)); + } + } + + if (node_id == 0) { + // check for valid node ID in settings + const uint8_t *eeprom = (const uint8_t *)EEPROM_START_ADD; + if (eeprom[0] == 1 && eeprom[176] <= 127) { + node_id = eeprom[176]; + } + } + + canardSetLocalNodeID(&canard, node_id); + + // initialise low level CAN peripheral hardware + sys_can_init(); #if 0 - if (fwupdate.node_id != 0) { - can_print("fwupdate startup"); - can_print(fwupdate.path); - } else { - can_print("bootloader startup"); - } + if (fwupdate.node_id != 0) { + can_print("fwupdate startup"); + can_print(fwupdate.path); + } else { + can_print("bootloader startup"); + } #endif } @@ -754,43 +754,43 @@ static void DroneCAN_Startup(void) */ bool DroneCAN_update() { - static uint64_t next_1hz_service_at; - static bool done_startup; - if (!done_startup) { - done_startup = true; - DroneCAN_Startup(); - } + static uint64_t next_1hz_service_at; + static bool done_startup; + if (!done_startup) { + done_startup = true; + DroneCAN_Startup(); + } - sys_can_disable_IRQ(); + sys_can_disable_IRQ(); - DroneCAN_processTxQueue(); + DroneCAN_processTxQueue(); - // see if we are still doing DNA - if (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) { - // we're still waiting for a DNA allocation of our node ID - if (millis32() > DNA.send_next_node_id_allocation_request_at_ms) { - request_DNA(); - } - sys_can_enable_IRQ(); - return false; + // see if we are still doing DNA + if (canardGetLocalNodeID(&canard) == CANARD_BROADCAST_NODE_ID) { + // we're still waiting for a DNA allocation of our node ID + if (millis32() > DNA.send_next_node_id_allocation_request_at_ms) { + request_DNA(); } + sys_can_enable_IRQ(); + return false; + } - const uint64_t ts = micros64(); + const uint64_t ts = micros64(); - if (ts >= next_1hz_service_at) { - next_1hz_service_at += 1000000ULL; - process1HzTasks(ts); - } + if (ts >= next_1hz_service_at) { + next_1hz_service_at += 1000000ULL; + process1HzTasks(ts); + } - if (fwupdate.node_id != 0) { - send_firmware_read(); - } + if (fwupdate.node_id != 0) { + send_firmware_read(); + } - DroneCAN_processTxQueue(); + DroneCAN_processTxQueue(); - sys_can_enable_IRQ(); + sys_can_enable_IRQ(); - return DroneCAN_boot_ok(); + return DroneCAN_boot_ok(); } /* @@ -798,27 +798,29 @@ bool DroneCAN_update() */ static void *memmem(const void *haystack, size_t haystacklen, const void *needle, size_t needlelen) { - while (haystacklen >= needlelen) { - char *p = (char *)memchr(haystack, *(const char *)needle, haystacklen-(needlelen-1)); - if (!p) return NULL; - if (memcmp(p, needle, needlelen) == 0) { - return p; - } - haystack = p+1; - haystacklen -= (p - (const char *)haystack) + 1; - } - return NULL; + while (haystacklen >= needlelen) { + char *p = (char *)memchr(haystack, *(const char *)needle, haystacklen-(needlelen-1)); + if (!p) { + return NULL; + } + if (memcmp(p, needle, needlelen) == 0) { + return p; + } + haystack = p+1; + haystacklen -= (p - (const char *)haystack) + 1; + } + return NULL; } static void set_reason(enum boot_code code, const char *reason) { - static uint64_t last_msg_us; - node_status.vendor_specific_status_code = code; - const uint64_t now_us = micros64(); - if (now_us - last_msg_us > 5000000UL) { - last_msg_us = now_us; - can_print(reason); - } + static uint64_t last_msg_us; + node_status.vendor_specific_status_code = code; + const uint64_t now_us = micros64(); + if (now_us - last_msg_us > 5000000UL) { + last_msg_us = now_us; + can_print(reason); + } } /* @@ -826,51 +828,51 @@ static void set_reason(enum boot_code code, const char *reason) */ bool DroneCAN_boot_ok(void) { - if (fwupdate.node_id != 0) { - // in fw update - return false; - } - if ((get_rtc_backup_register(0) & 0xFFFFU) == RTC_BKUP0_FWUPDATE) { - // waiting for firmware update - node_status.vendor_specific_status_code = FAIL_REASON_IN_UPDATE; - return false; - } - /* - check application signature - */ - uint32_t sig[2] = { APP_SIGNATURE_MAGIC1, APP_SIGNATURE_MAGIC2 }; - const uint32_t app_max_len = (128-18)*1024; - const uint8_t *fw_base = (const uint8_t *)MAIN_FW_START_ADDR; - struct app_signature *appsig = memmem(fw_base, app_max_len, sig, sizeof(sig)); - if (appsig == NULL || (((uint32_t)appsig) & 3) != 0) { - set_reason(FAIL_REASON_NO_APP_SIG, "no app signature"); - return false; - } - if (appsig->fwlen > app_max_len) { - set_reason(FAIL_REASON_BAD_LENGTH_APP, "bad app length"); - return false; - } - if (memcmp(AM32_MCU, appsig->mcu, strlen(AM32_MCU)) != 0) { - set_reason(FAIL_REASON_BAD_BOARD_ID, "bad board type"); - return false; - } - const uint8_t *appsigend = (const uint8_t *)(appsig+1); - const uint32_t crc1 = crc32(fw_base, (uint32_t)((uint8_t*)appsig - fw_base)); - const uint32_t crc2 = crc32(appsigend, appsig->fwlen - (uint32_t)(appsigend - fw_base)); - if (appsig->crc1 != crc1 || - appsig->crc2 != crc2) { - set_reason(FAIL_REASON_BAD_CRC, "bad firmware CRC"); - return false; - } + if (fwupdate.node_id != 0) { + // in fw update + return false; + } + if ((get_rtc_backup_register(0) & 0xFFFFU) == RTC_BKUP0_FWUPDATE) { + // waiting for firmware update + node_status.vendor_specific_status_code = FAIL_REASON_IN_UPDATE; + return false; + } + /* + check application signature + */ + uint32_t sig[2] = { APP_SIGNATURE_MAGIC1, APP_SIGNATURE_MAGIC2 }; + const uint32_t app_max_len = (128-18)*1024; + const uint8_t *fw_base = (const uint8_t *)MAIN_FW_START_ADDR; + struct app_signature *appsig = memmem(fw_base, app_max_len, sig, sizeof(sig)); + if (appsig == NULL || (((uint32_t)appsig) & 3) != 0) { + set_reason(FAIL_REASON_NO_APP_SIG, "no app signature"); + return false; + } + if (appsig->fwlen > app_max_len) { + set_reason(FAIL_REASON_BAD_LENGTH_APP, "bad app length"); + return false; + } + if (memcmp(AM32_MCU, appsig->mcu, strlen(AM32_MCU)) != 0) { + set_reason(FAIL_REASON_BAD_BOARD_ID, "bad board type"); + return false; + } + const uint8_t *appsigend = (const uint8_t *)(appsig+1); + const uint32_t crc1 = crc32(fw_base, (uint32_t)((uint8_t*)appsig - fw_base)); + const uint32_t crc2 = crc32(appsigend, appsig->fwlen - (uint32_t)(appsigend - fw_base)); + if (appsig->crc1 != crc1 || + appsig->crc2 != crc2) { + set_reason(FAIL_REASON_BAD_CRC, "bad firmware CRC"); + return false; + } - if (!have_raw_command) { - set_reason(FAIL_REASON_BAD_CRC, "no signal"); - return false; - } + if (!have_raw_command) { + set_reason(FAIL_REASON_BAD_CRC, "no signal"); + return false; + } - node_status.vendor_specific_status_code = CHECK_FW_OK; + node_status.vendor_specific_status_code = CHECK_FW_OK; - return true; + return true; } #endif // DRONECAN_SUPPORT diff --git a/bootloader/DroneCAN/regenerate.sh b/bootloader/DroneCAN/regenerate.sh index 093091cd..7c19ed47 100755 --- a/bootloader/DroneCAN/regenerate.sh +++ b/bootloader/DroneCAN/regenerate.sh @@ -2,24 +2,26 @@ # regenerate the C API from DSDL [ -d Src/DroneCAN ] || { - echo "Must be run from top level of AM32 source tree" - exit 1 + echo "Must be run from top level of AM32 source tree" + exit 1 } -download() { - /bin/rm -rf tmp - mkdir -p tmp +download() +{ + /bin/rm -rf tmp + mkdir -p tmp - echo "Cloning DSDL" - git clone --depth 1 https://github.com/DroneCAN/DSDL tmp/DSDL + echo "Cloning DSDL" +git clone --depth 1 https://github.com/DroneCAN/DSDL tmp/DSDL - echo "Cloning dronecan_dsdlc" - git clone --depth 1 https://github.com/DroneCAN/dronecan_dsdlc tmp/dronecan_dsdlc + echo "Cloning dronecan_dsdlc" +git clone --depth 1 https://github.com/DroneCAN/dronecan_dsdlc tmp/dronecan_dsdlc } -generate() { - echo "Running generator" - python3 tmp/dronecan_dsdlc/dronecan_dsdlc.py -O tmp/dsdl_generated tmp/DSDL/dronecan tmp/DSDL/uavcan tmp/DSDL/com tmp/DSDL/ardupilot +generate() +{ + echo "Running generator" + python3 tmp/dronecan_dsdlc/dronecan_dsdlc.py -O tmp/dsdl_generated tmp/DSDL/dronecan tmp/DSDL/uavcan tmp/DSDL/com tmp/DSDL/ardupilot } download @@ -28,19 +30,19 @@ generate # list of messages which we need to support, wildcards are added to get the sub-messages MSGS="uavcan.protocol.NodeStatus uavcan.protocol.HardwareVersion uavcan.protocol.SoftwareVersion uavcan.protocol.GetNodeInfo uavcan.equipment.esc uavcan.protocol.dynamic_node_id uavcan.protocol.param uavcan.protocol.file uavcan.protocol.RestartNode uavcan.protocol.RestartNode uavcan.protocol.debug uavcan.equipment.safety.ArmingStatus" -rm -rf Src/DroneCAN/dsdl_generated -mkdir -p Src/DroneCAN/dsdl_generated/src -mkdir -p Src/DroneCAN/dsdl_generated/include + rm -rf Src/DroneCAN/dsdl_generated + mkdir -p Src/DroneCAN/dsdl_generated/src + mkdir -p Src/DroneCAN/dsdl_generated/include -for m in $MSGS; do - echo "Copying $m" - cp tmp/dsdl_generated/src/$m* Src/DroneCAN/dsdl_generated/src/ - cp tmp/dsdl_generated/include/$m* Src/DroneCAN/dsdl_generated/include/ + for m in $MSGS; do +echo "Copying $m" +cp tmp/dsdl_generated/src/$m* Src/DroneCAN/dsdl_generated/src/ +cp tmp/dsdl_generated/include/$m* Src/DroneCAN/dsdl_generated/include/ done echo "#pragma once" > Src/DroneCAN/dsdl_generated/dronecan_msgs.h for f in $(/bin/ls Src/DroneCAN/dsdl_generated/include); do - echo "#include \"$f\"" >> Src/DroneCAN/dsdl_generated/dronecan_msgs.h -done + echo "#include \"$f\"" >> Src/DroneCAN/dsdl_generated/dronecan_msgs.h + done -rm -rf tmp + rm -rf tmp diff --git a/bootloader/DroneCAN/sys_can.h b/bootloader/DroneCAN/sys_can.h index ddb52dd3..7e01fd7b 100644 --- a/bootloader/DroneCAN/sys_can.h +++ b/bootloader/DroneCAN/sys_can.h @@ -10,18 +10,18 @@ CAN statistics shared by low level and high level code */ struct CANStats { - uint32_t num_commands; - uint32_t total_commands; - uint32_t num_receive; - uint32_t num_tx_interrupts; - uint32_t num_rx_interrupts; - uint32_t rx_errors; - uint32_t esr; - uint32_t rxframe_error; - int32_t rx_ecode; - uint32_t should_accept; - uint32_t on_receive; - uint64_t last_raw_command_us; + uint32_t num_commands; + uint32_t total_commands; + uint32_t num_receive; + uint32_t num_tx_interrupts; + uint32_t num_rx_interrupts; + uint32_t rx_errors; + uint32_t esr; + uint32_t rxframe_error; + int32_t rx_ecode; + uint32_t should_accept; + uint32_t on_receive; + uint64_t last_raw_command_us; }; extern struct CANStats canstats; diff --git a/bootloader/DroneCAN/sys_can_at32.c b/bootloader/DroneCAN/sys_can_at32.c index 1011d2f0..6efba795 100644 --- a/bootloader/DroneCAN/sys_can_at32.c +++ b/bootloader/DroneCAN/sys_can_at32.c @@ -19,7 +19,7 @@ */ void usleep(uint32_t usec) { - delayMicros(usec); + delayMicros(usec); } /* @@ -27,9 +27,9 @@ void usleep(uint32_t usec) */ void sys_can_getUniqueID(uint8_t id[16]) { - const uint8_t *uidbase = (const uint8_t *)0x1FFFF7E8; // 96 bit UID - memcpy(id, uidbase, 12); - memset(&id[12], 0, 4); + const uint8_t *uidbase = (const uint8_t *)0x1FFFF7E8; // 96 bit UID + memcpy(id, uidbase, 12); + memset(&id[12], 0, 4); } /** @@ -39,27 +39,27 @@ void sys_can_getUniqueID(uint8_t id[16]) */ static void can_gpio_config(void) { - gpio_init_type gpio_init_struct; + gpio_init_type gpio_init_struct; - crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); - crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); - gpio_pin_remap_config(CAN1_GMUX_0000,TRUE); // CAN_RX=PA11/CAN_TX=PA12 - // gpio_pin_remap_config(CAN1_GMUX_0010,TRUE); // CAN_RX=PB8/CAN_TX=PB9 + crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE); + crm_periph_clock_enable(CRM_IOMUX_PERIPH_CLOCK, TRUE); + gpio_pin_remap_config(CAN1_GMUX_0000,TRUE); // CAN_RX=PA11/CAN_TX=PA12 + // gpio_pin_remap_config(CAN1_GMUX_0010,TRUE); // CAN_RX=PB8/CAN_TX=PB9 - gpio_default_para_init(&gpio_init_struct); - /* can tx pin */ - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; - gpio_init_struct.gpio_mode = GPIO_MODE_MUX; - gpio_init_struct.gpio_pins = GPIO_PINS_12; - gpio_init_struct.gpio_pull = GPIO_PULL_NONE; - gpio_init(GPIOA, &gpio_init_struct); - /* can rx pin */ - gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; - gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; - gpio_init_struct.gpio_pins = GPIO_PINS_11; - gpio_init_struct.gpio_pull = GPIO_PULL_UP; - gpio_init(GPIOA, &gpio_init_struct); + gpio_default_para_init(&gpio_init_struct); + /* can tx pin */ + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL; + gpio_init_struct.gpio_mode = GPIO_MODE_MUX; + gpio_init_struct.gpio_pins = GPIO_PINS_12; + gpio_init_struct.gpio_pull = GPIO_PULL_NONE; + gpio_init(GPIOA, &gpio_init_struct); + /* can rx pin */ + gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER; + gpio_init_struct.gpio_mode = GPIO_MODE_INPUT; + gpio_init_struct.gpio_pins = GPIO_PINS_11; + gpio_init_struct.gpio_pull = GPIO_PULL_UP; + gpio_init(GPIOA, &gpio_init_struct); } /* @@ -67,72 +67,72 @@ static void can_gpio_config(void) */ void sys_can_init(void) { - nvic_priority_group_config(NVIC_PRIORITY_GROUP_4); - can_gpio_config(); + nvic_priority_group_config(NVIC_PRIORITY_GROUP_4); + can_gpio_config(); - can_base_type can_base_struct; - can_baudrate_type can_baudrate_struct; + can_base_type can_base_struct; + can_baudrate_type can_baudrate_struct; - crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE); - /* can base init */ - can_default_para_init(&can_base_struct); - can_base_struct.mode_selection = CAN_MODE_COMMUNICATE; - can_base_struct.ttc_enable = FALSE; - can_base_struct.aebo_enable = TRUE; - can_base_struct.aed_enable = TRUE; - can_base_struct.prsf_enable = FALSE; - can_base_struct.mdrsel_selection = CAN_DISCARDING_FIRST_RECEIVED; - can_base_struct.mmssr_selection = CAN_SENDING_BY_REQUEST; - can_base_init(CAN1, &can_base_struct); + crm_periph_clock_enable(CRM_CAN1_PERIPH_CLOCK, TRUE); + /* can base init */ + can_default_para_init(&can_base_struct); + can_base_struct.mode_selection = CAN_MODE_COMMUNICATE; + can_base_struct.ttc_enable = FALSE; + can_base_struct.aebo_enable = TRUE; + can_base_struct.aed_enable = TRUE; + can_base_struct.prsf_enable = FALSE; + can_base_struct.mdrsel_selection = CAN_DISCARDING_FIRST_RECEIVED; + can_base_struct.mmssr_selection = CAN_SENDING_BY_REQUEST; + can_base_init(CAN1, &can_base_struct); - crm_clocks_freq_type clocks; - crm_clocks_freq_get(&clocks); + crm_clocks_freq_type clocks; + crm_clocks_freq_get(&clocks); - /* can baudrate, set baudrate = pclk/(baudrate_div *(1 + bts1_size + bts2_size)) */ - can_baudrate_struct.baudrate_div = 6; - can_baudrate_struct.rsaw_size = CAN_RSAW_3TQ; - can_baudrate_struct.bts1_size = CAN_BTS1_8TQ; - can_baudrate_struct.bts2_size = CAN_BTS2_3TQ; - can_baudrate_set(CAN1, &can_baudrate_struct); + /* can baudrate, set baudrate = pclk/(baudrate_div *(1 + bts1_size + bts2_size)) */ + can_baudrate_struct.baudrate_div = 6; + can_baudrate_struct.rsaw_size = CAN_RSAW_3TQ; + can_baudrate_struct.bts1_size = CAN_BTS1_8TQ; + can_baudrate_struct.bts2_size = CAN_BTS2_3TQ; + can_baudrate_set(CAN1, &can_baudrate_struct); - /* can filter init */ - can_filter_init_type can_filter_init_struct; - can_filter_init_struct.filter_activate_enable = TRUE; - can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO0; - can_filter_init_struct.filter_number = 0; - can_filter_init_struct.filter_bit = CAN_FILTER_32BIT; - can_filter_init_struct.filter_id_high = 0; - can_filter_init_struct.filter_id_low = 0; - can_filter_init_struct.filter_mask_high = 0; - can_filter_init_struct.filter_mask_low = 0; - can_filter_init(CAN1, &can_filter_init_struct); + /* can filter init */ + can_filter_init_type can_filter_init_struct; + can_filter_init_struct.filter_activate_enable = TRUE; + can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO0; + can_filter_init_struct.filter_number = 0; + can_filter_init_struct.filter_bit = CAN_FILTER_32BIT; + can_filter_init_struct.filter_id_high = 0; + can_filter_init_struct.filter_id_low = 0; + can_filter_init_struct.filter_mask_high = 0; + can_filter_init_struct.filter_mask_low = 0; + can_filter_init(CAN1, &can_filter_init_struct); - can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO1; - can_filter_init_struct.filter_number = 1; - can_filter_init(CAN1, &can_filter_init_struct); + can_filter_init_struct.filter_fifo = CAN_FILTER_FIFO1; + can_filter_init_struct.filter_number = 1; + can_filter_init(CAN1, &can_filter_init_struct); - /* interrupt enable */ - can_interrupt_enable(CAN1, CAN_TCIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_RF1MIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_ETRIEN_INT, TRUE); - can_interrupt_enable(CAN1, CAN_EOIEN_INT, TRUE); + /* interrupt enable */ + can_interrupt_enable(CAN1, CAN_TCIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_RF0MIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_RF1MIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_ETRIEN_INT, TRUE); + can_interrupt_enable(CAN1, CAN_EOIEN_INT, TRUE); } void sys_can_enable_IRQ(void) { - nvic_irq_enable(CAN1_SE_IRQn, 0x00, 0x00); - nvic_irq_enable(CAN1_RX0_IRQn, 0x00, 0x00); - nvic_irq_enable(CAN1_RX1_IRQn, 0x00, 0x00); - nvic_irq_enable(CAN1_TX_IRQn, 0x00, 0x00); + nvic_irq_enable(CAN1_SE_IRQn, 0x00, 0x00); + nvic_irq_enable(CAN1_RX0_IRQn, 0x00, 0x00); + nvic_irq_enable(CAN1_RX1_IRQn, 0x00, 0x00); + nvic_irq_enable(CAN1_TX_IRQn, 0x00, 0x00); } void sys_can_disable_IRQ(void) { - nvic_irq_disable(CAN1_SE_IRQn); - nvic_irq_disable(CAN1_RX0_IRQn); - nvic_irq_disable(CAN1_RX1_IRQn); - nvic_irq_disable(CAN1_TX_IRQn); + nvic_irq_disable(CAN1_SE_IRQn); + nvic_irq_disable(CAN1_RX0_IRQn); + nvic_irq_disable(CAN1_RX1_IRQn); + nvic_irq_disable(CAN1_TX_IRQn); } /* @@ -141,24 +141,24 @@ void sys_can_disable_IRQ(void) */ int16_t sys_can_transmit(const CanardCANFrame* txf) { - can_tx_message_type tx_message_struct; - if (txf->id & CANARD_CAN_FRAME_EFF) { - tx_message_struct.id_type = CAN_ID_EXTENDED; - tx_message_struct.standard_id = 0; - tx_message_struct.extended_id = txf->id & CANARD_CAN_EXT_ID_MASK; - } else { - tx_message_struct.id_type = CAN_ID_STANDARD; - tx_message_struct.standard_id = txf->id & CANARD_CAN_STD_ID_MASK; - tx_message_struct.extended_id = 0; - } - tx_message_struct.frame_type = CAN_TFT_DATA; - tx_message_struct.dlc = txf->data_len; - memcpy(tx_message_struct.data, txf->data, txf->data_len); - const uint8_t transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); - if (transmit_mailbox == CAN_TX_STATUS_NO_EMPTY) { - return 0; - } - return 1; + can_tx_message_type tx_message_struct; + if (txf->id & CANARD_CAN_FRAME_EFF) { + tx_message_struct.id_type = CAN_ID_EXTENDED; + tx_message_struct.standard_id = 0; + tx_message_struct.extended_id = txf->id & CANARD_CAN_EXT_ID_MASK; + } else { + tx_message_struct.id_type = CAN_ID_STANDARD; + tx_message_struct.standard_id = txf->id & CANARD_CAN_STD_ID_MASK; + tx_message_struct.extended_id = 0; + } + tx_message_struct.frame_type = CAN_TFT_DATA; + tx_message_struct.dlc = txf->data_len; + memcpy(tx_message_struct.data, txf->data, txf->data_len); + const uint8_t transmit_mailbox = can_message_transmit(CAN1, &tx_message_struct); + if (transmit_mailbox == CAN_TX_STATUS_NO_EMPTY) { + return 0; + } + return 1; } /* @@ -167,28 +167,28 @@ int16_t sys_can_transmit(const CanardCANFrame* txf) */ int16_t sys_can_receive(CanardCANFrame *rx_frame) { - can_rx_message_type frm; - bool have_frame = false; - if (CAN1->rf0_bit.rf0mn) { - can_message_receive(CAN1, CAN_RX_FIFO0, &frm); - have_frame = true; - } else if (CAN1->rf1_bit.rf1mn) { - can_message_receive(CAN1, CAN_RX_FIFO1, &frm); - have_frame = true; - } - if (!have_frame) { - return 0; - } - if (frm.id_type == CAN_ID_EXTENDED) { - rx_frame->id = frm.extended_id | CANARD_CAN_FRAME_EFF; - } else if (frm.id_type == CAN_ID_STANDARD) { - rx_frame->id = frm.standard_id; - } else { - return 0; - } - rx_frame->data_len = frm.dlc; - memcpy(rx_frame->data, frm.data, rx_frame->data_len); - return 1; + can_rx_message_type frm; + bool have_frame = false; + if (CAN1->rf0_bit.rf0mn) { + can_message_receive(CAN1, CAN_RX_FIFO0, &frm); + have_frame = true; + } else if (CAN1->rf1_bit.rf1mn) { + can_message_receive(CAN1, CAN_RX_FIFO1, &frm); + have_frame = true; + } + if (!have_frame) { + return 0; + } + if (frm.id_type == CAN_ID_EXTENDED) { + rx_frame->id = frm.extended_id | CANARD_CAN_FRAME_EFF; + } else if (frm.id_type == CAN_ID_STANDARD) { + rx_frame->id = frm.standard_id; + } else { + return 0; + } + rx_frame->data_len = frm.dlc; + memcpy(rx_frame->data, frm.data, rx_frame->data_len); + return 1; } /** @@ -198,18 +198,18 @@ int16_t sys_can_receive(CanardCANFrame *rx_frame) */ void CAN1_RX0_IRQHandler(void) { - DroneCAN_receiveFrame(); + DroneCAN_receiveFrame(); } void CAN1_RX1_IRQHandler(void) { - DroneCAN_receiveFrame(); + DroneCAN_receiveFrame(); } void CAN1_TX_IRQHandler(void) { - CAN1->tsts = CAN_TSTS_TM0TCF_VAL | CAN_TSTS_TM1TCF_VAL | CAN_TSTS_TM2TCF_VAL; - DroneCAN_processTxQueue(); + CAN1->tsts = CAN_TSTS_TM0TCF_VAL | CAN_TSTS_TM1TCF_VAL | CAN_TSTS_TM2TCF_VAL; + DroneCAN_processTxQueue(); } /** @@ -220,42 +220,40 @@ void CAN1_TX_IRQHandler(void) void CAN1_SE_IRQHandler(void) { __IO uint32_t err_index = 0; - if (CAN1->ests_bit.etr) - { + if (CAN1->ests_bit.etr) { err_index = CAN1->ests & 0x70; CAN1->msts = CAN_MSTS_EOIF_VAL; CAN1->ests = 0; /* error type is stuff error */ - if (err_index == 0x00000010) - { - /* when stuff error occur: in order to ensure communication normally, - user must restart can or send a frame of highest priority message here - */ + if (err_index == 0x00000010) { + /* when stuff error occur: in order to ensure communication normally, + user must restart can or send a frame of highest priority message here + */ } } } static void ertc_init(void) { - static bool done_init; - if (done_init) { - return; - } - done_init = true; - crm_periph_clock_enable(CRM_PWC_PERIPH_CLOCK, TRUE); - pwc_battery_powered_domain_access(TRUE); + static bool done_init; + if (done_init) { + return; + } + done_init = true; + crm_periph_clock_enable(CRM_PWC_PERIPH_CLOCK, TRUE); + pwc_battery_powered_domain_access(TRUE); } uint32_t get_rtc_backup_register(uint8_t idx) { - ertc_init(); - return ertc_bpr_data_read((ertc_dt_type)idx); + ertc_init(); + return ertc_bpr_data_read((ertc_dt_type)idx); } void set_rtc_backup_register(uint8_t idx, uint32_t value) { - ertc_init(); - ertc_bpr_data_write((ertc_dt_type)idx, value); + ertc_init(); + ertc_bpr_data_write((ertc_dt_type)idx, value); } #endif // DRONECAN_SUPPORT && defined(ARTERY) diff --git a/bootloader/DroneCAN/sys_can_stm32.c b/bootloader/DroneCAN/sys_can_stm32.c index 354fd3a6..28edc06c 100644 --- a/bootloader/DroneCAN/sys_can_stm32.c +++ b/bootloader/DroneCAN/sys_can_stm32.c @@ -19,8 +19,8 @@ */ void usleep(uint32_t usec) { - const uint16_t start_us = (uint16_t)bl_timer_us(); - while ((uint16_t)(bl_timer_us() - start_us) < (uint16_t)usec) ; + const uint16_t start_us = (uint16_t)bl_timer_us(); + while ((uint16_t)(bl_timer_us() - start_us) < (uint16_t)usec) ; } /* @@ -28,12 +28,12 @@ void usleep(uint32_t usec) */ void sys_can_getUniqueID(uint8_t id[16]) { - const uint8_t *uidbase = (const uint8_t *)UID_BASE; - memcpy(id, uidbase, 12); + const uint8_t *uidbase = (const uint8_t *)UID_BASE; + memcpy(id, uidbase, 12); - // put CPU ID in last 4 bytes, handy for knowing the exact MCU we are on - const uint32_t cpuid = SCB->CPUID; - memcpy(&id[12], &cpuid, 4); + // put CPU ID in last 4 bytes, handy for knowing the exact MCU we are on + const uint32_t cpuid = SCB->CPUID; + memcpy(&id[12], &cpuid, 4); } /* @@ -41,7 +41,7 @@ void sys_can_getUniqueID(uint8_t id[16]) */ static void handleTxMailboxInterrupt(uint8_t mbox, bool txok) { - DroneCAN_processTxQueue(); + DroneCAN_processTxQueue(); } /* @@ -49,10 +49,10 @@ static void handleTxMailboxInterrupt(uint8_t mbox, bool txok) */ static void pollErrorFlagsFromISR() { - const uint8_t lec = (uint8_t)((BXCAN->ESR & CANARD_STM32_CAN_ESR_LEC_MASK) >> CANARD_STM32_CAN_ESR_LEC_SHIFT); - if (lec != 0) { - BXCAN->ESR = 0; - } + const uint8_t lec = (uint8_t)((BXCAN->ESR & CANARD_STM32_CAN_ESR_LEC_MASK) >> CANARD_STM32_CAN_ESR_LEC_SHIFT); + if (lec != 0) { + BXCAN->ESR = 0; + } } /* @@ -60,24 +60,24 @@ static void pollErrorFlagsFromISR() */ static void handleTxInterrupt(void) { - // TXOK == false means that there was a hardware failure - if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP0) { - const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK0; - BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP0; - handleTxMailboxInterrupt(0, txok); - } - if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP1) { - const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK1; - BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP1; - handleTxMailboxInterrupt(1, txok); - } - if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP2) { - const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK2; - BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP2; - handleTxMailboxInterrupt(2, txok); - } - - pollErrorFlagsFromISR(); + // TXOK == false means that there was a hardware failure + if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP0) { + const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK0; + BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP0; + handleTxMailboxInterrupt(0, txok); + } + if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP1) { + const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK1; + BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP1; + handleTxMailboxInterrupt(1, txok); + } + if (BXCAN->TSR & CANARD_STM32_CAN_TSR_RQCP2) { + const bool txok = BXCAN->TSR & CANARD_STM32_CAN_TSR_TXOK2; + BXCAN->TSR = CANARD_STM32_CAN_TSR_RQCP2; + handleTxMailboxInterrupt(2, txok); + } + + pollErrorFlagsFromISR(); } /* @@ -85,14 +85,14 @@ static void handleTxInterrupt(void) */ static void handleRxInterrupt(uint8_t fifo_index) { - volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &BXCAN->RF0R : &BXCAN->RF1R; - if ((*rfr_reg & CANARD_STM32_CAN_RFR_FMP_MASK) == 0) { - return; - } + volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &BXCAN->RF0R : &BXCAN->RF1R; + if ((*rfr_reg & CANARD_STM32_CAN_RFR_FMP_MASK) == 0) { + return; + } - DroneCAN_receiveFrame(); + DroneCAN_receiveFrame(); - pollErrorFlagsFromISR(); + pollErrorFlagsFromISR(); } /* @@ -100,17 +100,17 @@ static void handleRxInterrupt(uint8_t fifo_index) */ void CAN1_RX0_IRQHandler(void) { - handleRxInterrupt(0); + handleRxInterrupt(0); } void CAN1_RX1_IRQHandler(void) { - handleRxInterrupt(1); + handleRxInterrupt(1); } void CAN1_TX_IRQHandler(void) { - handleTxInterrupt(); + handleTxInterrupt(); } /* @@ -119,7 +119,7 @@ void CAN1_TX_IRQHandler(void) */ int16_t sys_can_transmit(const CanardCANFrame* txf) { - return canardSTM32Transmit(txf); + return canardSTM32Transmit(txf); } /* @@ -128,7 +128,7 @@ int16_t sys_can_transmit(const CanardCANFrame* txf) */ int16_t sys_can_receive(CanardCANFrame *rx_frame) { - return canardSTM32Receive(rx_frame); + return canardSTM32Receive(rx_frame); } /* @@ -136,9 +136,9 @@ int16_t sys_can_receive(CanardCANFrame *rx_frame) */ void sys_can_disable_IRQ(void) { - NVIC_DisableIRQ(CAN1_RX0_IRQn); - NVIC_DisableIRQ(CAN1_RX1_IRQn); - NVIC_DisableIRQ(CAN1_TX_IRQn); + NVIC_DisableIRQ(CAN1_RX0_IRQn); + NVIC_DisableIRQ(CAN1_RX1_IRQn); + NVIC_DisableIRQ(CAN1_TX_IRQn); } /* @@ -146,9 +146,9 @@ void sys_can_disable_IRQ(void) */ void sys_can_enable_IRQ(void) { - NVIC_EnableIRQ(CAN1_RX0_IRQn); - NVIC_EnableIRQ(CAN1_RX1_IRQn); - NVIC_EnableIRQ(CAN1_TX_IRQn); + NVIC_EnableIRQ(CAN1_RX0_IRQn); + NVIC_EnableIRQ(CAN1_RX1_IRQn); + NVIC_EnableIRQ(CAN1_TX_IRQn); } /* @@ -161,54 +161,54 @@ void sys_can_enable_IRQ(void) */ void sys_can_init(void) { - /* - setup CAN RX and TX pins - */ - LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_CAN1); - LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); + /* + setup CAN RX and TX pins + */ + LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_CAN1); + LL_AHB2_GRP1_EnableClock(LL_AHB2_GRP1_PERIPH_GPIOA); - // assume PA11/PA12 for now - LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; - GPIO_InitStruct.Pin = LL_GPIO_PIN_11 | LL_GPIO_PIN_12; - GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; - GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; - GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; - GPIO_InitStruct.Alternate = 9; // AF9==CAN - LL_GPIO_Init(GPIOA, &GPIO_InitStruct); + // assume PA11/PA12 for now + LL_GPIO_InitTypeDef GPIO_InitStruct = {0}; + GPIO_InitStruct.Pin = LL_GPIO_PIN_11 | LL_GPIO_PIN_12; + GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE; + GPIO_InitStruct.Pull = LL_GPIO_PULL_NO; + GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH; + GPIO_InitStruct.Alternate = 9; // AF9==CAN + LL_GPIO_Init(GPIOA, &GPIO_InitStruct); - /* - work out timing for 1MBps CAN - */ - CanardSTM32CANTimings timings; + /* + work out timing for 1MBps CAN + */ + CanardSTM32CANTimings timings; - // assume 80MHz peripheral clock - timings.bit_rate_prescaler = 8; - timings.bit_segment_1 = 8; - timings.bit_segment_2 = 1; - timings.max_resynchronization_jump_width = 1; + // assume 80MHz peripheral clock + timings.bit_rate_prescaler = 8; + timings.bit_segment_1 = 8; + timings.bit_segment_2 = 1; + timings.max_resynchronization_jump_width = 1; - canardSTM32Init(&timings, CanardSTM32IfaceModeNormal); + canardSTM32Init(&timings, CanardSTM32IfaceModeNormal); - /* - enable interrupt for CAN receive and transmit - */ - NVIC_SetPriority(CAN1_RX0_IRQn, 5); - NVIC_SetPriority(CAN1_RX1_IRQn, 5); - NVIC_SetPriority(CAN1_TX_IRQn, 5); + /* + enable interrupt for CAN receive and transmit + */ + NVIC_SetPriority(CAN1_RX0_IRQn, 5); + NVIC_SetPriority(CAN1_RX1_IRQn, 5); + NVIC_SetPriority(CAN1_TX_IRQn, 5); - BXCAN->IER = CANARD_STM32_CAN_IER_FMPIE0 | CANARD_STM32_CAN_IER_FMPIE1 | CANARD_STM32_CAN_IER_TMEIE; + BXCAN->IER = CANARD_STM32_CAN_IER_FMPIE0 | CANARD_STM32_CAN_IER_FMPIE1 | CANARD_STM32_CAN_IER_TMEIE; } uint32_t get_rtc_backup_register(uint8_t idx) { - const volatile uint32_t *bkp = &RTC->BKP0R; - return bkp[idx]; + const volatile uint32_t *bkp = &RTC->BKP0R; + return bkp[idx]; } void set_rtc_backup_register(uint8_t idx, uint32_t value) { - volatile uint32_t *bkp = &RTC->BKP0R; - bkp[idx] = value; + volatile uint32_t *bkp = &RTC->BKP0R; + bkp[idx] = value; } #endif // DRONECAN_SUPPORT && defined(MCU_L431) diff --git a/bootloader/main.c b/bootloader/main.c index 49355b69..d5142fe3 100644 --- a/bootloader/main.c +++ b/bootloader/main.c @@ -149,13 +149,13 @@ static uint16_t invalid_command; #define DEVINFO_MAGIC2 0x4eb863d9 static const struct { - uint32_t magic1; - uint32_t magic2; - const uint8_t deviceInfo[9]; + uint32_t magic1; + uint32_t magic2; + const uint8_t deviceInfo[9]; } devinfo __attribute__((section(".devinfo"))) = { - .magic1 = DEVINFO_MAGIC1, - .magic2 = DEVINFO_MAGIC2, - .deviceInfo = {'4','7','1',PIN_CODE,FLASH_SIZE_CODE,0x06,0x06,0x01,0x30} + .magic1 = DEVINFO_MAGIC1, + .magic2 = DEVINFO_MAGIC2, + .deviceInfo = {'4','7','1',PIN_CODE,FLASH_SIZE_CODE,0x06,0x06,0x01,0x30} }; typedef void (*pFunction)(void); @@ -191,9 +191,10 @@ static uint8_t payLoadBuffer[256]; static char rxbyte; static uint32_t address; -typedef union __attribute__ ((packed)) { - uint8_t bytes[2]; - uint16_t word; +typedef union __attribute__ ((packed)) +{ + uint8_t bytes[2]; + uint16_t word; } uint8_16_u; static uint16_t len; @@ -214,19 +215,19 @@ static uint16_t us_start; static void bl_timer_reset(void) { - us_start = bl_timer_us(); + us_start = bl_timer_us(); } static uint16_t bl_timer_elapsed(void) { - return bl_timer_us() - us_start; + return bl_timer_us() - us_start; } static void delayMicroseconds(uint16_t micros) { - bl_timer_reset(); - while (bl_timer_elapsed() < micros) { - } + bl_timer_reset(); + while (bl_timer_elapsed() < micros) { + } } /* @@ -236,46 +237,46 @@ static void jump() { #ifndef DISABLE_JUMP #if CHECK_EEPROM_BEFORE_JUMP - uint8_t value = *(uint8_t*)(EEPROM_START_ADD); - if (value != 0x01) { // check first byte of eeprom to see if its programmed, if not do not jump - invalid_command = 0; - return; - } + uint8_t value = *(uint8_t*)(EEPROM_START_ADD); + if (value != 0x01) { // check first byte of eeprom to see if its programmed, if not do not jump + invalid_command = 0; + return; + } #endif #ifndef DISABLE_APP_HEADER_CHECKS - /* - first word of the app is the stack pointer - make sure that it is in range - */ - const uint32_t *app = (uint32_t*)(MCU_FLASH_START + FIRMWARE_RELATIVE_START); - const uint32_t ram_start = 0x20000000; - const uint32_t ram_limit_kb = 64; - const uint32_t ram_end = ram_start+ram_limit_kb*1024; - if (app[0] < ram_start || app[0] > ram_end) { - invalid_command = 0; - return; - } - /* - 2nd word is the entry point of the main app. Ensure that is in range - */ - const uint32_t flash_limit_kb = 256; - if (app[1] < APPLICATION_ADDRESS || app[1] > APPLICATION_ADDRESS+flash_limit_kb*1024) { - // outside a 256k range, really unlikely to be a valid - // application, don't jump - invalid_command = 0; - return; - } + /* + first word of the app is the stack pointer - make sure that it is in range + */ + const uint32_t *app = (uint32_t*)(MCU_FLASH_START + FIRMWARE_RELATIVE_START); + const uint32_t ram_start = 0x20000000; + const uint32_t ram_limit_kb = 64; + const uint32_t ram_end = ram_start+ram_limit_kb*1024; + if (app[0] < ram_start || app[0] > ram_end) { + invalid_command = 0; + return; + } + /* + 2nd word is the entry point of the main app. Ensure that is in range + */ + const uint32_t flash_limit_kb = 256; + if (app[1] < APPLICATION_ADDRESS || app[1] > APPLICATION_ADDRESS+flash_limit_kb*1024) { + // outside a 256k range, really unlikely to be a valid + // application, don't jump + invalid_command = 0; + return; + } #endif // DISABLE_APP_HEADER_CHECKS #if DRONECAN_SUPPORT - if (!DroneCAN_boot_ok()) { - invalid_command = 0; - return; - } + if (!DroneCAN_boot_ok()) { + invalid_command = 0; + return; + } - sys_can_disable_IRQ(); + sys_can_disable_IRQ(); #endif - jump_to_application(); + jump_to_application(); #endif } @@ -284,154 +285,153 @@ static void jump() */ uint16_t crc16(const uint8_t* pBuff, uint16_t length) { - uint16_t ret = 0; - - for (int i = 0; i < length; i++) { - uint8_t xb = pBuff[i]; - for (uint8_t j = 0; j < 8; j++) - { - if (((xb & 0x01) ^ (ret & 0x0001)) != 0) { - ret >>= 1; - ret ^= 0xA001; - } else { - ret >>= 1; - } - xb = xb >> 1; - } - } - return ret; + uint16_t ret = 0; + + for (int i = 0; i < length; i++) { + uint8_t xb = pBuff[i]; + for (uint8_t j = 0; j < 8; j++) { + if (((xb & 0x01) ^ (ret & 0x0001)) != 0) { + ret >>= 1; + ret ^= 0xA001; + } else { + ret >>= 1; + } + xb = xb >> 1; + } + } + return ret; } static bool checkCrc(uint8_t* pBuff, uint16_t length) { - const uint16_t crcin = pBuff[length] | (pBuff[length+1]<<8); - const uint16_t crc2 = crc16(pBuff, length); + const uint16_t crcin = pBuff[length] | (pBuff[length+1]<<8); + const uint16_t crc2 = crc16(pBuff, length); - return crcin == crc2; + return crcin == crc2; } static void setReceive() { - gpio_mode_set_input(input_pin, GPIO_PULL_UP); - received = 0; + gpio_mode_set_input(input_pin, GPIO_PULL_UP); + received = 0; } static void setTransmit() { - // set high before we set as output to guarantee idle high - gpio_set(input_pin); - gpio_mode_set_output(input_pin, GPIO_OUTPUT_PUSH_PULL); + // set high before we set as output to guarantee idle high + gpio_set(input_pin); + gpio_mode_set_output(input_pin, GPIO_OUTPUT_PUSH_PULL); - // delay a bit to let the sender get setup for receiving - delayMicroseconds(BITTIME); + // delay a bit to let the sender get setup for receiving + delayMicroseconds(BITTIME); } static void send_ACK() { - setTransmit(); - serialwriteChar(0x30); // good ack! - setReceive(); - invalid_command = 0; + setTransmit(); + serialwriteChar(0x30); // good ack! + setReceive(); + invalid_command = 0; } static void send_BAD_ACK() { - setTransmit(); - serialwriteChar(0xC1); // bad command message. - setReceive(); - invalid_command++; + setTransmit(); + serialwriteChar(0xC1); // bad command message. + setReceive(); + invalid_command++; } static void send_BAD_CRC_ACK() { - setTransmit(); - serialwriteChar(0xC2); // bad command message. - setReceive(); - invalid_command++; + setTransmit(); + serialwriteChar(0xC2); // bad command message. + setReceive(); + invalid_command++; } static void sendDeviceInfo() { - setTransmit(); - sendString(devinfo.deviceInfo,sizeof(devinfo.deviceInfo)); - setReceive(); + setTransmit(); + sendString(devinfo.deviceInfo,sizeof(devinfo.deviceInfo)); + setReceive(); } static bool checkAddressWritable(uint32_t address) { - return address >= APPLICATION_ADDRESS; + return address >= APPLICATION_ADDRESS; } static void decodeInput() { - if (incoming_payload_no_command) { - len = payload_buffer_size; - if (checkCrc(rxBuffer,len)) { - memset(payLoadBuffer, 0, sizeof(payLoadBuffer)); // reset buffer - - for(int i = 0; i < len; i++){ - payLoadBuffer[i]= rxBuffer[i]; - } - send_ACK(); - incoming_payload_no_command = 0; - return; - }else{ - send_BAD_CRC_ACK(); - return; - } + if (incoming_payload_no_command) { + len = payload_buffer_size; + if (checkCrc(rxBuffer,len)) { + memset(payLoadBuffer, 0, sizeof(payLoadBuffer)); // reset buffer + + for (int i = 0; i < len; i++) { + payLoadBuffer[i]= rxBuffer[i]; + } + send_ACK(); + incoming_payload_no_command = 0; + return; + } else { + send_BAD_CRC_ACK(); + return; } + } - cmd = rxBuffer[0]; + cmd = rxBuffer[0]; - if (rxBuffer[16] == 0x7d) { - if(rxBuffer[8] == 13 && rxBuffer[9] == 66) { - sendDeviceInfo(); - rxBuffer[20]= 0; + if (rxBuffer[16] == 0x7d) { + if (rxBuffer[8] == 13 && rxBuffer[9] == 66) { + sendDeviceInfo(); + rxBuffer[20]= 0; - } - return; } + return; + } - if (rxBuffer[20] == 0x7d) { - if(rxBuffer[12] == 13 && rxBuffer[13] == 66) { - sendDeviceInfo(); - rxBuffer[20]= 0; - return; - } - + if (rxBuffer[20] == 0x7d) { + if (rxBuffer[12] == 13 && rxBuffer[13] == 66) { + sendDeviceInfo(); + rxBuffer[20]= 0; + return; } - if (rxBuffer[40] == 0x7d) { - if (rxBuffer[32] == 13 && rxBuffer[33] == 66) { - sendDeviceInfo(); - rxBuffer[20]= 0; - return; - } + + } + if (rxBuffer[40] == 0x7d) { + if (rxBuffer[32] == 13 && rxBuffer[33] == 66) { + sendDeviceInfo(); + rxBuffer[20]= 0; + return; } + } - if (cmd == CMD_RUN) { - // starts the main app - if((rxBuffer[1] == 0) && (rxBuffer[2] == 0) && (rxBuffer[3] == 0)) { - invalid_command = 101; - } + if (cmd == CMD_RUN) { + // starts the main app + if ((rxBuffer[1] == 0) && (rxBuffer[2] == 0) && (rxBuffer[3] == 0)) { + invalid_command = 101; } + } - if (cmd == CMD_PROG_FLASH) { - len = 2; - if (!checkCrc((uint8_t*)rxBuffer, len)) { - send_BAD_CRC_ACK(); + if (cmd == CMD_PROG_FLASH) { + len = 2; + if (!checkCrc((uint8_t*)rxBuffer, len)) { + send_BAD_CRC_ACK(); - return; - } + return; + } - if (!checkAddressWritable(address)) { - send_BAD_ACK(); + if (!checkAddressWritable(address)) { + send_BAD_ACK(); - return; - } + return; + } - if (address == EEPROM_START_ADD && payload_buffer_size > 2) { + if (address == EEPROM_START_ADD && payload_buffer_size > 2) { /* if the configuration client is writing the eeprom area then replace the bootloader version byte in the buffer @@ -439,146 +439,146 @@ static void decodeInput() less likely to need to erase any flash */ payLoadBuffer[2] = BOOTLOADER_VERSION; - } - - if (!save_flash_nolib((uint8_t*)payLoadBuffer, payload_buffer_size,address)) { - send_BAD_ACK(); - } else { - send_ACK(); - } - - return; } - if (cmd == CMD_SET_ADDRESS) { - // command set addressinput format is: CMD, 00 , High byte - // address, Low byte address, crclb ,crchb - len = 4; // package without 2 byte crc - if (!checkCrc((uint8_t*)rxBuffer, len)) { - send_BAD_CRC_ACK(); - return; - } - + if (!save_flash_nolib((uint8_t*)payLoadBuffer, payload_buffer_size,address)) { + send_BAD_ACK(); + } else { + send_ACK(); + } - // will send Ack 0x30 and read input after transfer out callback - invalid_command = 0; - address = MCU_FLASH_START + ((rxBuffer[2] << 8 | rxBuffer[3]) << ADDRESS_SHIFT); - send_ACK(); + return; + } - return; + if (cmd == CMD_SET_ADDRESS) { + // command set addressinput format is: CMD, 00 , High byte + // address, Low byte address, crclb ,crchb + len = 4; // package without 2 byte crc + if (!checkCrc((uint8_t*)rxBuffer, len)) { + send_BAD_CRC_ACK(); + return; } - if (cmd == CMD_SET_BUFFER) { - // for writing buffer rx buffer 0 = command byte. command set - // address, input , format is CMD, 00 , 00 or 01 (if buffer is 256), - // buffer_size, - len = 4; // package without 2 byte crc - if (!checkCrc((uint8_t*)rxBuffer, len)) { - send_BAD_CRC_ACK(); - return; - } + // will send Ack 0x30 and read input after transfer out callback + invalid_command = 0; + address = MCU_FLASH_START + ((rxBuffer[2] << 8 | rxBuffer[3]) << ADDRESS_SHIFT); + send_ACK(); - // no ack with command set buffer; - if(rxBuffer[2] == 0x01){ - payload_buffer_size = 256; // if nothing in this buffer - }else{ - payload_buffer_size = rxBuffer[3]; - } - incoming_payload_no_command = 1; - address_expected_increment = 256; - setReceive(); + return; + } + + if (cmd == CMD_SET_BUFFER) { + // for writing buffer rx buffer 0 = command byte. command set + // address, input , format is CMD, 00 , 00 or 01 (if buffer is 256), + // buffer_size, + len = 4; // package without 2 byte crc + if (!checkCrc((uint8_t*)rxBuffer, len)) { + send_BAD_CRC_ACK(); - return; + return; } - if (cmd == CMD_KEEP_ALIVE) { - len = 2; - if (!checkCrc((uint8_t*)rxBuffer, len)) { - send_BAD_CRC_ACK(); + // no ack with command set buffer; + if (rxBuffer[2] == 0x01) { + payload_buffer_size = 256; // if nothing in this buffer + } else { + payload_buffer_size = rxBuffer[3]; + } + incoming_payload_no_command = 1; + address_expected_increment = 256; + setReceive(); - return; - } + return; + } - setTransmit(); - serialwriteChar(0xC1); // bad command message. - setReceive(); + if (cmd == CMD_KEEP_ALIVE) { + len = 2; + if (!checkCrc((uint8_t*)rxBuffer, len)) { + send_BAD_CRC_ACK(); - return; + return; } - if (cmd == CMD_ERASE_FLASH) { - len = 2; - if (!checkCrc((uint8_t*)rxBuffer, len)) { - send_BAD_CRC_ACK(); - - return; - } - - if (!checkAddressWritable(address)) { - send_BAD_ACK(); + setTransmit(); + serialwriteChar(0xC1); // bad command message. + setReceive(); - return; - } + return; + } - send_ACK(); - return; - } + if (cmd == CMD_ERASE_FLASH) { + len = 2; + if (!checkCrc((uint8_t*)rxBuffer, len)) { + send_BAD_CRC_ACK(); - if (cmd == CMD_READ_EEPROM) { - eeprom_req = 1; + return; } - if (cmd == CMD_READ_FLASH_SIL) { - // for sending contents of flash memory at the memory location set in - // bootloader.c need to still set memory with data from set mem - // command - len = 2; - if (!checkCrc((uint8_t*)rxBuffer, len)) { - send_BAD_CRC_ACK(); + if (!checkAddressWritable(address)) { + send_BAD_ACK(); - return; - } + return; + } - count++; - uint16_t out_buffer_size = rxBuffer[1];// - if(out_buffer_size == 0){ - out_buffer_size = 256; - } - address_expected_increment = 128; + send_ACK(); + return; + } - setTransmit(); - uint8_t read_data[out_buffer_size + 3]; // make buffer 3 larger to fit CRC and ACK - memset(read_data, 0, sizeof(read_data)); - // read_flash((uint8_t*)read_data , address); // make sure read_flash reads two less than buffer. - read_flash_bin((uint8_t*)read_data , address, out_buffer_size); + if (cmd == CMD_READ_EEPROM) { + eeprom_req = 1; + } - const uint16_t crc = crc16(read_data,out_buffer_size); - read_data[out_buffer_size] = crc&0xFF; - read_data[out_buffer_size + 1] = crc>>8; - read_data[out_buffer_size + 2] = 0x30; - sendString(read_data, out_buffer_size+3); + if (cmd == CMD_READ_FLASH_SIL) { + // for sending contents of flash memory at the memory location set in + // bootloader.c need to still set memory with data from set mem + // command + len = 2; + if (!checkCrc((uint8_t*)rxBuffer, len)) { + send_BAD_CRC_ACK(); - setReceive(); + return; + } - return; + count++; + uint16_t out_buffer_size = rxBuffer[1];// + if (out_buffer_size == 0) { + out_buffer_size = 256; } + address_expected_increment = 128; setTransmit(); + uint8_t read_data[out_buffer_size + 3]; // make buffer 3 larger to fit CRC and ACK + memset(read_data, 0, sizeof(read_data)); + // read_flash((uint8_t*)read_data , address); // make sure read_flash reads two less than buffer. + read_flash_bin((uint8_t*)read_data, address, out_buffer_size); + + const uint16_t crc = crc16(read_data,out_buffer_size); + read_data[out_buffer_size] = crc&0xFF; + read_data[out_buffer_size + 1] = crc>>8; + read_data[out_buffer_size + 2] = 0x30; + sendString(read_data, out_buffer_size+3); - serialwriteChar(0xC1); // bad command message. - invalid_command++; setReceive(); + + return; + } + + setTransmit(); + + serialwriteChar(0xC1); // bad command message. + invalid_command++; + setReceive(); } #ifdef SERIAL_STATS // stats for debugging serial protocol struct { - uint32_t no_idle; - uint32_t no_start; - uint32_t bad_start; - uint32_t bad_stop; - uint32_t good; + uint32_t no_idle; + uint32_t no_start; + uint32_t bad_start; + uint32_t bad_stop; + uint32_t good; } stats; #endif @@ -589,165 +589,165 @@ struct { */ static bool serialreadChar() { - rxbyte=0; - bl_timer_reset(); + rxbyte=0; + bl_timer_reset(); - // UART is idle high, wait for it to be in the idle state - while (!gpio_read(input_pin)) { // wait for rx to go high - if (bl_timer_elapsed() > 20000U) { - /* - if we don't get a command for 20ms then assume we should - be trying to boot the main firmware, invalid_command 101 - triggers the jump immediately - */ - invalid_command = 101; + // UART is idle high, wait for it to be in the idle state + while (!gpio_read(input_pin)) { // wait for rx to go high + if (bl_timer_elapsed() > 20000U) { + /* + if we don't get a command for 20ms then assume we should + be trying to boot the main firmware, invalid_command 101 + triggers the jump immediately + */ + invalid_command = 101; #ifdef SERIAL_STATS - stats.no_idle++; + stats.no_idle++; #endif - return false; - } + return false; } + } - // now we need to wait for the start bit leading edge, which is low - bl_timer_reset(); - while (gpio_read(input_pin)) { - if (bl_timer_elapsed() > 5*BITTIME) { + // now we need to wait for the start bit leading edge, which is low + bl_timer_reset(); + while (gpio_read(input_pin)) { + if (bl_timer_elapsed() > 5*BITTIME) { #if DRONECAN_SUPPORT - if (DroneCAN_update()) { - jump(); - } + if (DroneCAN_update()) { + jump(); + } #endif - if (messagereceived) { - // we've been waiting too long, don't allow for long gaps - // between bytes + if (messagereceived) { + // we've been waiting too long, don't allow for long gaps + // between bytes #ifdef SERIAL_STATS - stats.no_start++; + stats.no_start++; #endif - return false; - } - } + return false; + } } + } - // wait to get the center of bit time. We want to sample at the - // middle of each bit - delayMicroseconds(HALFBITTIME); - if (gpio_read(input_pin)) { - // bad framing, we should be half-way through the start bit - // which should still be low + // wait to get the center of bit time. We want to sample at the + // middle of each bit + delayMicroseconds(HALFBITTIME); + if (gpio_read(input_pin)) { + // bad framing, we should be half-way through the start bit + // which should still be low #ifdef SERIAL_STATS - stats.bad_start++; + stats.bad_start++; #endif - return false; - } - - /* - now sample the 8 data bits - */ - int bits_to_read = 0; - while (bits_to_read < 8) { - delayMicroseconds(BITTIME); - rxbyte = rxbyte | gpio_read(input_pin) << bits_to_read; - bits_to_read++; - } + return false; + } - // wait till middle of stop bit, so we can check that too + /* + now sample the 8 data bits + */ + int bits_to_read = 0; + while (bits_to_read < 8) { delayMicroseconds(BITTIME); - if (!gpio_read(input_pin)) { - // bad framing, stop bit should be high + rxbyte = rxbyte | gpio_read(input_pin) << bits_to_read; + bits_to_read++; + } + + // wait till middle of stop bit, so we can check that too + delayMicroseconds(BITTIME); + if (!gpio_read(input_pin)) { + // bad framing, stop bit should be high #ifdef SERIAL_STATS - stats.bad_stop++; + stats.bad_stop++; #endif - return false; - } + return false; + } - // we got a good byte - messagereceived = 1; - receiveByte = rxbyte; + // we got a good byte + messagereceived = 1; + receiveByte = rxbyte; #ifdef SERIAL_STATS - stats.good++; + stats.good++; #endif - return true; + return true; } static void serialwriteChar(uint8_t data) { - // start bit is low - gpio_clear(input_pin); + // start bit is low + gpio_clear(input_pin); + delayMicroseconds(BITTIME); + + // send data bits + uint8_t bits_written = 0; + while (bits_written < 8) { + if (data & 0x01) { + gpio_set(input_pin); + } else { + // GPIO_BC(input_port) = input_pin; + gpio_clear(input_pin); + } + bits_written++; + data = data >> 1; delayMicroseconds(BITTIME); + } - // send data bits - uint8_t bits_written = 0; - while (bits_written < 8) { - if (data & 0x01) { - gpio_set(input_pin); - } else { - // GPIO_BC(input_port) = input_pin; - gpio_clear(input_pin); - } - bits_written++; - data = data >> 1; - delayMicroseconds(BITTIME); - } - - // send stop bit - gpio_set(input_pin); + // send stop bit + gpio_set(input_pin); - /* - note that we skip the delay by BITTIME for the full stop bit and - do it in sendString() instead to ensure when sending an ACK - immediately followed by a setReceive() on a slow MCU that we - start on the receive as soon as possible. - */ + /* + note that we skip the delay by BITTIME for the full stop bit and + do it in sendString() instead to ensure when sending an ACK + immediately followed by a setReceive() on a slow MCU that we + start on the receive as soon as possible. + */ } static void sendString(const uint8_t *data, int len) { - for(int i = 0; i < len; i++){ - serialwriteChar(data[i]); - // for multi-byte writes we add the stop bit delay - delayMicroseconds(BITTIME); - } + for (int i = 0; i < len; i++) { + serialwriteChar(data[i]); + // for multi-byte writes we add the stop bit delay + delayMicroseconds(BITTIME); + } } static void receiveBuffer() { - count = 0; - messagereceived = 0; - memset(rxBuffer, 0, sizeof(rxBuffer)); + count = 0; + messagereceived = 0; + memset(rxBuffer, 0, sizeof(rxBuffer)); - setReceive(); + setReceive(); + + for (uint32_t i = 0; i < sizeof(rxBuffer); i++) { + if (!serialreadChar()) { + break; + } + + if (incoming_payload_no_command) { + if (count == payload_buffer_size+2) { + break; + } + rxBuffer[i] = rxbyte; + count++; + } else { + if (bl_timer_elapsed() > 250) { + + count = 0; + + break; + } else { + rxBuffer[i] = rxbyte; + if (i == 257) { + invalid_command+=20; // needs one hundred to trigger a jump but will be reset on next set address commmand - for(uint32_t i = 0; i < sizeof(rxBuffer); i++){ - if (!serialreadChar()) { - break; - } - - if(incoming_payload_no_command) { - if(count == payload_buffer_size+2){ - break; - } - rxBuffer[i] = rxbyte; - count++; - } else { - if(bl_timer_elapsed() > 250){ - - count = 0; - - break; - } else { - rxBuffer[i] = rxbyte; - if(i == 257){ - invalid_command+=20; // needs one hundred to trigger a jump but will be reset on next set address commmand - - } - } - } - } - if (messagereceived) { - decodeInput(); + } + } } + } + if (messagereceived) { + decodeInput(); + } } #ifdef UPDATE_EEPROM_ENABLE @@ -789,61 +789,63 @@ static void update_EEPROM() #define pull_down_pin_count_interations 4000 // greater interations extend grace period for input devices booting with signal pin high static void checkForSignal() { - gpio_mode_set_input(input_pin, GPIO_PULL_DOWN); - - delayMicroseconds(500); + gpio_mode_set_input(input_pin, GPIO_PULL_DOWN); - for(int i = 0 ; i < pull_down_pin_count_interations ; i ++){ - if(!gpio_read(input_pin)){ - low_pin_count++; - } + delayMicroseconds(500); + + for (int i = 0 ; i < pull_down_pin_count_interations ; i ++) { + if (!gpio_read(input_pin)) { + low_pin_count++; + } - delayMicroseconds(10); - if (low_pin_count > low_pin_count_threshold) i = pull_down_pin_count_interations ; // end for loop if low_pin_count_threshold has already been exceeded + delayMicroseconds(10); + if (low_pin_count > low_pin_count_threshold) { + i = pull_down_pin_count_interations ; // end for loop if low_pin_count_threshold has already been exceeded } - if (low_pin_count > low_pin_count_threshold) { // pulled low & majority stayed low - jump to application + } + if (low_pin_count > low_pin_count_threshold) { // pulled low & majority stayed low - jump to application #if CHECK_SOFTWARE_RESET - if (!bl_was_software_reset()) { - jump(); - } + if (!bl_was_software_reset()) { + jump(); + } #else - jump(); + jump(); #endif - } + } - gpio_mode_set_input(input_pin, GPIO_PULL_UP); - - delayMicroseconds(500); + gpio_mode_set_input(input_pin, GPIO_PULL_UP); - for (int i = 0 ; i < 500; i++) { - if( !(gpio_read(input_pin))){ - low_pin_count++; - }else{ + delayMicroseconds(500); - } - delayMicroseconds(10); - } - if (low_pin_count == 0) { - return; // pulled high & never low in history - stay in bootloader only - } + for (int i = 0 ; i < 500; i++) { + if ( !(gpio_read(input_pin))) { + low_pin_count++; + } else { - low_pin_count = 0; + } + delayMicroseconds(10); + } + if (low_pin_count == 0) { + return; // pulled high & never low in history - stay in bootloader only + } - gpio_mode_set_input(input_pin, GPIO_PULL_NONE); + low_pin_count = 0; - delayMicroseconds(500); + gpio_mode_set_input(input_pin, GPIO_PULL_NONE); - for (int i = 0 ; i < 500; i ++) { - if( !(gpio_read(input_pin))){ - low_pin_count++; - } + delayMicroseconds(500); - delayMicroseconds(10); + for (int i = 0 ; i < 500; i ++) { + if ( !(gpio_read(input_pin))) { + low_pin_count++; } - if (low_pin_count > 0) { - jump(); // floating & low at least once - jump to application - } + delayMicroseconds(10); + } + + if (low_pin_count > 0) { + jump(); // floating & low at least once - jump to application + } } #ifdef BOOTLOADER_TEST_CLOCK @@ -852,15 +854,15 @@ static void checkForSignal() */ static void test_clock(void) { - setTransmit(); - while (1) { - gpio_clear(input_pin); - bl_timer_reset(); - while (bl_timer_elapsed() < 2000) ; - gpio_set(input_pin); - bl_timer_reset(); - while (bl_timer_elapsed() < 1000) ; - } + setTransmit(); + while (1) { + gpio_clear(input_pin); + bl_timer_reset(); + while (bl_timer_elapsed() < 2000) ; + gpio_set(input_pin); + bl_timer_reset(); + while (bl_timer_elapsed() < 1000) ; + } } #endif // BOOTLOADER_TEST_CLOCK @@ -870,11 +872,11 @@ static void test_clock(void) */ static void test_string(void) { - setTransmit(); - while (1) { - delayMicroseconds(10000); - sendString((uint8_t*)"HELLO_WORLD",11); - } + setTransmit(); + while (1) { + delayMicroseconds(10000); + sendString((uint8_t*)"HELLO_WORLD",11); + } } #endif // BOOTLOADER_TEST_STRING @@ -884,62 +886,62 @@ static void test_string(void) test operation of backup domain registers */ volatile struct { - uint32_t value; - uint32_t fail; + uint32_t value; + uint32_t fail; } bkup; static void test_rtc_backup(void) { - const uint8_t idx = 1; - while (true) { - bkup.value++; - set_rtc_backup_register(idx, bkup.value); - const uint32_t bkup_value2 = get_rtc_backup_register(idx); - if (bkup_value2 != bkup.value) { - bkup.fail++; - } - delayMicroseconds(1000); - } + const uint8_t idx = 1; + while (true) { + bkup.value++; + set_rtc_backup_register(idx, bkup.value); + const uint32_t bkup_value2 = get_rtc_backup_register(idx); + if (bkup_value2 != bkup.value) { + bkup.fail++; + } + delayMicroseconds(1000); + } } #endif int main(void) { - bl_clock_config(); - bl_timer_init(); - bl_gpio_init(); + bl_clock_config(); + bl_timer_init(); + bl_gpio_init(); #ifdef BOOTLOADER_TEST_CLOCK - test_clock(); + test_clock(); #endif #ifdef BOOTLOADER_TEST_STRING - test_string(); + test_string(); #endif #ifdef BOOTLOADER_TEST_BKUP - test_rtc_backup(); + test_rtc_backup(); #endif - checkForSignal(); + checkForSignal(); + + gpio_mode_set_input(input_pin, GPIO_PULL_NONE); - gpio_mode_set_input(input_pin, GPIO_PULL_NONE); - #ifdef USE_ADC_INPUT // go right to application - jump(); + jump(); #endif #ifdef UPDATE_EEPROM_ENABLE - update_EEPROM(); + update_EEPROM(); #endif - while (1) { - receiveBuffer(); - if (invalid_command > 100) { - jump(); - } + while (1) { + receiveBuffer(); + if (invalid_command > 100) { + jump(); + } #if DRONECAN_SUPPORT - if (DroneCAN_update()) { - jump(); - } -#endif + if (DroneCAN_update()) { + jump(); } +#endif + } } diff --git a/tools/CodeStyle/am32-astyle.sh b/tools/CodeStyle/am32-astyle.sh new file mode 100755 index 00000000..d2611682 --- /dev/null +++ b/tools/CodeStyle/am32-astyle.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env bash +# script for recormatting C code +# see https://astyle.sourceforge.net/astyle.html + +if [ -z "$1" ]; then + printf "\nArguments missing, Usage: '$0 '\n\n" + exit 1 +fi + +if [ $(uname) = "Darwin" ]; then + DIR=$(dirname $(greadlink -f $0)) +else + DIR=$(dirname $(readlink -f $0)) +fi + +astyle --options="${DIR}"/astylerc $* diff --git a/tools/CodeStyle/astylerc b/tools/CodeStyle/astylerc new file mode 100644 index 00000000..667b5501 --- /dev/null +++ b/tools/CodeStyle/astylerc @@ -0,0 +1,9 @@ +style=linux +keep-one-line-statements +add-braces +indent=spaces=2 +indent-col1-comments +min-conditional-indent=0 +suffix=none +lineend=linux +pad-header