arm64: PSCI+GICv2 SMP support

* SCTLR register definitions made 64-bit
* isb added after writing to SCTLR, to ensure we don't execute
  instructions requiring the MMU prior to the MMU being enabled
* GICv2 has per-cpu registers for IRQs < 32, so enabling or
  disabling those IRQs needs to be done on all cores
* MMU setup code in kernel and bootloader made per-cpu
* Signiture of arch_smp_boot_other_cpus is now the same on all
  architectures, with an addr_t page table

Change-Id: I13bef7ca4efc5504d643833664560a9a6a6a6964
Reviewed-on: https://review.haiku-os.org/c/haiku/+/10649
Reviewed-by: waddlesplash <[email protected]>

headers/private/kernel/arch/arm64/arm_registers.h

@@ -527,41 +527,41 @@
 #define	PAR_S_MASK		(0x1 << PAR_S_SHIFT)
 
 /* SCTLR_EL1 - System Control Register */
-#define	SCTLR_RES0	0xc8222440	/* Reserved ARMv8.0, write 0 */
+#define	SCTLR_RES0	0xc8222440UL	/* Reserved ARMv8.0, write 0 */
-#define	SCTLR_RES1	0x30d00800	/* Reserved ARMv8.0, write 1 */
+#define	SCTLR_RES1	0x30d00800UL	/* Reserved ARMv8.0, write 1 */
 
-#define	SCTLR_M		0x00000001
+#define	SCTLR_M		0x00000001UL
-#define	SCTLR_A		0x00000002
+#define	SCTLR_A		0x00000002UL
-#define	SCTLR_C		0x00000004
+#define	SCTLR_C		0x00000004UL
-#define	SCTLR_SA	0x00000008
+#define	SCTLR_SA	0x00000008UL
-#define	SCTLR_SA0	0x00000010
+#define	SCTLR_SA0	0x00000010UL
-#define	SCTLR_CP15BEN	0x00000020
+#define	SCTLR_CP15BEN	0x00000020UL
 /* Bit 6 is reserved */
-#define	SCTLR_ITD	0x00000080
+#define	SCTLR_ITD	0x00000080UL
-#define	SCTLR_SED	0x00000100
+#define	SCTLR_SED	0x00000100UL
-#define	SCTLR_UMA	0x00000200
+#define	SCTLR_UMA	0x00000200UL
 /* Bit 10 is reserved */
 /* Bit 11 is reserved */
-#define	SCTLR_I		0x00001000
+#define	SCTLR_I		0x00001000UL
-#define	SCTLR_EnDB	0x00002000 /* ARMv8.3 */
+#define	SCTLR_EnDB	0x00002000UL /* ARMv8.3 */
-#define	SCTLR_DZE	0x00004000
+#define	SCTLR_DZE	0x00004000UL
-#define	SCTLR_UCT	0x00008000
+#define	SCTLR_UCT	0x00008000UL
-#define	SCTLR_nTWI	0x00010000
+#define	SCTLR_nTWI	0x00010000UL
 /* Bit 17 is reserved */
-#define	SCTLR_nTWE	0x00040000
+#define	SCTLR_nTWE	0x00040000UL
-#define	SCTLR_WXN	0x00080000
+#define	SCTLR_WXN	0x00080000UL
 /* Bit 20 is reserved */
-#define	SCTLR_IESB	0x00200000 /* ARMv8.2 */
+#define	SCTLR_IESB	0x00200000UL /* ARMv8.2 */
 /* Bit 22 is reserved */
