Kernel: Start all processors on kernel boot

Processors don't do anything, except print hello message and halt.

kernel/arch/x86_64/GDT.cpp

@@ -3,8 +3,6 @@
 
 #include <string.h>
 
-extern "C" uintptr_t g_boot_stack_top[0];
-
 namespace Kernel::GDT
 {
 

kernel/arch/x86_64/PageTable.cpp

@@ -306,6 +306,7 @@ namespace Kernel
 
 	void PageTable::load()
 	{
+		SpinLockGuard _(m_lock);
 		asm volatile("movq %0, %%cr3" :: "r"(m_highest_paging_struct));
 		s_current = this;
 	}

kernel/arch/x86_64/boot.S

@@ -53,12 +53,9 @@ bananboot_start:
 bananboot_end:
 
 .section .bss, "aw", @nobits
- 	# Create stack
+	# reserve 4096 bytes of initial stack for each processor
-	.global g_boot_stack_bottom
+	g_processor_stacks:
-	g_boot_stack_bottom:
+		.skip 4096 * 64
-		.skip 16384
-	.global g_boot_stack_top
-	g_boot_stack_top:
 
 	.global g_kernel_cmdline
 	g_kernel_cmdline:
@@ -105,6 +102,13 @@ boot_gdtr:
 	.short . - boot_gdt - 1
 	.quad V2P(boot_gdt)
 
+.global g_ap_startup_done
+g_ap_startup_done:
+	.byte 0
+.global g_ap_running_count
+g_ap_running_count:
+	.byte 0
+
 .section .text
 
 has_cpuid:
@@ -151,6 +155,20 @@ enable_sse:
 	movl %eax, %cr4
 	ret
 
+# NOTE: return address in argument %edi
+initialize_pmode_stack:
+	movl $1, %eax
+	cpuid
+	shrl $24, %ebx
+
+	movw %bx, %gs
+
+	shll $12, %ebx
+	addl $V2P(g_processor_stacks) + 4096, %ebx
+	movl %ebx, %esp
+
+	jmp *%edi
+
 initialize_paging:
 	# enable PAE
 	movl %cr4, %ecx
@@ -178,10 +196,13 @@ initialize_paging:
 .type _start, @function
 _start:
 	# Initialize stack and multiboot info
-	movl $V2P(g_boot_stack_top), %esp
 	movl %eax, V2P(bootloader_magic)
 	movl %ebx, V2P(bootloader_info)
 
+	movl $V2P(1f), %edi
+	jmp initialize_pmode_stack
+1:
+
 	call check_requirements
 	call enable_sse
 
@@ -194,17 +215,15 @@ _start:
 .code64
 long_mode:
 	movw $0x10, %ax
-	movw %ax, %ss
-
-	# clear segment registers (unused in x86_64?)
-	movw $0x00, %ax
 	movw %ax, %ds
+	movw %ax, %ss
 	movw %ax, %es
-	movw %ax, %fs
+
-	movw %ax, %gs
+	# move stack pointer to higher half
+	movl %esp, %esp
+	addq $KERNEL_OFFSET, %rsp
 
 	# jump to higher half
-	movq $g_boot_stack_top, %rsp
 	movabsq $higher_half, %rcx
 	jmp *%rcx
 
@@ -227,3 +246,70 @@ system_halt:
 	cli
 1:	hlt
 	jmp 1b
+
+
+.section .ap_init, "ax"
+
+.code16
+.global ap_trampoline
+ap_trampoline:
+	cli
+	ljmpl $0x00, $ap_cs_clear
+
+ap_cs_clear:
+	xorw %ax, %ax
+	movw %ax, %ds
+
+	# load ap gdt and enter protected mode
+	lgdt ap_gdtr
+	movl %cr0, %eax
+	orb $1, %al
+	movl %eax, %cr0
+	ljmpl $0x08, $ap_protected_mode
+
+.code32
+ap_protected_mode:
+	movw $0x10, %ax
+	movw %ax, %ds
+	movw %ax, %ss
+	movw %ax, %es
+
+	movl $1f, %edi
+	jmp V2P(initialize_pmode_stack)
+1:
+
+	# load boot gdt, load boot page table and enter long mode
+	call V2P(initialize_paging)
+	lgdt V2P(boot_gdtr)
+	ljmpl $0x08, $ap_long_mode
+
+.code64
+ap_long_mode:
+	# move stack pointer to higher half
+	movl %esp, %esp
+	addq $KERNEL_OFFSET, %rsp
+
+	# jump to higher half
+	movabsq $ap_higher_half, %rcx
+	jmp *%rcx
+
+ap_higher_half:
+	# clear rbp for stacktrace
+	xorq %rbp, %rbp
+
+1:	pause
+	cmpb $0, g_ap_startup_done
+	jz 1b
+
+	lock incb g_ap_running_count
+
+	call ap_main
+	jmp system_halt
+
+ap_gdt:
+	.quad 0x0000000000000000 # null descriptor
+	.quad 0x00CF9A000000FFFF # 32 bit code
+	.quad 0x00CF92000000FFFF # 32 bit data
+ap_gdtr:
+	.short . - ap_gdt - 1
+	.quad ap_gdt

kernel/arch/x86_64/linker.ld

@@ -4,6 +4,13 @@ KERNEL_OFFSET = 0xFFFFFFFF80000000;
 
 SECTIONS
 {
+	. = 0xF000;
+	.ap_init ALIGN(4K) : AT(ADDR(.ap_init))
+	{
+		g_ap_init_addr = .;
+		*(.ap_init)
+	}
+
 	. = 0x00100000 + KERNEL_OFFSET;
 
 	g_kernel_start = .;

kernel/include/kernel/APIC.h

@@ -18,6 +18,8 @@ namespace Kernel
 		virtual BAN::ErrorOr<void> reserve_irq(uint8_t irq) override;
 		virtual BAN::Optional<uint8_t> get_free_irq() override;
 
+		virtual void initialize_multiprocessor() override;
+
 	private:
 		uint32_t read_from_local_apic(ptrdiff_t);
 		void write_to_local_apic(ptrdiff_t, uint32_t);

kernel/include/kernel/InterruptController.h

@@ -37,6 +37,8 @@ namespace Kernel
 		static void initialize(bool force_pic);
 		static InterruptController& get();
 
+		virtual void initialize_multiprocessor() = 0;
+
 		virtual BAN::ErrorOr<void> reserve_irq(uint8_t irq) = 0;
 		virtual BAN::Optional<uint8_t> get_free_irq() = 0;
 

kernel/include/kernel/PIC.h

@@ -16,6 +16,8 @@ namespace Kernel
 		virtual BAN::ErrorOr<void> reserve_irq(uint8_t irq) override;
 		virtual BAN::Optional<uint8_t> get_free_irq() override;
 
+		virtual void initialize_multiprocessor() override;
+
 		static void remap();
 		static void mask_all();
 

kernel/kernel/APIC.cpp

@@ -6,20 +6,27 @@
 #include <kernel/IDT.h>
 #include <kernel/Memory/PageTable.h>
 #include <kernel/MMIO.h>
+#include <kernel/Timer/Timer.h>
 
 #include <string.h>
 
 #define LAPIC_EIO_REG		0xB0
 #define LAPIC_SIV_REG		0xF0
 #define LAPIC_IS_REG		0x100
+#define LAPIC_ERROR_REG		0x280
+#define LAPIC_ICR_LO_REG	0x300
+#define LAPIC_ICR_HI_REG	0x310
 
 #define IOAPIC_MAX_REDIRS	0x01
 #define IOAPIC_REDIRS		0x10
 
-#define DEBUG_PRINT_PROCESSORS 0
-
 // https://uefi.org/specs/ACPI/6.5/05_ACPI_Software_Programming_Model.html#multiple-apic-description-table-madt-format
 
+extern uint8_t g_ap_init_addr[];
+
+extern volatile uint8_t g_ap_startup_done[];
+extern volatile uint8_t g_ap_running_count[];
+
 namespace Kernel
 {
 
@@ -182,18 +189,68 @@ namespace Kernel
 		uint32_t sivr = apic->read_from_local_apic(LAPIC_SIV_REG);
 		apic->write_to_local_apic(LAPIC_SIV_REG, sivr | 0x1FF);
 
-	#if DEBUG_PRINT_PROCESSORS
-		for (auto& processor : apic->m_processors)
-		{
-			dprintln("Processor{}", processor.processor_id);
-			dprintln("  lapic id: {}", processor.apic_id);
-			dprintln("  status:   {}", (processor.flags & Processor::Flags::Enabled) ? "enabled" : (processor.flags & Processor::Flags::OnlineCapable) ? "can be enabled" : "disabled");
-		}
-	#endif
-
 		return apic;
 	}
 
+	void APIC::initialize_multiprocessor()
+	{
+		constexpr auto udelay =
+			[](uint64_t us) {
+				uint64_t wake_time = SystemTimer::get().ns_since_boot() + us * 1000;
+				while (SystemTimer::get().ns_since_boot() < wake_time)
+					__builtin_ia32_pause();
+			};
+
+		const auto send_ipi =
+			[&](uint8_t processor, uint32_t data, uint64_t ud)
+			{
+				write_to_local_apic(LAPIC_ICR_HI_REG, (read_from_local_apic(LAPIC_ICR_HI_REG) & 0x00FFFFFF) | (processor << 24));
+				write_to_local_apic(LAPIC_ICR_LO_REG, data);
+				udelay(ud);
+				while (read_from_local_apic(LAPIC_ICR_LO_REG) & (1 << 12))
+					__builtin_ia32_pause();
+			};
+
+		const size_t ap_init_page = reinterpret_cast<vaddr_t>(g_ap_init_addr) / PAGE_SIZE;
+
+		dprintln("System has {} processors", m_processors.size());
+
+		uint8_t bsp_id = get_processor_id();
+		dprintln("BSP lapic id: {}", bsp_id);
+
+		for (auto& processor : m_processors)
+		{
+			if (processor.apic_id == bsp_id)
+				continue;
+
+			if (!(processor.flags & (Processor::Flags::Enabled | Processor::Flags::OnlineCapable)))
+			{
+				dwarnln("Skipping processor (lapic id {}) initialization", processor.apic_id);
+				continue;
+			}
+
+			dprintln("Trying to enable processor (lapic id {})", processor.apic_id);
+
+			write_to_local_apic(LAPIC_ERROR_REG, 0x00);
+			send_ipi(processor.processor_id, (read_from_local_apic(LAPIC_ICR_LO_REG) & 0xFFF00000) | 0x0000C500, 0);
+			send_ipi(processor.processor_id, (read_from_local_apic(LAPIC_ICR_LO_REG) & 0xFFF0F800) | 0x00008500, 0);
+
+			udelay(10 * 1000);
+
+			for (int i = 0; i < 2; i++)
+			{
+				write_to_local_apic(LAPIC_ERROR_REG, 0x00);
+				send_ipi(processor.processor_id, (read_from_local_apic(LAPIC_ICR_LO_REG) & 0xFFF0F800) | 0x00000600 | ap_init_page, 200);
+			}
+		}
+
+		*g_ap_startup_done = 1;
+
+		udelay(100);
+
+		dprintln("{} processors started", *g_ap_running_count);
+	}
+
 	uint32_t APIC::read_from_local_apic(ptrdiff_t offset)
 	{
 		return MMIO::read32(m_local_apic_vaddr + offset);

kernel/kernel/PIC.cpp

@@ -134,4 +134,9 @@ namespace Kernel
 		return IO::inb(port) & (1 << irq);
 	}
 
+	void PIC::initialize_multiprocessor()
+	{
+		dprintln("Only single processor supported with PIC");
+	}
+
 }

kernel/kernel/kernel.cpp

@@ -19,12 +19,13 @@
 #include <kernel/PCI.h>
 #include <kernel/PIC.h>
 #include <kernel/Process.h>
+#include <kernel/Processor.h>
 #include <kernel/Random.h>
 #include <kernel/Scheduler.h>
 #include <kernel/Syscall.h>
+#include <kernel/Terminal/FramebufferTerminal.h>
 #include <kernel/Terminal/Serial.h>
 #include <kernel/Terminal/VirtualTTY.h>
-#include <kernel/Terminal/FramebufferTerminal.h>
 #include <kernel/Timer/Timer.h>
 
 struct ParsedCommandLine
@@ -127,6 +128,8 @@ extern "C" void kernel_main(uint32_t boot_magic, uint32_t boot_info)
 	SystemTimer::initialize(cmdline.force_pic);
 	dprintln("Timers initialized");
 
+	InterruptController::get().initialize_multiprocessor();
+
 	DevFileSystem::initialize();
 	dprintln("devfs initialized");
 
@@ -206,3 +209,13 @@ static void init2(void*)
 
 	MUST(Process::create_userspace({ 0, 0, 0, 0 }, "/usr/bin/init"sv));
 }
+
+extern "C" void ap_main()
+{
+	using namespace Kernel;
+
+	dprintln("hello from processor {}", get_processor_id());
+
+	for (;;)
+		asm volatile("");
+}

script/qemu.sh

@@ -21,7 +21,7 @@ fi
 
 qemu-system-$BANAN_ARCH												\
 	-m 1G															\
-	-smp 2															\
+	-smp 4															\
 	$BIOS_ARGS														\
 	-drive format=raw,id=disk,file=${BANAN_DISK_IMAGE_PATH},if=none	\
 	-device e1000e,netdev=net										\