[linux_boot]新增 Linux EL2 启动、Stage-2 内存映射及 PSCI HVC 转发支持

ccj · ccj · commit b06846d7669b · 2026-05-07T18:01:55.000+08:00
diff --git a/bsp/qemu-virt64-aarch64/applications/linux_trampoline.S b/bsp/qemu-virt64-aarch64/applications/linux_trampoline.S
@@ -8,17 +8,206 @@
 
 #define LINUX_IMAGE_ENTRY       0x48200000
 #define LINUX_DTB_ADDRESS       0x4f000000
+#define LINUX_RAM_BASE          0x48000000
+#define LINUX_RAM_2M_BLOCKS     64
+#define VIRTIO_MMIO_BASE        0x0a000000
+#define GIC_2M_BASE             0x08000000
+#define GICD_BASE               0x08000000
+#define GICC_BASE               0x08010000
+#define GICH_BASE               0x08030000
+#define HCR_EL2_RW              (1 << 31)
+#define HCR_EL2_SWIO            (1 << 1)
+#define HCR_EL2_VM              (1 << 0)
+#define SPSR_EL1H_DAIF          0x3c5
+#define VTCR_EL2_RES1           (1 << 31)
+#define VTCR_EL2_SL0_LVL1       (1 << 6)
+#define VTCR_EL2_IRGN0_WBWA     (1 << 8)
+#define VTCR_EL2_ORGN0_WBWA     (1 << 10)
+#define VTCR_EL2_SH0_INNER      (3 << 12)
+#define S2_TABLE_DESC           0x3
+#define S2_BLOCK_NORMAL_RW      0x7fd
+#define S2_BLOCK_DEVICE_RW      0x4c1
+#define S2_PAGE_DEVICE_RW       0x4c3
 
+
+
+/* linux的启动入口,先搭建stage-2 */
     .section ".text.linux_trampoline", "ax"
     .align 6
     .global linux_cpu3_trampoline
 linux_cpu3_trampoline:
+    /*
+     * PSCI enters the target CPU at EL2 on QEMU virt with virtualization=on.
+     * Keep EL2 owned by our monitor, then enter Linux as an EL1 kernel.
+     */
+    mrs     x5, CurrentEL
+    lsr     x5, x5, #2
+    cmp     x5, #2
+    b.ne    .linux_enter_current_el /* 跳到 Linux 镜像入口 */
+
+    ldr     x5, =linux_cpu3_hyp_stack_top
+    mov     sp, x5
+
+    ldr     x5, =hyp_vectors
+    msr     vbar_el2, x5            /* EL2 异常有入口 */
+    isb
+
+    mrs     x5, cnthctl_el2
+    orr     x5, x5, #0x3
+    msr     cnthctl_el2, x5         /* 放开 EL1 访问物理计时器 */
+    msr     cntvoff_el2, xzr
+
+    bl      linux_stage2_setup
+
+    movz    x5, #(HCR_EL2_RW >> 16), lsl #16
+    orr     x5, x5, #HCR_EL2_SWIO
+    orr     x5, x5, #HCR_EL2_VM
+    msr     hcr_el2, x5             /* 再写 HCR_EL2 打开虚拟化翻译 */
+
+    mov     x5, #SPSR_EL1H_DAIF
+    msr     spsr_el2, x5
+
+    ldr     x0, =LINUX_DTB_ADDRESS
+    mov     x1, xzr
+    mov     x2, xzr
+    mov     x3, xzr
+    ldr     x5, =LINUX_IMAGE_ENTRY
+    msr     elr_el2, x5
+    eret
+
+.linux_enter_current_el:
     ldr     x0, =LINUX_DTB_ADDRESS
     mov     x1, xzr
     mov     x2, xzr
     mov     x3, xzr
     ldr     x4, =LINUX_IMAGE_ENTRY
     br      x4
 
