From 0f0f8ca61faf7e40ed78778fafa74ae4905aa053 Mon Sep 17 00:00:00 2001 From: David Garske Date: Fri, 27 Oct 2023 13:00:39 -0700 Subject: [PATCH] Added flattened device tree (FDT) support. Setting required FDT fields per ePAPR 1.1. --- arch.mk | 3 + hal/nxp_ppc.h | 6 + hal/nxp_t1024.c | 125 ++++++++++- include/fdt.h | 138 ++++++++++++ include/image.h | 3 +- src/boot_ppc.c | 10 + src/fdt.c | 577 ++++++++++++++++++++++++++++++++++++++++++++++++ src/image.c | 24 +- 8 files changed, 853 insertions(+), 33 deletions(-) create mode 100644 include/fdt.h create mode 100644 src/fdt.c diff --git a/arch.mk b/arch.mk index 75fb67cf0..e289e114e 100644 --- a/arch.mk +++ b/arch.mk @@ -424,6 +424,7 @@ ifeq ($(TARGET),nxp_t1024) LDFLAGS+=-Wl,--hash-style=both # generate both sysv and gnu symbol hash table LDFLAGS+=-Wl,--as-needed # remove weak functions not used OBJS+=src/boot_ppc_mp.o # support for spin table + OBJS+=src/fdt.o UPDATE_OBJS:=src/update_ram.o ifeq ($(SPMATH),1) MATH_OBJS += ./lib/wolfssl/wolfcrypt/src/sp_c32.o @@ -445,6 +446,7 @@ ifeq ($(TARGET),nxp_t2080) LDFLAGS+=-Wl,--hash-style=both # generate both sysv and gnu symbol hash table LDFLAGS+=-Wl,--as-needed # remove weak functions not used UPDATE_OBJS:=src/update_ram.o + OBJS+=src/fdt.o ifeq ($(SPMATH),1) MATH_OBJS += ./lib/wolfssl/wolfcrypt/src/sp_c32.o else @@ -750,6 +752,7 @@ BOOT_IMG?=test-app/image.bin ## Update mechanism ifeq ($(ARCH),AARCH64) CFLAGS+=-DMMU -DWOLFBOOT_DUALBOOT + OBJS+=src/fdt.o UPDATE_OBJS:=src/update_ram.o endif ifeq ($(DUALBANK_SWAP),1) diff --git a/hal/nxp_ppc.h b/hal/nxp_ppc.h index fc96a22e0..ad5b13c2c 100644 --- a/hal/nxp_ppc.h +++ b/hal/nxp_ppc.h @@ -32,7 +32,9 @@ #define CCSRBAR_SIZE BOOKE_PAGESZ_1M #define ENABLE_DDR + #ifndef DDR_SIZE #define DDR_SIZE (512UL * 1024UL * 1024UL) + #endif /* Memory used for transferring blocks to/from NAND. * Maps to eLBC FCM internal 8KB region (by hardware) */ @@ -87,7 +89,9 @@ #endif #define ENABLE_DDR + #ifndef DDR_SIZE #define DDR_SIZE (2048ULL * 1024ULL * 1024ULL) + #endif #define FLASH_BASE_ADDR 0xEC000000UL #define FLASH_BASE_PHYS_HIGH 0xFULL @@ -125,7 +129,9 @@ #define ENABLE_INTERRUPTS #define ENABLE_DDR + #ifndef DDR_SIZE #define DDR_SIZE (8192UL * 1024UL * 1024UL) + #endif #define FLASH_BASE_ADDR 0xE8000000UL #define FLASH_BASE_PHYS_HIGH 0x0ULL diff --git a/hal/nxp_t1024.c b/hal/nxp_t1024.c index c8ee868f3..1472bf35f 100644 --- a/hal/nxp_t1024.c +++ b/hal/nxp_t1024.c @@ -24,6 +24,7 @@ #include "string.h" #include "hal.h" #include "nxp_ppc.h" +#include "fdt.h" /* Tested on T1024E Rev 1.0, e5500 core 2.1, PVR 8024_1021 and SVR 8548_0010 */ /* IFC: CS0 NOR, CS1 MRAM, CS2 CPLD, CS3, MPU CPLD */ @@ -119,6 +120,7 @@ static void hal_flash_unlock_sector(uint32_t sector); /* QUICC Engine */ #define QE_MAX_RISC 1 +#define QE_MURAM_SIZE (24 * 1024) /* QE microcode/firmware address */ #ifndef QE_FW_ADDR @@ -152,6 +154,8 @@ static void hal_flash_unlock_sector(uint32_t sector); #define QE_RSP_TIBCR(n, i) ((volatile uint32_t*)(QE_RSP + ((n) * 0x100) + (i))) #define QE_RSP_ECCR(n) ((volatile uint32_t*)(QE_RSP + ((n) * 0x100) + 0xF0)) +#define QE_MURAM (QE_ENGINE_BASE + 0x110000UL) /* 24KB */ + #define QE_IRAM_IADD_AIE 0x80000000 /* Auto Increment Enable */ #define QE_IRAM_IADD_BADDR 0x00080000 /* Base Address */ #define QE_IRAM_READY 0x80000000 @@ -234,7 +238,7 @@ static void hal_flash_unlock_sector(uint32_t sector); #endif #define FMAN_BASE (CCSRBAR + 0x400000) -//#define QE_CEPIER ((volatile uint32_t*)(FMAN_BASE + 0x00CUL)) + @@ -619,22 +623,31 @@ enum ifc_amask_sizes { #ifdef ENABLE_BUS_CLK_CALC -static uint32_t hal_get_bus_clk(void) +static uint32_t hal_get_plat_clk(void) { - /* compute bus clock (system input * ratio) */ - uint32_t plat_clk, bus_clk; + /* compute platform clock (system input * ratio) */ + uint32_t plat_clk; uint32_t plat_ratio = get32(CLOCKING_PLLPGSR); /* see SYS_PLL_RAT in RCW */ /* mask and shift by 1 to get platform ratio */ plat_ratio = ((plat_ratio & 0x3E) >> 1); /* default is 4 (4:1) */ plat_clk = SYS_CLK * plat_ratio; - bus_clk = plat_clk / 2; + return plat_clk; +} + +static uint32_t hal_get_bus_clk(void) +{ + /* compute bus clock (platform clock / 2) */ + uint32_t bus_clk = hal_get_plat_clk() / 2; return bus_clk; } #else -#define hal_get_bus_clk() (uint32_t)((SYS_CLK * 4) / 2) +#define hal_get_plat_clk() (uint32_t)(SYS_CLK * 4) +#define hal_get_bus_clk() (uint32_t)(hal_get_plat_clk() / 2) #endif -#define DELAY_US ((hal_get_bus_clk() / 16) / 1000000) +#define TIMEBASE_CLK_DIV 16 +#define TIMEBASE_HZ (hal_get_plat_clk() / TIMEBASE_CLK_DIV) +#define DELAY_US (TIMEBASE_HZ / 1000000) static void udelay(uint32_t delay_us) { wait_ticks(delay_us * DELAY_US); @@ -1330,7 +1343,7 @@ static int hal_qe_init(void) set32(QE_SDMA_SDAQMR, 0); /* Allocate 2KB temporary buffer for sdma */ - sdma_base = 0; + sdma_base = 0; /* offset in QE_MURAM */ set32(QE_SDMA_SDEBCR, sdma_base & QE_SDEBCR_BA_MASK); /* Clear sdma status */ @@ -1370,6 +1383,10 @@ extern uint32_t _mp_page_start; extern uint32_t _spin_table; extern uint32_t _bootpg_addr; +#define SPIN_TABLE_ADDR() ((uint8_t*)(BOOT_ROM_ADDR + \ + ((uint32_t)&_spin_table - (uint32_t)&_mp_page_start))) + + /* Startup additional cores with spin table and synchronize the timebase */ static void hal_mp_up(uint32_t bootpg) { @@ -1382,8 +1399,7 @@ static void hal_mp_up(uint32_t bootpg) active_cores = (1 << whoami); /* current running cores */ /* Calculate location of spin table in BPTR */ - spin_table_addr = (uint8_t*)(BOOT_ROM_ADDR + - ((uint32_t)&_spin_table - (uint32_t)&_mp_page_start)); + spin_table_addr = SPIN_TABLE_ADDR(); wolfBoot_printf("MP: Starting core 2 (spin table %p)\n", spin_table_addr); @@ -1670,7 +1686,94 @@ void* hal_get_dts_address(void) { return (void*)WOLFBOOT_DTS_BOOT_ADDRESS; } -#endif + +int hal_dts_fixup(void* dts_addr) +{ +#ifndef BUILD_LOADER_STAGE1 + struct fdt_header *fdt = (struct fdt_header *)dts_addr; + int off; + uint32_t *reg; + + /* display FTD information */ + wolfBoot_printf("FDT: Version %d, Size %d\n", + fdt_version(fdt), fdt_totalsize(fdt)); + + /* expand total size */ + fdt->totalsize += 1024; /* expand by 1KB */ + wolfBoot_printf("FDT: Expanded (1KB) to %d bytes\n", fdt->totalsize); + + /* fixup the memory region - single bank */ + off = fdt_find_devtype(fdt, -1, "memory"); + if (off != -FDT_ERR_NOTFOUND) { + /* build addr/size as 64-bit */ + uint8_t ranges[sizeof(uint64_t) * 2], *p = ranges; + *(uint64_t*)p = cpu_to_fdt64(DDR_ADDRESS); + p += sizeof(uint64_t); + *(uint64_t*)p = cpu_to_fdt64(DDR_SIZE); + p += sizeof(uint64_t); + fdt_setprop(fdt, off, "reg", ranges, (int)(p - ranges)); + wolfBoot_printf("FDT: Set memory, start=0x%x, size=0x%x\n", + DDR_ADDRESS, (uint32_t)DDR_SIZE); + } + + /* fixup CPU status and, release address and enable method */ + off = fdt_find_devtype(fdt, -1, "cpu"); + while (off != -FDT_ERR_NOTFOUND) { + int core; + uintptr_t core_spin_table; + + reg = (uint32_t*)fdt_getprop(fdt, off, "reg", 0); + if (reg == NULL || *reg >= CPU_NUMCORES) + break; + core = *reg; + + /* calculate location of spin table for core */ + core_spin_table = (uintptr_t)(SPIN_TABLE_ADDR() + + (core * ENTRY_SIZE) + ENTRY_ADDR_LOWER); + + fdt_fixup_str(fdt, off, "cpu", "status", (core == 0) ? "okay" : "disabled"); + fdt_fixup_val(fdt, off, "cpu", "spin-table", (uint32_t)core_spin_table); + fdt_fixup_str(fdt, off, "cpu", "enable-method", "spin-table"); + fdt_fixup_val(fdt, off, "cpu", "timebase-frequency", TIMEBASE_HZ); + fdt_fixup_val(fdt, off, "cpu", "clock-frequency", hal_get_plat_clk()); + fdt_fixup_val(fdt, off, "cpu", "bus-frequency", hal_get_plat_clk()); + + off = fdt_find_devtype(fdt, off, "cpu"); + } + + /* fixup the soc clock */ + off = fdt_find_devtype(fdt, -1, "soc"); + if (off != -FDT_ERR_NOTFOUND) { + fdt_fixup_val(fdt, off, "soc", "bus-frequency", hal_get_plat_clk()); + } + + /* fixup the serial clocks */ + off = fdt_find_devtype(fdt, -1, "serial"); + while (off != -FDT_ERR_NOTFOUND) { + fdt_fixup_val(fdt, off, "serial", "clock-frequency", hal_get_bus_clk()); + off = fdt_find_devtype(fdt, off, "serial"); + } + + /* fixup the QE bridge and bus blocks */ + off = fdt_find_devtype(fdt, -1, "qe"); + if (off != -FDT_ERR_NOTFOUND) { + fdt_fixup_val(fdt, off, "qe", "clock-frequency", hal_get_bus_clk()); + fdt_fixup_val(fdt, off, "qe", "bus-frequency", hal_get_bus_clk()); + fdt_fixup_val(fdt, off, "qe", "brg-frequency", hal_get_bus_clk()/2); + } + + /* mpic clock */ + off = fdt_find_devtype(fdt, -1, "open-pic"); + if (off != -FDT_ERR_NOTFOUND) { + fdt_fixup_val(fdt, off, "open-pic", "clock-frequency", hal_get_bus_clk()); + } + + +#endif /* !BUILD_LOADER_STAGE1 */ + return 0; +} +#endif /* MMU */ + #if defined(ENABLE_DDR) && defined(TEST_DDR) diff --git a/include/fdt.h b/include/fdt.h new file mode 100644 index 000000000..87d0ebb45 --- /dev/null +++ b/include/fdt.h @@ -0,0 +1,138 @@ +/* fdt.h + * + * Functions to help with flattened device tree (DTB) parsing + * + * + * Copyright (C) 2023 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 + */ + +/* Based on work from "libfdt" by David Gibson */ + +#ifndef FDT_H +#define FDT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#ifdef BIG_ENDIAN_ORDER +#define FDT_MAGIC 0xD00DFEEDUL +#else +#define FDT_MAGIC 0xEDFE0DD0UL +#endif +#define FDT_SW_MAGIC (~FDT_MAGIC) + +struct fdt_header { + uint32_t magic; + uint32_t totalsize; + uint32_t off_dt_struct; + uint32_t off_dt_strings; + uint32_t off_mem_rsvmap; + uint32_t version; + uint32_t last_comp_version; + uint32_t boot_cpuid_phys; + uint32_t size_dt_strings; + uint32_t size_dt_struct; +}; + +struct fdt_reserve_entry { + uint64_t address; + uint64_t size; +}; + +struct fdt_prop { + uint32_t len; + uint32_t nameoff; +}; + +struct fdt_node_header { + uint32_t tag; + char name[0]; +}; + +struct fdt_property { + uint32_t tag; + uint32_t len; + uint32_t nameoff; + char data[0]; +}; + +#define FDT_TAGSIZE sizeof(uint32_t) +#define FDT_ALIGN(x, a) (((x) + (a) - 1) & ~((a) - 1)) +#define FDT_TAGALIGN(x) (FDT_ALIGN((x), FDT_TAGSIZE)) + +#define FDT_BEGIN_NODE 0x00000001UL +#define FDT_END_NODE 0x00000002UL +#define FDT_PROP 0x00000003UL +#define FDT_NOP 0x00000004UL +#define FDT_END 0x00000009UL + +#define FDT_ERR_NOTFOUND 1 +#define FDT_ERR_NOSPACE 3 +#define FDT_ERR_BADOFFSET 4 +#define FDT_ERR_TRUNCATED 8 +#define FDT_ERR_BADVERSION 10 +#define FDT_ERR_BADSTRUCTURE 11 +#define FDT_ERR_INTERNAL 13 + + +uint32_t cpu_to_fdt32(uint32_t x); +uint64_t cpu_to_fdt64(uint64_t x); +uint32_t fdt32_to_cpu(uint32_t x); +uint64_t fdt64_to_cpu(uint64_t x); + +int fdt_next_node(const void *fdt, int offset, int *depth); +const void *fdt_getprop(const void *fdt, int nodeoffset, const char *name, int *lenp); +int fdt_setprop(void *fdt, int nodeoffset, const char *name, const void *val, int len); + +int fdt_fixup_str(void* fdt, int off, const char* node, const char* name, const char* str); +int fdt_fixup_val(void* fdt, int off, const char* node, const char* name, uint32_t val); +int fdt_find_devtype(void* fdt, int startoff, const char* node); + +#define fdt_get_header(fdt, field) (fdt32_to_cpu(((const struct fdt_header *)(fdt))->field)) +#define fdt_magic(fdt) (fdt_get_header(fdt, magic)) +#define fdt_totalsize(fdt) (fdt_get_header(fdt, totalsize)) +#define fdt_off_dt_struct(fdt) (fdt_get_header(fdt, off_dt_struct)) +#define fdt_off_dt_strings(fdt) (fdt_get_header(fdt, off_dt_strings)) +#define fdt_off_mem_rsvmap(fdt) (fdt_get_header(fdt, off_mem_rsvmap)) +#define fdt_version(fdt) (fdt_get_header(fdt, version)) +#define fdt_last_comp_version(fdt) (fdt_get_header(fdt, last_comp_version)) +#define fdt_boot_cpuid_phys(fdt) (fdt_get_header(fdt, boot_cpuid_phys)) +#define fdt_size_dt_strings(fdt) (fdt_get_header(fdt, size_dt_strings)) +#define fdt_size_dt_struct(fdt) (fdt_get_header(fdt, size_dt_struct)) + +#define fdt_set_header(fdt, field, val) (((struct fdt_header *)fdt)->field = cpu_to_fdt32(val)) +#define fdt_set_magic(fdt, val) (fdt_set_header(fdt, magic, (val))) +#define fdt_set_totalsize(fdt, val) (fdt_set_header(fdt, totalsize, (val))) +#define fdt_set_off_dt_struct(fdt, val) (fdt_set_header(fdt, off_dt_struct, (val))) +#define fdt_set_off_dt_strings(fdt, val) (fdt_set_header(fdt, off_dt_strings, (val))) +#define fdt_set_off_mem_rsvmap(fdt, val) (fdt_set_header(fdt, off_mem_rsvmap, (val))) +#define fdt_set_version(fdt, val) (fdt_set_header(fdt, version, (val))) +#define fdt_set_last_comp_version(fdt, val) (fdt_set_header(fdt, last_comp_version, (val))) +#define fdt_set_boot_cpuid_phys(fdt, val) (fdt_set_header(fdt, boot_cpuid_phys, (val))) +#define fdt_set_size_dt_strings(fdt, val) (fdt_set_header(fdt, size_dt_strings, (val))) +#define fdt_set_size_dt_struct(fdt, val) (fdt_set_header(fdt, size_dt_struct, (val))) + +#ifdef __cplusplus +} +#endif + +#endif /* !FDT_H */ diff --git a/include/image.h b/include/image.h index dfade4b95..9315e02e5 100644 --- a/include/image.h +++ b/include/image.h @@ -668,7 +668,8 @@ static inline int wb_flash_write_verify_word(struct wolfBoot_image *img, #define UBOOT_IMG_HDR_SZ 64 /* --- Flattened Device Tree Blob */ -#define UBOOT_FDT_MAGIC 0xEDFE0DD0UL +#include "fdt.h" + #ifndef EXT_ENCRYPTED #define WOLFBOOT_MAX_SPACE (WOLFBOOT_PARTITION_SIZE - \ diff --git a/src/boot_ppc.c b/src/boot_ppc.c index 1df610556..e63b1ece6 100644 --- a/src/boot_ppc.c +++ b/src/boot_ppc.c @@ -105,6 +105,12 @@ void __attribute((weak)) hal_early_init(void) { } +#ifdef MMU +int __attribute((weak)) hal_dts_fixup(void* dts_addr) +{ + return 0; +} +#endif void boot_entry_C(void) { @@ -172,6 +178,10 @@ void do_boot(const uint32_t *app_offset) uintptr_t r7, uintptr_t r8, uintptr_t r9); boot_entry entry = (boot_entry)app_offset; +#ifdef MMU + hal_dts_fixup((uint32_t*)dts_offset); +#endif + #ifndef BUILD_LOADER_STAGE1 /* invalidate cache */ flush_cache((uint32_t)app_offset, L1_CACHE_SZ); diff --git a/src/fdt.c b/src/fdt.c new file mode 100644 index 000000000..260d73ce3 --- /dev/null +++ b/src/fdt.c @@ -0,0 +1,577 @@ +/* fdt.c + * + * Functions to help with flattened device tree (DTB) parsing + * + * + * Copyright (C) 2023 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 + */ + +/* Based on work from "libfdt" by David Gibson */ + +#if defined(MMU) && !defined(BUILD_LOADER_STAGE1) + +#include /* for wolfCrypt hash/sign routines */ + +#include "fdt.h" +#include "hal.h" +#include "printf.h" +#include "string.h" + +#define WOLFSSL_MISC_INCLUDED /* allow misc.c code to be inlined */ +#include /* for ByteReverseWord32 and ByteReverseWord64 */ + +uint32_t cpu_to_fdt32(uint32_t x) +{ +#ifdef BIG_ENDIAN_ORDER + return x; +#else + return ByteReverseWord32(x); +#endif +} +uint64_t cpu_to_fdt64(uint64_t x) +{ +#ifdef BIG_ENDIAN_ORDER + return x; +#else + return ByteReverseWord64(x); +#endif +} + +uint32_t fdt32_to_cpu(uint32_t x) +{ +#ifdef BIG_ENDIAN_ORDER + return x; +#else + return ByteReverseWord32(x); +#endif +} +uint64_t fdt64_to_cpu(uint64_t x) +{ +#ifdef BIG_ENDIAN_ORDER + return x; +#else + return ByteReverseWord64(x); +#endif +} + +static inline const void *fdt_offset_ptr_(const void *fdt, int offset) +{ + return (const char*)fdt + fdt_off_dt_struct(fdt) + offset; +} +static inline void *fdt_offset_ptr_w_(void *fdt, int offset) +{ + return (void*)(uintptr_t)fdt_offset_ptr_(fdt, offset); +} +static inline int fdt_data_size_(void *fdt) +{ + return fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt); +} + +const void *fdt_offset_ptr(const void *fdt, int offset, unsigned int len) +{ + unsigned int uoffset = offset; + unsigned int absoffset = offset + fdt_off_dt_struct(fdt); + + if (offset < 0) { + return NULL; + } + if ((absoffset < uoffset) + || ((absoffset + len) < absoffset) + || (absoffset + len) > fdt_totalsize(fdt)) { + return NULL; + } + if (fdt_version(fdt) >= 0x11) { + if (((uoffset + len) < uoffset) + || ((offset + len) > fdt_size_dt_struct(fdt))) { + return NULL; + } + } + return fdt_offset_ptr_(fdt, offset); +} + +uint32_t fdt_next_tag(const void *fdt, int startoffset, int *nextoffset) +{ + const uint32_t *tagp, *lenp; + uint32_t tag; + int offset = startoffset; + const char *p; + + *nextoffset = -FDT_ERR_TRUNCATED; + tagp = fdt_offset_ptr(fdt, offset, FDT_TAGSIZE); + if (!tagp) { + return FDT_END; /* premature end */ + } + tag = fdt32_to_cpu(*tagp); + offset += FDT_TAGSIZE; + + *nextoffset = -FDT_ERR_BADSTRUCTURE; + switch (tag) { + case FDT_BEGIN_NODE: + /* skip name */ + do { + p = fdt_offset_ptr(fdt, offset++, 1); + } while (p && (*p != '\0')); + if (!p) + return FDT_END; /* premature end */ + break; + + case FDT_PROP: + lenp = fdt_offset_ptr(fdt, offset, sizeof(*lenp)); + if (!lenp) { + return FDT_END; /* premature end */ + } + /* skip-name offset, length and value */ + offset += sizeof(struct fdt_property) - FDT_TAGSIZE + + fdt32_to_cpu(*lenp); + if (fdt_version(fdt) < 0x10 && fdt32_to_cpu(*lenp) >= 8 && + ((offset - fdt32_to_cpu(*lenp)) % 8) != 0) { + offset += 4; + } + break; + + case FDT_END: + case FDT_END_NODE: + case FDT_NOP: + break; + + default: + return FDT_END; + } + + if (!fdt_offset_ptr(fdt, startoffset, offset - startoffset)) { + return FDT_END; /* premature end */ + } + *nextoffset = FDT_TAGALIGN(offset); + return tag; +} + +static int fdt_check_node_offset_(const void *fdt, int offset) +{ + if ((offset < 0) || (offset % FDT_TAGSIZE) + || (fdt_next_tag(fdt, offset, &offset) != FDT_BEGIN_NODE)) { + return -FDT_ERR_BADOFFSET; + } + return offset; +} + +static int fdt_check_prop_offset_(const void *fdt, int offset) +{ + if ((offset < 0) || (offset % FDT_TAGSIZE) + || (fdt_next_tag(fdt, offset, &offset) != FDT_PROP)) { + return -FDT_ERR_BADOFFSET; + } + return offset; +} + +int fdt_next_node(const void *fdt, int offset, int *depth) +{ + int nextoffset = 0; + uint32_t tag; + + if (offset >= 0) + if ((nextoffset = fdt_check_node_offset_(fdt, offset)) < 0) + return nextoffset; + + do { + offset = nextoffset; + tag = fdt_next_tag(fdt, offset, &nextoffset); + + switch (tag) { + case FDT_PROP: + case FDT_NOP: + break; + + case FDT_BEGIN_NODE: + if (depth) + (*depth)++; + break; + + case FDT_END_NODE: + if (depth && ((--(*depth)) < 0)) + return nextoffset; + break; + + case FDT_END: + if ((nextoffset >= 0) + || ((nextoffset == -FDT_ERR_TRUNCATED) && !depth)) + return -FDT_ERR_NOTFOUND; + else + return nextoffset; + } + } while (tag != FDT_BEGIN_NODE); + + return offset; +} + +static int fdt_next_property_(const void *fdt, int offset) +{ + uint32_t tag; + int nextoffset; + + do { + tag = fdt_next_tag(fdt, offset, &nextoffset); + + switch (tag) { + case FDT_END: + if (nextoffset >= 0) + return -FDT_ERR_BADSTRUCTURE; + else + return nextoffset; + + case FDT_PROP: + return offset; + } + offset = nextoffset; + } while (tag == FDT_NOP); + + return -FDT_ERR_NOTFOUND; +} + +int fdt_first_property_offset(const void *fdt, int nodeoffset) +{ + int offset; + if ((offset = fdt_check_node_offset_(fdt, nodeoffset)) < 0) { + return offset; + } + return fdt_next_property_(fdt, offset); +} +int fdt_next_property_offset(const void *fdt, int offset) +{ + if ((offset = fdt_check_prop_offset_(fdt, offset)) < 0) { + return offset; + } + return fdt_next_property_(fdt, offset); +} + +static const struct fdt_property *fdt_get_property_by_offset_(const void *fdt, + int offset, + int *lenp) +{ + int err; + const struct fdt_property *prop; + + if ((err = fdt_check_prop_offset_(fdt, offset)) < 0) { + if (lenp) { + *lenp = err; + } + return NULL; + } + prop = fdt_offset_ptr_(fdt, offset); + if (lenp) { + *lenp = fdt32_to_cpu(prop->len); + } + return prop; +} + +const char *fdt_get_string(const void *fdt, int stroffset, int *lenp) +{ + const char *s = (const char*)fdt + fdt_off_dt_strings(fdt) + stroffset; + if (lenp) { + *lenp = strlen(s); + } + return s; +} + +static int fdt_string_eq_(const void *fdt, int stroffset, + const char *s, int len) +{ + int slen; + const char *p = fdt_get_string(fdt, stroffset, &slen); + return p && (slen == len) && (memcmp(p, s, len) == 0); +} + +static const struct fdt_property *fdt_get_property_namelen_(const void *fdt, + int offset, const char *name, int namelen, int *lenp, int *poffset) +{ + for (offset = fdt_first_property_offset(fdt, offset); + offset >= 0; + offset = fdt_next_property_offset(fdt, offset)) + { + const struct fdt_property *prop = + fdt_get_property_by_offset_(fdt, offset, lenp); + if (!prop) { + offset = -FDT_ERR_INTERNAL; + break; + } + if (fdt_string_eq_(fdt, fdt32_to_cpu(prop->nameoff), name, namelen)) { + if (poffset) { + *poffset = offset; + } + return prop; + } + } + if (lenp) { + *lenp = offset; + } + return NULL; +} + +const struct fdt_property *fdt_get_property_namelen(const void *fdt, int offset, + const char *name, int namelen, int *lenp) +{ + return fdt_get_property_namelen_(fdt, offset, name, namelen, lenp, NULL); +} + +const struct fdt_property *fdt_get_property(const void *fdt, int nodeoffset, + const char *name, int *lenp) +{ + return fdt_get_property_namelen(fdt, nodeoffset, name, strlen(name), lenp); +} + +struct fdt_property *fdt_get_property_w(void *fdt, int nodeoffset, + const char *name, int *lenp) +{ + return (struct fdt_property*)(uintptr_t) + fdt_get_property(fdt, nodeoffset, name, lenp); +} + +static void fdt_del_last_string_(void *fdt, const char *s) +{ + int newlen = strlen(s) + 1; + fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) - newlen); +} + +static int fdt_splice_(void *fdt, void *splicepoint, int oldlen, int newlen) +{ + char *p, *end; + p = splicepoint; + end = (char*)fdt + fdt_data_size_(fdt); + if (((p + oldlen) < p) || ((p + oldlen) > end)) { + return -FDT_ERR_BADOFFSET; + } + if ((p < (char*)fdt) || ((end - oldlen + newlen) < (char*)fdt)) { + return -FDT_ERR_BADOFFSET; + } + if ((end - oldlen + newlen) > ((char*)fdt + fdt_totalsize(fdt))) { + return -FDT_ERR_NOSPACE; + } + memmove(p + newlen, p + oldlen, end - p - oldlen); + return 0; +} + +static int fdt_splice_struct_(void *fdt, void *p, int oldlen, int newlen) +{ + int err, delta; + + delta = newlen - oldlen; + if ((err = fdt_splice_(fdt, p, oldlen, newlen))) { + return err; + } + fdt_set_size_dt_struct(fdt, fdt_size_dt_struct(fdt) + delta); + fdt_set_off_dt_strings(fdt, fdt_off_dt_strings(fdt) + delta); + return 0; +} + +static int fdt_resize_property_(void *fdt, int nodeoffset, const char *name, + int len, struct fdt_property **prop) +{ + int err, oldlen; + + *prop = fdt_get_property_w(fdt, nodeoffset, name, &oldlen); + if (!*prop) { + return oldlen; + } + if ((err = fdt_splice_struct_(fdt, (*prop)->data, FDT_TAGALIGN(oldlen), + FDT_TAGALIGN(len)))) { + return err; + } + (*prop)->len = cpu_to_fdt32(len); + return 0; +} + +static int fdt_splice_string_(void *fdt, int newlen) +{ + int err; + void *p = (char*)fdt + fdt_off_dt_strings(fdt) + fdt_size_dt_strings(fdt); + + if ((err = fdt_splice_(fdt, p, 0, newlen))) { + return err; + } + fdt_set_size_dt_strings(fdt, fdt_size_dt_strings(fdt) + newlen); + return 0; +} + +const char *fdt_find_string_(const char *strtab, int tabsize, const char *s) +{ + int len = strlen(s) + 1; + const char *last = strtab + tabsize - len; + const char *p; + + for (p = strtab; p <= last; p++) { + if (memcmp(p, s, len) == 0) { + return p; + } + } + return NULL; +} + +static int fdt_find_add_string_(void *fdt, const char *s, int *allocated) +{ + int err, len; + char *strtab, *new; + const char *p; + + strtab = (char*)fdt + fdt_off_dt_strings(fdt); + len = strlen(s) + 1; + *allocated = 0; + p = fdt_find_string_(strtab, fdt_size_dt_strings(fdt), s); + if (p) { /* found it */ + return (p - strtab); + } + new = strtab + fdt_size_dt_strings(fdt); + err = fdt_splice_string_(fdt, len); + if (err) { + return err; + } + *allocated = 1; + + memcpy(new, s, len); + return (new - strtab); +} + +static int fdt_add_property_(void *fdt, int nodeoffset, const char *name, + int len, struct fdt_property **prop) +{ + int err, proplen, nextoffset, namestroff, allocated; + + if ((nextoffset = fdt_check_node_offset_(fdt, nodeoffset)) < 0) { + return nextoffset; + } + namestroff = fdt_find_add_string_(fdt, name, &allocated); + if (namestroff < 0) { + return namestroff; + } + + *prop = fdt_offset_ptr_w_(fdt, nextoffset); + proplen = sizeof(**prop) + FDT_TAGALIGN(len); + + err = fdt_splice_struct_(fdt, *prop, 0, proplen); + if (err) { + /* Delete the string if we failed to add it */ + if (allocated) + fdt_del_last_string_(fdt, name); + return err; + } + + (*prop)->tag = cpu_to_fdt32(FDT_PROP); + (*prop)->nameoff = cpu_to_fdt32(namestroff); + (*prop)->len = cpu_to_fdt32(len); + return 0; +} + +int fdt_setprop_placeholder(void *fdt, int nodeoffset, const char *name, + int len, void **prop_data) +{ + int err; + struct fdt_property *prop; + + err = fdt_totalsize(fdt); + if (err < 0) { + return err; + } + err = fdt_resize_property_(fdt, nodeoffset, name, len, &prop); + if (err == -FDT_ERR_NOTFOUND) { + err = fdt_add_property_(fdt, nodeoffset, name, len, &prop); + } + if (err) { + return err; + } + *prop_data = prop->data; + return 0; +} + +int fdt_setprop(void *fdt, int nodeoffset, const char *name, + const void *val, int len) +{ + int err; + void *prop_data; + + err = fdt_setprop_placeholder(fdt, nodeoffset, name, len, &prop_data); + if (err) { + return err; + } + if (len) { + memcpy(prop_data, val, len); + } + return 0; +} + +const void *fdt_getprop_namelen(const void *fdt, int nodeoffset, + const char *name, int namelen, int *lenp) +{ + int poffset; + const struct fdt_property *prop; + + prop = fdt_get_property_namelen_(fdt, nodeoffset, name, namelen, lenp, + &poffset); + if (!prop) { + return NULL; + } + + /* Handle realignment */ + if (fdt_version(fdt) < 0x10 && + (poffset + sizeof(*prop)) % 8 && fdt32_to_cpu(prop->len) >= 8) { + return prop->data + 4; + } + return prop->data; +} + +const void *fdt_getprop(const void *fdt, int nodeoffset, const char *name, + int *lenp) +{ + return fdt_getprop_namelen(fdt, nodeoffset, name, strlen(name), lenp); +} + +/* FTD Fixup API's */ +int fdt_fixup_str(void* fdt, int off, const char* node, const char* name, + const char* str) +{ + wolfBoot_printf("FDT: Set %s (%d), %s=%s\n", node, off, name, str); + fdt_setprop(fdt, off, name, str, strlen(str)+1); + return off; +} + +int fdt_fixup_val(void* fdt, int off, const char* node, const char* name, + uint32_t val) +{ + wolfBoot_printf("FDT: Set %s (%d), %s=%u\n", node, off, name, val); + val = cpu_to_fdt32(val); + fdt_setprop(fdt, off, name, &val, sizeof(val)); + return off; +} + +int fdt_find_devtype(void* fdt, int startoff, const char* node) +{ + int len, off; + const void* val; + const char* propname = "device_type"; + int nodelen = strlen(node)+1; + + for (off = fdt_next_node(fdt, startoff, NULL); + off >= 0; + off = fdt_next_node(fdt, off, NULL)) + { + val = fdt_getprop(fdt, off, propname, &len); + if (val && (len == nodelen) && (memcmp(val, node, len) == 0)) { + return off; + } + } + return off; /* return error from fdt_next_node() */ +} +#endif /* MMU && !BUILD_LOADER_STAGE1 */ diff --git a/src/image.c b/src/image.c index c272c74ce..8b2974f7c 100644 --- a/src/image.c +++ b/src/image.c @@ -884,14 +884,6 @@ int wolfBoot_open_image_address(struct wolfBoot_image *img, uint8_t *image) } #ifdef MMU -#ifndef WOLFBOOT_TPM /* tpm2_types.h has ByteReverseWord32 */ - #define WOLFSSL_MISC_INCLUDED /* allow misc.c code to be inlined */ - #include /* for ByteReverseWord32 */ -#endif /* !WOLFBOOT_TPM */ -static uint32_t wb_reverse_word32(uint32_t x) -{ - return ByteReverseWord32(x); -} /** * @brief Get the size of the Device Tree Blob (DTB). @@ -904,20 +896,10 @@ static uint32_t wb_reverse_word32(uint32_t x) */ int wolfBoot_get_dts_size(void *dts_addr) { - uint32_t hdr[2], magic, size; - - memcpy(hdr, dts_addr, sizeof(hdr)); - -#ifdef BIG_ENDIAN_ORDER - magic = wb_reverse_word32(hdr[0]); - size = hdr[1]; -#else - magic = hdr[0]; - size = wb_reverse_word32(hdr[1]); -#endif - return (magic == UBOOT_FDT_MAGIC) ? (int)size : -1; + return fdt_totalsize(dts_addr); } -#endif + +#endif /* MMU */ #ifdef WOLFBOOT_FIXED_PARTITIONS