-#define	SCTLR_SPAN	0x00800000 /* ARMv8.1 */
+#define	SCTLR_SPAN	0x00800000UL /* ARMv8.1 */
-#define	SCTLR_EOE	0x01000000
+#define	SCTLR_EOE	0x01000000UL
-#define	SCTLR_EE	0x02000000
+#define	SCTLR_EE	0x02000000UL
-#define	SCTLR_UCI	0x04000000
+#define	SCTLR_UCI	0x04000000UL
-#define	SCTLR_EnDA	0x08000000 /* ARMv8.3 */
+#define	SCTLR_EnDA	0x08000000UL /* ARMv8.3 */
-#define	SCTLR_nTLSMD	0x10000000 /* ARMv8.2 */
+#define	SCTLR_nTLSMD	0x10000000UL /* ARMv8.2 */
-#define	SCTLR_LSMAOE	0x20000000 /* ARMv8.2 */
+#define	SCTLR_LSMAOE	0x20000000UL /* ARMv8.2 */
-#define	SCTLR_EnIB	0x40000000 /* ARMv8.3 */
+#define	SCTLR_EnIB	0x40000000UL /* ARMv8.3 */
-#define	SCTLR_EnIA	0x80000000 /* ARMv8.3 */
+#define	SCTLR_EnIA	0x80000000UL /* ARMv8.3 */
 
 /* SPSR_EL1 */
 /*

headers/private/kernel/boot/platform/efi/arch_smp.h

@@ -9,33 +9,34 @@
 #include <boot/menu.h>
 
 
-#if defined(__riscv) || defined(__ARM__) || defined(__aarch64__)
-// These platforms take inventory of cpu cores from fdt
-
-struct platform_cpu_info {
-	uint32 id; // hart id on riscv
 #if defined(__riscv)
+struct platform_cpu_info {
+	uint32 id;
 	uint32 phandle;
 	uint32 plicContext;
-#endif
 };
 
-#if defined(__riscv)
 extern uint32 gBootHart;
-#endif
+void arch_smp_register_cpu(platform_cpu_info** cpu);
+platform_cpu_info* arch_smp_find_cpu(uint32 phandle);
+#elif defined(__ARM__)
+struct platform_cpu_info {
+	uint32 id;
+};
+
+void arch_smp_register_cpu(platform_cpu_info** cpu);
+#elif defined(__aarch64__)
+struct platform_cpu_info {
+	uint32 id;
+	uint64 mpidr;
+};
 
 void arch_smp_register_cpu(platform_cpu_info** cpu);
 #endif
 
-
 int arch_smp_get_current_cpu(void);
 void arch_smp_init_other_cpus(void);
-#ifdef __riscv
+void arch_smp_boot_other_cpus(addr_t pageTable, uint64 kernelEntry, addr_t virtKernelArgs);
-platform_cpu_info* arch_smp_find_cpu(uint32 phandle);
-void arch_smp_boot_other_cpus(uint64 satp, uint64 kernelEntry, addr_t virtKernelArgs);
-#else
-void arch_smp_boot_other_cpus(uint32 pml4, uint64 kernelEntry, addr_t virtKernelArgs);
-#endif
 void arch_smp_add_safemode_menus(Menu *menu);
 void arch_smp_init(void);
 

src/system/boot/platform/efi/arch/arm/arch_smp.cpp

@@ -61,7 +61,7 @@ arch_smp_init_other_cpus(void)
 
 
 void
-arch_smp_boot_other_cpus(uint32 pml4, uint64 kernelEntry, addr_t virtKernelArgs)
+arch_smp_boot_other_cpus(addr_t pml4, uint64 kernelEntry, addr_t virtKernelArgs)
 {
 	// One cpu for now.
 }

src/system/boot/platform/efi/arch/arm64/aarch64.h

@@ -146,6 +146,7 @@ static inline void _arch_mmu_set_sctlr(uint64 sctlr)
 			WRITE_SPECIALREG(SCTLR_EL3, sctlr);
 			break;
 	}
+	asm("isb");
 }
 
 

src/system/boot/platform/efi/arch/arm64/arch_acpi.cpp

@@ -8,6 +8,7 @@
 #include <boot/platform.h>
 #include <boot/stage2.h>
 #include <arch_acpi.h>
+#include <arch_smp.h>
 
 #include "serial.h"
 #include "acpi.h"
@@ -94,6 +95,13 @@ arch_handle_acpi()
 				acpi_gic_interface *acpi_gicc = (acpi_gic_interface*)desc;
 				if (acpi_gicc->cpu_interface_num == 0)
 					gicc_base = acpi_gicc->base_address;
+
+				platform_cpu_info* cpu = NULL;
+				arch_smp_register_cpu(&cpu);
+				if (cpu == NULL)
+					continue;
+				cpu->id = acpi_gicc->cpu_interface_num;
+				cpu->mpidr = acpi_gicc->mpidr;
 			} else if (desc->type == ACPI_MADT_GIC_DISTRIBUTOR) {
 				acpi_gic_distributor *acpi_gicd = (acpi_gic_distributor*)desc;
 				gicd_base = acpi_gicd->base_address;

src/system/boot/platform/efi/arch/arm64/arch_smp.cpp

@@ -16,6 +16,9 @@
 #include <boot/stage2.h>
 #include <boot/menu.h>
 
+#include "mmu.h"
+#include "aarch64.h"
+
 
 //#define TRACE_SMP
 #ifdef TRACE_SMP
@@ -25,17 +28,47 @@
 #endif
 
 
+#define PSCI_CPU_ON 0xc4000003UL
+
+
+extern "C" void arch_enter_kernel(struct kernel_args* kernelArgs,
+	addr_t kernelEntry, addr_t kernelStackTop, uint32 cpu);
+void arm64_common_cpu_startup();
+
+
+struct secondary_startup_info {
+	uint64 ttbr0;
+	uint64 ttbr1;
+	uint64 sctlr;
+	uint64 tcr;
+	uint64 mair;
+	uint64 stack;
+	uint64 kernelEntry;
+	uint64 kernelArgs;
+	uint32 id;
+};
+
+
+static platform_cpu_info sCpus[SMP_MAX_CPUS];
+uint32 sCpuCount = 0;
+
+
 void
 arch_smp_register_cpu(platform_cpu_info** cpu)
 {
-        dprintf("TODO: arch_smp_register_cpu()\n");
+	uint32 newCount = sCpuCount + 1;
+	if (newCount > SMP_MAX_CPUS) {
+		*cpu = NULL;
+		return;
+	}
+	*cpu = &sCpus[sCpuCount];
+	sCpuCount = newCount;
 }
 
 
 int
 arch_smp_get_current_cpu(void)
 {
-	// One cpu for now.
 	return 0;
 }
 
@@ -43,16 +76,104 @@ arch_smp_get_current_cpu(void)
 void
 arch_smp_init_other_cpus(void)
 {
-	// One cpu for now.
+	gKernelArgs.num_cpus = sCpuCount;
-	gKernelArgs.num_cpus = 1;
+
+	if (get_safemode_boolean(B_SAFEMODE_DISABLE_SMP, false)) {
+		// SMP has been disabled!
+		TRACE("smp disabled per safemode setting\n");
+		gKernelArgs.num_cpus = 1;
+	}
+
+	if (gKernelArgs.num_cpus < 2)
+		return;
+
+	for (uint32 i = 1; i < gKernelArgs.num_cpus; i++) {
+		// create a final stack the trampoline code will put the ap processor on
+		void * stack = NULL;
+		const size_t size = KERNEL_STACK_SIZE + KERNEL_STACK_GUARD_PAGES * B_PAGE_SIZE;
+		if (platform_allocate_region(&stack, size, 0) != B_OK) {
+			panic("Unable to allocate AP stack");
+		}
+		memset(stack, 0, size);
+		gKernelArgs.cpu_kstack[i].start = fix_address((uint64_t)stack);
+		gKernelArgs.cpu_kstack[i].size = size;
+	}
 	return;
 }
 
 
-void
+static void
-arch_smp_boot_other_cpus(uint32 pml4, uint64 kernelEntry, addr_t virtKernelArgs)
+arm64_secondary_startup()
 {
-	// One cpu for now.
+	asm("ldr x1, [x0, #0]");
+	asm("msr TTBR0_EL1, x1");
+	asm("ldr x1, [x0, #8]");
+	asm("msr TTBR1_EL1, x1");
+	asm("ldr x1, [x0, #16]");
+	asm("msr SCTLR_EL1, x1");
+	asm("ldr x1, [x0, #24]");
+	asm("msr TCR_EL1, x1");
+	asm("ldr x1, [x0, #32]");
+	asm("msr MAIR_EL1, x1");
+	asm("ldr x1, [x0, #40]");
+	asm("mov sp, x1");
+	asm("ldr x1, =0x300000");
+	asm("msr CPACR_EL1, x1");
+	asm("b arm64_secondary_startup2");
+}
+
+
+extern "C" void
+arm64_secondary_startup2(secondary_startup_info *startup)
+{
+	arm64_common_cpu_startup();
+
+	arch_enter_kernel((struct kernel_args *)startup->kernelArgs, startup->kernelEntry,
+		gKernelArgs.cpu_kstack[startup->id].start + gKernelArgs.cpu_kstack[startup->id].size,
+		startup->id);
+}
+
+
+void
+arch_smp_boot_other_cpus(addr_t ttbr1, uint64 kernelEntry, addr_t virtKernelArgs)
+{
+	for (uint32 i = 0; i < sCpuCount; i++) {
+		platform_cpu_info* cpu = &sCpus[i];
+
+		if (cpu->id == 0)
+			continue;
+
+		void* stack = NULL;
+		platform_kernel_address_to_bootloader_address(
+			gKernelArgs.cpu_kstack[cpu->id].start,
+			&stack);
+		secondary_startup_info* info = new secondary_startup_info {
+			.ttbr0 = READ_SPECIALREG(TTBR0_EL1),
+			.ttbr1 = ttbr1,
+			.sctlr = READ_SPECIALREG(SCTLR_EL1)
+				& ~(SCTLR_C | SCTLR_M),
+			.tcr = READ_SPECIALREG(TCR_EL1),
+			.mair = READ_SPECIALREG(MAIR_EL1),
+			.stack = ((uint64)stack) + gKernelArgs.cpu_kstack[cpu->id].size,
+			.kernelEntry = kernelEntry,
+			.kernelArgs = virtKernelArgs,
+			.id = cpu->id
+		};
+
+		asm(
+			"mov x0, %0\n"
+			"mov x1, %1\n"
+			"mov x2, %2\n"
+			"mov x3, %3\n"
+			"hvc #0"
+			::
+			"r" (PSCI_CPU_ON),
+			"r" (cpu->mpidr),
+			"r" ((uint64)&arm64_secondary_startup),
+			"r" ((uint64)info)
+			: "x0", "x1", "x2", "x3"
+		);
+	}
 }
 
 
@@ -75,5 +196,4 @@ arch_smp_add_safemode_menus(Menu *menu)
 void
 arch_smp_init(void)
 {
-	// One cpu for now.
 }

src/system/boot/platform/efi/arch/arm64/arch_start.cpp

@@ -13,6 +13,7 @@
 #include "generic_mmu.h"
 #include "mmu.h"
 #include "serial.h"
+#include "smp.h"
 
 #include "aarch64.h"
 
@@ -24,7 +25,7 @@ extern void arch_mmu_setup_EL1(uint64 tcr);
 
 // From entry.S
 extern "C" void arch_enter_kernel(struct kernel_args* kernelArgs,
-	addr_t kernelEntry, addr_t kernelStackTop);
+	addr_t kernelEntry, addr_t kernelStackTop, uint32 cpu);
 
 // From arch_mmu.cpp
 extern void arch_mmu_post_efi_setup(size_t memoryMapSize,
@@ -43,6 +44,37 @@ arch_convert_kernel_args(void)
 }
 
 
+void
+arm64_common_cpu_startup()
+{
+	// If we have E2H available, we want to also enable TGE
+	// so exceptions don't get taken to EL1
+	uint64 el = arch_exception_level();
+	bool e2h = false;
+	if (el == 2) {
+		uint64 hcr = READ_SPECIALREG(HCR_EL2);
+		if ((hcr & HCR_E2H) != 0) {
+			e2h = true;
+			WRITE_SPECIALREG(HCR_EL2, hcr | HCR_TGE);
+		}
+	}
+
+	// EL2 with E2H enabled behaves as a superset of EL1
+	//
+	// EL2 without E2H enabled does not, so we need to drop to EL1
+	if (el == 1 || e2h) {
+		arch_mmu_setup_EL1(READ_SPECIALREG(TCR_EL1));
+		WRITE_SPECIALREG(CNTKCTL_EL1, 0b11);
+	} else {
+		arch_mmu_setup_EL1(READ_SPECIALREG(TCR_EL2));
+		arch_cache_disable();
+		_arch_transition_EL2_EL1();
+	}
+
+	arch_cache_enable();
+}
+
+
 void
 arch_start_kernel(addr_t kernelEntry)
 {
@@ -128,7 +160,7 @@ arch_start_kernel(addr_t kernelEntry)
 	}
 
 	// Generate page tables for use after ExitBootServices.
-	arch_mmu_generate_post_efi_page_tables(
+	uint64 ttbr1 = arch_mmu_generate_post_efi_page_tables(
 		memoryMapSize, memoryMap, descriptorSize, descriptorVersion);
 
 	// Attempt to fetch the memory map and exit boot services.
@@ -168,38 +200,15 @@ arch_start_kernel(addr_t kernelEntry)
 	serial_init();
 	serial_enable();
 
-	// If we have E2H available, we want to also enable TGE
+	arm64_common_cpu_startup();
-	// so exceptions don't get taken to EL1
-	bool e2h = false;
-	if (el == 2) {
-		uint64 hcr = READ_SPECIALREG(HCR_EL2);
-		if ((hcr & HCR_E2H) != 0) {
-			e2h = true;
-			WRITE_SPECIALREG(HCR_EL2, hcr | HCR_TGE);
-		}
-	}
 
-	// EL2 with E2H enabled behaves as a superset of EL1
+	smp_boot_other_cpus(ttbr1, kernelEntry, (addr_t)&gKernelArgs);
-	//
-	// EL2 without E2H enabled does not, so we need to drop to EL1
-	if (el == 1 || e2h) {
-		arch_mmu_setup_EL1(READ_SPECIALREG(TCR_EL1));
-		WRITE_SPECIALREG(CNTKCTL_EL1, 0b11);
-	} else {
-		arch_mmu_setup_EL1(READ_SPECIALREG(TCR_EL2));
-		arch_cache_disable();
-		_arch_transition_EL2_EL1();
-	}
-
-	arch_cache_enable();
-
-	// smp_boot_other_cpus(final_pml4, kernelEntry, (addr_t)&gKernelArgs);
 
 	if (arch_mmu_read_access(kernelEntry)
 		&& arch_mmu_read_access(gKernelArgs.cpu_kstack[0].start)) {
 		// Enter the kernel!
 		arch_enter_kernel(&gKernelArgs, kernelEntry,
-			gKernelArgs.cpu_kstack[0].start + gKernelArgs.cpu_kstack[0].size);
+			gKernelArgs.cpu_kstack[0].start + gKernelArgs.cpu_kstack[0].size, 0);
 	} else {
 		// _arch_exception_panic("Kernel or Stack memory not accessible\n", __LINE__);
 		panic("Kernel or Stack memory not accessible\n");

src/system/boot/platform/efi/arch/arm64/entry.S

@@ -9,11 +9,12 @@
 	.text
 
 /*	status_t arch_enter_kernel(struct kernel_args *kernelArgs,
-		addr_t kernelEntry, addr_t kernelStackTop);
+		addr_t kernelEntry, addr_t kernelStackTop, uint32 cpu);
 
 	x0	- kernelArgs
 	x1	- kernelEntry
 	x2	- kernelStackTop
+	x3	- cpu
 */
 FUNCTION(arch_enter_kernel):
 
