Added XMODEM update

pull/455/head
Daniele Lacamera 2024-06-12 16:07:52 +02:00 committed by David Garske
parent aff2072694
commit 9d563ae60f
6 changed files with 406 additions and 19 deletions

View File

@ -366,10 +366,23 @@ static void clock_pll_on(void)
}
#if (TZ_SECURE())
#define NVIC_ISER_BASE (0xE000E100)
#define NVIC_ICER_BASE (0xE000E180)
#define NVIC_IPRI_BASE (0xE000E400)
#define NVIC_USART3_IRQ 60
/* Cortex M-33 has an extra register to set up non-secure interrupts */
#define NVIC_ITNS_BASE (0xE000E380)
static void periph_unsecure(void)
{
uint32_t pin;
volatile uint32_t reg;
volatile uint32_t *nvic_itns;
uint32_t nvic_reg_pos, nvic_reg_off;
/*Enable clock for User LED GPIOs */
RCC_AHB2_CLOCK_ER|= LED_AHB2_ENABLE;
@ -412,6 +425,13 @@ static void periph_unsecure(void)
TZSC_SECCFGR1 = reg;
}
/* Set USART3 interrupt as non-secure */
nvic_reg_pos = NVIC_USART3_IRQ / 32;
nvic_reg_off = NVIC_USART3_IRQ % 32;
nvic_itns = ((volatile uint32_t *)(NVIC_ITNS_BASE + 4 * nvic_reg_pos));
*nvic_itns |= (1 << nvic_reg_off);
/* Disable GPIOs clock used previously for accessing SECCFGR registers */
#if 0
RCC_AHB2_CLOCK_ER &= ~GPIOA_AHB2_CLOCK_ER;

View File

@ -23,7 +23,9 @@
#ifndef STM32H5_DEF_INCLUDED
#define STM32H5_DEF_INCLUDED
/* Assembly helpers */
#ifndef DMB
#define DMB() __asm__ volatile ("dmb")
#endif
#define ISB() __asm__ volatile ("isb")
#define DSB() __asm__ volatile ("dsb")
@ -409,8 +411,14 @@
#define UART_CR3_HDSEL (1 << 3)
#define UART_CR3_DEM (1 << 14)
#define UART_CR3_IREN (1 << 1)
#define UART_CR3_RXFTIE (1 << 28)
#define UART_ISR_TX_EMPTY (1 << 7)
#define UART_ISR_RX_NOTEMPTY (1 << 5)
#define UART_EPE (1 << 0) /* Parity error */
#define UART_EFE (1 << 1) /* Framing error */
#define UART_ENE (1 << 2) /* Noise error */
#define UART_ORE (1 << 3) /* Overrun error */
/* OTP FLASH AREA */

View File

