|
42 | 42 | /* Fixed addresses (provided by the linker script) */ |
43 | 43 | extern void *kernel_addr, *update_addr, *dts_addr; |
44 | 44 |
|
| 45 | +#if defined(HAVE_FIPS) |
| 46 | +void cm4_mmu_enable(void); /* defined below; called from hal_init */ |
| 47 | +void cm4_mmu_disable(void); /* defined below; called from hal_prepare_boot */ |
| 48 | +#endif |
| 49 | + |
45 | 50 | #if defined(DEBUG_UART) |
46 | 51 | static void uart_tx(char c) |
47 | 52 | { |
@@ -129,12 +134,135 @@ void hal_init(void) |
129 | 134 | wolfBoot_printf("wolfBoot CM4 (BCM2711 Cortex-A72) hal_init, EL%d\n", |
130 | 135 | (int)((el >> 2) & 0x3)); |
131 | 136 | #endif |
| 137 | +#if defined(HAVE_FIPS) |
| 138 | + /* Bring up Normal cacheable memory before the FIPS POST, which uses |
| 139 | + * unaligned / SIMD accesses that the MMU-off Device memory rejects. */ |
| 140 | + cm4_mmu_enable(); |
| 141 | +#endif |
132 | 142 | } |
133 | 143 |
|
134 | 144 | void hal_prepare_boot(void) |
135 | 145 | { |
| 146 | +#if defined(HAVE_FIPS) |
| 147 | + /* Undo cm4_mmu_enable() before handoff: flush the app out of the D-cache |
| 148 | + * and return to the MMU-off state the application expects. */ |
| 149 | + cm4_mmu_disable(); |
| 150 | +#endif |
136 | 151 | } |
137 | 152 |
|
| 153 | +#if defined(HAVE_FIPS) |
| 154 | +/* Minimal identity-mapped MMU + caches for the CM4. wolfBoot's simple startup |
| 155 | + * runs with the MMU off, so all memory is Device-nGnRnE, which faults on the |
| 156 | + * unaligned / 128-bit SIMD accesses the FIPS module and newlib printf perform. |
| 157 | + * Mapping DDR as Normal (cacheable) permits those accesses and speeds up the |
| 158 | + * crypto; the peripheral region (incl. 0xFE000000) stays Device. |
| 159 | + * Four 1GB block descriptors cover the 32-bit VA space at translation level 1. */ |
| 160 | +#define MMU_BLOCK_NORMAL 0x0000000000000701ULL /* block, AttrIdx0, AF, SH inner */ |
| 161 | +#define MMU_BLOCK_DEVICE 0x0000000000000405ULL /* block, AttrIdx1, AF, SH none */ |
| 162 | + |
| 163 | +static volatile uint64_t cm4_l1_table[512] __attribute__((aligned(4096))); |
| 164 | + |
| 165 | +void cm4_mmu_enable(void) |
| 166 | +{ |
| 167 | + unsigned long sctlr; |
| 168 | + int i; |
| 169 | + |
| 170 | + /* 0-3GB DDR -> Normal; 3-4GB peripherals (0xFE000000) -> Device. */ |
| 171 | + for (i = 0; i < 4; i++) { |
| 172 | + uint64_t base = (uint64_t)i << 30; |
| 173 | + cm4_l1_table[i] = base | ((i == 3) ? MMU_BLOCK_DEVICE : MMU_BLOCK_NORMAL); |
| 174 | + } |
| 175 | + /* MAIR: Attr0 = 0xFF Normal WB write-alloc, Attr1 = 0x00 Device-nGnRnE. */ |
| 176 | + __asm__ volatile("msr mair_el2, %0" :: "r"(0x00000000000000FFUL)); |
| 177 | + __asm__ volatile("msr ttbr0_el2, %0" |
| 178 | + :: "r"((uint64_t)(uintptr_t)cm4_l1_table)); |
| 179 | + /* TCR_EL2: T0SZ=32 (32-bit VA), 4KB granule, WB cacheable inner-shareable |
| 180 | + * table walks, 36-bit PA. */ |
| 181 | + __asm__ volatile("msr tcr_el2, %0" :: "r"(0x0000000000013520UL)); |
| 182 | + __asm__ volatile("isb"); |
| 183 | + __asm__ volatile("tlbi alle2"); |
| 184 | + __asm__ volatile("dsb sy"); |
| 185 | + __asm__ volatile("ic iallu"); |
| 186 | + __asm__ volatile("dsb sy"); |
| 187 | + __asm__ volatile("isb"); |
| 188 | + /* SCTLR_EL2: enable MMU (M), data cache (C), instruction cache (I). */ |
| 189 | + __asm__ volatile("mrs %0, sctlr_el2" : "=r"(sctlr)); |
| 190 | + sctlr |= (1UL << 0) | (1UL << 2) | (1UL << 12); |
| 191 | + __asm__ volatile("msr sctlr_el2, %0" :: "r"(sctlr)); |
| 192 | + __asm__ volatile("isb"); |
| 193 | +} |
| 194 | + |
| 195 | +/* Clean+invalidate all data cache levels by set/way (dc cisw). */ |
| 196 | +static void cm4_dcache_flush_all(void) |
| 197 | +{ |
| 198 | + uint64_t clidr, ccsidr; |
| 199 | + unsigned int level, loc, ctype, linesize, ways, sets, way, set, wayshift; |
| 200 | + |
| 201 | + __asm__ volatile("dsb sy"); |
| 202 | + __asm__ volatile("mrs %0, clidr_el1" : "=r"(clidr)); |
| 203 | + loc = (unsigned int)((clidr >> 24) & 0x7); /* Level of Coherency */ |
| 204 | + for (level = 0; level < loc; level++) { |
| 205 | + ctype = (unsigned int)((clidr >> (level * 3)) & 0x7); |
| 206 | + if (ctype < 2) /* no data/unified cache at this level */ |
| 207 | + continue; |
| 208 | + __asm__ volatile("msr csselr_el1, %0" :: "r"((uint64_t)(level << 1))); |
| 209 | + __asm__ volatile("isb"); |
| 210 | + __asm__ volatile("mrs %0, ccsidr_el1" : "=r"(ccsidr)); |
| 211 | + linesize = (unsigned int)(ccsidr & 0x7) + 4; /* log2(bytes) */ |
| 212 | + ways = (unsigned int)((ccsidr >> 3) & 0x3FF); /* assoc - 1 */ |
| 213 | + sets = (unsigned int)((ccsidr >> 13) & 0x7FFF); /* sets - 1 */ |
| 214 | + wayshift = (unsigned int)__builtin_clz(ways); |
| 215 | + for (set = 0; set <= sets; set++) { |
| 216 | + for (way = 0; way <= ways; way++) { |
| 217 | + uint64_t val = ((uint64_t)(level << 1)) |
| 218 | + | ((uint64_t)way << wayshift) |
| 219 | + | ((uint64_t)set << linesize); |
| 220 | + __asm__ volatile("dc cisw, %0" :: "r"(val)); |
| 221 | + } |
| 222 | + } |
| 223 | + } |
| 224 | + __asm__ volatile("dsb sy"); |
| 225 | + __asm__ volatile("isb"); |
| 226 | +} |
| 227 | + |
| 228 | +/* Tear down the MMU/caches before boot handoff: clean the freshly-copied app |
| 229 | + * out of the D-cache to memory, disable the MMU and caches, and invalidate the |
| 230 | + * I-cache/TLB. Returns the CPU to the MMU-off state the application (and the |
| 231 | + * ARM64 Linux boot protocol) expects. */ |
| 232 | +void cm4_mmu_disable(void) |
| 233 | +{ |
| 234 | + unsigned long sctlr; |
| 235 | + |
| 236 | + cm4_dcache_flush_all(); |
| 237 | + __asm__ volatile("mrs %0, sctlr_el2" : "=r"(sctlr)); |
| 238 | + sctlr &= ~((1UL << 0) | (1UL << 2) | (1UL << 12)); /* clear M, C, I */ |
| 239 | + __asm__ volatile("msr sctlr_el2, %0" :: "r"(sctlr)); |
| 240 | + __asm__ volatile("isb"); |
| 241 | + __asm__ volatile("ic iallu"); |
| 242 | + __asm__ volatile("tlbi alle2"); |
| 243 | + __asm__ volatile("dsb sy"); |
| 244 | + __asm__ volatile("isb"); |
| 245 | +} |
| 246 | +#endif /* HAVE_FIPS */ |
| 247 | + |
| 248 | +#if defined(DEBUG) && defined(DEBUG_UART) |
| 249 | +/* CM4 bring-up diagnostic: exception handler invoked from cm4_vectors in |
| 250 | + * src/boot_aarch64_start.S. Dumps the fault syndrome so a data/instruction |
| 251 | + * abort shows up over UART instead of hanging silently. Built only with |
| 252 | + * DEBUG + DEBUG_UART. ESR_EL2[31:26] = exception class. */ |
| 253 | +void cm4_fault_handler(unsigned long esr, unsigned long elr, unsigned long far); |
| 254 | +void cm4_fault_handler(unsigned long esr, unsigned long elr, unsigned long far) |
| 255 | +{ |
| 256 | + wolfBoot_printf("\n*** CM4 EXCEPTION ***\n"); |
| 257 | + wolfBoot_printf("ESR_EL2=0x%08x EC=0x%02x\n", |
| 258 | + (unsigned)esr, (unsigned)((esr >> 26) & 0x3F)); |
| 259 | + wolfBoot_printf("ELR_EL2=0x%08x%08x\n", |
| 260 | + (unsigned)(elr >> 32), (unsigned)elr); |
| 261 | + wolfBoot_printf("FAR_EL2=0x%08x%08x\n", |
| 262 | + (unsigned)(far >> 32), (unsigned)far); |
| 263 | +} |
| 264 | +#endif /* DEBUG && DEBUG_UART */ |
| 265 | + |
138 | 266 | #if defined(HAVE_FIPS) |
139 | 267 | /* FIPS DRBG entropy seed from the BCM2711 RNG200 hardware TRNG. Registered via |
140 | 268 | * CUSTOM_RAND_GENERATE_SEED in include/user_settings.h. The RNG200 has NIST |
@@ -176,18 +304,6 @@ int wolfBoot_fips_seed(unsigned char* output, unsigned int sz) |
176 | 304 | for (i = 0; i < n; i++) |
177 | 305 | output[pos++] = (unsigned char)(word >> (i * 8)); |
178 | 306 | } |
179 | | - |
180 | | -#if defined(DEBUG_UART) |
181 | | - { |
182 | | - static int traced = 0; |
183 | | - if (!traced) { |
184 | | - wolfBoot_printf("RNG200 seed %02x %02x %02x %02x %02x %02x %02x %02x\n", |
185 | | - output[0], output[1], output[2], output[3], |
186 | | - output[4], output[5], output[6], output[7]); |
187 | | - traced = 1; |
188 | | - } |
189 | | - } |
190 | | -#endif |
191 | 307 | return 0; |
192 | 308 | } |
193 | 309 | #endif /* HAVE_FIPS */ |
|
0 commit comments