@@ -23,7 +24,7 @@ FUNCTION(arch_enter_kernel):
 	// set up kernel _start args
 	//mov	x0,x0	// kernelArgs
 	mov		x4,x1
-	mov		x1,#0	// currentCPU=0
+	mov		x1,x3	// currentCPU
 
 	// clear frame pointer to ensure the kernel doesn't
 	// try to continue a stacktrace back out here

src/system/boot/platform/efi/arch/riscv64/arch_smp.cpp

@@ -200,7 +200,7 @@ arch_smp_init_other_cpus(void)
 
 
 void
-arch_smp_boot_other_cpus(uint64 satp, uint64 kernel_entry, addr_t virtKernelArgs)
+arch_smp_boot_other_cpus(addr_t satp, uint64 kernel_entry, addr_t virtKernelArgs)
 {
 	dprintf("arch_smp_boot_other_cpus(%p, %p)\n", (void*)satp, (void*)kernel_entry);
 

src/system/boot/platform/efi/arch/x86/arch_smp.cpp

@@ -253,7 +253,7 @@ arch_smp_init_other_cpus(void)
 
 
 void
-arch_smp_boot_other_cpus(uint32 pagedir, uint64 kernelEntry, addr_t virtKernelArgs)
+arch_smp_boot_other_cpus(addr_t pagedir, uint64 kernelEntry, addr_t virtKernelArgs)
 {
 	TRACE("trampolining other cpus\n");
 

src/system/kernel/arch/arm/arch_int_gicv2.cpp

@@ -7,11 +7,16 @@
 #include <interrupt_controller.h>
 #include <kernel.h>
 #include <vm/vm.h>
+#include <smp.h>
 
 #include "arch_int_gicv2.h"
 #include "gicv2_regs.h"
 
 
+#define ICI_IRQ 0
+#define SHARED_IRQ_BASE 32
+
+
 GICv2InterruptController::GICv2InterruptController(uint32_t gicd_addr, uint32_t gicc_addr)
 : InterruptController()
 {
@@ -43,19 +48,46 @@ GICv2InterruptController::GICv2InterruptController(uint32_t gicd_addr, uint32_t
 	fGicdRegs[GICD_REG_ICENABLER] = 0xffffffff;
 	fGicdRegs[GICD_REG_ICENABLER+1] = 0xffffffff;
 
+	// enable GICD
+	fGicdRegs[GICD_REG_CTLR] = 0x03;
+
+	_PerCpuInit();
+	call_all_cpus_sync([](void *arg, int cpu) {
+		GICv2InterruptController* self = (GICv2InterruptController *)arg;
+		self->_PerCpuInit();
+	}, this);
+
+	EnableInterrupt(ICI_IRQ);
+}
+
+
+void GICv2InterruptController::_PerCpuInit()
+{
 	// set PMR and BPR
 	fGiccRegs[GICC_REG_PMR] = 0xff;
 	fGiccRegs[GICC_REG_BPR] = 0x07;
 
 	// enable GICC
-	fGiccRegs[GICC_REG_CTLR] = 0x03;
+	fGiccRegs[GICC_REG_CTLR] = 0x01;
-
-	// enable GICD
-	fGicdRegs[GICD_REG_CTLR] = 0x03;
 }
 
 
 void GICv2InterruptController::EnableInterrupt(int32 irq)
+{
+	_EnableInterrupt(irq);
+	if (irq < SHARED_IRQ_BASE) {
+		call_all_cpus_sync([](void *arg, int cpu) {
+			int32 irq = (int32)(addr_t)arg;
+			GICv2InterruptController *self =
+				(GICv2InterruptController *)InterruptController::Get();
+
+			self->_EnableInterrupt(irq);
+		}, (void*)(addr_t)irq);
+	}
+}
+
+
+void GICv2InterruptController::_EnableInterrupt(int32 irq)
 {
 	uint32_t ena_reg = GICD_REG_ISENABLER + irq / 32;
 	uint32_t ena_val = 1 << (irq % 32);
@@ -69,6 +101,21 @@ void GICv2InterruptController::EnableInterrupt(int32 irq)
 
 
 void GICv2InterruptController::DisableInterrupt(int32 irq)
+{
+	_DisableInterrupt(irq);
+	if (irq < SHARED_IRQ_BASE) {
+		call_all_cpus_sync([](void *arg, int cpu) {
+			int32 irq = (int32)(addr_t)arg;
+			GICv2InterruptController *self =
+				(GICv2InterruptController *)InterruptController::Get();
+
+			self->_DisableInterrupt(irq);
+		}, (void*)(addr_t)irq);
+	}
+}
+
+
+void GICv2InterruptController::_DisableInterrupt(int32 irq)
 {
 	fGicdRegs[GICD_REG_ICENABLER + irq / 32] = 1 << (irq % 32);
 }
@@ -80,9 +127,23 @@ void GICv2InterruptController::HandleInterrupt()
 	uint32_t irqnr = iar & 0x3FF;
 	if ((irqnr == 1022) || (irqnr == 1023)) {
 		dprintf("spurious interrupt\n");
+	} else if (irqnr == ICI_IRQ) {
+		smp_intercpu_interrupt_handler(smp_get_current_cpu());
 	} else {
 		io_interrupt_handler(irqnr, true);
 	}
 
 	fGiccRegs[GICC_REG_EOIR] = iar;
 }
+
+
+void GICv2InterruptController::SendMulticastIci(CPUSet& cpuSet)
+{
+	fGicdRegs[GICD_REG_SGIR] = (cpuSet.Bits(0) << 16);
+}
+
+
+void GICv2InterruptController::SendBroadcastIci()
+{
+	fGicdRegs[GICD_REG_SGIR] = (0b01 << 24);
+}

src/system/kernel/arch/arm/arch_int_gicv2.h

@@ -15,7 +15,13 @@ public:
 	void EnableInterrupt(int32 irq);
 	void DisableInterrupt(int32 irq);
 	void HandleInterrupt();
+	void SendMulticastIci(CPUSet& cpuSet);
+	void SendBroadcastIci();
 private:
+	void _PerCpuInit();
+	void _EnableInterrupt(int32 irq);
+	void _DisableInterrupt(int32 irq);
+
 	volatile uint32_t *fGicdRegs;
 	volatile uint32_t *fGiccRegs;
 };

src/system/kernel/arch/arm/soc.h

@@ -3,6 +3,7 @@
 
 class InterruptController;
 
+#include <arch/smp.h>
 #include <drivers/bus/FDT.h>
 #include <private/kernel/interrupts.h>
 #include <private/kernel/timer.h>
@@ -16,6 +17,14 @@ public:
 
 	virtual void HandleInterrupt() = 0;
 
+	virtual void SendMulticastIci(CPUSet& cpuSet) {
+		panic("SendMulticastIci unimplemented");
+	}
+
+	virtual void SendBroadcastIci() {
+		panic("SendBroadcastIci unimplemented");
+	}
+
 	static InterruptController* Get() {
 		return sInstance;
 	}

src/system/kernel/arch/arm64/arch_cpu.cpp

@@ -26,6 +26,24 @@ arch_cpu_preboot_init_percpu(kernel_args *args, int curr_cpu)
 status_t
 arch_cpu_init_percpu(kernel_args *args, int curr_cpu)
 {
+	uint64_t tcr = READ_SPECIALREG(TCR_EL1);
+	uint64_t mmfr1 = READ_SPECIALREG(ID_AA64MMFR1_EL1);
+
+	uint64_t hafdbs = ID_AA64MMFR1_HAFDBS(mmfr1);
+	if (hafdbs == ID_AA64MMFR1_HAFDBS_AF) {
+		tcr |= (1UL << 39);
+	}
+	if (hafdbs == ID_AA64MMFR1_HAFDBS_AF_DBS) {
+		tcr |= (1UL << 40) | (1UL << 39);
+	}
+
+	tcr |= TCR_SH1_IS | TCR_IRGN1_WBWA | TCR_ORGN1_WBWA;
+	tcr |= TCR_SH0_IS | TCR_IRGN0_WBWA | TCR_ORGN0_WBWA;
+	tcr &= ~TCR_T0SZ(0x1f);
+	tcr |= TCR_T0SZ(16);
+
+	WRITE_SPECIALREG(TCR_EL1, tcr);
+
 	return 0;
 }
 
@@ -33,6 +51,13 @@ arch_cpu_init_percpu(kernel_args *args, int curr_cpu)
 status_t
 arch_cpu_init(kernel_args *args)
 {
+	for (uint32 i = 0; i < args->num_cpus; i++) {
+		cpu_ent* cpu = &gCPU[i];
+
+		cpu->topology_id[CPU_TOPOLOGY_PACKAGE] = 0;
+		cpu->topology_id[CPU_TOPOLOGY_CORE] = i;
+		cpu->topology_id[CPU_TOPOLOGY_SMT] = 0;
+	}
 	return B_OK;
 }
 

src/system/kernel/arch/arm64/arch_smp.cpp

@@ -8,6 +8,9 @@
 #include <arch/smp.h>
 #include <debug.h>
 #include <interrupts.h>
+#include <smp.h>
+
+#include "soc.h"
 
 
 status_t
@@ -27,16 +30,24 @@ arch_smp_per_cpu_init(kernel_args *args, int32 cpu)
 void
 arch_smp_send_multicast_ici(CPUSet& cpuSet)
 {
+	InterruptController *ic = InterruptController::Get();
+	ic->SendMulticastIci(cpuSet);
 }
 
 
 void
 arch_smp_send_ici(int32 target_cpu)
 {
+	InterruptController *ic = InterruptController::Get();
+	CPUSet cpuSet;
+	cpuSet.SetBit(target_cpu);
+	ic->SendMulticastIci(cpuSet);
 }
 
 
 void
 arch_smp_send_broadcast_ici()
 {
+	InterruptController *ic = InterruptController::Get();
+	ic->SendBroadcastIci();
 }

src/system/kernel/arch/arm64/arch_vm_translation_map.cpp

@@ -51,52 +51,28 @@ arch_vm_translation_map_create_map(bool kernel, VMTranslationMap** _map)
 status_t
 arch_vm_translation_map_init(kernel_args* args, VMPhysicalPageMapper** _physicalPageMapper)
 {
-	dprintf("arch_vm_translation_map_init\n");
-
 	// nuke TTBR0 mapping, we use identity mapping in kernel space at KERNEL_PMAP_BASE
 	memset((void*) READ_SPECIALREG(TTBR0_EL1), 0, B_PAGE_SIZE);
 
-	uint64_t tcr = READ_SPECIALREG(TCR_EL1);
-	uint32_t t0sz = tcr & 0x1f;
-	uint32_t t1sz = (tcr >> 16) & 0x1f;
-	uint32_t tg0 = (tcr >> 14) & 0x3;
-	uint32_t tg1 = (tcr >> 30) & 0x3;
-	uint64_t ttbr0 = READ_SPECIALREG(TTBR0_EL1);
-	uint64_t ttbr1 = READ_SPECIALREG(TTBR1_EL1);
 	uint64_t mair = READ_SPECIALREG(MAIR_EL1);
 	uint64_t mmfr1 = READ_SPECIALREG(ID_AA64MMFR1_EL1);
 	uint64_t mmfr2 = READ_SPECIALREG(ID_AA64MMFR2_EL1);
-	uint64_t sctlr = READ_SPECIALREG(SCTLR_EL1);
 
 	ASSERT(VMSAv8TranslationMap::fHwFeature == 0);
 	uint64_t hafdbs = ID_AA64MMFR1_HAFDBS(mmfr1);
 	if (hafdbs == ID_AA64MMFR1_HAFDBS_AF) {
 		VMSAv8TranslationMap::fHwFeature = VMSAv8TranslationMap::HW_ACCESS;
-		tcr |= (1UL << 39);
 	}
 	if (hafdbs == ID_AA64MMFR1_HAFDBS_AF_DBS) {
 		VMSAv8TranslationMap::fHwFeature
 			= VMSAv8TranslationMap::HW_ACCESS | VMSAv8TranslationMap::HW_DIRTY;
-		tcr |= (1UL << 40) | (1UL << 39);
 	}
-
 	if (ID_AA64MMFR2_CNP(mmfr2) == ID_AA64MMFR2_CNP_IMPL) {
 		VMSAv8TranslationMap::fHwFeature |= VMSAv8TranslationMap::HW_COMMON_NOT_PRIVATE;
 	}
 
-	tcr |= TCR_SH1_IS | TCR_IRGN1_WBWA | TCR_ORGN1_WBWA;
-	tcr |= TCR_SH0_IS | TCR_IRGN0_WBWA | TCR_ORGN0_WBWA;
-	tcr &= ~TCR_T0SZ(0x1f);
-	tcr |= TCR_T0SZ(16);
-
 	VMSAv8TranslationMap::fMair = mair;
 
-	WRITE_SPECIALREG(TCR_EL1, tcr);
-
-	dprintf("vm config: MMFR1: %lx, MMFR2: %lx, TCR: %lx\nTTBR0: %lx, TTBR1: %lx\nT0SZ: %u, "
-			"T1SZ: %u, TG0: %u, TG1: %u, MAIR: %lx, SCTLR: %lx\n",
-		mmfr1, mmfr2, tcr, ttbr0, ttbr1, t0sz, t1sz, tg0, tg1, mair, sctlr);
-
 	*_physicalPageMapper = new (&sPhysicalPageMapperData) PMAPPhysicalPageMapper();
 
 	// Create an empty page table for use when we don't want a userspace page table.