@ -141,11 +141,18 @@ static int uart1_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
return 0;
}
static void uart1_clear_errors(void)
{
UART1_ICR = UART1_ISR & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
}
static int uart1_tx(const uint8_t c)
{
volatile uint32_t reg;
do {
reg = UART1_ISR;
if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
uart1_clear_errors();
} while ((reg & UART_ISR_TX_EMPTY) == 0);
UART1_TDR = c;
return 1;
@ -156,6 +163,8 @@ static int uart1_rx(uint8_t *c)
volatile uint32_t reg;
int i = 0;
reg = UART1_ISR;
if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
uart1_clear_errors();
if (reg & UART_ISR_RX_NOTEMPTY) {
*c = (uint8_t)UART1_RDR;
return 1;
@ -209,11 +218,18 @@ static int uart3_init(uint32_t bitrate, uint8_t data, char parity, uint8_t stop)
return 0;
}
static void uart3_clear_errors(void)
{
UART3_ICR = UART3_ISR & (UART_ENE | UART_EPE | UART_ORE | UART_EFE);
}
static int uart3_tx(const uint8_t c)
{
volatile uint32_t reg;
do {
reg = UART3_ISR;
if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
uart3_clear_errors();
} while ((reg & UART_ISR_TX_EMPTY) == 0);
UART3_TDR = c;
return 1;
@ -224,6 +240,8 @@ static int uart3_rx(uint8_t *c)
volatile uint32_t reg;
int i = 0;
reg = UART3_ISR;
if (reg & (UART_ENE | UART_EPE | UART_ORE | UART_EFE))
uart3_clear_errors();
if (reg & UART_ISR_RX_NOTEMPTY) {
*c = (uint8_t)UART3_RDR;
return 1;

View File

@ -28,6 +28,7 @@
#include <sys/stat.h>
#include "system.h"
#include "hal.h"
#include "hal/stm32h5.h"
#include "uart_drv.h"
#include "wolfboot/wolfboot.h"
#include "wolfcrypt/benchmark/benchmark.h"
@ -45,10 +46,19 @@ extern const CK_FUNCTION_LIST wolfpkcs11nsFunctionList;
volatile unsigned int jiffies = 0;
/* Usart irq-based read function */
static uint8_t uart_buf_rx[256];
static uint32_t uart_rx_bytes = 0;
static uint32_t uart_processed = 0;
static int uart_rx_isr(unsigned char *c, int len);
static int uart_poll(void);
#define LED_BOOT_PIN (4) /* PG4 - Nucleo - Red Led */
#define LED_USR_PIN (0) /* PB0 - Nucleo - Green Led */
#define LED_EXTRA_PIN (4) /* PF4 - Nucleo - Orange Led */
#define NVIC_USART3_IRQN (60)
/*Non-Secure */
#define RCC_BASE (0x44020C00) /* RM0481 - Table 3 */
#define GPIOG_BASE 0x42021800
@ -176,6 +186,9 @@ static int cmd_random(const char *args);
static int cmd_benchmark(const char *args);
static int cmd_test(const char *args);
static int cmd_timestamp(const char *args);
static int cmd_update(const char *args);
static int cmd_update_xmodem(const char *args);
static int cmd_reboot(const char *args);
@ -201,9 +214,165 @@ struct console_command COMMANDS[] =
{ cmd_timestamp, "timestamp", "print the current timestamp"},
{ cmd_benchmark, "benchmark", "run the wolfCrypt benchmark"},
{ cmd_test, "test", "run the wolfCrypt test"},
{ cmd_update_xmodem, "update", "update the firmware via XMODEM"},
{ cmd_reboot, "reboot", "reboot the system"},
{ NULL, "", ""}
};
#define AIRCR *(volatile uint32_t *)(0xE000ED0C)
#define AIRCR_VKEY (0x05FA << 16)
# define AIRCR_SYSRESETREQ (1 << 2)
int cmd_reboot(const char *args)
{
(void)args;
AIRCR = AIRCR_SYSRESETREQ | AIRCR_VKEY;
while(1)
asm volatile("wfi");
return 0; /* Never happens */
}
#define XSOH 0x01
#define XEOT 0x04
#define XACK 0x06
#define XNAK 0x15
#define XCAN 0x18
static uint8_t crc8(uint8_t *data, size_t len)
{
uint8_t checksum = 0;
for (int i = 0; i < len; i++) {
checksum += data[i];
}
return checksum;
}
#define XMODEM_PAYLOAD_SIZE 128
#define XMODEM_PACKET_SIZE (3 + XMODEM_PAYLOAD_SIZE + 1)
#define XMODEM_TIMEOUT 1000 /* milliseconds */
static void xcancel(void)
{
int i;
for (i = 0; i < 10; i++)
uart_tx(XCAN);
}
static uint8_t xpkt_payload[XMODEM_PAYLOAD_SIZE];
static int cmd_update_xmodem(const char *args)
{
int ret = -1;
uint8_t xpkt[XMODEM_PACKET_SIZE];
uint32_t dst_flash = (uint32_t)WOLFBOOT_PARTITION_UPDATE_ADDRESS;
uint8_t pkt_num = 0, pkt_num_expected=0xFF;
uint32_t pkt_size = XMODEM_PACKET_SIZE;
uint32_t update_ver = 0;
uint32_t now = jiffies;
uint32_t i = 0;
uint8_t pkt_num_inv;
uint8_t crc, calc_crc;
printf("Erasing update partition...");
fflush(stdout);
hal_flash_unlock();
//hal_flash_erase(dst_flash, WOLFBOOT_PARTITION_SIZE);
printf("Done.\r\n");
printf("Waiting for XMODEM transfer...\r\n");
while (1) {
now = jiffies;
i = 0;
while(i < XMODEM_PACKET_SIZE) {
ret = uart_rx_isr(&xpkt[i], XMODEM_PACKET_SIZE - i);
if (ret == 0) {
if(jiffies > (now + XMODEM_TIMEOUT)) {
now = jiffies;
if (i == 0)
uart_tx(XNAK);
i = 0;
} else {
asm volatile("wfi");
}
} else {
now = jiffies;
i += ret;
}
}
if (xpkt[0] == XEOT) {
ret = 0;
uart_tx(XACK);
extra_led_on();
break;
}
if (xpkt[0] != XSOH) {
continue;
}
pkt_num = xpkt[1];
pkt_num_inv = ~xpkt[2];
if (pkt_num == pkt_num_inv) {
if (pkt_num_expected == 0xFF) /* re-sync */
(pkt_num_expected = pkt_num);
else if (pkt_num_expected != pkt_num) {
uart_tx(XNAK);
continue;
}
if ((pkt_num / 0x10) & 0x01)
extra_led_on();
else
extra_led_off();
/* Packet is valid */
crc = xpkt[XMODEM_PACKET_SIZE - 1];
calc_crc = crc8(xpkt, XMODEM_PACKET_SIZE - 1);
if (crc == calc_crc) {
uint32_t t_size;
/* CRC is valid */
memcpy(xpkt_payload, xpkt + 3, XMODEM_PAYLOAD_SIZE);
ret = hal_flash_write(dst_flash, xpkt_payload, XMODEM_PAYLOAD_SIZE);
if (ret != 0) {
xcancel();
printf("Error writing to flash\r\n");
break;
}
uart_tx(XACK);
pkt_num++;
pkt_num_expected++;
dst_flash += XMODEM_PAYLOAD_SIZE;
t_size = *((uint32_t *)(WOLFBOOT_PARTITION_UPDATE_ADDRESS + 4));
if ((uint32_t)dst_flash >= (WOLFBOOT_PARTITION_UPDATE_ADDRESS + t_size)) {
ret = 0;
extra_led_off();
break;
}
uart_tx(XACK);
} else {
uart_tx(XNAK);
}
} else {
uart_tx(XNAK); /* invalid packet number received */
}
}
for (i = 0; i < 10; i++)
uart_tx('\r');
printf("End of transfer. ret: %d\r\n", ret);
update_ver = wolfBoot_update_firmware_version();
if (update_ver != 0) {
printf("New firmware version: 0x%lx\r\n", update_ver);
} else {
printf("No valid image in update partition\r\n");
}
hal_flash_lock();
return ret;
}
static int cmd_help(const char *args)
{
int i;
@ -231,6 +400,12 @@ static int cmd_info(const char *args)
printf("Firmware version : 0x%lx\r\n", wolfBoot_current_firmware_version());
if (update_fw_version != 0) {
printf("Candidate firmware version : 0x%lx\r\n", update_fw_version);
if (update_fw_version > cur_fw_version) {
wolfBoot_update_trigger();
printf("'reboot' to initiate update.\r\n");
} else {
printf("Update image older than current.\r\n");
}
} else {
printf("No image in update partition.\r\n");
}
@ -443,7 +618,7 @@ static void console_loop(void)
fflush(stdout);
idx = 0;
do {
ret = uart_rx((uint8_t *)&c);
ret = uart_rx_isr((uint8_t *)&c, 1);
if (ret > 0) {
if (c == '\r')
break;
@ -453,20 +628,51 @@ static void console_loop(void)
if (idx > 0) {
cmd[idx] = 0;
if (parse_cmd(cmd) == -2) {
printf("Unknown command\r\n");
printf("Unknown command: %s\r\n", cmd);
}
}
}
}
void isr_usart3(void)
{
volatile uint32_t reg;
usr_led_on();
reg = UART3_ISR;
if (reg & UART_ISR_RX_NOTEMPTY) {
if (uart_rx_bytes >= 255)
reg = UART3_RDR;
else
uart_buf_rx[uart_rx_bytes++] = (unsigned char)(UART3_RDR & 0xFF);
}
}
static int uart_rx_isr(unsigned char *c, int len)
{
UART3_CR1 &= ~UART_ISR_RX_NOTEMPTY;
if (len > (uart_rx_bytes - uart_processed))
len = (uart_rx_bytes - uart_processed);
if (len > 0) {
memcpy(c, uart_buf_rx + uart_processed, len);
uart_processed += len;
if (uart_processed >= uart_rx_bytes) {
uart_processed = 0;
uart_rx_bytes = 0;
usr_led_off();
}
}
UART3_CR1 |= UART_ISR_RX_NOTEMPTY;
return len;
}
static int uart_poll(void)
{
return (uart_rx_bytes > uart_processed)?1:0;
}
void main(void)
{
int ret;
uint32_t rand;
uint32_t i;
uint32_t klen = 200;
int otherkey_slot;
uint32_t app_version;
@ -478,7 +684,15 @@ void main(void)
app_version = wolfBoot_current_firmware_version();
nvic_irq_setprio(NVIC_USART3_IRQN, 0);
nvic_irq_enable(NVIC_USART3_IRQN);
uart_init(115200, 8, 'N', 1);
UART3_CR1 |= UART_ISR_RX_NOTEMPTY;
UART3_CR3 |= UART_CR3_RXFTIE;
printf("========================\r\n");
printf("STM32H5 wolfBoot demo Application\r\n");
printf("Copyright 2024 wolfSSL Inc\r\n");
@ -523,18 +737,7 @@ __attribute__((weak)) int _read(int file, char *ptr, int len)
(void)file;
int DataIdx;
int ret;
for (DataIdx = 0; DataIdx < len; DataIdx++)
{
do {
ret = uart_rx((uint8_t *)ptr);
if (ret > 0)
ptr++;
} while (ret == 0);
if (ret == 0)
break;
}
return DataIdx;
return -1;
}
int _write(int file, char *ptr, int len)

View File

@ -33,6 +33,10 @@ extern unsigned int _start_heap;
extern void isr_tim2(void);
#endif
#ifdef PLATFORM_stm32h5
extern void isr_usart3(void);
#endif
#ifdef APP_HAS_SYSTICK
extern void isr_systick(void);
#endif
@ -294,6 +298,139 @@ void (* const IV[])(void) =
isr_empty, // UCPD1_IRQHandler
isr_empty, // ICACHE_IRQHandler
isr_empty, // OTFDEC1_IRQHandler
//
#elif defined(PLATFORM_stm32h5)
isr_empty, // WWDG_IRQHandler
isr_empty, // PVD_PVM_IRQHandler
isr_empty, // RTC_IRQHandler
isr_empty, // RTC_S_IRQHandler
isr_empty, // TAMP_IRQHandler
isr_empty, // RAMCFG_IRQHandler
isr_empty, // FLASH_IRQHandler
isr_empty, // FLASH_S_IRQHandler
isr_empty, // GTZC_IRQHandler
isr_empty, // RCC_IRQHandler
isr_empty, // RCC_S_IRQHandler
isr_empty, // EXTI0_IRQHandler
isr_empty, // EXTI1_IRQHandler
isr_empty, // EXTI2_IRQHandler
isr_empty, // EXTI3_IRQHandler
isr_empty, // EXTI4_IRQHandler
isr_empty, // EXTI5_IRQHandler
isr_empty, // EXTI6_IRQHandler
isr_empty, // EXTI7_IRQHandler
isr_empty, // EXTI8_IRQHandler
isr_empty, // EXTI9_IRQHandler
isr_empty, // EXTI10_IRQHandler
isr_empty, // EXTI11_IRQHandler
isr_empty, // EXTI12_IRQHandler
isr_empty, // EXTI13_IRQHandler
isr_empty, // EXTI14_IRQHandler
isr_empty, // EXTI15_IRQHandler
isr_empty, // GPDMA1CH0_IRQHandler
isr_empty, // GPDMA1CH1_IRQHandler
isr_empty, // GPDMA1CH2_IRQHandler
isr_empty, // GPDMA1CH3_IRQHandler
isr_empty, // GPDMA1CH4_IRQHandler
isr_empty, // GPDMA1CH5_IRQHandler
isr_empty, // GPDMA1CH6_IRQHandler
isr_empty, // GPDMA1CH7_IRQHandler
isr_empty, // IWDG_IRQHandler
isr_empty, // SAES_IRQHandler
isr_empty, // ADC1_IRQHandler
isr_empty, // DAC1_IRQHandler
isr_empty, // FDCAN1_IT0_IRQHandler
isr_empty, // FDCAN1_IT1_IRQHandler
isr_empty, // TIM1_BRK_IRQHandler
isr_empty, // TIM1_UP_IRQHandler
isr_empty, // TIM1_TRG_COM_IRQHandler
isr_empty, // TIM1_CC_IRQHandler
isr_empty, // TIM2_IRQHandler
isr_empty, // TIM3_IRQHandler
isr_empty, // TIM4_IRQHandler
isr_empty, // TIM5_IRQHandler
isr_empty, // TIM6_IRQHandler
isr_empty, // TIM7_IRQHandler
isr_empty, // I2C1_EV_IRQHandler
isr_empty, // I2C1_ER_IRQHandler
isr_empty, // I2C2_EV_IRQHandler
isr_empty, // I2C2_ER_IRQHandler
isr_empty, // SPI1_IRQHandler
isr_empty, // SPI2_IRQHandler
isr_empty, // SPI3_IRQHandler
isr_empty, // USART1_IRQHandler
isr_empty, // USART2_IRQHandler
isr_usart3, // USART3_IRQHandler
isr_empty, // UART4_IRQHandler
isr_empty, // UART5_IRQHandler
isr_empty, // LPUART1_IRQHandler
isr_empty, // LPTIM1_IRQHandler
isr_empty, // TIM8_BRK_IRQHandler
isr_empty, // TIM8_UP_IRQHandler
isr_empty, // TIM8_TRG_COM_IRQHandler
isr_empty, // TIM8_CC_IRQHandler
isr_empty, // ADC2_IRQHandler
isr_empty, // LPTIM2_IRQHandler
isr_empty, // TIM15_IRQHandler
isr_empty, // TIM16_IRQHandler
isr_empty, // TIM17_IRQHandler
isr_empty, // USB_FS_IRQHandler
isr_empty, // CRS_IRQHandler
isr_empty, // UCPD1_IRQHandler
isr_empty, // FMC_IRQHandler
isr_empty, // OCTOSPI1_IRQHandler
isr_empty, // SDMMC1_IRQHandler
isr_empty, // I2C3_EV_IRQHandler
isr_empty, // I2C3_ER_IRQHandler
isr_empty, // SPI4_IRQHandler
isr_empty, // SPI5_IRQHandler
isr_empty, // SPI6_IRQHandler
isr_empty, // USART6_IRQHandler
isr_empty, // USART10_IRQHandler
isr_empty, // USART11_IRQHandler
isr_empty, // SAI1_IRQHandler
isr_empty, // SAI2_IRQHandler
isr_empty, // GPDMA2CH0_IRQHandler
isr_empty, // GPDMA2CH1_IRQHandler
isr_empty, // GPDMA2CH2_IRQHandler
isr_empty, // GPDMA2CH3_IRQHandler
isr_empty, // GPDMA2CH4_IRQHandler
isr_empty, // GPDMA2CH5_IRQHandler
isr_empty, // GPDMA2CH6_IRQHandler
isr_empty, // GPDMA2CH7_IRQHandler
isr_empty, // UART7_IRQHandler
isr_empty, // UART8_IRQHandler
isr_empty, // UART9_IRQHandler
isr_empty, // UART12_IRQHandler
isr_empty, // SDMMC2_IRQHandler
isr_empty, // FPU_IRQHandler
isr_empty, // ICACHE_IRQHandler
isr_empty, // DCACHE_IRQHandler
isr_empty, // ETH1_IRQHandler
isr_empty, // DCMI_PSSI_IRQHandler
isr_empty, // FDCAN2_IT0_IRQHandler
isr_empty, // FDCAN2_IT1_IRQHandler
isr_empty, // CORDIC_IRQHandler
isr_empty, // FMAC_IRQHandler
isr_empty, // DTS_IRQHandler
isr_empty, // RNG_IRQHandler
isr_empty, // OTFDEC1_IRQHandler
isr_empty, // AES_IRQHandler
isr_empty, // HASH_IRQHandler
isr_empty, // PKA_IRQHandler
isr_empty, // CEC_IRQHandler
isr_empty, // TIM12_IRQHandler
isr_empty, // TIM13_IRQHandler
isr_empty, // TIM14_IRQHandler
isr_empty, // I3C1_EV_IRQHandler
isr_empty, // I3C1_ER_IRQHandler
isr_empty, // I2C4_EV_IRQHandler
isr_empty, // I2C4_ER_IRQHandler
isr_empty, // LPTIM3_IRQHandler
isr_empty, // LPTIM4_IRQHandler
isr_empty, // LPTIM5_IRQHandler
isr_empty, // LPTIM6_IRQHandler
#elif defined(STM32) /* For STM32 */
isr_empty, // NVIC_WWDG_IRQ 0
isr_empty, // PVD_IRQ 1

View File

@ -28,12 +28,13 @@
#define PLL_FULL_MASK (0x7F037FFF)
/* Assembly helpers */
#ifndef DMB
#define DMB() asm volatile ("dmb");
#endif
#define WFI() asm volatile ("wfi");
/* Master clock setting */
void clock_config(void);
void flash_set_waitstates(void);
/* NVIC */