x86-64: SMP init

Cargo.toml

@@ -2,6 +2,7 @@
 name = "yggdrasil-kernel"
 version = "0.1.0"
 edition = "2021"
+build = "build.rs"
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 

build.rs

@@ -0,0 +1,46 @@
+use std::{
+    env,
+    io::{self, Write},
+    path::PathBuf,
+    process::Command,
+};
+
+fn build_x86_64() {
+    const DEFAULT_8086_AS: &str = "nasm";
+    const AP_BOOTSTRAP_S: &str = "src/arch/x86_64/boot/ap_boot.S";
+
+    println!("cargo:rerun-if-changed={}", AP_BOOTSTRAP_S);
+
+    let out_dir = env::var("OUT_DIR").unwrap();
+    let assembler = env::var("AS8086").unwrap_or(DEFAULT_8086_AS.to_owned());
+
+    let ap_bootstrap_out = PathBuf::from(out_dir).join("__x86_64_ap_boot.bin");
+
+    // Assemble the code
+    let output = Command::new(assembler.as_str())
+        .args([
+            "-fbin",
+            "-o",
+            ap_bootstrap_out.to_str().unwrap(),
+            AP_BOOTSTRAP_S,
+        ])
+        .output()
+        .unwrap();
+
+    if !output.status.success() {
+        io::stderr().write_all(&output.stderr).ok();
+        panic!("{}: could not assemble {}", assembler, AP_BOOTSTRAP_S);
+    }
+}
+
+fn main() {
+    let arch = env::var("CARGO_CFG_TARGET_ARCH").unwrap();
+
+    println!("cargo:rerun-if-changed=build.rs");
+
+    match arch.as_str() {
+        "x86_64" => build_x86_64(),
+        "aarch64" => (),
+        _ => panic!("Unknown target arch: {:?}", arch),
+    }
+}

src/arch/mod.rs

@@ -82,4 +82,7 @@ pub trait Architecture {
 
     /// Returns the local CPU's interrupt mask
     fn interrupt_mask() -> bool;
+
+    /// Returns the count of present CPUs, including the BSP
+    fn cpu_count() -> usize;
 }

src/arch/x86_64/apic/ioapic.rs

@@ -244,7 +244,12 @@ impl InitializableDevice for IoApic {
 
         infoln!("Maximum GSI number: {}", max_gsi);
 
-        let inner = Inner { regs, max_gsi };
+        let mut inner = Inner { regs, max_gsi };
+
+        // Mask all GSIs
+        for gsi in 0..max_gsi {
+            inner.set_gsi_enabled(gsi, false);
+        }
 
         self.inner.init(IrqSafeSpinlock::new(inner));
         self.isa_redirections.init(isa_redirections);

src/arch/x86_64/apic/local.rs

@@ -5,9 +5,14 @@ use tock_registers::{
     registers::{ReadOnly, ReadWrite, WriteOnly},
 };
 
-use crate::{arch::x86_64::registers::MSR_IA32_APIC_BASE, mem::ConvertAddress, util::OneTimeInit};
+use crate::{
+    arch::x86_64::registers::MSR_IA32_APIC_BASE, device::interrupt::IpiDeliveryTarget,
+    mem::ConvertAddress, util::OneTimeInit,
+};
 
-use super::APIC_TIMER_VECTOR;
+use super::{
+    APIC_IPI_VECTOR, APIC_LINT0_VECTOR, APIC_LINT1_VECTOR, APIC_SPURIOUS_VECTOR, APIC_TIMER_VECTOR,
+};
 
 const TIMER_INTERVAL: u32 = 150000;
 
@@ -20,7 +25,8 @@ register_bitfields! {
         ApicId OFFSET(24) NUMBITS(8) []
     ],
     SpuriousVector [
-        SoftwareEnable OFFSET(8) NUMBITS(1),
+        Vector OFFSET(0) NUMBITS(8) [],
+        SoftwareEnable OFFSET(8) NUMBITS(1) [],
     ],
     TimerLocalVectorEntry [
         Vector OFFSET(0) NUMBITS(8) [],
@@ -39,10 +45,40 @@ register_bitfields! {
             Masked = 1,
             Unmasked = 0,
         ],
-        Nmi OFFSET(8) NUMBITS(3) [
+        DeliveryMode OFFSET(8) NUMBITS(3) [
-            IsNmi = 4,
+            Nmi = 4,
+            ExtINT = 7
         ],
+    ],
+    ICR0 [
+        Vector OFFSET(0) NUMBITS(8) [],
+        Destination OFFSET(8) NUMBITS(3) [
+            Normal = 1,
+            Lowest = 2,
+            SMI = 3,
+            NMI = 4,
+            INIT = 5,
+            SIPI = 6
+        ],
+        DeliveryStatus OFFSET(12) NUMBITS(1) [],
+        INIT0 OFFSET(14) NUMBITS(1) [
+            Deassert = 0,
+            Assert = 1,
+        ],
+        INIT1 OFFSET(15) NUMBITS(1) [
+            Deassert = 1,
+            Assert = 0,
+        ],
+        DestinationType OFFSET(18) NUMBITS(3) [
+            Physical = 0,
+            This = 1,
+            All = 2,
+            AllExceptThis = 3,
         ]
+    ],
+    ICR1 [
+        PhysicalDestination OFFSET(24) NUMBITS(4) []
+    ],
 }
 
 register_structs! {
@@ -51,22 +87,32 @@ register_structs! {
         (0x00 => _0),
         (0x20 => Id: ReadOnly<u32, Id::Register>),
         (0x24 => _1),
+        (0x80 => TaskPriorityRegister: ReadWrite<u32>),
+        (0x84 => _13),
         (0xB0 => EndOfInterrupt: WriteOnly<u32>),
         (0xB4 => _2),
         (0xF0 => SpuriousVector: ReadWrite<u32, SpuriousVector::Register>),
         (0xF4 => _3),
+        (0x100 => ISR0: ReadOnly<u32>),
+        (0x104 => _14),
+        (0x280 => ErrorStatus: ReadOnly<u32>),
+        (0x284 => _4),
+        (0x300 => ICR0: ReadWrite<u32, ICR0::Register>),
+        (0x304 => _5),
+        (0x310 => ICR1: ReadWrite<u32, ICR1::Register>),
+        (0x314 => _6),
         (0x320 => TimerLocalVectorEntry: ReadWrite<u32, TimerLocalVectorEntry::Register>),
-        (0x324 => _4),
+        (0x324 => _7),
         (0x350 => LInt0: ReadWrite<u32, LocalVectorEntry::Register>),
-        (0x354 => _5),
+        (0x354 => _8),
         (0x360 => LInt1: ReadWrite<u32, LocalVectorEntry::Register>),
-        (0x364 => _6),
+        (0x364 => _9),
         (0x380 => TimerInitCount: ReadWrite<u32>),
-        (0x384 => _7),
+        (0x384 => _10),
         (0x390 => TimerCurrentCount: ReadOnly<u32>),
-        (0x394 => _8),
+        (0x394 => _11),
         (0x3E0 => TimerDivideConfig: ReadWrite<u32>),
-        (0x3E4 => _9),
+        (0x3E4 => _12),
         (0x530 => @END),
     }
 }
@@ -93,19 +139,34 @@ impl LocalApic {
         }
 
         Self::enable();
+        // Configure spurious interrupt handler
+        regs.SpuriousVector.write(
+            SpuriousVector::SoftwareEnable::SET
+                + SpuriousVector::Vector.val(APIC_SPURIOUS_VECTOR + 32),
+        );
+
+        // Configure task priority register
+        regs.TaskPriorityRegister.set(0);
 
         // Enable timer
-        regs.TimerInitCount.set(TIMER_INTERVAL);
         regs.TimerDivideConfig.set(0x3);
+        regs.TimerInitCount.set(TIMER_INTERVAL);
 
+        // Configure local interrupt vectors
         regs.TimerLocalVectorEntry.write(
             TimerLocalVectorEntry::Vector.val(APIC_TIMER_VECTOR + 32)
                 + TimerLocalVectorEntry::Mask::Unmasked
                 + TimerLocalVectorEntry::TimerMode::Periodic,
         );
-
+        // LINT0 unmasked, leave LINT1 masked
-        regs.SpuriousVector
+        regs.LInt0.write(
-            .modify(SpuriousVector::SoftwareEnable::SET);
+            LocalVectorEntry::Mask::Unmasked
+                + LocalVectorEntry::Vector.val(APIC_LINT0_VECTOR + 32)
+                + LocalVectorEntry::DeliveryMode::ExtINT,
+        );
+        regs.LInt1.write(
+            LocalVectorEntry::Mask::Masked + LocalVectorEntry::Vector.val(APIC_LINT1_VECTOR + 32),
+        );
 
         Self { regs }
     }
