diff --git a/.gitignore b/.gitignore index c88e451a..b1d50f6f 100644 --- a/.gitignore +++ b/.gitignore @@ -35,6 +35,7 @@ *.i*86 *.x86_64 *.hex +*.rom # Debug files *.dSYM/ diff --git a/Makefile b/Makefile index 075484e8..8819d2f4 100644 --- a/Makefile +++ b/Makefile @@ -4,7 +4,7 @@ LD:=$(CROSS_COMPILE)gcc AS:=$(CROSS_COMPILE)gcc OBJCOPY:=$(CROSS_COMPILE)objcopy SIZE:=$(CROSS_COMPILE)size -BOOT0_OFFSET?=`cat include/target.h |grep WOLFBOOT_PARTITION_BOOT_ADDRESS | sed -e "s/.*[ ]//g"` +BOOT0_OFFSET?=`cat include/target.h |grep WOLFBOOT_PARTITION_BOOT_ADDRESS | head -1 | sed -e "s/.*[ ]//g"` BOOT_IMG?=test-app/image.bin SIGN?=ED25519 TARGET?=stm32f4 @@ -17,6 +17,7 @@ SWAP?=1 CORTEX_M0?=0 NO_ASM?=0 EXT_FLASH?=0 +SPI_FLASH?=0 ALLOW_DOWNGRADE?=0 NVM_FLASH_WRITEONCE?=0 LSCRIPT:=hal/$(TARGET).ld @@ -71,7 +72,7 @@ ifeq ($(FASTMATH),1) endif CFLAGS+=-mthumb -Wall -Wextra -Wno-main -Wstack-usage=1024 -ffreestanding -Wno-unused \ - -Ilib/bootutil/include -Iinclude/ -Ilib/wolfssl -nostartfiles \ + -I. -Ilib/bootutil/include -Iinclude/ -Ilib/wolfssl -nostartfiles \ -DWOLFSSL_USER_SETTINGS \ -mthumb -mlittle-endian -mthumb-interwork \ -DPLATFORM_$(TARGET) @@ -81,10 +82,17 @@ ifeq ($(TARGET),kinetis) OBJS+= $(KINETIS_DRIVERS)/drivers/fsl_clock.o $(KINETIS_DRIVERS)/drivers/fsl_ftfx_flash.o $(KINETIS_DRIVERS)/drivers/fsl_ftfx_cache.o $(KINETIS_DRIVERS)/drivers/fsl_ftfx_controller.o endif +ifeq ($(SPI_FLASH),1) + EXT_FLASH=1 + CFLAGS+= -DSPI_FLASH=1 + OBJS+= src/spi_flash.o hal/spi/spi_drv_$(TARGET).o +endif + ifeq ($(EXT_FLASH),1) CFLAGS+= -DEXT_FLASH=1 -DPART_UPDATE_EXT=1 -DPART_SWAP_EXT=1 endif + ifeq ($(ALLOW_DOWNGRADE),1) CFLAGS+= -DALLOW_DOWNGRADE endif diff --git a/hal/spi/spi_drv_stm32f4.c b/hal/spi/spi_drv_stm32f4.c new file mode 100644 index 00000000..c527e0d9 --- /dev/null +++ b/hal/spi/spi_drv_stm32f4.c @@ -0,0 +1,158 @@ +/* spi_drv.h + * + * Driver for the SPI back-end of the SPI_FLASH module. + * + * Example implementation for stm32F4, using SPI1. + * + * Pinout: see spi_drv_stm32f4.h + * + * Copyright (C) 2018 wolfSSL Inc. + * + * This file is part of wolfBoot. + * + * wolfBoot is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * wolfBoot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA + */ +#include +#include "spi_drv.h" +#include "spi_drv_stm32f4.h" + +void spi_cs_off(void) +{ + int i; + GPIOE_BSRR |= (1 << SPI_FLASH_PIN); + while(!(GPIOE_ODR & (1 << SPI_FLASH_PIN))) + ; + for(i = 0; i < 168000; i++) + ; +} + +void spi_cs_on(void) +{ + GPIOE_BSRR |= (1 << (SPI_FLASH_PIN + 16)); + while(GPIOE_ODR & (1 << SPI_FLASH_PIN)) + ; +} + + +static void spi_flash_pin_setup(void) +{ + uint32_t reg; + AHB1_CLOCK_ER |= GPIOE_AHB1_CLOCK_ER; + reg = GPIOE_MODE & ~ (0x03 << (SPI_FLASH_PIN * 2)); + GPIOE_MODE = reg | (1 << (SPI_FLASH_PIN * 2)); + + reg = GPIOE_PUPD & ~(0x03 << (SPI_FLASH_PIN * 2)); + GPIOE_PUPD = reg | (0x01 << (SPI_FLASH_PIN * 2)); + + reg = GPIOE_OSPD & ~(0x03 << (SPI_FLASH_PIN * 2)); + GPIOE_OSPD |= (0x03 << (SPI_FLASH_PIN * 2)); + +} + +static void spi1_pins_setup(void) +{ + uint32_t reg; + AHB1_CLOCK_ER |= GPIOB_AHB1_CLOCK_ER; + /* Set mode = AF */ + reg = GPIOB_MODE & ~ (0x03 << (SPI1_CLOCK_PIN * 2)); + GPIOB_MODE = reg | (2 << (SPI1_CLOCK_PIN * 2)); + reg = GPIOB_MODE & ~ (0x03 << (SPI1_MOSI_PIN * 2)); + GPIOB_MODE = reg | (2 << (SPI1_MOSI_PIN * 2)); + reg = GPIOB_MODE & ~ (0x03 << (SPI1_MISO_PIN * 2)); + GPIOB_MODE = reg | (2 << (SPI1_MISO_PIN * 2)); + + /* Alternate function: use low pins (5,6,7) */ + reg = GPIOB_AFL & ~(0xf << ((SPI1_CLOCK_PIN) * 4)); + GPIOB_AFL = reg | (SPI1_PIN_AF << ((SPI1_CLOCK_PIN) * 4)); + reg = GPIOB_AFL & ~(0xf << ((SPI1_MOSI_PIN) * 4)); + GPIOB_AFL = reg | (SPI1_PIN_AF << ((SPI1_MOSI_PIN) * 4)); + reg = GPIOB_AFL & ~(0xf << ((SPI1_MISO_PIN) * 4)); + GPIOB_AFL = reg | (SPI1_PIN_AF << ((SPI1_MISO_PIN) * 4)); +} + +static void spi_pins_release(void) +{ + uint32_t reg; + /* Set mode = 0 */ + GPIOB_MODE &= ~ (0x03 << (SPI1_CLOCK_PIN * 2)); + GPIOB_MODE &= ~ (0x03 << (SPI1_MOSI_PIN * 2)); + GPIOB_MODE &= ~ (0x03 << (SPI1_MISO_PIN * 2)); + + /* Alternate function clear */ + GPIOB_AFL &= ~(0xf << ((SPI1_CLOCK_PIN) * 4)); + GPIOB_AFL &= ~(0xf << ((SPI1_MOSI_PIN) * 4)); + GPIOB_AFL &= ~(0xf << ((SPI1_MISO_PIN) * 4)); + + /* Floating */ + GPIOB_PUPD &= ~ (0x03 << (SPI1_CLOCK_PIN * 2)); + GPIOB_PUPD &= ~ (0x03 << (SPI1_MOSI_PIN * 2)); + GPIOB_PUPD &= ~ (0x03 << (SPI1_MISO_PIN * 2)); + + /* Release CS */ + GPIOE_MODE &= ~ (0x03 << (SPI_FLASH_PIN * 2)); + GPIOE_PUPD &= ~ (0x03 << (SPI_FLASH_PIN * 2)); + + /* Disable GPIOB+GPIOE clock */ + AHB1_CLOCK_ER &= ~(GPIOB_AHB1_CLOCK_ER | GPIOE_AHB1_CLOCK_ER); +} + +static void spi1_reset(void) +{ + APB2_CLOCK_RST |= SPI1_APB2_CLOCK_ER_VAL; + APB2_CLOCK_RST &= ~SPI1_APB2_CLOCK_ER_VAL; +} + +uint8_t spi_read(void) +{ + volatile uint32_t reg; + do { + reg = SPI1_SR; + } while(!(reg & SPI_SR_RX_NOTEMPTY)); + return (uint8_t)SPI1_DR; +} + +void spi_write(const char byte) +{ + int i; + volatile uint32_t reg; + do { + reg = SPI1_SR; + } while ((reg & SPI_SR_TX_EMPTY) == 0); + SPI1_DR = byte; + do { + reg = SPI1_SR; + } while ((reg & SPI_SR_TX_EMPTY) == 0); +} + + +void spi_init(int polarity, int phase) +{ + spi1_pins_setup(); + spi_flash_pin_setup(); + APB2_CLOCK_ER |= SPI1_APB2_CLOCK_ER_VAL; + spi1_reset(); + SPI1_CR1 = SPI_CR1_MASTER | (5 << 3) | (polarity << 1) | (phase << 0); + SPI1_CR2 |= SPI_CR2_SSOE; + SPI1_CR1 |= SPI_CR1_SPI_EN; +} + +void spi_release(void) +{ + spi1_reset(); + SPI1_CR2 &= ~SPI_CR2_SSOE; + SPI1_CR1 = 0; + spi_pins_release(); +} + diff --git a/hal/spi/spi_drv_stm32f4.h b/hal/spi/spi_drv_stm32f4.h new file mode 100644 index 00000000..b65f35f4 --- /dev/null +++ b/hal/spi/spi_drv_stm32f4.h @@ -0,0 +1,67 @@ +#ifndef SPI_DRV_STM32_H_INCLUDED +#define SPI_DRV_STM32_H_INCLUDED +#include +/** SPI settings **/ + +#define SPI1 (0x40013000)/* SPI1 base address */ +#define SPI_FLASH_PIN 1 /* Flash CS connected to GPIOE1 */ +#define SPI1_PIN_AF 5 /* Alternate function for SPI pins */ +#define SPI1_CLOCK_PIN 3 /* SPI_SCK: PB3 */ +#define SPI1_MISO_PIN 4 /* SPI_MISO PB4 */ +#define SPI1_MOSI_PIN 5 /* SPI_MOSI PB5 */ + +#define SPI1_CR1 (*(volatile uint32_t *)(SPI1)) +#define SPI1_CR2 (*(volatile uint32_t *)(SPI1 + 0x04)) +#define SPI1_SR (*(volatile uint32_t *)(SPI1 + 0x08)) +#define SPI1_DR (*(volatile uint32_t *)(SPI1 + 0x0c)) + +#define SPI_CR1_CLOCK_PHASE (1 << 0) +#define SPI_CR1_CLOCK_POLARITY (1 << 1) +#define SPI_CR1_MASTER (1 << 2) +#define SPI_CR1_BAUDRATE (0x07 << 3) +#define SPI_CR1_SPI_EN (1 << 6) +#define SPI_CR1_LSBFIRST (1 << 7) +#define SPI_CR1_SSI (1 << 8) +#define SPI_CR1_SSM (1 << 9) +#define SPI_CR1_16BIT_FORMAT (1 << 11) +#define SPI_CR1_TX_CRC_NEXT (1 << 12) +#define SPI_CR1_HW_CRC_EN (1 << 13) +#define SPI_CR1_BIDIOE (1 << 14) +#define SPI_CR2_SSOE (1 << 2) + + +#define SPI_SR_RX_NOTEMPTY (1 << 0) +#define SPI_SR_TX_EMPTY (1 << 1) +#define SPI_SR_BUSY (1 << 7) + +#define APB2_CLOCK_ER (*(volatile uint32_t *)(0x40023844)) +#define APB2_CLOCK_RST (*(volatile uint32_t *)(0x40023824)) +#define SPI1_APB2_CLOCK_ER_VAL (1 << 12) + + +#define CLOCK_SPEED (168000000) + + +#define AHB1_CLOCK_ER (*(volatile uint32_t *)(0x40023830)) +#define GPIOB_AHB1_CLOCK_ER (1 << 1) +#define GPIOE_AHB1_CLOCK_ER (1 << 4) +#define GPIOB_BASE 0x40020400 +#define GPIOE_BASE 0x40021000 + +#define GPIOB_MODE (*(volatile uint32_t *)(GPIOB_BASE + 0x00)) +#define GPIOB_AFL (*(volatile uint32_t *)(GPIOB_BASE + 0x20)) +#define GPIOB_AFH (*(volatile uint32_t *)(GPIOB_BASE + 0x24)) +#define GPIOB_OSPD (*(volatile uint32_t *)(GPIOB_BASE + 0x08)) +#define GPIOB_PUPD (*(volatile uint32_t *)(GPIOB_BASE + 0x0c)) +#define GPIOB_BSRR (*(volatile uint32_t *)(GPIOB_BASE + 0x18)) +#define GPIOE_MODE (*(volatile uint32_t *)(GPIOE_BASE + 0x00)) +#define GPIOE_AFL (*(volatile uint32_t *)(GPIOE_BASE + 0x20)) +#define GPIOE_AFH (*(volatile uint32_t *)(GPIOE_BASE + 0x24)) +#define GPIOE_OSPD (*(volatile uint32_t *)(GPIOE_BASE + 0x08)) +#define GPIOE_PUPD (*(volatile uint32_t *)(GPIOE_BASE + 0x0c)) +#define GPIOE_BSRR (*(volatile uint32_t *)(GPIOE_BASE + 0x18)) +#define GPIOE_ODR (*(volatile uint32_t *)(GPIOE_BASE + 0x14)) +#define GPIO_MODE_AF (2) + + +#endif diff --git a/hal/stm32f4.c b/hal/stm32f4.c index 09cb835d..940bb10d 100644 --- a/hal/stm32f4.c +++ b/hal/stm32f4.c @@ -326,81 +326,10 @@ void hal_init(void) void hal_prepare_boot(void) { +#ifdef SPI_FLASH + spi_release(); +#endif + clock_pll_off(); } - -#ifdef EXT_FLASH -#define SIM_ADDRESS (0x40000) - -int ext_flash_read(uint32_t address, uint8_t *data, int len) -{ - int i; - uint32_t val; - address += SIM_ADDRESS; - for (i = 0; i < len; i++) - data[i] = *((uint8_t *)(address + i)); - return len; -} - -int ext_flash_write(uint32_t address, const uint8_t *data, int len) -{ - int i; - uint32_t val; - address += SIM_ADDRESS; - flash_wait_complete(); - clear_errors(); - /* Set 8-bit write */ - FLASH_CR &= (~(0x03 << 8)); - for (i = 0; i < len; i++) { - FLASH_CR |= FLASH_CR_PG; - *((uint8_t *)(address + i)) = data[i]; - flash_wait_complete(); - FLASH_CR &= ~FLASH_CR_PG; - } - return 0; -} - -void ext_flash_unlock(void) -{ - FLASH_CR |= FLASH_CR_LOCK; - FLASH_KEYR = FLASH_KEY1; - FLASH_KEYR = FLASH_KEY2; -} - -void ext_flash_lock(void) -{ - FLASH_CR |= FLASH_CR_LOCK; -} - - -int ext_flash_erase(uint32_t address, int len) -{ - int start = -1, end = -1; - uint32_t end_address; - int i; - address += SIM_ADDRESS; - end_address = address + len - 1; - - if (address < flash_sector[0] || end_address > FLASH_TOP) - return -1; - for (i = 0; i < 8; i++) - { - if ((address >= flash_sector[i]) && (address < flash_sector[i + 1])) { - start = i; - } - if ((end_address >= flash_sector[i]) && (end_address < flash_sector[i + 1])) { - end = i; - } - if (start > 0 && end > 0) - break; - } - if (start < 0 || end < 0) - return -1; - for (i = start; i <= end; i++) - flash_erase_sector(i); - return 0; -} - - -#endif diff --git a/include/hal.h b/include/hal.h index 825bb619..359c7c98 100644 --- a/include/hal.h +++ b/include/hal.h @@ -12,12 +12,31 @@ void hal_flash_lock(void); void hal_prepare_boot(void); #ifdef EXT_FLASH -/* external flash interface */ -int ext_flash_write(uint32_t address, const uint8_t *data, int len); -int ext_flash_read(uint32_t address, uint8_t *data, int len); -int ext_flash_erase(uint32_t address, int len); -void ext_flash_lock(void); -void ext_flash_unlock(void); -#endif + +# ifndef SPI_FLASH + /* external flash interface */ + int ext_flash_write(uint32_t address, const uint8_t *data, int len); + int ext_flash_read(uint32_t address, uint8_t *data, int len); + int ext_flash_erase(uint32_t address, int len); + void ext_flash_lock(void); + void ext_flash_unlock(void); +# else +# include "spi_flash.h" +# define ext_flash_lock() do{}while(0) +# define ext_flash_unlock() do{}while(0) +# define ext_flash_read spi_flash_read +# define ext_flash_write spi_flash_write + + static int ext_flash_erase(uint32_t address, int len) + { + uint32_t end = address + len - 1; + uint32_t p; + for (p = address; p <= end; p += SPI_FLASH_SECTOR_SIZE) + spi_flash_sector_erase(p); + return 0; + } +# endif /* !SPI_FLASH */ + +#endif /* EXT_FLASH */ #endif /* H_HAL_FLASH_ */ diff --git a/include/spi_drv.h b/include/spi_drv.h new file mode 100644 index 00000000..c3e3e640 --- /dev/null +++ b/include/spi_drv.h @@ -0,0 +1,38 @@ +/* spi_drv.h + * + * Driver for the SPI back-end of the SPI_FLASH module. + * + * * Compile with SPI_FLASH=1 + * * Define your platform specific SPI driver in spi_drv_$PLATFORM.c, + * implementing the spi_ calls below. + * + * + * Copyright (C) 2018 wolfSSL Inc. + * + * This file is part of wolfBoot. + * + * wolfBoot is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * wolfBoot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA + */ +#ifndef SPI_DRV_H_INCLUDED +#define SPI_DRV_H_INCLUDED +#include + +void spi_init(int polarity, int phase); +void spi_write(const char byte); +uint8_t spi_read(void); +void spi_cs_on(void); +void spi_cs_off(void); + +#endif diff --git a/include/spi_flash.h b/include/spi_flash.h new file mode 100644 index 00000000..74c649cb --- /dev/null +++ b/include/spi_flash.h @@ -0,0 +1,47 @@ +/* spi_flash.h + * + * Generic implementation of the read/write/erase + * functionalities, on top of the spi_drv.h HAL. + * + * Compile with SPI_FLASH=1 + * + * + * Copyright (C) 2018 wolfSSL Inc. + * + * This file is part of wolfBoot. + * + * wolfBoot is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * wolfBoot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA + */ +#ifndef SPI_FLASH_DRI_H +#define SPI_FLASH_DRI_H + +#ifdef SPI_FLASH +#include + +#define SPI_FLASH_SECTOR_SIZE (4096) +#define SPI_FLASH_PAGE_SIZE (256) + +uint16_t spi_flash_probe(void); +void spi_flash_sector_erase(uint32_t address); +int spi_flash_read(uint32_t address, void *data, int len); +int spi_flash_write(uint32_t address, const void *data, int len); + +#else + +#define spi_flash_probe() do{}while(0) + +#endif /* SPI_FLASH */ + +#endif /* GUARD */ diff --git a/include/target.h b/include/target.h index d6e095e4..658f599e 100644 --- a/include/target.h +++ b/include/target.h @@ -6,11 +6,25 @@ * Ensure that your firmware entry point is * at FLASH_AREA_IMAGE_0_OFFSET + 0x100 */ -#define WOLFBOOT_SECTOR_SIZE 0x20000 -#define WOLFBOOT_PARTITION_SIZE 0x20000 -#define WOLFBOOT_PARTITION_BOOT_ADDRESS 0x20000 -#define WOLFBOOT_PARTITION_UPDATE_ADDRESS 0x40000 -#define WOLFBOOT_PARTITION_SWAP_ADDRESS 0x60000 +# define WOLFBOOT_SECTOR_SIZE 0x20000 +# define WOLFBOOT_PARTITION_BOOT_ADDRESS 0x20000 + +#ifdef EXT_FLASH + +/* Test configuration with 1MB external memory */ +/* (Addresses are relative to the beginning of the ext)*/ + +# define WOLFBOOT_PARTITION_SIZE 0x80000 +# define WOLFBOOT_PARTITION_UPDATE_ADDRESS 0x00000 +# define WOLFBOOT_PARTITION_SWAP_ADDRESS 0x80000 + +#else + +/* Test configuration with internal memory */ +# define WOLFBOOT_PARTITION_SIZE 0x20000 +# define WOLFBOOT_PARTITION_UPDATE_ADDRESS 0x40000 +# define WOLFBOOT_PARTITION_SWAP_ADDRESS 0x60000 +#endif #endif diff --git a/src/loader.c b/src/loader.c index ab6826c5..e1a89aae 100644 --- a/src/loader.c +++ b/src/loader.c @@ -18,9 +18,10 @@ * along with this program; if not, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA */ -#include -#include -#include +#include "loader.h" +#include "image.h" +#include "hal.h" +#include "spi_flash.h" extern void do_boot(const uint32_t *app_offset); @@ -38,10 +39,10 @@ static int wolfBoot_copy_sector(struct wolfBoot_image *src, struct wolfBoot_imag if (dst->part == PART_SWAP) dst_sector_offset = 0; #ifdef EXT_FLASH - uint8_t buffer[FLASHBUFFER_SIZE]; if (PART_IS_EXT(src)) { + uint8_t buffer[FLASHBUFFER_SIZE]; wb_flash_erase(dst, dst_sector_offset, WOLFBOOT_SECTOR_SIZE); - while (pos < WOLFBOOT_SECTOR_SIZE) + while (pos < WOLFBOOT_SECTOR_SIZE) { if (src_sector_offset + pos < (src->fw_size + IMAGE_HEADER_SIZE + FLASHBUFFER_SIZE)) { ext_flash_read((uint32_t)(src->hdr) + src_sector_offset + pos, (void *)buffer, FLASHBUFFER_SIZE); wb_flash_write(dst, dst_sector_offset + pos, buffer, FLASHBUFFER_SIZE); @@ -176,6 +177,7 @@ static void wolfBoot_start(void) int main(void) { hal_init(); + spi_flash_probe(); wolfBoot_start(); while(1) ; diff --git a/src/spi_flash.c b/src/spi_flash.c new file mode 100644 index 00000000..32acb535 --- /dev/null +++ b/src/spi_flash.c @@ -0,0 +1,239 @@ +/* spi_flash.c + * + * Generic implementation of the read/write/erase + * functionalities, on top of the spi_drv.h HAL. + * + * + * Copyright (C) 2018 wolfSSL Inc. + * + * This file is part of wolfBoot. + * + * wolfBoot is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * wolfBoot is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1335, USA + */ +#include "spi_drv.h" +#include "spi_flash.h" + + +#define MDID 0x90 +#define RDSR 0x05 +#define WRSR 0x01 +# define ST_BUSY (1 << 0) +# define ST_WEL (1 << 1) +# define ST_BP0 (1 << 2) +# define ST_BP1 (1 << 3) +# define ST_BP2 (1 << 4) +# define ST_BP3 (1 << 5) +# define ST_AAI (1 << 6) +# define ST_BRO (1 << 7) +#define WREN 0x06 +#define WRDI 0x04 +#define SECTOR_ERASE 0x20 +#define BYTE_READ 0x03 +#define BYTE_WRITE 0x02 +#define AUTOINC 0xAD +#define EWSR 0x50 +#define EBSY 0x70 +#define DBSY 0x80 + + +static enum write_mode { + WB_WRITEPAGE = 0x00, + SST_SINGLEBYTE = 0x01 +} chip_write_mode = WB_WRITEPAGE; + +static void write_address(uint32_t address) +{ + spi_write((address & 0xFF0000) >> 16); + spi_read(); + spi_write((address & 0xFF00) >> 8); + spi_read(); + spi_write((address & 0xFF)); + spi_read(); +} + +static uint8_t read_status(void) +{ + uint8_t status; + spi_cs_on(); + spi_write(RDSR); + spi_read(); + spi_write(0xFF); + status = spi_read(); + spi_cs_off(); + return status; +} + +static void spi_cmd(uint8_t cmd) +{ + spi_cs_on(); + spi_write(cmd); + spi_read(); + spi_cs_off(); +} + +static void flash_write_enable(void) +{ + uint8_t status; + do { + spi_cmd(WREN); + status = read_status(); + } while ((status & ST_WEL) == 0); +} + +static void flash_write_disable(void) +{ + uint8_t status; + spi_cmd(WRDI); +} +static void wait_busy(void) +{ + uint8_t status; + do { + status = read_status(); + } while(status & ST_BUSY); +} + +static int spi_flash_write_page(uint32_t address, const void *data, int len) +{ + const uint8_t *buf = data; + int j = 0; + while (len > 0) { + wait_busy(); + flash_write_enable(); + wait_busy(); + + spi_cs_on(); + spi_write(BYTE_WRITE); + spi_read(); + write_address(address); + do { + spi_write(buf[j++]); + address++; + spi_read(); + len--; + } while ((address & (SPI_FLASH_PAGE_SIZE - 1)) != 0); + spi_cs_off(); + } + wait_busy(); + return j; +} + +static int spi_flash_write_sb(uint32_t address, const void *data, int len) +{ + const uint8_t *buf = data; + const uint8_t verify; + int j = 0; + wait_busy(); + if (len < 1) + return -1; + while (len > 0) { + flash_write_enable(); + spi_cs_on(); + spi_write(BYTE_WRITE); + spi_read(); + write_address(address); + spi_write(buf[j]); + spi_read(); + spi_cs_off(); + wait_busy(); + spi_flash_read(address, &verify, 1); + if ((verify & ~(buf[j])) == 0) { + j++; + len--; + address++; + } + wait_busy(); + } + return j; +} + +/* --- */ + +uint16_t spi_flash_probe(void) +{ + uint8_t manuf, product, b0; + int i; + spi_init(0,0); + wait_busy(); + spi_cs_on(); + spi_write(MDID); + b0 = spi_read(); + + write_address(0); + spi_write(0xFF); + manuf = spi_read(); + spi_write(0xFF); + product = spi_read(); + spi_cs_off(); + if (manuf == 0xBF) + chip_write_mode = SST_SINGLEBYTE; + if (manuf == 0xEF) + chip_write_mode = WB_WRITEPAGE; + +#ifndef READONLY + spi_cmd(EWSR); + spi_cs_on(); + spi_write(WRSR); + spi_read(); + spi_write(0x00); + spi_read(); + spi_cs_off(); +#endif + return (uint16_t)(manuf << 8 | product); +} + + +void spi_flash_sector_erase(uint32_t address) +{ + uint8_t status; + address &= (~(SPI_FLASH_SECTOR_SIZE - 1)); + + wait_busy(); + flash_write_enable(); + spi_cs_on(); + spi_write(SECTOR_ERASE); + spi_read(); + write_address(address); + spi_cs_off(); + wait_busy(); +} + +int spi_flash_read(uint32_t address, void *data, int len) +{ + uint8_t *buf = data; + int i = 0; + wait_busy(); + spi_cs_on(); + spi_write(BYTE_READ); + spi_read(); + write_address(address); + while (len > 0) { + spi_write(0xFF); + buf[i++] = spi_read(); + len--; + } + spi_cs_off(); + return i; +} + +int spi_flash_write(uint32_t address, const void *data, int len) +{ + if (chip_write_mode == SST_SINGLEBYTE) + return spi_flash_write_sb(address, data, len); + if (chip_write_mode == WB_WRITEPAGE) + return spi_flash_write_page(address, data, len); + return -1; +} + diff --git a/test-app/Makefile b/test-app/Makefile index 1b5a2d12..4eef45e1 100644 --- a/test-app/Makefile +++ b/test-app/Makefile @@ -12,6 +12,10 @@ ifeq ($(EXT_FLASH),1) CFLAGS+=-DEXT_FLASH=1 -DPART_UPDATE_EXT=1 endif +ifeq ($(SPI_FLASH),1) + CFLAGS+=-DSPI_FLASH + OBJS+=../hal/spi/spi_drv_$(TARGET).o ../src/spi_flash.o +endif image.bin: image.elf $(OBJCOPY) -O binary $^ $@ diff --git a/test-app/main.c b/test-app/main.c index a4ab0199..44c18ed0 100644 --- a/test-app/main.c +++ b/test-app/main.c @@ -29,6 +29,7 @@ #include "led.h" #include "hal.h" #include "wolfboot/wolfboot.h" +#include "spi_flash.h" #ifdef PLATFORM_stm32f4 @@ -198,6 +199,8 @@ void main(void) { clock_config(); led_pwm_setup(); pwm_init(CPU_FREQ, 0); + + spi_flash_probe(); /* Dim the led by altering the PWM duty-cicle * in isr_tim2 (timer.c) * diff --git a/tools/test.mk b/tools/test.mk index 360d866e..d3b11ee1 100644 --- a/tools/test.mk +++ b/tools/test.mk @@ -1,9 +1,12 @@ TEST_UPDATE_VERSION?=2 EXPVER=tools/test-expect-version/test-expect-version +SPI_CHIP=SST25VF080B $(EXPVER): make -C tools/test-expect-version +# Testbed actions + test-update: test-app/image.bin FORCE @$(SIGN_TOOL) test-app/image.bin $(PRIVATE_KEY) $(TEST_UPDATE_VERSION) 131072 >/dev/null @dd if=test-app/image.bin.v$(TEST_UPDATE_VERSION).signed of=test-update.bin bs=1 count=131067 @@ -12,8 +15,17 @@ test-update: test-app/image.bin FORCE @sleep 2 @sudo st-flash --reset write test-update.bin 0x08040000 || \ (make test-reset && sleep 1 && sudo st-flash --reset write test-update.bin 0x08040000) || \ - (make test-reset && sleep 1 && sudo st-flash --reset write test-update.bin 0x08040000) + (make test-reset && sleep 1 && sudo st-flash --reset write test-update.bin 0x08040000) +test-update-ext: + @$(SIGN_TOOL) test-app/image.bin $(PRIVATE_KEY) $(TEST_UPDATE_VERSION) 524288 >/dev/null + @$$(dd if=/dev/zero bs=1M count=1 | tr '\000' '\377' > test-update.rom) + @dd if=test-app/image.bin.v$(TEST_UPDATE_VERSION).signed of=test-update.rom bs=1 count=524283 conv=notrunc + @printf "pBOOT" | dd of=test-update.rom obs=1 seek=524283 count=5 conv=notrunc + flashrom -c $(SPI_CHIP) -p linux_spi:dev=/dev/spidev0.0 -w update.rom + @make test-reset + @sleep 2 + @make clean test-erase: FORCE @echo Mass-erasing the internal flash: @@ -74,7 +86,7 @@ test-02-forward-update-allow-downgrade: $(EXPVER) FORCE test-03-rollback: $(EXPVER) FORCE @echo Creating and uploading factory image... - @make test-factory ALLOW_DOWNGRADE=1 + @make test-factory @echo Expecting version '1' @$$(test `$(EXPVER)` -eq 1) @echo @@ -100,5 +112,44 @@ test-11-forward-update-no-downgrade-ECC: $(EXPVER) FORCE test-13-rollback-ECC: $(EXPVER) FORCE @make test-03-rollback SIGN=ECC256 +test-21-forward-update-no-downgrade-SPI: FORCE + @echo Creating and uploading factory image... + @make test-factory SPI_FLASH=1 + @echo Expecting version '1' + @$$(test `$(EXPVER)` -eq 1) + @echo + @echo Creating and uploading update image... + @make test-update-ext TEST_UPDATE_VERSION=4 SPI_FLASH=1 + @echo Expecting version '4' + @$$(test `$(EXPVER)` -eq 4) + @echo + @echo Creating and uploading update image... + @make test-update-ext TEST_UPDATE_VERSION=1 SPI_FLASH=1 + @echo Expecting version '4' + @$$(test `$(EXPVER)` -eq 4) + @make clean + @echo TEST PASSED + +test-23-rollback-SPI: + @echo Creating and uploading factory image... + @make test-factory SPI_FLASH=1 + @echo Expecting version '1' + @$$(test `$(EXPVER)` -eq 1) + @echo + @echo Creating and uploading update image... + @make test-update-ext TEST_UPDATE_VERSION=4 SPI_FLASH=1 + @echo Expecting version '4' + @$$(test `$(EXPVER)` -eq 4) + @echo + @echo Creating and uploading update image... + @make test-update-ext TEST_UPDATE_VERSION=5 SPI_FLASH=1 + @echo Expecting version '5' + @$$(test `$(EXPVER)` -eq 5) + @echo + @echo Resetting to trigger rollback... + @make test-reset + @$$(test `$(EXPVER)` -eq 4) + @make clean + @echo TEST PASSED test-all: clean test-01-forward-update-no-downgrade test-02-forward-update-allow-downgrade test-03-rollback test-11-forward-update-no-downgrade-ECC test-13-rollback-ECC