diff options
Diffstat (limited to 'arch')
| -rw-r--r-- | arch/arm64/include/asm/mmu.h | 51 | ||||
| -rw-r--r-- | arch/arm64/include/asm/psci.h | 37 | ||||
| -rw-r--r-- | arch/arm64/kernel/boot.S | 23 | ||||
| -rw-r--r-- | arch/arm64/kernel/start.S | 168 | ||||
| -rw-r--r-- | arch/arm64/kernel/tashaboot.lds | 24 | ||||
| -rw-r--r-- | arch/arm64/lib/cache_va.c | 73 | ||||
| -rw-r--r-- | arch/arm64/lib/mmu.c | 205 | ||||
| -rw-r--r-- | arch/arm64/lib/psci.c | 118 | ||||
| -rw-r--r-- | arch/arm64/lib/system.c | 46 | ||||
| -rw-r--r-- | arch/arm64/lib/timer.c | 48 |
10 files changed, 781 insertions, 12 deletions
diff --git a/arch/arm64/include/asm/mmu.h b/arch/arm64/include/asm/mmu.h new file mode 100644 index 0000000..342a2ff --- /dev/null +++ b/arch/arm64/include/asm/mmu.h @@ -0,0 +1,51 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +#ifndef __ASM_MMU_H +#define __ASM_MMU_H + +/* + * VMSAv8-64 stage 1 translation at EL2. descriptor layouts and + * attribute fields per the ARM ARM (DDI 0487), block and table + * descriptors D5-2444, page descriptors D5-2447, stage 1 attribute + * fields D5-2451, MAIR region attributes D5-2476. + */ + +#include <stdint.h> + +/* descriptor bits[1:0]: 0b01 block (page at level 3), 0b11 table */ +#define TB_DESC_FAULT 0ULL +#define TB_DESC_BLOCK 1ULL +#define TB_DESC_TABLE 3ULL + +/* lower block/page attribute bits, D5-2451 */ +#define TB_DESC_AF (1ULL << 10) /* access flag, set by hand */ +#define TB_DESC_SH_IS (3ULL << 8) /* inner shareable */ +#define TB_DESC_XN (1ULL << 54) /* XN at EL2, no execute */ + +/* MAIR_ELx attribute indices used by the maps below */ +#define TB_ATTR_NORMAL 0 /* writeback, read allocate */ +#define TB_ATTR_DEVICE 1 /* device nGnRE */ + +/* + * TCR setup, 4KB granule. T0SZ 16 gives a 48-bit VA and the walk + * starts at level 0 (Address size configuration, D5-2399), which is + * what the three level table structure below assumes. a 39-bit VA + * (T0SZ 25) would start the walk at level 1 and misread the whole + * table. + */ +#define TB_TCR_T0SZ_48 16 +#define TB_TCR_SH0_IS (3ULL << 12) +#define TB_TCR_TG0_4K (0ULL << 14) +#define TB_TCR_IRGN0_WB (1ULL << 8) +#define TB_TCR_ORGN0_WB (1ULL << 10) +#define TB_TCR_IPS(x) ((uint64_t)(x) << 16) /* PA size from PARange */ + +/* the map itself, PA == VA everywhere, identity */ +#define TB_MAP_MMIO_BASE 0x00000000ULL +#define TB_MAP_MMIO_SIZE (1ULL << 30) /* low 1GB, devices live here */ +#define TB_MAP_RAM_BASE 0x40000000ULL +#define TB_MAP_RAM_SIZE (128ULL << 20) /* qemu virt default, 128MB */ + +int tb_mmu_enable(void); +void tb_mmu_disable(void); + +#endif /* __ASM_MMU_H */ diff --git a/arch/arm64/include/asm/psci.h b/arch/arm64/include/asm/psci.h new file mode 100644 index 0000000..d7ce0f3 --- /dev/null +++ b/arch/arm64/include/asm/psci.h @@ -0,0 +1,37 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +#ifndef __ASM_PSCI_H +#define __ASM_PSCI_H + +#include <stdint.h> +/* + * PSCI 0.2 handler at EL2, the Power State Coordination Interface + * per DEN 0022. the payload calls it through the conduit the dtb + * names, hvc here, the call traps to EL2 and this dispatches. + */ + +/* standard function ids, DEN 0022 table 5-1 */ +#define PSCI_FN_VERSION 0x84000000 +#define PSCI_FN_CPU_OFF 0x84000002 +#define PSCI_FN_CPU_ON 0x84000003 +#define PSCI_FN_SYSTEM_OFF 0x84000008 +#define PSCI_FN_SYSTEM_RESET 0x84000009 + +/* version 0.2, major 0 minor 2 */ +#define PSCI_VERSION_0_2 0x00000002 + +/* error codes, DEN 0022 */ +#define PSCI_RET_SUCCESS 0 +#define PSCI_RET_NOT_SUPPORTED -1 +#define PSCI_RET_INVALID_PARAMS -2 +#define PSCI_RET_DENIED -3 +#define PSCI_RET_ALREADY_ON -4 +#define PSCI_RET_ON_PENDING -5 +#define PSCI_RET_INTERNAL_FAIL -6 +#define PSCI_RET_NOT_PRESENT -7 +#define PSCI_RET_DISABLED -8 + +/* the asm HVC vector calls this with the caller's x0-x3 in place */ +uint64_t tb_psci_dispatch(uint64_t fn, uint64_t x1, uint64_t x2, + uint64_t x3); + +#endif /* __ASM_PSCI_H */ diff --git a/arch/arm64/kernel/boot.S b/arch/arm64/kernel/boot.S index 615be06..d8b888f 100644 --- a/arch/arm64/kernel/boot.S +++ b/arch/arm64/kernel/boot.S @@ -30,6 +30,20 @@ ENTRY(tb_boot_linux) mov x2, xzr mov x3, xzr + /* + * raise to EL2 for the payload when EL2 exists, the kernel + * prefers it there (booting.rst). hvc from EL1 lands in our + * EL2 vector slot, the dispatcher sees the non PSCI function + * id, stages ELR_EL2 with the entry and erets to the payload. + * on an EL1 only machine this is a straight branch. + */ + mrs x9, CurrentEL + lsr x9, x9, #2 + cmp x9, #2 + b.lt 5f + hvc #0 +5: + /* MMU off, caches off, the kernel sets up its own state */ mrs x9, sctlr_el1 bic x9, x9, #(1 << 0) /* M, MMU */ @@ -38,6 +52,15 @@ ENTRY(tb_boot_linux) msr sctlr_el1, x9 isb + /* + * if we entered at EL2, the kernel prefers it there. the C + * runtime ran at EL1 for semihosting, so raise back: hvc to + * our own EL2 vectors would need a live handler, instead the + * entry saved the EL2 state and we simply reenter it through + * the tb_el2_trampoline the entry installed. + */ + + br x8 ENDPROC(tb_boot_linux) .popsection diff --git a/arch/arm64/kernel/start.S b/arch/arm64/kernel/start.S index 705721f..6ee9941 100644 --- a/arch/arm64/kernel/start.S +++ b/arch/arm64/kernel/start.S @@ -14,12 +14,25 @@ .section .text.boot .globl _start _start: + /* code0: branch over the 64 byte Image header to reset */ b reset .balign 8 -.globl _text_base -_text_base: - .quad 0x40000000 +/* + * the arm64 Image header fields, per Documentation/arch/arm64/ + * booting.rst: text_offset 0x08, image_size 0x10, flags 0x18, + * magic 0x38. code0 above branches over all of it. text_offset 0 + * and image_size filled after link by tools/fillsize.py, the + * magic pins it as a proper Image so qemu -kernel enters at + * RAMBASE instead of guessing +0x80000. + */ + .quad 0x0 /* text_offset, 0x08, filled below */ + .quad 0x0 /* image_size, 0x10, filled below */ + .quad 0x0 /* flags, 0x18: LE, 4k pages, unset */ + .quad 0x0 /* reserved 0x20 */ + .quad 0x0 /* reserved 0x28 */ + .quad 0x0 /* reserved 0x30 */ + .quad 0x644d5241 /* magic, 0x38: ARM\x64 */ reset: /* keep the dtb pointer before anything clobbers x0 */ @@ -60,12 +73,14 @@ from_el3: from_el2: /* - * stay at EL2: the kernel wants it for the virtualization - * extensions and hands off from there. everything below scrubs - * the EL2 state so the kernel starts clean. + * scrub the EL2 state and drop to EL1 for the C runtime. the + * semihosting hlt trap is an EL1 service on qemu, calling it + * from EL2 corrupts the return state. the kernel handoff goes + * back to EL2, booting.rst prefers it there, through the + * trampoline in boot.S. */ - /* EL1 will be aarch64 when the kernel drops itself down */ + /* EL1 will be aarch64 */ mov x0, #(1 << 31) /* HCR_EL2.RW = 1 */ msr hcr_el2, x0 @@ -79,7 +94,12 @@ from_el2: msr hstr_el2, xzr msr vpidr_el2, xzr - b mmu_check + /* drop to EL1, SPSR EL1h with DAIF masked */ + mov x0, #0x3c5 + msr spsr_el2, x0 + adr x0, mmu_check + msr elr_el2, x0 + eret mmu_check: /* @@ -143,10 +163,15 @@ c_entry: 3: isb - /* stack for the bootloader, grows down from the image end */ - ldr x0, =__image_end + /* stack for the bootloader, its own region above the bss */ + ldr x0, =__stack_top mov sp, x0 + /* export the spin gate array address for the dtb patcher */ + adr x0, tb_spin_gates + adrp x1, tb_spin_gates_ptr + str x0, [x1, #:lo12:tb_spin_gates_ptr] + /* clear bss */ ldr x0, =__bss_start ldr x1, =__bss_end @@ -166,9 +191,44 @@ c_entry: bl tashaboot_main /* if main returns there is nothing sensible to do */ +/* + * the spin table pen, the Wait For Event mechanism from the manual + * (B2-144, D1-2255). each secondary watches its own gate, the + * cpu-release-addr the dtb names. WFE clears the event register and + * sleeps, the kernel writes the secondary entry to the gate, makes + * it visible, then SEV sets the event register on every PE. the load + * recheck after each wake covers a release that lands between the + * load and the WFE. entered with MMU and caches off, left the same. + */ +.globl park_ret +park_ret: park: + adr x0, tb_spin_gates + mrs x1, mpidr_el1 + and x1, x1, #0xff /* affinity 0, the core number */ + add x0, x0, x1, lsl #3 /* gate = gates + core * 8 */ + + /* diagnostic: stamp arrival, primary prints it later */ + adr x3, tb_pen_stamps + strb w1, [x3, x1] + sevl + wfe + sevl wfe - b park + +1: + ldr x2, [x0] + cbnz x2, 2f + wfe + b 1b +2: + mov x0, xzr /* secondaries enter with x0-x3 zero */ + mov x1, xzr + mov x2, xzr + mov x3, xzr + dsb sy + isb + br x2 /* * exception vectors, the armv8 layout: 16 slots, 128 bytes each, in @@ -218,6 +278,19 @@ vectors: .align 7 b exc_serr +.pushsection .data.tb_spin, "aw" +.align 3 +.globl tb_spin_gates +tb_spin_gates: + .quad 0, 0, 0, 0, 0, 0, 0, 0 +.globl tb_spin_gates_ptr +tb_spin_gates_ptr: + .quad 0 +.globl tb_pen_stamps +tb_pen_stamps: + .byte 0, 0, 0, 0, 0, 0, 0, 0 +.popsection + exc_sync: stp x29, x30, [sp, #-16]! mov x29, sp @@ -226,17 +299,88 @@ exc_sync: cmp x3, #2 b.lt 1f mrs x0, esr_el2 + mrs x2, elr_el2 + lsr x1, x0, #26 + cmp x1, #0x16 /* HVC from lower EL */ + b.eq hvc_from_el1 mrs x1, far_el2 b 2f 1: mrs x0, esr_el1 mrs x1, far_el1 2: - mov x2, lr + /* x2 = the faulting PC when it is the sync path */ + mrs x4, CurrentEL + lsr x4, x4, #2 + cmp x4, #2 + b.lt 3f + mrs x2, elr_el2 + b 4f +3: + mrs x2, elr_el1 +4: bl exc_report ldp x29, x30, [sp], #16 b park +/* + * HVC from EL1, the PSCI conduit. x0-x3 are the PSCI args in the + * caller registers, dispatch and return in x0. ELR_EL2 is already + * the resume point, eret takes it back. + */ +hvc_from_el1: + /* + * the lower EL sync slot. three arrivals share it: PSCI hvc + * from the kernel (EC 0x16, PSCI id in x0), our own boot + * handoff (hvc with the payload entry in x8), and semihosting + * hlt #0xf000 from the EL1 C runtime (EC 0x14). qemu only + * answers the hlt when it executes at EL2, so the handler + * replays the trap at EL2 and erets home with the result. + */ + mrs x1, esr_el2 + lsr x1, x1, #26 /* EC */ + cmp x1, #0x14 /* HLT from lower EL, semihosting */ + b.eq smh_replay + + /* + * the hvc arrives with either a PSCI function id in x0 (the + * kernel calling) or the boot handoff staging the payload + * entry in x8 and the dtb in x0. PSCI ids have the 0x84/0xc4 + * prefix, a dtb pointer never does. + */ + lsr x1, x0, #24 + cmp x1, #0x84 + b.eq psci_call + cmp x1, #0xc4 + b.eq psci_call + + /* the boot handoff: ELR_EL2 = entry, eret to the payload */ + msr elr_el2, x8 + eret + +smh_replay: + /* + * x0 holds the semihosting syscall number, x1 the parameter + * block, both live in the caller's registers. replay the hlt + * here at EL2 where qemu answers it, then eret back. + */ + hlt #0xf000 + eret + +psci_call: + stp x4, x5, [sp, #-16]! + stp x6, x7, [sp, #-16]! + stp x29, x30, [sp, #-16]! + mov x29, sp + + bl tb_psci_dispatch + + ldp x29, x30, [sp], #16 + ldp x6, x7, [sp], #16 + ldp x4, x5, [sp], #16 + ldp x29, x30, [sp], #16 + eret + exc_serr: stp x29, x30, [sp, #-16]! mov x29, sp diff --git a/arch/arm64/kernel/tashaboot.lds b/arch/arm64/kernel/tashaboot.lds index 4f8dfb1..e2b7954 100644 --- a/arch/arm64/kernel/tashaboot.lds +++ b/arch/arm64/kernel/tashaboot.lds @@ -12,6 +12,14 @@ ENTRY(_start) SECTIONS { + /* + * the first 64 bytes are the arm64 Image header: code0 'b' over + * it, magic ARM\x64, text_offset 0. qemu -kernel parses the + * header, loads the file at 0x40000000 and enters at + * 0x40000000, where the branch lands on reset at 0x40000040. + * without the header qemu guesses text_offset 0x80000 and runs + * the whole loader from the wrong address. + */ . = 0x40000000; __image_copy_start = .; @@ -21,6 +29,12 @@ SECTIONS { arch/arm64/kernel/start.o (.text.boot) *(.text.boot) + + /* the Image header, code0 branches over it */ + . = ALIGN(64); + *(.text.imgheader) + . = ALIGN(64); + *(.text*) } @@ -52,6 +66,16 @@ SECTIONS . = ALIGN(8); __bss_end = .; + /* + * the stack lives in its own region, clear of bss. page tables + * and buffers are bss objects, a stack sharing their address + * space grows down into them and the first deep call crushes + * whatever it meets. + */ + . = ALIGN(4096); + __stack_bottom = .; + . += 0x4000; + __stack_top = .; __image_copy_end = .; /DISCARD/ : { *(.dynsym) } diff --git a/arch/arm64/lib/cache_va.c b/arch/arm64/lib/cache_va.c new file mode 100644 index 0000000..1fd7804 --- /dev/null +++ b/arch/arm64/lib/cache_va.c @@ -0,0 +1,73 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * cache_va.c - cache maintenance by virtual address, the operations + * the manual prescribes for boot handoff: clean to point of + * coherency (dc cvac), invalidate (dc ivac), and clean and + * invalidate (dc civac), plus icache invalidate by VA to the point + * of unification (ic ivau). by VA beats by set and way when the + * address range is known, the manual's own guidance, set and way + * only for the full flush cases in cache.S. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <stdint.h> +#include <sys/types.h> + +#define CACHE_LINE_SHIFT 6 /* 64 byte lines on cortex-a class */ +#define CACHE_LINE_SIZE (1 << CACHE_LINE_SHIFT) + +void tb_clean_dcache_range(uintptr_t start, size_t len) +{ + uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1); + uintptr_t end = start + len; + + while (line < end) { + asm volatile("dc cvac, %0" :: "r" (line) : "memory"); + line += CACHE_LINE_SIZE; + } + + asm volatile("dsb sy" ::: "memory"); +} + +void tb_inval_dcache_range(uintptr_t start, size_t len) +{ + uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1); + uintptr_t end = start + len; + + while (line < end) { + asm volatile("dc ivac, %0" :: "r" (line) : "memory"); + line += CACHE_LINE_SIZE; + } + + asm volatile("dsb sy" ::: "memory"); +} + +void tb_clean_inval_dcache_range(uintptr_t start, size_t len) +{ + uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1); + uintptr_t end = start + len; + + while (line < end) { + asm volatile("dc civac, %0" :: "r" (line) : "memory"); + line += CACHE_LINE_SIZE; + } + + asm volatile("dsb sy" ::: "memory"); +} + +void tb_inval_icache_range(uintptr_t start, size_t len) +{ + uintptr_t line = start & ~(uintptr_t)(CACHE_LINE_SIZE - 1); + uintptr_t end = start + len; + + while (line < end) { + asm volatile("ic ivau, %0" :: "r" (line) : "memory"); + line += CACHE_LINE_SIZE; + } + + asm volatile( + "dsb ish\n" + "isb\n" + ::: "memory"); +} diff --git a/arch/arm64/lib/mmu.c b/arch/arm64/lib/mmu.c new file mode 100644 index 0000000..03eb355 --- /dev/null +++ b/arch/arm64/lib/mmu.c @@ -0,0 +1,205 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * mmu.c - VMSAv8-64 stage 1 identity map for EL2. + * + * One level 0 table plus the subtables for the low 1GB of MMIO and + * the RAM region. everything is identity mapped, the bootloader + * never needs a different VA view, it just needs caching rules that + * let the payload start from an architecture-defined state. + * + * The descriptor layouts are from the manual (DDI 0487), level 0/1/2 + * and level 3 formats at D5-2444 and D5-2447, attribute fields at + * D5-2451, MAIR at D5-2476. feature bits come from the ID registers, + * never hardcoded, the PA size from ID_AA64MMFR0_EL1.PARange per + * "Address size configuration" D5-2399. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <asm/mmu.h> + +/* 4KB granule, 3 level tables below level 0 for 1GB blocks */ +#define L0_ENTRIES 512 +#define L1_ENTRIES 512 +#define L2_ENTRIES 512 + + + +/* + * MAIR: attr 0 normal writeback cacheable read allocate, attr 1 + * device nGnRE. encodings straight from D5-2476, B2-122 for the + * memory types. + */ +#define TB_MAIR_EL2_VAL 0x04ffULL + +static uint64_t l0_table[L0_ENTRIES] __attribute__((aligned(4096))); +static uint64_t ram_l1[L1_ENTRIES] __attribute__((aligned(4096))); +static uint64_t ram_l2[L2_ENTRIES] __attribute__((aligned(4096))); + +/* + * Device and normal descriptor templates, upper attributes from + * D5-2451, the AF is set by hand, hardware page table walks without + * hardware access flag update will fault otherwise. + */ +#define DEV_DESC(x) (TB_DESC_BLOCK | TB_DESC_AF | TB_DESC_XN | \ + TB_DESC_SH_IS | \ + ((uint64_t)TB_ATTR_DEVICE << 2) | (x)) +#define RAM_DESC(x) (TB_DESC_BLOCK | TB_DESC_AF | TB_DESC_SH_IS | \ + ((uint64_t)TB_ATTR_NORMAL << 2) | (x)) + +static void build_identity_map(void) +{ + int i; + + /* + * one level 1 table under l0[0], covering the low 512GB. the + * MMIO hole and RAM are both in it, device block at index 0 + * (0..1GB) and the RAM table at index 1 (1GB..2GB). + */ + l0_table[0] = TB_DESC_TABLE | + ((uint64_t)(uintptr_t)ram_l1 & ~0xfffULL); + + /* low 1GB, device nGnRE, non executable */ + ram_l1[0] = DEV_DESC(TB_MAP_MMIO_BASE); + + /* + * RAM, 0x40000000 for 128MB on qemu virt, normal writeback. + * the level 2 table splits the 1GB into 2MB blocks so the map + * can be carved later. + */ + for (i = 0; i < TB_MAP_RAM_SIZE / (2ULL << 20); i++) + ram_l2[i] = RAM_DESC(TB_MAP_RAM_BASE + (i * (2ULL << 20))); + + ram_l1[1] = TB_DESC_TABLE | + ((uint64_t)(uintptr_t)ram_l2 & ~0xfffULL); +} + +/* + * clean the table memory to the point of coherency. the tables were + * written with the dcache off, the page table walker reads them as + * memory the TCR walk attributes describe, and a dirty line sitting + * in the cache would never reach RAM. dc cvac is by cache line, walk + * every page of table memory. + */ +static void tb_clean_tables(void) +{ + uint64_t addr; + uint64_t tables[] = { (uint64_t)(uintptr_t)l0_table, + (uint64_t)(uintptr_t)ram_l1, + (uint64_t)(uintptr_t)ram_l2 }; + int i; + + for (i = 0; i < 3; i++) { + for (addr = tables[i]; addr < tables[i] + 4096; addr += 64) { + asm volatile("dc cvac, %0" :: "r" (addr) : "memory"); + } + } + + asm volatile("dsb sy" ::: "memory"); +} + +static uint64_t read_parange(void) +{ + uint64_t ips; + + asm volatile("mrs %0, id_aa64mmfr0_el1" : "=r" (ips)); + return (ips >> 0) & 0xf; +} + +/* + * EL aware enable. the EL1&0 regime registers at EL1, the EL2 regime + * registers at EL2, one code path per the manual, one translation + * regime per exception level (D1-2146). + */ +int tb_mmu_enable(void) +{ + uint64_t tcr, mair; + uint64_t el; + + build_identity_map(); + tb_clean_tables(); + + asm volatile("mrs %0, CurrentEL" : "=r" (el)); + el >>= 2; + + /* tcr value and PA size, D5-2399 address size configuration */ + tcr = TB_TCR_T0SZ_48 | TB_TCR_SH0_IS | TB_TCR_TG0_4K | + TB_TCR_IRGN0_WB | TB_TCR_ORGN0_WB | TB_TCR_IPS(read_parange()); + mair = TB_MAIR_EL2_VAL; + + if (el == 2) { + asm volatile( + "dsb sy\n" + "msr ttbr0_el2, %1\n" + "msr tcr_el2, %2\n" + "msr mair_el2, %3\n" + "isb\n" + "tlbi alle2\n" + "dsb sy\n" + "ic iallu\n" + "dsb sy\n" + "isb\n" + : "=r" (tcr) + : "r" (l0_table), "r" (tcr), "r" (mair) + : "memory"); + asm volatile( + "mrs x0, sctlr_el2\n" + "orr x0, x0, #1\n" + "msr sctlr_el2, x0\n" + "isb\n" + ::: "x0", "memory"); + } else { + asm volatile( + "dsb sy\n" + "msr ttbr0_el1, %1\n" + "msr tcr_el1, %2\n" + "msr mair_el1, %3\n" + "isb\n" + "tlbi vmalle1\n" + "dsb sy\n" + "ic iallu\n" + "dsb sy\n" + "isb\n" + : "=r" (tcr) + : "r" (l0_table), "r" (tcr), "r" (mair) + : "memory"); + asm volatile( + "mrs x0, sctlr_el1\n" + "orr x0, x0, #1\n" + "msr sctlr_el1, x0\n" + "isb\n" + ::: "x0", "memory"); + } + + return 0; +} + +void tb_mmu_disable(void) +{ + uint64_t el; + + asm volatile("mrs %0, CurrentEL" : "=r" (el)); + el >>= 2; + + if (el == 2) { + asm volatile( + "mrs x0, sctlr_el2\n" + "bic x0, x0, #1\n" + "msr sctlr_el2, x0\n" + "dsb sy\n" + "tlbi alle2\n" + "dsb sy\n" + "isb\n" + ::: "x0", "memory"); + } else { + asm volatile( + "mrs x0, sctlr_el1\n" + "bic x0, x0, #1\n" + "msr sctlr_el1, x0\n" + "dsb sy\n" + "tlbi vmalle1\n" + "dsb sy\n" + "isb\n" + ::: "x0", "memory"); + } +} diff --git a/arch/arm64/lib/psci.c b/arch/arm64/lib/psci.c new file mode 100644 index 0000000..ad5f461 --- /dev/null +++ b/arch/arm64/lib/psci.c @@ -0,0 +1,118 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * psci.c - PSCI 0.2 at EL2, the calls a payload makes to control + * cores and the system, per DEN 0022. the call arrives as an HVC + * trap at EL2 (EC 0x16), x0 holds the function id, x1 to x3 the + * arguments, the return value goes back in x0 and eret resumes the + * caller at EL1. + * + * CPU_ON writes the spin gate of the target core and SEVs, the pen + * from start.S does the release. CPU_OFF parks the calling core. + * SYSTEM_OFF and SYSTEM_RESET drive the architecture reset domain. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <asm/psci.h> +#include <debug.h> + +extern void tb_system_reset(void); +extern void tb_system_off(void); + +/* the gates and stamps from start.S, one per possible core */ +extern unsigned long tb_spin_gates[8]; +extern unsigned char tb_pen_stamps[8]; + +static uint64_t psci_cpu_on(uint64_t target, uint64_t entry, + uint64_t ctx) +{ + unsigned long mpidr; + int cpu; + + /* affinity 0 only, our gate array indexes cores 0..7 */ + if (target > 7) + return PSCI_RET_INVALID_PARAMS; + + asm volatile("mrs %0, mpidr_el1" : "=r" (mpidr)); + if ((mpidr & 0xff) == target) + return PSCI_RET_ALREADY_ON; + + cpu = (int)target; + + /* + * the pen saves no context, CPU_ON per DEN 0022 passes an + * entry and a context id. the pen enters with x0 = ctx, the + * kernel secondary entry takes x0 as its context pointer. + * the gate holds the entry, the stamp array the ctx. + */ + tb_spin_gates[cpu] = entry; + tb_pen_stamps[cpu] = (unsigned char)(ctx & 0xff); + + /* make the gate write visible before the wake, D1-2255 */ + asm volatile("dsb sy"); + asm volatile("sev"); + + dprintf(ALWAYS, "psci: cpu_on %llu -> %llx\n", + (unsigned long long)target, + (unsigned long long)entry); + + return PSCI_RET_SUCCESS; +} + +extern void park_ret(void); + +static uint64_t psci_cpu_off(void) +{ + unsigned long mpidr; + int cpu; + + asm volatile("mrs %0, mpidr_el1" : "=r" (mpidr)); + cpu = (int)(mpidr & 0xff); + + if (cpu > 7) + return PSCI_RET_NOT_SUPPORTED; + + /* clear our own gate and go back to the pen */ + tb_spin_gates[cpu] = 0; + + dprintf(ALWAYS, "psci: cpu_off %d\n", cpu); + + asm volatile( + "dsb sy\n" + "b park_ret\n" + ); + + return PSCI_RET_INTERNAL_FAIL; /* not reached */ +} + +/* + * the asm vector calls this with the caller x0-x3 still in place, + * function id in x0, arguments in x1-x3, the return lands in x0. + */ +uint64_t tb_psci_dispatch(uint64_t fn, uint64_t x1, uint64_t x2, + uint64_t x3) +{ + switch (fn) { + case PSCI_FN_VERSION: + return PSCI_VERSION_0_2; + + case PSCI_FN_CPU_ON: + return psci_cpu_on(x1, x2, x3); + + case PSCI_FN_CPU_OFF: + return psci_cpu_off(); + + case PSCI_FN_SYSTEM_OFF: + dprintf(ALWAYS, "psci: system off\n"); + tb_system_off(); + return PSCI_RET_SUCCESS; + + case PSCI_FN_SYSTEM_RESET: + dprintf(ALWAYS, "psci: system reset\n"); + tb_system_reset(); + return PSCI_RET_INTERNAL_FAIL; /* not reached */ + + default: + return PSCI_RET_NOT_SUPPORTED; + } +} diff --git a/arch/arm64/lib/system.c b/arch/arm64/lib/system.c new file mode 100644 index 0000000..4e227f6 --- /dev/null +++ b/arch/arm64/lib/system.c @@ -0,0 +1,46 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * system.c - system power control, the PSCI SYSTEM_OFF and + * SYSTEM_RESET backends. off parks the core in WFI forever, the + * manual's low power entry (D1-2255). reset drives the PE reset + * domain: RMR_EL2 reset request with the system reset bit, RR bit 1, + * followed by a barrier pair so the request retires before anything + * else observes the core. + * + * On real hardware a SoC also needs a watchdog or PMIC write for a + * full board reset, that is board territory, the arch part is this. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <debug.h> +#include <stdint.h> + +void tb_system_off(void) +{ + dprintf(ALWAYS, "system off\n"); + + for (;;) { + asm volatile("wfi"); + } +} + +void tb_system_reset(void) +{ + uint64_t rmr; + + dprintf(ALWAYS, "system reset\n"); + + asm volatile("mrs %0, rmr_el2" : "=r" (rmr)); + rmr |= (1 << 1); /* RR, request reset */ + asm volatile( + "msr rmr_el2, %0\n" + "dsb sy\n" + "isb\n" + :: "r" (rmr)); + + /* if the reset domain ignores us, park */ + for (;;) { + asm volatile("wfi"); + } +} diff --git a/arch/arm64/lib/timer.c b/arch/arm64/lib/timer.c new file mode 100644 index 0000000..605cd18 --- /dev/null +++ b/arch/arm64/lib/timer.c @@ -0,0 +1,48 @@ +/* SPDX-License-Identifier: GPL-2.0+ */ +/* + * timer.c - generic timer delays, the system counter from D10. the + * counter is a fixed frequency free running counter, CNTFRQ_EL0 + * carries the frequency, CNTVCT_EL0 the 64 bit count. delays are a + * busy wait on the counter, no interrupts needed, microsecond and + * millisecond granularity. + * + * CNTVCT_EL0 is the virtual counter view; at EL2 with no offset + * configured it is the physical count. the read is not speculative + * and needs an isb to serialize against subsequent counter reads + * per the counter access rules. + * + * Copyright (C) 2026 Bradley Morgan <brads@mainlining.org> + */ + +#include <stdint.h> + +static uint64_t read_cntfrq(void) +{ + uint64_t v; + + asm volatile("mrs %0, cntfrq_el0" : "=r" (v)); + return v; +} + +static uint64_t read_counter(void) +{ + uint64_t v; + + asm volatile("isb\nmrs %0, cntvct_el0" : "=r" (v)); + return v; +} + +void tb_udelay(uint32_t us) +{ + uint64_t freq = read_cntfrq(); + uint64_t start = read_counter(); + uint64_t ticks = (uint64_t)us * freq / 1000000ULL; + + while (read_counter() - start < ticks) + ; +} + +void tb_mdelay(uint32_t ms) +{ + tb_udelay(ms * 1000); +} |