+/* 真正建 Stage-2 */
+/* 4 块内存缓冲区，运行时被手工填成 Stage-2 页表 */
+linux_stage2_setup:
+    ldr     x6, =linux_stage2_l1
+    mov     x7, x6
+    mov     x8, #0x4000
+
+.linux_stage2_clear:
+    str     xzr, [x7], #8
+    subs    x8, x8, #8
+    b.ne    .linux_stage2_clear
+
+    ldr     x6, =linux_stage2_l1
+    ldr     x7, =linux_stage2_l2_low
+    ldr     x8, =linux_stage2_l2_high
+    ldr     x9, =linux_stage2_l3_gic
+
+    orr     x10, x7, #S2_TABLE_DESC
+    str     x10, [x6, #0]
+    orr     x10, x8, #S2_TABLE_DESC
+    str     x10, [x6, #8]
+
+    orr     x10, x9, #S2_TABLE_DESC
+    str     x10, [x7, #(64 * 8)]
+
+    ldr     x10, =VIRTIO_MMIO_BASE
+    lsr     x11, x10, #21
+    and     x11, x11, #0x1ff
+    movz    x12, #S2_BLOCK_DEVICE_RW
+    orr     x13, x10, x12
+    str     x13, [x7, x11, lsl #3]
+
+    ldr     x10, =LINUX_RAM_BASE
+    mov     x11, #LINUX_RAM_2M_BLOCKS
+    movz    x12, #S2_BLOCK_NORMAL_RW
+
+.linux_stage2_map_ram:
+    lsr     x13, x10, #21
+    and     x13, x13, #0x1ff
+    orr     x14, x10, x12
+    str     x14, [x8, x13, lsl #3]
+    add     x10, x10, #0x200000
+    subs    x11, x11, #1
+    b.ne    .linux_stage2_map_ram
+
+    /*
+     * Keep GICD writable for now so Linux can finish its early interrupt
+     * controller bring-up.  Once console output is stable again we can
+     * re-introduce finer-grained GICD trapping incrementally.
+     */
+    ldr     x10, =GICD_BASE
+    mov     x11, #16
+    movz    x12, #S2_PAGE_DEVICE_RW     /* 如果设置为只读权限就会被stage-2拦截 */
+
+.linux_stage2_map_gicd:
+    lsr     x13, x10, #12
+    and     x13, x13, #0x1ff
+    orr     x14, x10, x12
+    str     x14, [x9, x13, lsl #3]
+    add     x10, x10, #0x1000
+    subs    x11, x11, #1
+    b.ne    .linux_stage2_map_gicd
+
+    ldr     x10, =GICC_BASE
+    mov     x11, #16
+    movz    x12, #S2_PAGE_DEVICE_RW
+
+.linux_stage2_map_gicc:
+    lsr     x13, x10, #12
+    and     x13, x13, #0x1ff
+    orr     x14, x10, x12
+    str     x14, [x9, x13, lsl #3]
+    add     x10, x10, #0x1000
+    subs    x11, x11, #1
+    b.ne    .linux_stage2_map_gicc
+
+    ldr     x10, =GICH_BASE
+    mov     x11, #32
+    movz    x12, #S2_PAGE_DEVICE_RW
+
+.linux_stage2_map_gich_gicv:
+    lsr     x13, x10, #12
+    and     x13, x13, #0x1ff
+    orr     x14, x10, x12
+    str     x14, [x9, x13, lsl #3]
+    add     x10, x10, #0x1000
+    subs    x11, x11, #1
+    b.ne    .linux_stage2_map_gich_gicv
+
+    ldr     x6, =linux_stage2_l1
+    movz    x7, #1, lsl #48
+    orr     x6, x6, x7
+    msr     vttbr_el2, x6
+
+    mrs     x7, id_aa64mmfr0_el1
+    and     x7, x7, #0xf
+    lsl     x7, x7, #16
+    movz    x6, #(VTCR_EL2_RES1 >> 16), lsl #16
+    orr     x6, x6, #32
+    orr     x6, x6, #VTCR_EL2_SL0_LVL1
+    orr     x6, x6, #VTCR_EL2_IRGN0_WBWA
+    orr     x6, x6, #VTCR_EL2_ORGN0_WBWA
+    orr     x6, x6, #VTCR_EL2_SH0_INNER
+    orr     x6, x6, x7
+    msr     vtcr_el2, x6
+
+    dsb     sy
+    tlbi    vmalls12e1
+    dsb     sy
+    isb
+    ret
+
     .global linux_cpu3_trampoline_end
 linux_cpu3_trampoline_end:
+
+    .section ".bss.noclean.linux_hyp", "aw"
+    .align 12
+linux_stage2_l1:
+    .space 4096
+linux_stage2_l2_low:
+    .space 4096
+linux_stage2_l2_high:
+    .space 4096
+linux_stage2_l3_gic:
+    .space 4096
+linux_cpu3_hyp_stack:
+    .space 4096
+linux_cpu3_hyp_stack_top:
diff --git a/bsp/qemu-virt64-aarch64/linux.dts b/bsp/qemu-virt64-aarch64/linux.dts
@@ -13,7 +13,7 @@
         cpu_on = <0xc4000003>;
         cpu_off = <0x84000002>;
         cpu_suspend = <0xc4000001>;
-        method = "smc";
+        method = "hvc";
         compatible = "arm,psci-1.0", "arm,psci-0.2", "arm,psci";
     };
 
diff --git a/libcpu/aarch64/common/hyp_vector_gcc.S b/libcpu/aarch64/common/hyp_vector_gcc.S
@@ -8,13 +8,19 @@
 
 #define ESR_EL2_EC_SHIFT               26
 #define ESR_EL2_EC_HVC64               0x16
+#define ESR_EL2_EC_DABT_LOW            0x24
+#define ESR_EL2_DABT_WNR               (1 << 6)
 
 #define HYPERCALL_VERSION_ID           0xc5000000
 #define HYPERCALL_DEBUG_ID             0xc5000001
 #define HYPERCALL_CPU_ON_ID            0xc5000003
 #define HYP_VERSION_0_1                0x00010000
 #define HYP_CUSTOM_TEST_VALUE          0x48564321
 #define PSCI_0_2_FN64_CPU_ON           0xc4000003
+#define PSCI_32_PREFIX                 0x84
+#define PSCI_64_PREFIX                 0xc4
+#define GICD_BASE                      0x08000000
+#define GICD_END                       0x08010000
 
 .text
 .align 11
@@ -45,7 +51,10 @@ hyp_vectors:
 
     /* Exception from lower EL, AArch64 */
     .org (HYP_VBAR + 0x400)
-    b hyp_sync_lower_aarch64
+    b hyp_sync_lower_aarch64    /* EL1/EL0 触发同步异常后，EL2 的统一分发入口 */
+    /* ESR_EL2.EC == HVC64 主要是 Linux 发 PSCI 的 hvc，或者你自定义的 hypercall */
+    /* ESR_EL2.EC == DABT_LOW EL1 对某个地址读/写时发生数据访问异常 */
+
     .org (HYP_VBAR + 0x480)
     b hyp_panic
     .org (HYP_VBAR + 0x500)
@@ -63,11 +72,33 @@ hyp_vectors:
     .org (HYP_VBAR + 0x780)
     b hyp_panic
 
+
+
+
+
+
+
+
+
 hyp_sync_lower_aarch64:
+    sub     sp, sp, #16
+    stp     x16, x17, [sp]
+
     mrs     x16, esr_el2
     lsr     x17, x16, #ESR_EL2_EC_SHIFT
     cmp     x17, #ESR_EL2_EC_HVC64
-    b.ne    hyp_panic
+    b.eq    hyp_hvc_lower_aarch64
+    cmp     x17, #ESR_EL2_EC_DABT_LOW
+    b.eq    hyp_dabt_lower_aarch64
+    ldp     x16, x17, [sp], #16
+    b       hyp_panic
+
+
+
+
+/* hypercall */
+hyp_hvc_lower_aarch64:
+    add     sp, sp, #16
 
     movz    w16, #(HYPERCALL_VERSION_ID >> 16), lsl #16
     cmp     w0, w16
@@ -83,6 +114,16 @@ hyp_sync_lower_aarch64:
     cmp     w0, w16
     b.eq    hyp_hvc_cpu_on
 
+    /*
+     * Linux is given PSCI method = "hvc".  EL2 owns the HVC trap and
+     * proxies standard PSCI calls down to QEMU/EL3 with SMC.
+     */
+    lsr     w16, w0, #24
+    cmp     w16, #PSCI_32_PREFIX
+    b.eq    hyp_hvc_psci_proxy
+    cmp     w16, #PSCI_64_PREFIX
+    b.eq    hyp_hvc_psci_proxy
+
     b       hyp_hvc_not_supported
 
 hyp_hvc_version:
@@ -117,13 +158,97 @@ hyp_hvc_cpu_on:
     mov     x3, xzr
     eret
 
+.global hyp_hvc_psci_proxy
+hyp_hvc_psci_proxy:
+    smc     #0
+    eret
+
 hyp_hvc_not_supported:
     mov     x0, #-1
     mov     x1, xzr
     mov     x2, xzr
     mov     x3, xzr
     eret
 
+
+
+
+/* 数据访问的问题 */
+hyp_dabt_lower_aarch64:
+    sub     sp, sp, #64
+    stp     x16, x17, [sp, #0]
+    stp     x18, x19, [sp, #16]
+    stp     x20, x21, [sp, #32]
+    stp     x22, x23, [sp, #48]
+
+    /*
+     * Stage-2 permission fault on the real GIC distributor.  For now this
+     * drops guest GICD writes instead of letting Linux modify global GICD
+     * state directly.  Reads should be handled by the read-only mapping.
+     */
+    tbz     x16, #6, hyp_dabt_restore_panic
+
+    mrs     x18, hpfar_el2
+    lsl     x18, x18, #8
+    mrs     x19, far_el2
+    and     x19, x19, #0xfff
+    orr     x18, x18, x19
+
+    ldr     x19, =GICD_BASE
+    cmp     x18, x19
+    b.lo    hyp_dabt_restore_panic
+    ldr     x19, =GICD_END
+    cmp     x18, x19
+    b.hs    hyp_dabt_restore_panic
+
+    ldr     x19, =hyp_gicd_write_trap_count
+    ldr     x20, [x19]
+    add     x20, x20, #1
+    str     x20, [x19]
+
+    ldr     x19, =hyp_last_gicd_write_ipa
+    str     x18, [x19]
+    ldr     x19, =hyp_last_gicd_write_esr
+    str     x16, [x19]
+
+    mrs     x20, elr_el2
+    add     x20, x20, #4
+    msr     elr_el2, x20
+
+    ldp     x16, x17, [sp, #0]
+    ldp     x18, x19, [sp, #16]
+    ldp     x20, x21, [sp, #32]
+    ldp     x22, x23, [sp, #48]
+    add     sp, sp, #64
+    ldp     x16, x17, [sp], #16
+    eret
+
+hyp_dabt_restore_panic:
+    ldp     x16, x17, [sp, #0]
+    ldp     x18, x19, [sp, #16]
+    ldp     x20, x21, [sp, #32]
+    ldp     x22, x23, [sp, #48]
+    add     sp, sp, #64
+    ldp     x16, x17, [sp], #16
+    b       hyp_panic
+
+
+
+
+
+
 hyp_panic:
     wfe
     b       hyp_panic
+
+    .section ".bss.noclean.hyp", "aw"
+    .align 3
+    .global hyp_gicd_write_trap_count
+hyp_gicd_write_trap_count:
+    .quad   0
+    .global hyp_last_gicd_write_ipa
+hyp_last_gicd_write_ipa:
+    .quad   0
+    .global hyp_last_gicd_write_esr
+hyp_last_gicd_write_esr:
+    .quad   0
diff --git a/libcpu/aarch64/cortex-a/entry_point.S b/libcpu/aarch64/cortex-a/entry_point.S