@@ -115,6 +176,85 @@ impl LocalApic {
         self.regs.EndOfInterrupt.set(0);
     }
 
+    /// Performs an application processor startup sequence.
+    ///
+    /// # Safety
+    ///
+    /// Unsafe: only meant to be called by the BSP during SMP init.
+    pub unsafe fn wakeup_cpu(&self, apic_id: u32, entry_vector: usize) {
+        infoln!("Waking up apic{}, entry = {:#x}", apic_id, entry_vector);
+
+        while self.regs.ICR0.matches_all(ICR0::DeliveryStatus::SET) {
+            core::hint::spin_loop();
+        }
+
+        let entry_vector = entry_vector >> 12;
+
+        // INIT assert
+        self.regs.ICR1.write(ICR1::PhysicalDestination.val(apic_id));
+
+        self.regs.ICR0.write(
+            ICR0::Destination::INIT
+                + ICR0::DestinationType::Physical
+                + ICR0::INIT0::Assert
+                + ICR0::INIT1::Assert,
+        );
+
+        while self.regs.ICR0.matches_all(ICR0::DeliveryStatus::SET) {
+            core::hint::spin_loop();
+        }
+
+        // INIT deassert
+        self.regs.ICR1.write(ICR1::PhysicalDestination.val(apic_id));
+
+        self.regs.ICR0.write(
+            ICR0::Destination::INIT
+                + ICR0::DestinationType::Physical
+                + ICR0::INIT0::Deassert
+                + ICR0::INIT1::Deassert,
+        );
+
+        while self.regs.ICR0.matches_all(ICR0::DeliveryStatus::SET) {
+            core::hint::spin_loop();
+        }
+
+        // Send another SIPI type IPI because the spec says so
+        self.regs.ICR1.write(ICR1::PhysicalDestination.val(apic_id));
+
+        self.regs.ICR0.write(
+            ICR0::Vector.val(entry_vector as u32)
+                + ICR0::Destination::SIPI
+                + ICR0::DestinationType::Physical,
+        );
+
+        while self.regs.ICR0.matches_all(ICR0::DeliveryStatus::SET) {
+            core::hint::spin_loop();
+        }
+    }
+
+    /// Issues an interprocessor interrupt for the target.
+    ///
+    /// # Safety
+    ///
+    /// Unsafe: this function may break the control flow on the target processors.
+    pub unsafe fn send_ipi(&self, target: IpiDeliveryTarget) {
+        while self.regs.ICR0.matches_all(ICR0::DeliveryStatus::SET) {
+            core::hint::spin_loop();
+        }
+
+        match target {
+            IpiDeliveryTarget::AllExceptLocal => {
+                self.regs.ICR1.write(ICR1::PhysicalDestination.val(0));
+                self.regs.ICR0.write(
+                    ICR0::Vector.val(APIC_IPI_VECTOR + 32)
+                        + ICR0::Destination::Normal
+                        + ICR0::DestinationType::AllExceptThis,
+                );
+            }
+            IpiDeliveryTarget::Specified(_) => todo!(),
+        }
+    }
+
     #[inline]
     fn base() -> usize {
         MSR_IA32_APIC_BASE.read_base() as usize
@@ -127,6 +267,8 @@ impl LocalApic {
 
     #[inline]
     fn enable() {
-        MSR_IA32_APIC_BASE.modify(MSR_IA32_APIC_BASE::ApicEnable::SET);
+        MSR_IA32_APIC_BASE.modify(
+            MSR_IA32_APIC_BASE::ApicEnable::SET + MSR_IA32_APIC_BASE::ExtendedEnable::CLEAR,
+        );
     }
 }

src/arch/x86_64/apic/mod.rs

@@ -1,5 +1,7 @@
 //! x86-64 APIC interface (Local + I/O)
 
+use core::arch::global_asm;
+
 use abi::error::Error;
 
 use crate::{
@@ -16,7 +18,15 @@ pub mod local;
 // I/O APIC 0..MAX_EXTERNAL_VECTORS range is mapped to BSP Local APIC 2..
 
 /// Fixed IRQ vector for Local APIC timer
-pub const APIC_TIMER_VECTOR: u32 = 0;
+pub const APIC_TIMER_VECTOR: u32 = 0x00;
+/// Fixed IRQ vector for LINT0 line
+pub const APIC_LINT0_VECTOR: u32 = 0x01;
+/// Fixed IRQ vector for LINT1 line
+pub const APIC_LINT1_VECTOR: u32 = 0x02;
+/// Fixed vector for inter-processor interrupt
+pub const APIC_IPI_VECTOR: u32 = 0x03;
+/// Fixed vector for spurious interrupt
+pub const APIC_SPURIOUS_VECTOR: u32 = 0xDF;
 /// Start of the I/O APIC IRQ range
 pub const APIC_EXTERNAL_OFFSET: u32 = 4;
 /// Maximum number of APIC vectors allocated for handling IRQs from I/O APIC
@@ -56,7 +66,8 @@ impl Table {
 /// Fills the IDT with interrupt vectors for this APIC
 pub fn setup_vectors(idt: &mut [exception::Entry]) {
     extern "C" {
-        static __x86_64_apic_vectors: [usize; 16];
+        // IRQ vectors
+        static __x86_64_apic_vectors: [usize; 224];
     }
 
     for (i, &entry) in unsafe { __x86_64_apic_vectors.iter() }.enumerate() {
@@ -68,32 +79,44 @@ pub fn setup_vectors(idt: &mut [exception::Entry]) {
     }
 }
 
-fn apic_irq_inner(vector: u32) {
+unsafe extern "C" fn irq_handler(vector: usize, frame: *mut IrqFrame) {
     let cpu = Cpu::local();
-
-    if vector == APIC_TIMER_VECTOR {
-        cpu.local_apic().clear_interrupt();
-        unsafe { Cpu::local().queue().yield_cpu() }
-    } else if (APIC_EXTERNAL_OFFSET..APIC_EXTERNAL_OFFSET + MAX_EXTERNAL_VECTORS).contains(&vector)
-    {
-        PLATFORM.ioapic.handle_irq(vector - APIC_EXTERNAL_OFFSET);
-        cpu.local_apic().clear_interrupt();
-    } else {
-        panic!("Got an interrupt on undefined vector: {}", vector);
-    }
-}
-
-/// Main handler for APIC interrupts.
-///
-/// # Safety
-///
-/// Only meant to be called from the assembly irq vector.
-pub unsafe extern "C" fn __x86_64_apic_irq_handler(vector: u32, frame: *mut IrqFrame) {
     let frame = &mut *frame;
 
-    apic_irq_inner(vector);
+    PLATFORM.ioapic.handle_irq(vector as u32);
+    cpu.local_apic().clear_interrupt();
 
     if let Some(process) = Process::get_current() {
         process.handle_signal(frame);
     }
 }
+
+unsafe extern "C" fn local_timer_irq_handler(frame: *mut IrqFrame) {
+    let frame = &mut *frame;
+    let cpu = Cpu::local();
+    // Clear interrupt before switching, because otherwise we won't receive the next one
+    cpu.local_apic().clear_interrupt();
+    cpu.queue().yield_cpu();
+
+    if let Some(process) = Process::get_current() {
+        process.handle_signal(frame);
+    }
+}
+
+unsafe extern "C" fn dummy_irq_handler() {
+    todo!()
+}
+
+unsafe extern "C" fn ipi_handler() {
+    let cpu = Cpu::local();
+    todo!("Processor {} received an IPI", cpu.id());
+}
+
+global_asm!(
+    include_str!("vectors.S"),
+    local_timer_irq_handler = sym local_timer_irq_handler,
+    irq_handler = sym irq_handler,
+    ipi_handler = sym ipi_handler,
+    dummy_irq_handler = sym dummy_irq_handler,
+    options(att_syntax)
+);

src/arch/x86_64/apic/vectors.S

@@ -0,0 +1,172 @@
+.altmacro
+
+.global __x86_64_apic_vectors
+
+.set IRQ_REG_RAX, 0 * 8
+.set IRQ_REG_RCX, 1 * 8
+.set IRQ_REG_RDX, 2 * 8
+.set IRQ_REG_RBX, 3 * 8
+.set IRQ_REG_RSI, 4 * 8
+.set IRQ_REG_RDI, 5 * 8
+.set IRQ_REG_RBP, 6 * 8
+.set IRQ_REG_R8, 7 * 8
+.set IRQ_REG_R9, 8 * 8
+.set IRQ_REG_R10, 9 * 8
+.set IRQ_REG_R11, 10 * 8
+.set IRQ_REG_R12, 11 * 8
+.set IRQ_REG_R13, 12 * 8
+.set IRQ_REG_R14, 13 * 8
+.set IRQ_REG_R15, 14 * 8
+
+// 15 registers + stack align word if needed
+.set IRQ_STATE_SIZE, 15 * 8
+
+.macro SWAPGS_IF_NEEDED, cs_off
+    cmpq $0x08, \cs_off(%rsp)
+    je 1f
+    swapgs
+1:
+.endm
+
+.macro IRQ_SAVE_STATE
+    // Save state
+    subq $IRQ_STATE_SIZE, %rsp
+
+    movq %rax, IRQ_REG_RAX(%rsp)
+    movq %rcx, IRQ_REG_RCX(%rsp)
+    movq %rdx, IRQ_REG_RDX(%rsp)
+    movq %rbx, IRQ_REG_RBX(%rsp)
+    movq %rsi, IRQ_REG_RSI(%rsp)
+    movq %rdi, IRQ_REG_RDI(%rsp)
+    movq %rbp, IRQ_REG_RBP(%rsp)
+    movq %r8,  IRQ_REG_R8(%rsp)
+    movq %r9,  IRQ_REG_R9(%rsp)
+    movq %r10, IRQ_REG_R10(%rsp)
+    movq %r11, IRQ_REG_R11(%rsp)
+    movq %r12, IRQ_REG_R12(%rsp)
+    movq %r13, IRQ_REG_R13(%rsp)
+    movq %r14, IRQ_REG_R14(%rsp)
+    movq %r15, IRQ_REG_R15(%rsp)
+
+    // Save current stack into %rbp
+    movq %rsp, %rbp
+
+    // Force correct stack alignment
+    orq $0xF, %rsp
+    xorq $0xF, %rsp
+.endm
+
+.macro IRQ_RESTORE_STATE
+    // Restore the stack pointer
+    movq %rbp, %rsp
+
+    // Restore state
+    movq IRQ_REG_RAX(%rsp), %rax
+    movq IRQ_REG_RCX(%rsp), %rcx
+    movq IRQ_REG_RDX(%rsp), %rdx
+    movq IRQ_REG_RBX(%rsp), %rbx
+    movq IRQ_REG_RSI(%rsp), %rsi
+    movq IRQ_REG_RDI(%rsp), %rdi
+    movq IRQ_REG_RBP(%rsp), %rbp
+    movq IRQ_REG_R8(%rsp),  %r8
+    movq IRQ_REG_R9(%rsp),  %r9
+    movq IRQ_REG_R10(%rsp), %r10
+    movq IRQ_REG_R11(%rsp), %r11
+    movq IRQ_REG_R12(%rsp), %r12
+    movq IRQ_REG_R13(%rsp), %r13
+    movq IRQ_REG_R14(%rsp), %r14
+    movq IRQ_REG_R15(%rsp), %r15
+
+    addq $IRQ_STATE_SIZE, %rsp
+.endm
+
+.macro IRQ_VECTOR, n
+irq_vector_\n:
+    // %rsp + 0: %rip
+    // %rsp + 8: %cs
+    SWAPGS_IF_NEEDED 8
+
+    IRQ_SAVE_STATE
+
+    // Force correct segment registers
+    mov $0x10, %ax
+    mov %ax, %ss
+    mov %ax, %ds
+    mov %ax, %es
+
+    mov $\n, %rdi
+    mov %rbp, %rsi
+    call {irq_handler}
+
+    IRQ_RESTORE_STATE
+
+    SWAPGS_IF_NEEDED 8
+
+    iretq
+.endm
+
+.macro IRQ_VECTOR_ENTRY, n
+.quad irq_vector_\n
+.endm
+
+.macro IRQ_VECTORS, start, end
+.set i, 0
+.rept \end - \start
+    IRQ_VECTOR %i
+    .set i, i+1
+.endr
+.endm
+
+.macro IRQ_VECTOR_ENTRIES, start, end
+.set i, 0
+.rept \end - \start
+    IRQ_VECTOR_ENTRY %i
+    .set i, i+1
+.endr
+.endm
+
+.section .text
+local_timer_vector:
+    SWAPGS_IF_NEEDED 8
+    IRQ_SAVE_STATE
+
+    mov %rbp, %rdi
+    call {local_timer_irq_handler}
+
+    IRQ_RESTORE_STATE
+    SWAPGS_IF_NEEDED 8
+    iretq
+
+ipi_vector:
+    SWAPGS_IF_NEEDED 8
+    IRQ_SAVE_STATE
+
+    call {ipi_handler}
+    jmp .
+
+dummy_vector:
+    SWAPGS_IF_NEEDED 8
+    IRQ_SAVE_STATE
+
+    call {dummy_irq_handler}
+    jmp .
+
+IRQ_VECTORS 4, 255
+
+.section .rodata
+// 224 vectors: 256 - 32 (exceptions)
+.p2align 4
+.type __x86_64_apic_vectors, @object
+__x86_64_apic_vectors:
+    // Local timer IRQ: 0
+    .quad local_timer_vector
+    // Dummy entries (currently): 1..=2
+    .quad dummy_vector
+    .quad dummy_vector
+    // IPI vector: 3
+    .quad ipi_vector
+    // Regular IRQ vectors: 4..=222
+    IRQ_VECTOR_ENTRIES 4, 223
+    // Spurious interrupt vector: 223
+    .quad dummy_vector
+.size __x86_64_apic_vectors, . - __x86_64_apic_vectors

src/arch/x86_64/boot/ap_boot.S

@@ -0,0 +1,110 @@
+[org 0x7000]
+[bits 16]
+
+; Data at 0x6000
+
+; Layout:
+; +0x00: cr3 (only u32)
+; +0x08: stack_base
+; +0x10: stack_size
+; +0x18: entry
+
+__x86_64_ap_bootstrap:
+    cli
+
+    ; Reset DS
+    mov ax, 0
+    mov ds, ax
+
+    ; Disable NMI
+    in al, 0x70
+    or al, 0x80
+    out 0x70, al
+
+    ; Load GDT32 and enable protected mode
+    lgdt [gdt32_ptr]
+    mov eax, cr0
+    or al, 1
+    mov cr0, eax
+
+    jmp 0x08:ap_start_32
+
+[bits 32]
+ap_start_32:
+    cli
+
+    ; Proper DS
+    mov ax, 0x10
+    mov ds, ax
+
+    ; Enable PSE+PAE
+    mov eax, cr4
+    or eax, (1 << 5) | (1 << 4)
+    mov cr4, eax
+
+    ; Load CR3
+    mov eax, dword [0x6000 + 0x00]
+    mov cr3, eax
+
+    ; Enable EFER.LME
+    mov ecx, 0xC0000080
+    rdmsr
+    or eax, 1 << 8
+    wrmsr
+
+    ; Enable paging
+    mov eax, cr0
+    or eax, 1 << 31
+    mov cr0, eax
+
+    ; Load GDT64
+    lgdt [gdt64_ptr]
+
+    jmp 0x08:ap_start_64
+
+[bits 64]
+ap_start_64:
+    mov rax, 0x10
+    mov ds, rax
+    mov es, rax
+    mov ss, rax
+
+    ; Load stack
+    mov rsp, qword [0x6000 + 0x08]
+    add rsp, qword [0x6000 + 0x10]
+    mov rbp, rsp
+
+    ; Jump to kernel entry
+    mov rax, qword [0x6000 + 0x18]
+    jmp rax
+
+align 4
+gdt32:
+    ; NULL
+    dq 0
+    ; CS32
+    dq 0xCF98000000FFFF
+    ; DS32
+    dq 0xCF92000000FFFF
+gdt32_end:
+
+align 4
+gdt32_ptr:
+    ; limit
+    dw gdt32_end - gdt32 - 1
+    dd gdt32
+
+align 4
+gdt64:
+    ; NULL
+    dq 0
+    ; CS64
+    dq 0x00209A0000000000
+    ; DS64
+    dq 0x0000920000000000
+gdt64_end:
+
+align 4
+gdt64_ptr:
+    dw gdt64_end - gdt64 - 1
+    dd gdt64

src/arch/x86_64/boot/mod.rs

@@ -1,7 +1,6 @@
 //! x86-64 boot and entry functions
-use core::arch::global_asm;
+use core::{arch::global_asm, sync::atomic::Ordering};
 
-use git_version::git_version;
 use yboot_proto::{
     v1::{FramebufferOption, MemoryMap},
     LoadProtocolHeader, LoadProtocolV1, KERNEL_MAGIC, LOADER_MAGIC, PROTOCOL_VERSION_1,
@@ -9,21 +8,18 @@ use yboot_proto::{
 
 use crate::{
     arch::{
-        x86_64::{apic::local::LocalApic, cpu::Cpu, cpuid, exception, syscall, FB_CONSOLE},
+        x86_64::{
-        Architecture, ArchitectureImpl, PLATFORM,
+            apic::local::LocalApic, cpu::Cpu, cpuid, exception, kernel_main, syscall,
+            CPU_INIT_FENCE,
         },
-    debug,
+        Architecture, ArchitectureImpl,
-    device::{display::console::add_console_autoflush, platform::Platform},
-    fs::{devfs, Initrd, INITRD_DATA},
-    mem::{
-        heap,
-        phys::{self, PageUsage},
-        ConvertAddress, KERNEL_VIRT_OFFSET,
     },
+    device::platform::Platform,
+    mem::KERNEL_VIRT_OFFSET,
     task,
 };
 
-use super::ARCHITECTURE;
+use super::{smp::CPU_COUNT, ARCHITECTURE};
 
 const BOOT_STACK_SIZE: usize = 65536;
 
@@ -64,85 +60,38 @@ static mut YBOOT_DATA: LoadProtocolV1 = LoadProtocolV1 {
     },
 };
 
-fn setup_initrd() {
-    let initrd_start = unsafe { YBOOT_DATA.initrd_address } as usize;
-    let initrd_end = initrd_start + unsafe { YBOOT_DATA.initrd_size } as usize;
-
-    if initrd_start == 0 || initrd_end <= initrd_start {
-        infoln!("No initrd loaded");
-        return;
-    }
-
-    let start_aligned = initrd_start & !0xFFF;
-    let end_aligned = initrd_end & !0xFFF;
-
-    let data = unsafe {
-        core::slice::from_raw_parts(
-            initrd_start.virtualize() as *const _,
-            initrd_end - initrd_start,
-        )
-    };
-
-    let initrd = Initrd {
-        phys_page_start: start_aligned,
-        phys_page_len: end_aligned - start_aligned,
-        data,
-    };
-
-    INITRD_DATA.init(initrd);
-}
-
 unsafe extern "C" fn __x86_64_upper_entry() -> ! {
     ArchitectureImpl::set_interrupt_mask(true);
 
     ARCHITECTURE.init_mmu(true);
     core::arch::asm!("wbinvd");
 
-    ARCHITECTURE
+    kernel_main(&YBOOT_DATA)
-        .yboot_framebuffer
+}
-        .init(YBOOT_DATA.opt_framebuffer);
-    ARCHITECTURE.init_primary_debug_sink();
 
-    debug::init();
+/// Application processor entry point
-    infoln!("Yggdrasil kernel git {} starting", git_version!());
+pub extern "C" fn __x86_64_ap_entry() -> ! {
+    let cpu_id = CPU_COUNT.load(Ordering::Acquire);
+    // Still not initialized: GDT, IDT, CPU features, syscall, kernel_gs_base
     cpuid::feature_gate();
 
-    if YBOOT_DATA.memory_map.address > 0xFFFFFFFF {
+    infoln!("cpu{} initializing", cpu_id);
-        errorln!("Unhandled case: memory map is above 4GiB");
+    unsafe {
-        loop {
+        ARCHITECTURE.init_mmu(false);
-            ArchitectureImpl::wait_for_interrupt();
+        core::arch::asm!("wbinvd");
-        }
-    }
 
-    if YBOOT_DATA.rsdp_address != 0 {
+        Cpu::init_local(LocalApic::new(), cpu_id as u32);
-        infoln!("ACPI RSDP at {:#x}", YBOOT_DATA.rsdp_address);
-        PLATFORM.init_rsdp(YBOOT_DATA.rsdp_address as _);
-    }
-
-    // Setup physical memory allocation
-    setup_initrd();
-    ArchitectureImpl::init_physical_memory(&YBOOT_DATA.memory_map);
-
-    // Allocate memory for the kernel heap
-    let heap_base = phys::alloc_pages_contiguous(16, PageUsage::Used)
-        .expect("Could not allocate a block for heap");
-    heap::init_heap(heap_base.virtualize(), 16 * 0x1000);
-
-    // Add console to flush list
-    add_console_autoflush(FB_CONSOLE.get());
-
-    // Also initializes local APIC
-    Cpu::init_local(LocalApic::new(), 0);
         syscall::init_syscall();
-    exception::init_exceptions(0);
+        exception::init_exceptions(cpu_id);
 
-    devfs::init();
+        ARCHITECTURE.init(false).unwrap();
+    }
 
-    PLATFORM.init(true).unwrap();
+    CPU_COUNT.fetch_add(1, Ordering::Release);
 
-    task::init().expect("Failed to initialize the scheduler");
+    CPU_INIT_FENCE.wait_one();
 
-    task::enter()
+    unsafe { task::enter() }
 }
 
 global_asm!(

src/arch/x86_64/cpu.rs

@@ -35,7 +35,9 @@ impl Cpu {
     ///
     /// Only meant to be called once per each CPU during their init.
     pub unsafe fn init_local(local_apic: LocalApic, id: u32) {
-        let tss_address = gdt::init(id);
+        let tss_address = gdt::init();
+
+        infoln!("Initialize CPU with id {}", id);
 
         let this = Box::new(Cpu {
             this: null_mut(),
@@ -64,7 +66,11 @@ impl Cpu {
 
     /// Returns the system ID of the CPU
     pub fn local_id() -> u32 {
-        Self::local().id()
+        if let Some(cpu) = Self::get_local() {
+            cpu.id()
+        } else {
+            0
+        }
     }
 
     /// Returns this CPU's local data structure or None if it hasn't been set up yet

src/arch/x86_64/exception.rs

@@ -4,7 +4,7 @@ use core::{arch::global_asm, mem::size_of_val};
 use abi::{arch::SavedFrame, primitive_enum, process::Signal};
 
 use crate::{
-    arch::x86_64::apic::{self, __x86_64_apic_irq_handler},
+    arch::x86_64::apic,
     task::{context::TaskFrame, process::Process},
 };
 
@@ -288,7 +288,18 @@ fn user_exception_inner(kind: ExceptionKind, frame: &ExceptionFrame) {
 }
 
 fn kernel_exception_inner(kind: ExceptionKind, frame: &ExceptionFrame) -> ! {
-    fatalln!("{:?} in KERNEL", kind);
+    let cr3: usize;
+    let cr2: usize;
+    unsafe {
+        core::arch::asm!("movq %cr3, {0}", out(reg) cr3, options(att_syntax));
+        core::arch::asm!("movq %cr2, {0}", out(reg) cr2, options(att_syntax));
+    }
+
+    fatalln!("{:?} in KERNEL, frame {:p}, cr3 = {:#x}", kind, frame, cr3);
+    if kind == ExceptionKind::PageFault {
+        fatalln!("cr2 = {:#x}", cr2);
+    }
+
     fatalln!("CS:RIP = {:#x}:{:#x}", frame.cs, frame.rip);
     fatalln!("SS:RSP = {:#x}:{:#x}", frame.ss, frame.rsp);
 
@@ -315,7 +326,8 @@ extern "C" fn __x86_64_exception_handler(frame: *mut ExceptionFrame) {
 /// # Safety
 ///
 /// Only meant to be called once per each CPU during their init.
-pub unsafe fn init_exceptions(_cpu_index: usize) {
+pub unsafe fn init_exceptions(cpu_index: usize) {
+    if cpu_index == 0 {
         extern "C" {
             static __x86_64_exception_vectors: [usize; 32];
         }
@@ -325,6 +337,7 @@ pub unsafe fn init_exceptions(_cpu_index: usize) {
         }
 
         apic::setup_vectors(&mut IDT[32..]);
+    }
 
     let idtr = Pointer {
         limit: size_of_val(&IDT) as u16 - 1,
@@ -337,6 +350,5 @@ pub unsafe fn init_exceptions(_cpu_index: usize) {
 global_asm!(
     include_str!("vectors.S"),
     exception_handler = sym __x86_64_exception_handler,
-    apic_irq_handler = sym __x86_64_apic_irq_handler,
     options(att_syntax)
 );

src/arch/x86_64/gdt.rs

@@ -1,6 +1,7 @@
 //! x86-64 Global Descriptor Table interface
-// TODO TSS
+use core::mem::size_of;
-use core::mem::{size_of, size_of_val};
+
+use alloc::boxed::Box;
 
 #[allow(dead_code)]
 #[repr(packed)]
@@ -80,6 +81,30 @@ impl Entry {
         flags: 0,
         limit_lo: 0,
     };
+    const RING0_CS64: Self = Entry::new(
+        0,
+        0,
+        Entry::FLAG_LONG,
+        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_EXECUTE,
+    );
+    const RING0_DS64: Self = Entry::new(
+        0,
+        0,
+        0,
+        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_WRITE,
+    );
+    const RING3_DS64: Self = Entry::new(
+        0,
+        0,
+        0,
+        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_RING3 | Entry::ACC_WRITE,
+    );
+    const RING3_CS64: Self = Entry::new(
+        0,
+        0,
+        Entry::FLAG_LONG,
+        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_RING3 | Entry::ACC_EXECUTE,
+    );
 
     const fn new(base: u32, limit: u32, flags: u8, access: u8) -> Self {
         Self {
@@ -91,70 +116,46 @@ impl Entry {
             limit_lo: (limit & 0xFFFF) as u16,
         }
     }
+
+    const fn tss_low(base: u32, limit: u32) -> Self {
+        Self::new(
+            base,
+            limit,
+            Entry::FLAG_LONG,
+            Entry::ACC_ACCESS | Entry::ACC_PRESENT | Entry::ACC_EXECUTE,
+        )
+    }
 }
 
-const SIZE: usize = 7;
+// NULL, CS64, DS64, DS64, CS64, TSS, TSSext
-
+const GDT_SIZE: usize = 7;
-// TODO per-CPU
-#[no_mangle]
-static mut TSS: Tss = Tss::NULL;
-
-static mut GDT: [Entry; SIZE] = [
-    // 0x00, NULL
-    Entry::NULL,
-    // 0x08, Ring0 CS64
-    Entry::new(
-        0,
-        0,
-        Entry::FLAG_LONG,
-        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_EXECUTE,
-    ),
-    // 0x10, Ring0 DS64
-    Entry::new(
-        0,
-        0,
-        0,
-        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_WRITE,
-    ),
-    // 0x18 (0x1B), Ring3 DS64
-    Entry::new(
-        0,
-        0,
-        0,
-        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_RING3 | Entry::ACC_WRITE,
-    ),
-    // 0x20 (0x23), Ring3 CS64
-    Entry::new(
-        0,
-        0,
-        Entry::FLAG_LONG,
-        Entry::ACC_PRESENT | Entry::ACC_SYSTEM | Entry::ACC_RING3 | Entry::ACC_EXECUTE,
-    ),
-    // 0x28, TSS
-    Entry::NULL,
-    Entry::NULL,
-];
 
 /// Initializes the global descriptor table.
 ///
 /// # Safety
 ///
 /// Only meant to be called by the CPUs during their early init.
-pub unsafe fn init(_id: u32) -> usize {
+pub unsafe fn init() -> usize {
-    let tss_addr = &TSS as *const _ as usize;
+    // Won't be deallocated, so leaks are not a concern
+    let tss_addr = Box::into_raw(Box::new(Tss::NULL)) as usize;
+    let mut gdt = Box::new([
+        Entry::NULL,
+        Entry::RING0_CS64,
+        Entry::RING0_DS64,
+        Entry::RING3_DS64,
+        Entry::RING3_CS64,
+        Entry::tss_low(tss_addr as u32, (size_of::<Tss>() - 1) as u32),
+        Entry::NULL,
+    ]);
 
-    GDT[5] = Entry::new(
+    let tss_high = &mut gdt[6] as *mut _ as *mut u64;
-        tss_addr as u32,
+    tss_high.write_unaligned((tss_addr >> 32) as u64);
-        size_of_val(&TSS) as u32 - 1,
+
-        Entry::FLAG_LONG,
+    let gdt_addr = Box::into_raw(gdt) as usize;
-        Entry::ACC_ACCESS | Entry::ACC_PRESENT | Entry::ACC_EXECUTE,
-    );
-    let tss_upper = &mut GDT[6] as *mut _ as *mut u64;
-    tss_upper.write_unaligned((tss_addr >> 32) as u64);
 
     let gdtr = Pointer {
-        limit: size_of_val(&GDT) as u16 - 1,
+        limit: (GDT_SIZE * size_of::<Entry>()) as u16 - 1,
-        offset: &GDT as *const _ as usize,
+        offset: gdt_addr,
     };
 
     core::arch::asm!(

src/arch/x86_64/mod.rs

@@ -1,21 +1,28 @@
 //! x86-64 architecture and platform implementation
-use core::ptr::NonNull;
+use core::{ptr::NonNull, sync::atomic::Ordering};
 
 use abi::error::Error;
-use acpi::{AcpiHandler, AcpiTables, HpetInfo, InterruptModel, PhysicalMapping};
+use acpi::{
-use yboot_proto::{v1::FramebufferOption, AvailableRegion, IterableMemoryMap};
+    platform::ProcessorInfo, AcpiHandler, AcpiTables, HpetInfo, InterruptModel, PhysicalMapping,
+};
+use cpu::Cpu;
+use git_version::git_version;
+use yboot_proto::{v1::FramebufferOption, AvailableRegion, IterableMemoryMap, LoadProtocolV1};
 
 use crate::{
-    arch::x86_64::table::{init_fixed_tables, KERNEL_TABLES},
+    arch::x86_64::{
-    debug::DebugSink,
+        apic::local::LocalApic,
+        table::{init_fixed_tables, KERNEL_TABLES},
+    },
+    debug::{self, DebugSink},
     device::{
         display::{
-            console::{ConsoleBuffer, ConsoleRow, DisplayConsole},
+            console::{add_console_autoflush, ConsoleBuffer, ConsoleRow, DisplayConsole},
             fb_console::FramebufferConsole,
             linear_fb::LinearFramebuffer,
         },
         input::KeyboardDevice,
-        interrupt::{ExternalInterruptController, InterruptSource},
+        interrupt::{ExternalInterruptController, InterruptSource, IpiDeliveryTarget},
         platform::Platform,
         timer::TimestampSource,
         tty::CombinedTerminal,
@@ -23,12 +30,15 @@ use crate::{
     },
     fs::{
         devfs::{self, CharDeviceType},
-        INITRD_DATA,
+        Initrd, INITRD_DATA,
     },
     mem::{
-        phys::{self, reserved::reserve_region, PhysicalMemoryRegion},
+        heap,
+        phys::{self, reserved::reserve_region, PageUsage, PhysicalMemoryRegion},
         ConvertAddress,
     },
+    sync::SpinFence,
+    task,
     util::OneTimeInit,
 };
 
@@ -36,9 +46,10 @@ use self::{
     apic::{ioapic::IoApic, IrqNumber},
     intrinsics::IoPort,
     peripherals::{hpet::Hpet, ps2::PS2Controller},
+    smp::CPU_COUNT,
 };
 
-use super::Architecture;
+use super::{Architecture, CpuMessage};
 
 #[macro_use]
 pub mod intrinsics;
@@ -52,9 +63,12 @@ pub mod exception;
 pub mod gdt;
 pub mod peripherals;
 pub mod registers;
+pub mod smp;
 pub mod syscall;
 pub mod table;
 
+static CPU_INIT_FENCE: SpinFence = SpinFence::new();
+
 /// Helper trait to provide abstract access to available memory regions
 pub trait AbstractAvailableRegion {
     /// Returns page-aligned physical start address of the region
@@ -134,6 +148,8 @@ pub struct X86_64 {
     yboot_framebuffer: OneTimeInit<FramebufferOption>,
     rsdp_phys_base: OneTimeInit<usize>,
 
+    acpi_processor_info: OneTimeInit<ProcessorInfo>,
+
     // TODO make this fully dynamic?
     combined_terminal: OneTimeInit<CombinedTerminal>,
 
@@ -183,6 +199,10 @@ impl Architecture for X86_64 {
         // If IF is zero, interrupts are disabled (masked)
         flags & (1 << 9) == 0
     }
+
+    fn cpu_count() -> usize {
+        CPU_COUNT.load(Ordering::Acquire)
+    }
 }
 
 impl Platform for X86_64 {
@@ -201,6 +221,10 @@ impl Platform for X86_64 {
             let hpet = HpetInfo::new(&acpi_tables).unwrap();
             let platform_info = acpi_tables.platform_info().unwrap();
 
+            if let Some(processor_info) = platform_info.processor_info {
+                self.acpi_processor_info.init(processor_info);
+            }
+
             let InterruptModel::Apic(apic_info) = platform_info.interrupt_model else {
                 panic!("Processor does not have APIC");
             };
@@ -268,6 +292,16 @@ impl Platform for X86_64 {
     fn timestamp_source(&self) -> &dyn TimestampSource {
         &self.timer
     }
+
+    unsafe fn send_ipi(&self, target: IpiDeliveryTarget, _msg: CpuMessage) -> Result<(), Error> {
+        let Some(local_apic) = Cpu::get_local().map(|cpu| cpu.local_apic()) else {
+            panic!("Local APIC has not been initialized yet");
+        };
+
+        local_apic.send_ipi(target);
+
+        Ok(())
+    }
 }
 
 impl X86_64 {
@@ -338,6 +372,8 @@ pub static ARCHITECTURE: X86_64 = X86_64 {
     rsdp_phys_base: OneTimeInit::new(),
     yboot_framebuffer: OneTimeInit::new(),
 
+    acpi_processor_info: OneTimeInit::new(),
+
     combined_terminal: OneTimeInit::new(),
 
     ioapic: IoApic::new(),
@@ -352,3 +388,98 @@ static FB_CONSOLE: OneTimeInit<FramebufferConsole> = OneTimeInit::new();
 const EARLY_BUFFER_LINES: usize = 32;
 static mut EARLY_CONSOLE_BUFFER: [ConsoleRow; EARLY_BUFFER_LINES] =
     [ConsoleRow::zeroed(); EARLY_BUFFER_LINES];
+
+fn setup_initrd(initrd_start: usize, initrd_end: usize) {
+    if initrd_start == 0 || initrd_end <= initrd_start {
+        infoln!("No initrd loaded");
+        return;
+    }
+
+    let start_aligned = initrd_start & !0xFFF;
+    let end_aligned = initrd_end & !0xFFF;
+
+    let data = unsafe {
+        core::slice::from_raw_parts(
+            initrd_start.virtualize() as *const _,
+            initrd_end - initrd_start,
+        )
+    };
+
+    let initrd = Initrd {
+        phys_page_start: start_aligned,
+        phys_page_len: end_aligned - start_aligned,
+        data,
+    };
+
+    INITRD_DATA.init(initrd);
+}
+
+fn kernel_main(boot_data: &LoadProtocolV1) -> ! {
+    ARCHITECTURE
+        .yboot_framebuffer
+        .init(boot_data.opt_framebuffer);
+
+    unsafe {
+        ARCHITECTURE.init_primary_debug_sink();
+    }
+
+    debug::init();
+    infoln!("Yggdrasil kernel git {} starting", git_version!());
+
+    cpuid::feature_gate();
+
+    if boot_data.memory_map.address > 0xFFFFFFFF {
+        errorln!("Unhandled case: memory map is above 4GiB");
+        loop {
+            X86_64::wait_for_interrupt();
+        }
+    }
+
+    if boot_data.rsdp_address != 0 {
+        infoln!("ACPI RSDP at {:#x}", boot_data.rsdp_address);
+        ARCHITECTURE.init_rsdp(boot_data.rsdp_address as _);
+    }
+
+    // Reserve initrd
+    let initrd_start = boot_data.initrd_address as usize;
+    let initrd_end = initrd_start + boot_data.initrd_size as usize;
+
+    setup_initrd(initrd_start, initrd_end);
+
+    // Setup physical memory allocation
+    unsafe {
+        X86_64::init_physical_memory(&boot_data.memory_map);
+    }
+
+    // Allocate memory for the kernel heap
+    let heap_base = phys::alloc_pages_contiguous(16, PageUsage::Used)
+        .expect("Could not allocate a block for heap");
+    unsafe {
+        heap::init_heap(heap_base.virtualize(), 16 * 0x1000);
+    }
+
+    // Add console to flush list
+    add_console_autoflush(FB_CONSOLE.get());
+
+    // Also initializes local APIC
+    unsafe {
+        Cpu::init_local(LocalApic::new(), 0);
+        syscall::init_syscall();
+
+        // TODO per-CPU IDTs?
+        exception::init_exceptions(0);
+
+        devfs::init();
+
+        ARCHITECTURE.init(true).unwrap();
+
+        if let Some(info) = ARCHITECTURE.acpi_processor_info.try_get() {
+            smp::start_ap_cores(info);
+        }
+
+        CPU_INIT_FENCE.signal();
+
+        task::init().expect("Failed to initialize the scheduler");
+        task::enter()
+    }
+}

src/arch/x86_64/peripherals/ps2/mod.rs

@@ -184,30 +184,7 @@ impl InterruptSource for PS2Controller {
             count += 1;
         }
 
-        Ok(true)
+        Ok(count != 0)
-
-        // let status = self.command.read();
-
-        // if status & 1 == 0 {
-        //     return Ok(false);
-        // }
-
-        // let key = self.data.read();
-
-        // if key == 0xE0 {
-        //     self.data.read();
-        //     infoln!("TODO: handle 0xE0");
-        //     return Ok(true);
-        // }
-
-        // self.data.read();
-
-        // if key < 128 {
-        //     let terminal = self.tty.lock();
-
-        // }
-
-        // Ok(true)
     }
 }
 

src/arch/x86_64/registers/mod.rs

@@ -76,6 +76,8 @@ mod msr_ia32_apic_base {
             AddressPage OFFSET(12) NUMBITS(40) [],
             #[doc = "If set, the APIC is enabled"]
             ApicEnable OFFSET(11) NUMBITS(1) [],
+            #[doc = "If set, x2APIC mode is enabled"]
+            ExtendedEnable OFFSET(10) NUMBITS(1) [],
             #[doc = "If set, this CPU is a bootstrap processor"]
             BootstrapCpuCore OFFSET(8) NUMBITS(1) [],
         ]

src/arch/x86_64/smp.rs

@@ -0,0 +1,124 @@
+//! x86-64 multiprocessing implementation
+use core::{
+    mem::size_of,
+    sync::atomic::{AtomicUsize, Ordering},
+};
+
+use acpi::platform::{ProcessorInfo, ProcessorState};
+
+use crate::{
+    arch::{x86_64::boot::__x86_64_ap_entry, Architecture, ArchitectureImpl},
+    mem::{
+        phys::{self, PageUsage},
+        ConvertAddress,
+    },
+    task::Cpu,
+};
+
+use super::table::KERNEL_TABLES;
+
+/// The number of CPUs present in the system
+pub static CPU_COUNT: AtomicUsize = AtomicUsize::new(1);
+
+static AP_BOOTSTRAP_BIN: &[u8] = include_bytes!(concat!(env!("OUT_DIR"), "/__x86_64_ap_boot.bin"));
+
+const AP_STACK_PAGES: usize = 8;
+const AP_BOOTSTRAP_DATA: usize = 0x6000;
+const AP_BOOTSTRAP_CODE: usize = 0x7000;
+
+#[allow(dead_code)]
+struct ApBootstrapData {
+    cr3: usize,
+    stack_base: usize,
+    stack_size: usize,
+    entry: usize,
+}
+
+unsafe fn load_ap_bootstrap_code() {
+    let src_ptr = AP_BOOTSTRAP_BIN.as_ptr();
+    let dst_ptr = AP_BOOTSTRAP_CODE as *mut u8;
+
+    let size = AP_BOOTSTRAP_BIN.len();
+
+    assert!(size != 0, "Empty bootstrap code");
+    assert!(
+        AP_BOOTSTRAP_CODE + size < 0x100000,
+        "Invalid bootstrap code placement: is not below 1MiB"
+    );
+
+    let src_slice = core::slice::from_raw_parts(src_ptr, size);
+    let dst_slice = core::slice::from_raw_parts_mut(dst_ptr.virtualize(), size);
+
+    dst_slice.copy_from_slice(src_slice);
+}
+
+unsafe fn load_ap_bootstrap_data(src: &ApBootstrapData) {
+    let src_ptr = src as *const _ as *const u8;
+    let dst_ptr = AP_BOOTSTRAP_DATA as *mut u8;
+    let size = size_of::<ApBootstrapData>();
+
+    assert!(
+        AP_BOOTSTRAP_DATA + size < 0x100000,
+        "Invalid bootstrap data placement: is not below 1MiB"
+    );
+
+    let src_slice = core::slice::from_raw_parts(src_ptr, size);
+    let dst_slice = core::slice::from_raw_parts_mut(dst_ptr.virtualize(), size);
+
+    dst_slice.copy_from_slice(src_slice);
+    core::arch::asm!("wbinvd");
+}
+
+unsafe fn start_ap_core(apic_id: u32) {
+    assert!(ArchitectureImpl::interrupt_mask());
+
+    let bsp_cpu = Cpu::local();
+    let bsp_apic = bsp_cpu.local_apic();
+
+    let cr3 = KERNEL_TABLES.physical_address();
+    let stack_base = phys::alloc_pages_contiguous(AP_STACK_PAGES, PageUsage::Used)
+        .unwrap()
+        .virtualize();
+    let stack_size = AP_STACK_PAGES * 0x1000;
+
+    let data = ApBootstrapData {
+        cr3,
+        stack_base,
+        stack_size,
+        entry: __x86_64_ap_entry as usize,
+    };
+
+    load_ap_bootstrap_data(&data);
+
+    let cpu_count = CPU_COUNT.load(Ordering::Acquire);
+
+    // Send an IPI to wake up the AP
+    bsp_apic.wakeup_cpu(apic_id, AP_BOOTSTRAP_CODE);
+
+    while cpu_count == CPU_COUNT.load(Ordering::Acquire) {
+        core::hint::spin_loop();
+    }
+
+    infoln!("cpu{} up", cpu_count);
+}
+
+/// Starts up application processors specified by ACPI MADT.
+///
+/// # Safety
+///
+/// Only meant to be called once by the BSP.
+pub unsafe fn start_ap_cores(info: &ProcessorInfo) {
+    let aps = &info.application_processors;
+
+    if aps.is_empty() {
+        return;
+    }
+
+    load_ap_bootstrap_code();
+
+    for ap in aps {
+        if ap.is_ap && ap.state == ProcessorState::WaitingForSipi {
+            start_ap_core(ap.local_apic_id);
+        }
+    }
+}

src/arch/x86_64/table/fixed.rs

@@ -132,6 +132,8 @@ pub unsafe fn init_fixed_tables() {
 
     // Global L0
     let addr = KERNEL_TABLES.l1.physical_address();
-    // No lower mapping anymore
+
+    // Keep the lower mapping for AP bootstrapping
+    KERNEL_TABLES.l0[0] = PageEntry::table(addr, PageAttributes::empty());
     KERNEL_TABLES.l0[511] = PageEntry::table(addr, PageAttributes::empty());
 }

src/arch/x86_64/vectors.S

@@ -65,96 +65,7 @@ __x86_64_exc_\n:
 1:
 .endm
 
-.set IRQ_REG_RAX, 0 * 8
-.set IRQ_REG_RCX, 1 * 8
-.set IRQ_REG_RDX, 2 * 8
-.set IRQ_REG_RBX, 3 * 8
-.set IRQ_REG_RSI, 4 * 8
-.set IRQ_REG_RDI, 5 * 8
-.set IRQ_REG_RBP, 6 * 8
-.set IRQ_REG_R8, 7 * 8
-.set IRQ_REG_R9, 8 * 8
-.set IRQ_REG_R10, 9 * 8
-.set IRQ_REG_R11, 10 * 8
-.set IRQ_REG_R12, 11 * 8
-.set IRQ_REG_R13, 12 * 8
-.set IRQ_REG_R14, 13 * 8
-.set IRQ_REG_R15, 14 * 8
-
-// 15 registers + stack align word if needed
-.set IRQ_STATE_SIZE, 15 * 8
-
-.macro apic_vector, n
-__x86_64_apic_irq_\n:
-    // %rsp + 0: %rip
-    // %rsp + 8: %cs
-    SWAPGS_IF_NEEDED 8
-
-    // Save state
-    subq $IRQ_STATE_SIZE, %rsp
-
-    movq %rax, IRQ_REG_RAX(%rsp)
-    movq %rcx, IRQ_REG_RCX(%rsp)
-    movq %rdx, IRQ_REG_RDX(%rsp)
-    movq %rbx, IRQ_REG_RBX(%rsp)
-    movq %rsi, IRQ_REG_RSI(%rsp)
-    movq %rdi, IRQ_REG_RDI(%rsp)
-    movq %rbp, IRQ_REG_RBP(%rsp)
-    movq %r8,  IRQ_REG_R8(%rsp)
-    movq %r9,  IRQ_REG_R9(%rsp)
-    movq %r10, IRQ_REG_R10(%rsp)
-    movq %r11, IRQ_REG_R11(%rsp)
-    movq %r12, IRQ_REG_R12(%rsp)
-    movq %r13, IRQ_REG_R13(%rsp)
-    movq %r14, IRQ_REG_R14(%rsp)
-    movq %r15, IRQ_REG_R15(%rsp)
-
-    // Save current stack into %rsi (arg 2) + %rbp
-    movq %rsp, %rsi
-    movq %rsp, %rbp
-
-    // Force correct stack alignment
-    orq $0xF, %rsp
-    xorq $0xF, %rsp
-
-    // Force correct segment registers
-    mov $0x10, %ax
-    mov %ax, %ss
-    mov %ax, %ds
-    mov %ax, %es
-
-    mov $\n, %rdi
-    call {apic_irq_handler}
-
-    // Restore the stack pointer
-    movq %rbp, %rsp
-
-    // Restore state
-    movq IRQ_REG_RAX(%rsp), %rax
-    movq IRQ_REG_RCX(%rsp), %rcx
-    movq IRQ_REG_RDX(%rsp), %rdx
-    movq IRQ_REG_RBX(%rsp), %rbx
-    movq IRQ_REG_RSI(%rsp), %rsi
-    movq IRQ_REG_RDI(%rsp), %rdi
-    movq IRQ_REG_RBP(%rsp), %rbp
-    movq IRQ_REG_R8(%rsp),  %r8
-    movq IRQ_REG_R9(%rsp),  %r9
-    movq IRQ_REG_R10(%rsp), %r10
-    movq IRQ_REG_R11(%rsp), %r11
-    movq IRQ_REG_R12(%rsp), %r12
-    movq IRQ_REG_R13(%rsp), %r13
-    movq IRQ_REG_R14(%rsp), %r14
-    movq IRQ_REG_R15(%rsp), %r15
-
-    addq $IRQ_STATE_SIZE, %rsp
-
-    SWAPGS_IF_NEEDED 8
-
-    iretq
-.endm
-
 .global __x86_64_exception_vectors
-.global __x86_64_apic_vectors
 
 .section .text
 __x86_64_exc_common:
@@ -162,6 +73,7 @@ __x86_64_exc_common:
     // %rsp + 8: error code
     // %rsp + 16: %rip
     // %rsp + 24: %cs
+
     SWAPGS_IF_NEEDED 24
 
     EXC_SAVE_STATE
@@ -221,23 +133,6 @@ ISR_NERR 29
 ISR_YERR 30
 ISR_NERR 31
 
-apic_vector 0
-apic_vector 1
-apic_vector 2
-apic_vector 3
-apic_vector 4
-apic_vector 5
-apic_vector 6
-apic_vector 7
-apic_vector 8
-apic_vector 9
-apic_vector 10
-apic_vector 11
-apic_vector 12
-apic_vector 13
-apic_vector 14
-apic_vector 15
-
 .section .rodata
 .global __x86_64_exception_vectors
 .p2align 4
@@ -275,23 +170,4 @@ __x86_64_exception_vectors:
     .quad __x86_64_exc_30
     .quad __x86_64_exc_31
 
-.p2align 4
-__x86_64_apic_vectors:
-    .quad __x86_64_apic_irq_0
-    .quad __x86_64_apic_irq_1
-    .quad __x86_64_apic_irq_2
-    .quad __x86_64_apic_irq_3
-    .quad __x86_64_apic_irq_4
-    .quad __x86_64_apic_irq_5
-    .quad __x86_64_apic_irq_6
-    .quad __x86_64_apic_irq_7
-    .quad __x86_64_apic_irq_8
-    .quad __x86_64_apic_irq_9
-    .quad __x86_64_apic_irq_10
-    .quad __x86_64_apic_irq_11
-    .quad __x86_64_apic_irq_12
-    .quad __x86_64_apic_irq_13
-    .quad __x86_64_apic_irq_14
-    .quad __x86_64_apic_irq_15
-
 .section .text

src/debug.rs

@@ -58,7 +58,7 @@ macro_rules! log_print_raw {
 
 macro_rules! log_print {
     ($level:expr, $($args:tt)+) => {
-        log_print_raw!($level, "{}:{}: {}", file!(), line!(), format_args!($($args)+))
+        log_print_raw!($level, "cpu{}:{}:{}: {}", $crate::task::Cpu::local_id(), file!(), line!(), format_args!($($args)+))
     };
 }
 

src/device/interrupt.rs

@@ -44,17 +44,6 @@ pub trait InterruptSource: Device {
 //     /// Returns the unique ID of this local interrupt controller
 //     fn id(&self) -> Self::Id;
 //
-//     /// Sends a message to the requested set of CPUs through an interprocessor interrupt.
-//     ///
-//     /// # Note
-//     ///
-//     /// u64 limits the number of targetable CPUs to (only) 64. Platform-specific implementations
-//     /// may impose narrower restrictions.
-//     ///
-//     /// # Safety
-//     ///
-//     /// As the call may alter the flow of execution on CPUs, this function is unsafe.
-//     unsafe fn send_ipi(&self, target: IpiDeliveryTarget, msg: CpuMessage) -> Result<(), Error>;
 // }
 
 /// Interface for a device responsible for routing and handling IRQs from external sources

src/device/platform.rs

@@ -2,9 +2,12 @@
 
 use abi::error::Error;
 
-use crate::debug::DebugSink;
+use crate::{arch::CpuMessage, debug::DebugSink};
 
-use super::{interrupt::ExternalInterruptController, timer::TimestampSource};
+use super::{
+    interrupt::{ExternalInterruptController, IpiDeliveryTarget},
+    timer::TimestampSource,
+};
 
 /// Platform interface for interacting with a general hardware set
 pub trait Platform {
@@ -53,4 +56,16 @@ pub trait Platform {
     ///
     /// May not be initialized at the moment of calling.
     fn timestamp_source(&self) -> &dyn TimestampSource;
+
+    /// Sends a message to the requested set of CPUs through an interprocessor interrupt.
+    ///
+    /// # Note
+    ///
+    /// u64 limits the number of targetable CPUs to (only) 64. Platform-specific implementations
+    /// may impose narrower restrictions.
+    ///
+    /// # Safety
+    ///
+    /// As the call may alter the flow of execution on CPUs, this function is unsafe.
+    unsafe fn send_ipi(&self, target: IpiDeliveryTarget, msg: CpuMessage) -> Result<(), Error>;
 }

src/panic.rs

@@ -2,9 +2,11 @@
 use core::sync::atomic::{AtomicBool, Ordering};
 
 use crate::{
-    arch::{Architecture, ArchitectureImpl},
+    arch::{Architecture, ArchitectureImpl, CpuMessage, PLATFORM},
     debug::{debug_internal, LogLevel},
+    device::{interrupt::IpiDeliveryTarget, platform::Platform},
     sync::SpinFence,
+    task::{sched::CpuQueue, Cpu},
 };
 
 // Just a fence to ensure secondary panics don't trash the screen
@@ -37,14 +39,14 @@ fn panic_handler(pi: &core::panic::PanicInfo) -> ! {
         .is_ok()
     {
         // Let other CPUs know we're screwed
-        // unsafe {
+        unsafe {
-        //     PLATFORM
+            PLATFORM
-        //         .interrupt_controller()
+                .send_ipi(IpiDeliveryTarget::AllExceptLocal, CpuMessage::Panic)
-        //         .send_ipi(IpiDeliveryTarget::AllExceptLocal, CpuMessage::Panic)
+                .ok();
-        //         .ok();
+        }
-        // }
 
         log_print_raw!(LogLevel::Fatal, "--- BEGIN PANIC ---\n");
+        log_print_raw!(LogLevel::Fatal, "In CPU {}", Cpu::local_id());
         log_print_raw!(LogLevel::Fatal, "Kernel panic ");
 
         if let Some(location) = pi.location() {
@@ -64,6 +66,21 @@ fn panic_handler(pi: &core::panic::PanicInfo) -> ! {
             debug_internal(*msg, LogLevel::Fatal);
             log_print_raw!(LogLevel::Fatal, "\n");
         }
+
+        for (i, queue) in CpuQueue::all().enumerate() {
+            log_print_raw!(LogLevel::Fatal, "queue{}:\n", i);
+            let lock = unsafe { queue.grab() };
+            for item in lock.iter() {
+                log_print_raw!(
+                    LogLevel::Fatal,
+                    "* {} {:?} {:?}\n",
+                    item.id(),
+                    item.name(),
+                    item.state()
+                );
+            }
+        }
+
         log_print_raw!(LogLevel::Fatal, "---  END PANIC  ---\n");
 
         log_print_raw!(LogLevel::Fatal, "X");

src/proc/wait.rs

@@ -70,10 +70,10 @@ impl Wait {
 
                 drop(tick_lock);
 
-                if proc.state() != ProcessState::Terminated {
                 unsafe {
                     proc.set_wait_status(WaitStatus::Done);
                 }
+                if proc.state() == ProcessState::Suspended {
                     proc.enqueue_somewhere();
                 }
             }
@@ -174,7 +174,9 @@ pub fn tick(now: Duration) {
         if now > item.deadline {
             let t = cursor.remove_current().unwrap();
 
+            if t.process.state() == ProcessState::Suspended {
                 t.process.enqueue_somewhere();
+            }
         } else {
             cursor.move_next();
         }

src/sync.rs

@@ -111,6 +111,16 @@ impl<T> IrqSafeSpinlock<T> {
             _irq: irq_guard,
         }
     }
+
+    /// Returns an unsafe reference to the inner value.
+    ///
+    /// # Safety
+    ///
+    /// Unsafe: explicitly ignores proper access sharing.
+    #[allow(clippy::mut_from_ref)]
+    pub unsafe fn grab(&self) -> &mut T {
+        unsafe { &mut *self.inner.value.get() }
+    }
 }
 
 impl<'a, T> Deref for IrqSafeSpinlockGuard<'a, T> {

src/syscall/mod.rs

@@ -347,8 +347,6 @@ fn syscall_handler(func: SyscallFunction, args: &[u64]) -> Result<usize, Error>
         SyscallFunction::SetSignalEntry => {
             let entry = args[0] as usize;
             let sp = args[1] as usize;
-            let proc = Process::current();
-            warnln!("SetSignalEntry({}, {:#x}, {:#x})", proc.id(), entry, sp);
 
             Process::current().set_signal_entry(entry, sp);
 

src/task/mod.rs

@@ -6,6 +6,7 @@ use abi::error::Error;
 use alloc::{rc::Rc, string::String, vec::Vec};
 
 use crate::{
+    arch::{Architecture, ArchitectureImpl},
     kernel_main,
     sync::{IrqSafeSpinlock, SpinFence},
     task::sched::CpuQueue,
@@ -75,9 +76,7 @@ pub fn spawn_kernel_closure<S: Into<String>, F: Fn() + Send + 'static>(
 
 /// Sets up CPU queues and gives them some processes to run
 pub fn init() -> Result<(), Error> {
-    // XXX
+    let cpu_count = ArchitectureImpl::cpu_count();
-    // let cpu_count = CPU_COUNT.load(Ordering::Acquire);
-    let cpu_count = 1;
 
     // Create a queue for each CPU
     sched::init_queues(Vec::from_iter((0..cpu_count).map(|_| CpuQueue::new())));

src/task/process.rs

@@ -14,6 +14,7 @@ use atomic_enum::atomic_enum;
 use vfs::VnodeRef;
 
 use crate::{
+    arch::{Architecture, ArchitectureImpl},
     mem::{table::AddressSpace, ForeignPointer},
     proc::{
         io::ProcessIo,
@@ -204,6 +205,11 @@ impl Process {
         self.inner.lock().group_id
     }
 
+    /// Returns the CPU number this task in running on (or the last one)
+    pub fn cpu_id(&self) -> u32 {
+        self.cpu_id.load(Ordering::Acquire)
+    }
+
     /// Selects a suitable CPU queue and submits the process for execution.
     ///
     /// # Panics
@@ -225,6 +231,7 @@ impl Process {
     ///
     /// Currently, the code will panic if the process is queued/executing on any queue.
     pub fn enqueue_to(self: Rc<Self>, queue: &CpuQueue) {
+        let _irq = IrqGuard::acquire();
         let current_state = self.state.swap(ProcessState::Ready, Ordering::SeqCst);
 
         if current_state == ProcessState::Terminated {
@@ -390,6 +397,7 @@ impl CurrentProcess {
     pub unsafe fn new(inner: Rc<Process>) -> Self {
         // XXX
         // assert_eq!(DAIF.read(DAIF::I), 1);
+        assert!(ArchitectureImpl::interrupt_mask());
         Self(inner)
     }
 

src/task/sched.rs

@@ -88,8 +88,14 @@ impl CpuQueueInner {
                     return Some(task);
                 }
                 // Drop suspended tasks from the queue
-                ProcessState::Suspended | ProcessState::Terminated => (),
+                ProcessState::Suspended | ProcessState::Terminated | ProcessState::Running => (),
-                e => panic!("Unexpected process state in CpuQueue: {:?}", e),
+                // e => panic!(
+                //     "Unexpected process state in CpuQueue: {:?} ({} {:?}, cpu_id={})",
+                //     e,
+                //     task.id(),
+                //     task.name(),
+                //     task.cpu_id()
+                // ),
             }
         }
 
@@ -106,7 +112,8 @@ impl CpuQueueInner {
 impl CpuQueue {
     /// Constructs an empty queue with its own idle task
     pub fn new() -> Self {
-        let idle = TaskContext::kernel(__idle, 0).expect("Could not construct an idle task");
+        let idle = TaskContext::kernel(__idle, Cpu::local_id() as usize)
+            .expect("Could not construct an idle task");
 
         Self {
             inner: {
@@ -209,6 +216,7 @@ impl CpuQueue {
     /// Only meant to be called from Process impl. The function does not set any process accounting
     /// information, which may lead to invalid states.
     pub unsafe fn enqueue(&self, p: Rc<Process>) {
+        assert_eq!(p.state(), ProcessState::Ready);
         self.inner.lock().queue.push_back(p);
     }
 
@@ -240,6 +248,16 @@ impl CpuQueue {
         self.inner.lock()
     }
 
+    /// Returns an unsafe reference to the queue.
+    ///
+    /// # Safety
+    ///
+    /// Only meant to be called to dump the queue contents when panicking.
+    #[allow(clippy::mut_from_ref)]
+    pub unsafe fn grab(&self) -> &mut CpuQueueInner {
+        self.inner.grab()
+    }
+
     /// Returns the process currently being executed.
     ///
     /// # Note