riscv64/smp: Implement multi-processor support

* Working under qemu smp 1,2+
* Working on SiFive Unmatched
* x86_64 efi not broken by smp_boot_other_cpus change

Change-Id: I32ebc17913e46ed082be9ade8f56448bbf12f16e
Reviewed-on: https://review.haiku-os.org/c/haiku/+/4705
Tested-by: Commit checker robot <[email protected]>
Reviewed-by: Alex von Gluck IV <[email protected]>

headers/os/drivers/bus/FDT.h

@@ -12,20 +12,20 @@
 struct fdt_bus;
 struct fdt_device;
 
-struct fdt_bus_module_info {
+typedef struct fdt_bus_module_info {
 	driver_module_info info;
-	device_node* (*node_by_phandle)(fdt_bus* bus, int phandle);
-};
+	device_node* (*node_by_phandle)(struct fdt_bus* bus, int phandle);
+} fdt_bus_module_info;
 
-struct fdt_device_module_info{
+typedef struct fdt_device_module_info {
 	driver_module_info info;
-	device_node* (*get_bus)(fdt_device* dev);
-	const char* (*get_name)(fdt_device* dev);
-	const void* (*get_prop)(fdt_device* dev, const char* name, int* len);
-	bool (*get_reg)(fdt_device* dev, uint32 ord, uint64* regs, uint64* len);
-	bool (*get_interrupt)(fdt_device* dev, uint32 ord,
+	device_node* (*get_bus)(struct fdt_device* dev);
+	const char* (*get_name)(struct fdt_device* dev);
+	const void* (*get_prop)(struct fdt_device* dev, const char* name, int* len);
+	bool (*get_reg)(struct fdt_device* dev, uint32 ord, uint64* regs, uint64* len);
+	bool (*get_interrupt)(struct fdt_device* dev, uint32 ord,
 		device_node** interruptController, uint64* interrupt);
-};
+} fdt_device_module_info;
 
 
 #endif // _DRIVERS_BUS_FDT_H

headers/private/kernel/arch/riscv64/arch_cpu.h

@@ -45,7 +45,7 @@ clear_ac()
 
 
 typedef struct arch_cpu_info {
-	int null;
+	uint64 hartId;
 } arch_cpu_info;
 
 

headers/private/kernel/arch/riscv64/arch_debug.h

@@ -10,6 +10,7 @@
 
 
 struct kernel_args;
+struct iframe;
 
 struct arch_debug_registers {
 };
@@ -17,7 +18,7 @@ struct arch_debug_registers {
 
 void WritePC(addr_t pc);
 void DoStackTrace(addr_t fp, addr_t pc);
-void WriteTrapInfo();
+void WriteTrapInfo(iframe* frame);
 
 status_t arch_debug_init_early(kernel_args *args);
 

headers/private/kernel/arch/riscv64/arch_int.h

@@ -15,6 +15,8 @@
 #define NUM_IO_VECTORS	256
 
 
+#ifdef __cplusplus
+
 static inline void
 arch_int_enable_interrupts_inline(void)
 {
@@ -69,5 +71,7 @@ enum {
 
 extern "C" status_t MSyscall(uint64 op, ...);
 
+#endif
+
 
 #endif /* _KERNEL_ARCH_RISCV64_INT_H */

headers/private/kernel/arch/riscv64/arch_kernel_args.h

@@ -38,7 +38,6 @@ typedef struct {
 	// MNative hooks, or SBI
 	uint32 machine_platform;
 
-	uint bootHart;
 	uint64 timerFrequency; // in Hz
 
 	// All following address are virtual
@@ -50,6 +49,9 @@ typedef struct {
 	addr_range	clint;
 
 	uart_info	uart;
+
+	uint32 hartIds[SMP_MAX_CPUS];
+	uint32 plicContexts[SMP_MAX_CPUS];
 } _PACKED arch_kernel_args;
 
 #endif	/* KERNEL_ARCH_RISCV64_KERNEL_ARGS_H */

headers/private/kernel/arch/riscv64/arch_thread_types.h

@@ -14,6 +14,11 @@ namespace BKernel {
 
 
 struct iframe {
+	uint64 status;
+	uint64 cause;
+	uint64 tval;
+	uint64 align1; // structure need to be 16 byte aligned
+
 	uint64 ra;
 	uint64 t6;
 	uint64 sp;
@@ -48,6 +53,7 @@ struct iframe {
 	uint64 epc;
 };
 
+
 struct arch_context {
 	uint64 ra;    //  0
 	uint64 s[12]; // 12
@@ -60,12 +66,15 @@ struct fpu_context {
 	uint64 fcsr;
 };
 
+struct __attribute__((aligned(16))) arch_stack {
+	BKernel::Thread* thread;
+};
 
 struct arch_thread {
-	BKernel::Thread* thread;
 	arch_context context;
 	fpu_context fpuContext;
 	iframe* userFrame;
+	uint64 oldA0;
 };
 
 struct arch_team {
@@ -84,13 +93,12 @@ struct arch_fork_arg {
 extern "C" {
 #endif
 
-int arch_setjmp(arch_context* ctx);
-void arch_longjmp(arch_context* ctx, int val);
+void arch_context_switch(arch_context* from, arch_context* to);
 void save_fpu(fpu_context* ctx);
 void restore_fpu(fpu_context* ctx);
 void arch_thread_entry();
-void arch_enter_userspace(void *arg1, void *arg2, addr_t sp);
-void arch_longjmp_iframe(iframe* frame);
+void arch_load_user_iframe(arch_stack* stackHeader, iframe* frame)
+	__attribute__ ((noreturn));
 
 #ifdef __cplusplus
 }

headers/private/kernel/arch/riscv64/arch_traps.h

@@ -9,79 +9,79 @@
 
 
 # NOTE: this macro don't save SP, it should be saved manually
-.macro PushTrapFrame
-	addi sp, sp, -256
+.macro PushTrapFrame extSize
+	addi sp, sp, -(\extSize + 256)
 
-	sd ra,   0*8(sp)
-	sd t6,   1*8(sp)
-#	sd sp,   2*8(sp) # sp
-	sd gp,   3*8(sp)
-	sd tp,   4*8(sp)
-	sd t0,   5*8(sp)
-	sd t1,   6*8(sp)
-	sd t2,   7*8(sp)
-	sd t5,   8*8(sp)
-	sd s1,   9*8(sp)
-	sd a0,  10*8(sp)
-	sd a1,  11*8(sp)
-	sd a2,  12*8(sp)
-	sd a3,  13*8(sp)
-	sd a4,  14*8(sp)
-	sd a5,  15*8(sp)
-	sd a6,  16*8(sp)
-	sd a7,  17*8(sp)
-	sd s2,  18*8(sp)
-	sd s3,  19*8(sp)
-	sd s4,  20*8(sp)
-	sd s5,  21*8(sp)
-	sd s6,  22*8(sp)
-	sd s7,  23*8(sp)
-	sd s8,  24*8(sp)
-	sd s9,  25*8(sp)
-	sd s10, 26*8(sp)
-	sd s11, 27*8(sp)
-	sd t3,  28*8(sp)
-	sd t4,  29*8(sp)
-	sd fp,  30*8(sp)
+	sd ra,  \extSize +  0*8(sp)
+	sd t6,  \extSize +  1*8(sp)
+#	sd sp,  \extSize +  2*8(sp) # sp
+	sd gp,  \extSize +  3*8(sp)
+	sd tp,  \extSize +  4*8(sp)
+	sd t0,  \extSize +  5*8(sp)
+	sd t1,  \extSize +  6*8(sp)
+	sd t2,  \extSize +  7*8(sp)
+	sd t5,  \extSize +  8*8(sp)
+	sd s1,  \extSize +  9*8(sp)
+	sd a0,  \extSize + 10*8(sp)
+	sd a1,  \extSize + 11*8(sp)
+	sd a2,  \extSize + 12*8(sp)
+	sd a3,  \extSize + 13*8(sp)
+	sd a4,  \extSize + 14*8(sp)
+	sd a5,  \extSize + 15*8(sp)
+	sd a6,  \extSize + 16*8(sp)
+	sd a7,  \extSize + 17*8(sp)
+	sd s2,  \extSize + 18*8(sp)
+	sd s3,  \extSize + 19*8(sp)
+	sd s4,  \extSize + 20*8(sp)
+	sd s5,  \extSize + 21*8(sp)
+	sd s6,  \extSize + 22*8(sp)
+	sd s7,  \extSize + 23*8(sp)
+	sd s8,  \extSize + 24*8(sp)
+	sd s9,  \extSize + 25*8(sp)
+	sd s10, \extSize + 26*8(sp)
+	sd s11, \extSize + 27*8(sp)
+	sd t3,  \extSize + 28*8(sp)
+	sd t4,  \extSize + 29*8(sp)
+	sd fp,  \extSize + 30*8(sp)
 
-	addi fp, sp, 256
+	addi fp, sp, \extSize + 256
 .endm
 
 
-.macro PopTrapFrame
-	ld ra,   0*8(sp)
-	ld t6,   1*8(sp)
-#	ld sp,   2*8(sp) restore later
-	ld gp,   3*8(sp)
-#	ld tp,   4*8(sp)
-	ld t0,   5*8(sp)
-	ld t1,   6*8(sp)
-	ld t2,   7*8(sp)
-	ld t5,   8*8(sp)
-	ld s1,   9*8(sp)
-	ld a0,  10*8(sp)
-	ld a1,  11*8(sp)
-	ld a2,  12*8(sp)
-	ld a3,  13*8(sp)
-	ld a4,  14*8(sp)
-	ld a5,  15*8(sp)
-	ld a6,  16*8(sp)
-	ld a7,  17*8(sp)
-	ld s2,  18*8(sp)
-	ld s3,  19*8(sp)
-	ld s4,  20*8(sp)
-	ld s5,  21*8(sp)
-	ld s6,  22*8(sp)
-	ld s7,  23*8(sp)
-	ld s8,  24*8(sp)
-	ld s9,  25*8(sp)
-	ld s10, 26*8(sp)
-	ld s11, 27*8(sp)
-	ld t3,  28*8(sp)
-	ld t4,  29*8(sp)
-	ld fp,  30*8(sp)
+.macro PopTrapFrame extSize
+	ld ra,  \extSize +  0*8(sp)
+	ld t6,  \extSize +  1*8(sp)
+#	ld sp,  \extSize +  2*8(sp) restore later
+	ld gp,  \extSize +  3*8(sp)
+#	ld tp,  \extSize +  4*8(sp)
+	ld t0,  \extSize +  5*8(sp)
+	ld t1,  \extSize +  6*8(sp)
+	ld t2,  \extSize +  7*8(sp)
+	ld t5,  \extSize +  8*8(sp)
+	ld s1,  \extSize +  9*8(sp)
+	ld a0,  \extSize + 10*8(sp)
+	ld a1,  \extSize + 11*8(sp)
+	ld a2,  \extSize + 12*8(sp)
+	ld a3,  \extSize + 13*8(sp)
+	ld a4,  \extSize + 14*8(sp)
+	ld a5,  \extSize + 15*8(sp)
+	ld a6,  \extSize + 16*8(sp)
+	ld a7,  \extSize + 17*8(sp)
+	ld s2,  \extSize + 18*8(sp)
+	ld s3,  \extSize + 19*8(sp)
+	ld s4,  \extSize + 20*8(sp)
+	ld s5,  \extSize + 21*8(sp)
+	ld s6,  \extSize + 22*8(sp)
+	ld s7,  \extSize + 23*8(sp)
+	ld s8,  \extSize + 24*8(sp)
+	ld s9,  \extSize + 25*8(sp)
+	ld s10, \extSize + 26*8(sp)
+	ld s11, \extSize + 27*8(sp)
+	ld t3,  \extSize + 28*8(sp)
+	ld t4,  \extSize + 29*8(sp)
+	ld fp,  \extSize + 30*8(sp)
 
-	ld sp,   2*8(sp)
+	ld sp,  \extSize +  2*8(sp)
 .endm
 
 

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

@@ -1,5 +1,5 @@
 /*
- * Copyright 2013-2019 Haiku, Inc. All rights reserved.
+ * Copyright 2013-2021 Haiku, Inc. All rights reserved.
  * Distributed under the terms of the MIT License.
  */
 #ifndef KERNEL_BOOT_PLATFORM_EFI_ARCH_SMP_H
@@ -13,9 +13,17 @@
 // These platforms take inventory of cpu cores from fdt
 
 struct platform_cpu_info {
-	uint32 id;
+	uint32 id; // hart id on riscv
+#if defined(__riscv)
+	uint32 phandle;
+	uint32 plicContext;
+#endif
 };
 
+#if defined(__riscv)
+extern uint32 gBootHart;
+#endif
+
 void arch_smp_register_cpu(platform_cpu_info** cpu);
 #endif
 
@@ -23,6 +31,7 @@ void arch_smp_register_cpu(platform_cpu_info** cpu);
 int arch_smp_get_current_cpu(void);
 void arch_smp_init_other_cpus(void);
 #ifdef __riscv
+platform_cpu_info* arch_smp_find_cpu(uint32 phandle);
 void arch_smp_boot_other_cpus(uint64 satp, uint64 kernel_entry);
 #else
 void arch_smp_boot_other_cpus(uint32 pml4, uint64 kernel_entry);

headers/private/kernel/vm/VMCache.h

@@ -376,7 +376,7 @@ vm_page::IncrementWiredCount()
 inline void
 vm_page::DecrementWiredCount()
 {
-	ASSERT(fWiredCount > 0);
+	ASSERT_PRINT(fWiredCount > 0, "page: %#" B_PRIx64, physical_page_number * B_PAGE_SIZE);
 
 	if (--fWiredCount == 0)
 		cache_ref->cache->DecrementWiredPagesCount();

headers/private/kernel/vm/vm_types.h

@@ -230,10 +230,9 @@ vm_page_debug_access_start(vm_page* page)
 	thread_id previousThread = atomic_test_and_set(&page->accessing_thread,
 		threadID, -1);
 	if (previousThread != -1) {
-		panic("Invalid concurrent access to page %p (start), currently "
-			"accessed by: %" B_PRId32
-			"@! page -m %p; sc %" B_PRId32 "; cache _cache", page,
-			previousThread, page, previousThread);
+		panic("Invalid concurrent access to page 0x%" B_PRIXPHYSADDR " (start), currently "
+			"accessed by: %" B_PRId32 "@! page -m %p; sc %" B_PRId32 "; cache _cache",
+			page->physical_page_number * B_PAGE_SIZE, previousThread, page, previousThread);
 	}
 }
 
@@ -245,9 +244,9 @@ vm_page_debug_access_end(vm_page* page)
 	thread_id previousThread = atomic_test_and_set(&page->accessing_thread, -1,
 		threadID);
 	if (previousThread != threadID) {
-		panic("Invalid concurrent access to page %p (end) by current thread, "
-			"current accessor is: %" B_PRId32
-			"@! page -m %p; sc %" B_PRId32 "; cache _cache", page,
+		panic("Invalid concurrent access to page 0x%" B_PRIXPHYSADDR " (end) by "
+			"current thread, current accessor is: %" B_PRId32 "@! page -m %p; "
+			"sc %" B_PRId32 "; cache _cache", page->physical_page_number * B_PAGE_SIZE,
 			previousThread, page, previousThread);
 	}
 }
@@ -258,10 +257,9 @@ vm_page_debug_access_check(vm_page* page)
 {
 	thread_id thread = page->accessing_thread;
 	if (thread != thread_get_current_thread_id()) {
-		panic("Invalid concurrent access to page %p (check), currently "
-			"accessed by: %" B_PRId32
-			"@! page -m %p; sc %" B_PRId32 "; cache _cache", page, thread, page,
-			thread);
+		panic("Invalid concurrent access to page 0x%" B_PRIXPHYSADDR " (check), currently "
+			"accessed by: %" B_PRId32 "@! page -m %p; sc %" B_PRId32 "; cache _cache",
+			page->physical_page_number * B_PAGE_SIZE, thread, page, thread);
 	}
 }
 

headers/private/system/arch/riscv64/arch_cpu_defs.h

@@ -11,6 +11,8 @@
 #define B_ALWAYS_INLINE __attribute__((always_inline)) inline
 
 
+#ifdef __cplusplus
+
 enum {
 	modeU = 0,
 	modeS = 1,
@@ -327,6 +329,10 @@ static B_ALWAYS_INLINE void FlushTlbAllAsid(uint64 asid) {
 static B_ALWAYS_INLINE void FlushTlbPageAsid(uint64 page, uint64 asid) {
 	asm volatile("sfence.vma %0, %0" : : "r" (page), "r" (asid) : "memory");}
 
+// flush instruction cache
+static B_ALWAYS_INLINE void FenceI() {
+	asm volatile("fence.i" : : : "memory");}
+
 static B_ALWAYS_INLINE uint64 Sp() {
 	uint64 x; asm volatile("mv %0, sp" : "=r" (x)); return x;}
 static B_ALWAYS_INLINE void SetSp(uint64 x) {
@@ -352,6 +358,8 @@ static B_ALWAYS_INLINE void Wfi() {asm volatile("wfi");}
 static B_ALWAYS_INLINE void Mret() {asm volatile("mret");}
 static B_ALWAYS_INLINE void Sret() {asm volatile("sret");}
 
+#endif // __cplusplus
+
 
 #define SPINLOCK_PAUSE()	do {} while (false)
 

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

@@ -63,7 +63,8 @@ WritePteFlags(uint32 flags)
 
 
 static void
-DumpPageWrite(uint64_t virtAdr, uint64_t physAdr, size_t size, uint64 flags, uint64& firstVirt, uint64& firstPhys, uint64& firstFlags, uint64& len)
+DumpPageWrite(uint64_t virtAdr, uint64_t physAdr, size_t size, uint64 flags, uint64& firstVirt,
+	uint64& firstPhys, uint64& firstFlags, uint64& len)
 {
 	if (virtAdr == firstVirt + len && physAdr == firstPhys + len && flags == firstFlags) {
 		len += size;
@@ -71,7 +72,8 @@ DumpPageWrite(uint64_t virtAdr, uint64_t physAdr, size_t size, uint64 flags, uin
 		if (len != 0) {
 			dprintf("  0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR,
 				firstVirt, firstVirt + (len - 1));
-			dprintf(": 0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR ", %#" B_PRIxADDR ", ", firstPhys, firstPhys + (len - 1), len);
+			dprintf(": 0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR ", %#" B_PRIxADDR ", ",
+				firstPhys, firstPhys + (len - 1), len);
 			WritePteFlags(firstFlags); dprintf("\n");
 		}
 		firstVirt = virtAdr;
@@ -83,7 +85,8 @@ DumpPageWrite(uint64_t virtAdr, uint64_t physAdr, size_t size, uint64 flags, uin
 
 
 static void
-DumpPageTableInt(Pte* pte, uint64_t virtAdr, uint32_t level, uint64& firstVirt, uint64& firstPhys, uint64& firstFlags, uint64& len)
+DumpPageTableInt(Pte* pte, uint64_t virtAdr, uint32_t level, uint64& firstVirt, uint64& firstPhys,
+	uint64& firstFlags, uint64& len)
 {
 	for (uint32 i = 0; i < pteCount; i++) {
 		if (((1 << pteValid) & pte[i].flags) != 0) {

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

@@ -6,6 +6,7 @@
 
 #include "arch_smp.h"
 
+#include <algorithm>
 #include <string.h>
 
 #include <KernelExport.h>
@@ -28,13 +29,14 @@
 #endif
 
 
-extern "C" void arch_enter_kernel(uint64 satp, struct kernel_args *kernelArgs,
-        addr_t kernelEntry, addr_t kernelStackTop);
+typedef status_t (*KernelEntry) (kernel_args *bootKernelArgs, int currentCPU);
 
 
 struct CpuEntryInfo {
-	uint64 satp;
-	uint64 kernelEntry;
+	uint64 satp;		//  0
+	uint64 stackBase;	//  8
+	uint64 stackSize;	// 16
+	KernelEntry kernelEntry;// 24
 };
 
 
@@ -43,11 +45,85 @@ uint32 sCpuCount = 0;
 
 
 static void
-CpuEntry(int hartId, CpuEntryInfo* info)
+arch_cpu_dump_hart_status(uint64 status)
 {
-	arch_enter_kernel(info->satp, &gKernelArgs, info->kernelEntry,
-		gKernelArgs.cpu_kstack[hartId].start
-		+ gKernelArgs.cpu_kstack[hartId].size);
+	switch (status) {
+		case SBI_HART_STATE_STARTED:
+			dprintf("started");
+			break;
+		case SBI_HART_STATE_STOPPED:
+			dprintf("stopped");
+			break;
+		case SBI_HART_STATE_START_PENDING:
+			dprintf("startPending");
+			break;
+		case SBI_HART_STATE_STOP_PENDING:
+			dprintf("stopPending");
+			break;
+		case SBI_HART_STATE_SUSPENDED:
+			dprintf("suspended");
+			break;
+		case SBI_HART_STATE_SUSPEND_PENDING:
+			dprintf("suspendPending");
+			break;
+		case SBI_HART_STATE_RESUME_PENDING:
+			dprintf("resumePending");
+			break;
+		default:
+			dprintf("?(%" B_PRIu64 ")", status);
+	}
+}
+
+
+static void
+arch_cpu_dump_hart()
+{
+	dprintf("  hart status:\n");
+	for (uint32 i = 0; i < sCpuCount; i++) {
+		dprintf("    hart %" B_PRIu32 ": ", i);
+		sbiret res = sbi_hart_get_status(sCpus[i].id);
+		if (res.error < 0)
+			dprintf("error: %" B_PRIu64 , res.error);
+		else {
+			arch_cpu_dump_hart_status(res.value);
+		}
+		dprintf("\n");
+	}
+}
+
+
+static void __attribute__((naked))
+arch_cpu_entry(int hartId, CpuEntryInfo* info)
+{
+	// enable MMU
+	asm("ld t0, 0(a1)");   // CpuEntryInfo::satp
+	asm("csrw satp, t0");
+	asm("sfence.vma");
+
+	// setup stack
+	asm("ld sp, 8(a1)");   // CpuEntryInfo::stackBase
+	asm("ld t0, 16(a1)");  // CpuEntryInfo::stackSize
+	asm("add sp, sp, t0");
+	asm("li fp, 0");
+
+	asm("tail arch_cpu_entry2");
+}
+
+
+extern "C" void
+arch_cpu_entry2(int hartId, CpuEntryInfo* info)
+{
+	dprintf("%s(%d)\n", __func__, hartId);
+
+	uint32 cpu = 0;
+	while (cpu < sCpuCount && !(sCpus[cpu].id == (uint32)hartId))
+		cpu++;
+
+	if (!(cpu < sCpuCount))
+		panic("CPU for hart id %d not found\n", hartId);
+
+	info->kernelEntry(&gKernelArgs, cpu);
+	for (;;) {}
 }
 
 
@@ -65,6 +141,17 @@ arch_smp_register_cpu(platform_cpu_info** cpu)
 }
 
 
+platform_cpu_info*
+arch_smp_find_cpu(uint32 phandle)
+{
+	for (uint32 i = 0; i < sCpuCount; i++) {
+		if (sCpus[i].phandle == phandle)
+			return &sCpus[i];
+	}
+	return NULL;
+}
+
+
 int
 arch_smp_get_current_cpu(void)
 {
@@ -75,9 +162,18 @@ arch_smp_get_current_cpu(void)
 void
 arch_smp_init_other_cpus(void)
 {
-	// TODO: SMP code disabled for now
-	gKernelArgs.num_cpus = 1;
-	return;
+	gKernelArgs.num_cpus = sCpuCount;
+
+	// make boot CPU first as expected by kernel
+	for (uint32 i = 1; i < sCpuCount; i++) {
+		if (sCpus[i].id == gBootHart)
+			std::swap(sCpus[i], sCpus[0]);
+	}
+
+	for (uint32 i = 0; i < sCpuCount; i++) {
+		gKernelArgs.arch_args.hartIds[i] = sCpus[i].id;
+		gKernelArgs.arch_args.plicContexts[i] = sCpus[i].plicContext;
+	}
 
 	if (get_safemode_boolean(B_SAFEMODE_DISABLE_SMP, false)) {
 		// SMP has been disabled!
@@ -85,8 +181,6 @@ arch_smp_init_other_cpus(void)
 		gKernelArgs.num_cpus = 1;
 	}
 
-	gKernelArgs.num_cpus = sCpuCount;
-
 	if (gKernelArgs.num_cpus < 2)
 		return;
 
@@ -108,35 +202,34 @@ arch_smp_init_other_cpus(void)
 void
 arch_smp_boot_other_cpus(uint64 satp, uint64 kernel_entry)
 {
-	// TODO: SMP code disabled for now
-	return;
+	dprintf("arch_smp_boot_other_cpus(%p, %p)\n", (void*)satp, (void*)kernel_entry);
 
-	dprintf("arch_smp_boot_other_cpus()\n");
+	arch_cpu_dump_hart();
 	for (uint32 i = 0; i < sCpuCount; i++) {
-		// TODO: mhartid 0 may not exist, or it may not be a core
-		// you're interested in (FU540/FU740 hart 0 is mgmt core.)
-		if (0 != sCpus[i].id) {
+		if (sCpus[i].id != gBootHart) {
 			sbiret res;
-			dprintf("starting CPU %" B_PRIu32 "\n", sCpus[i].id);
+			dprintf("  starting CPU %" B_PRIu32 "\n", sCpus[i].id);
 
-			res = sbi_hart_get_status(sCpus[i].id);
-			dprintf("[PRE] sbi_hart_get_status() -> (%ld, %ld)\n",
-				res.error, res.value);
+			dprintf("  stack: %#" B_PRIx64 " - %#" B_PRIx64 "\n",
+				gKernelArgs.cpu_kstack[i].start, gKernelArgs.cpu_kstack[i].start
+				+ gKernelArgs.cpu_kstack[i].size - 1);
 
-			CpuEntryInfo info = {.satp = satp, .kernelEntry = kernel_entry};
-			res = sbi_hart_start(sCpus[i].id, (addr_t)&CpuEntry, (addr_t)&info);
-			dprintf("sbi_hart_start() -> (%ld, %ld)\n", res.error, res.value);
+			CpuEntryInfo* info = new(std::nothrow) CpuEntryInfo{
+				.satp = satp,
+				.stackBase = gKernelArgs.cpu_kstack[i].start,
+				.stackSize = gKernelArgs.cpu_kstack[i].size,
+				.kernelEntry = (KernelEntry)kernel_entry
+			};
+			res = sbi_hart_start(sCpus[i].id, (addr_t)&arch_cpu_entry, (addr_t)info);
 
 			for (;;) {
 				res = sbi_hart_get_status(sCpus[i].id);
 				if (res.error < 0 || res.value == SBI_HART_STATE_STARTED)
 					break;
 			}
-
-			dprintf("[POST] sbi_hart_get_status() -> (%ld, %ld)\n",
-				res.error, res.value);
 		}
 	}
+	arch_cpu_dump_hart();
 }
 
 

src/system/boot/platform/efi/dtb.cpp

@@ -9,6 +9,7 @@
 
 // TODO: split arch-depending code to per-arch source
 
+#include <arch_cpu_defs.h>
 #include <arch_smp.h>
 #include <arch/generic/debug_uart_8250.h>
 #if defined(__riscv)
@@ -40,7 +41,8 @@ extern "C" {
 static void* sDtbTable = NULL;
 static uint32 sDtbSize = 0;
 
-static uint32 sBootHart = 0;
+// TODO: gBootHart is riscy, move
+uint32 gBootHart = 0;
 static uint64 sTimerFrequency = 10000000;
 
 static addr_range sPlic = {0};
@@ -422,7 +424,7 @@ HandleFdt(const void* fdt, int node, uint32 addressCells, uint32 sizeCells)
 	const char* name = fdt_get_name(fdt, node, NULL);
 	if (strcmp(name, "chosen") == 0) {
 		if (uint32* prop = (uint32*)fdt_getprop(fdt, node, "boot-hartid", NULL))
-			sBootHart = fdt32_to_cpu(*prop);
+			gBootHart = fdt32_to_cpu(*prop);
 	} else if (strcmp(name, "cpus") == 0) {
 		if (uint32* prop = (uint32*)fdt_getprop(fdt, node, "timebase-frequency", NULL))
 			sTimerFrequency = fdt32_to_cpu(*prop);
@@ -433,6 +435,9 @@ HandleFdt(const void* fdt, int node, uint32 addressCells, uint32 sizeCells)
 
 	if (deviceType != NULL) {
 		if (strcmp(deviceType, "cpu") == 0) {
+			// TODO: improve incompatible CPU detection
+			if (!(fdt_getprop(fdt, node, "mmu-type", NULL) != NULL))
+				return;
 			platform_cpu_info* info;
 			arch_smp_register_cpu(&info);
 			if (info == NULL)
@@ -441,6 +446,14 @@ HandleFdt(const void* fdt, int node, uint32 addressCells, uint32 sizeCells)
 				"reg", NULL));
 			dprintf("cpu\n");
 			dprintf("  id: %" B_PRIu32 "\n", info->id);
+
+			int subNode = fdt_subnode_offset(fdt, node, "interrupt-controller");
+			if (subNode < 0) {
+				dprintf("  [!] no interrupt controller\n");
+			} else {
+				info->phandle = fdt_get_phandle(fdt, subNode);
+				dprintf("  phandle: %" B_PRIu32 "\n", info->phandle);
+			}
 		}
 	}
 
@@ -459,6 +472,24 @@ HandleFdt(const void* fdt, int node, uint32 addressCells, uint32 sizeCells)
 	if (HasFdtString(compatible, compatibleLen, "riscv,plic0")
 		|| HasFdtString(compatible, compatibleLen, "sifive,plic-1.0.0")) {
 		GetReg(fdt, node, addressCells, sizeCells, 0, sPlic);
+		int propSize;
+		if (uint32* prop = (uint32*)fdt_getprop(fdt, node, "interrupts-extended", &propSize)) {
+			dprintf("PLIC contexts\n");
+			uint32 contextId = 0;
+			for (uint32 *it = prop; (uint8_t*)it - (uint8_t*)prop < propSize; it += 2) {
+				uint32 phandle = fdt32_to_cpu(*it);
+				uint32 interrupt = fdt32_to_cpu(*(it + 1));
+				if (interrupt == sExternInt) {
+					platform_cpu_info* cpuInfo = arch_smp_find_cpu(phandle);
+					dprintf("  context %" B_PRIu32 ": %" B_PRIu32 "\n", contextId, phandle);
+					if (cpuInfo != NULL) {
+						cpuInfo->plicContext = contextId;
+						dprintf("    cpu id: %" B_PRIu32 "\n", cpuInfo->id);
+					}
+				}
+				contextId++;
+			}
+		}
 		return;
 	}
 
@@ -556,7 +587,6 @@ dtb_set_kernel_args()
 	// pack into proper location if the architecture cares
 	if (sDtbTable != NULL) {
 		#if defined(__ARM__) || defined(__riscv)
-
 		// libfdt requires 8-byte alignment
 		gKernelArgs.arch_args.fdt = (void*)(addr_t)kernel_args_malloc(sDtbSize, 8);
 
@@ -568,8 +598,7 @@ dtb_set_kernel_args()
 	}
 
 #ifdef __riscv
-	dprintf("bootHart: %" B_PRIu32 "\n", sBootHart);
-	gKernelArgs.arch_args.bootHart = sBootHart;
+	dprintf("bootHart: %" B_PRIu32 "\n", gBootHart);
 	dprintf("timerFrequency: %" B_PRIu64 "\n", sTimerFrequency);
 	gKernelArgs.arch_args.timerFrequency = sTimerFrequency;
 

src/system/boot/platform/efi/smp.cpp

@@ -48,12 +48,12 @@ smp_init_other_cpus(void)
 
 
 void
-smp_boot_other_cpus(uint32 pml4, uint64 kernel_entry)
+smp_boot_other_cpus(addr_t pageTable, addr_t kernelEntry)
 {
 	if (gKernelArgs.num_cpus < 2)
 		return;
 
-	arch_smp_boot_other_cpus(pml4, kernel_entry);
+	arch_smp_boot_other_cpus(pageTable, kernelEntry);
 }
 
 

src/system/boot/platform/efi/smp.h

@@ -19,7 +19,7 @@ extern "C" {
 
 extern void smp_init(void);
 extern void smp_init_other_cpus(void);
-extern void smp_boot_other_cpus(uint32 pml4, uint64 kernel_entry);
+extern void smp_boot_other_cpus(addr_t pageTable, addr_t kernelEntry);
 extern int smp_get_current_cpu(void);
 
 #ifdef __cplusplus

src/system/kernel/arch/riscv64/Jamfile

@@ -35,3 +35,5 @@ KernelMergeObject kernel_arch_riscv64.o :
 	$(TARGET_KERNEL_PIC_CCFLAGS) -Wno-unused
 	:
 ;
+
+CreateAsmStructOffsetsHeader asm_offsets.h : asm_offsets.cpp : $(TARGET_KERNEL_ARCH) ;

src/system/kernel/arch/riscv64/RISCV64VMTranslationMap.cpp

@@ -15,13 +15,12 @@
 #include <vm/VMAddressSpace.h>
 #include <vm/VMCache.h>
 #include <slab/Slab.h>
+#include <platform/sbi/sbi_syscalls.h>
 
 #include <util/AutoLock.h>
 #include <util/ThreadAutoLock.h>
 
 
-//#define DISABLE_MODIFIED_FLAGS 1
-
 //#define DO_TRACE
 #ifdef DO_TRACE
 #	define TRACE(x...) dprintf(x)
@@ -32,6 +31,84 @@
 #define NOT_IMPLEMENTED_PANIC() \
 	panic("not implemented: %s\n", __PRETTY_FUNCTION__)
 
+extern uint32 gPlatform;
+
+
+static void
+WriteVmPage(vm_page* page)
+{
+	dprintf("0x%08" B_PRIxADDR " ",
+		(addr_t)(page->physical_page_number * B_PAGE_SIZE));
+	switch (page->State()) {
+		case PAGE_STATE_ACTIVE:
+			dprintf("A");
+			break;
+		case PAGE_STATE_INACTIVE:
+			dprintf("I");
+			break;
+		case PAGE_STATE_MODIFIED:
+			dprintf("M");
+			break;
+		case PAGE_STATE_CACHED:
+			dprintf("C");
+			break;
+		case PAGE_STATE_FREE:
+			dprintf("F");
+			break;
+		case PAGE_STATE_CLEAR:
+			dprintf("L");
+			break;
+		case PAGE_STATE_WIRED:
+			dprintf("W");
+			break;
+		case PAGE_STATE_UNUSED:
+			dprintf("-");
+			break;
+	}
+	dprintf(" ");
+	if (page->busy)
+		dprintf("B");
+	else
+		dprintf("-");
+
+	if (page->busy_writing)
+		dprintf("W");
+	else
+		dprintf("-");
+
+	if (page->accessed)
+		dprintf("A");
+	else
+		dprintf("-");
+
+	if (page->modified)
+		dprintf("M");
+	else
+		dprintf("-");
+
+	if (page->unused)
+		dprintf("U");
+	else
+		dprintf("-");
+
+	dprintf(" usage:%3u", page->usage_count);
+	dprintf(" wired:%5u", page->WiredCount());
+
+	bool first = true;
+	vm_page_mappings::Iterator iterator = page->mappings.GetIterator();
+	vm_page_mapping* mapping;
+	while ((mapping = iterator.Next()) != NULL) {
+		if (first) {
+			dprintf(": ");
+			first = false;
+		} else
+			dprintf(", ");
+
+		dprintf("%" B_PRId32 " (%s)", mapping->area->id, mapping->area->name);
+		mapping = mapping->page_link.next;
+	}
+}
+
 
 static void
 FreePageTable(page_num_t ppn, bool isKernel, uint32 level = 2)
@@ -50,6 +127,7 @@ FreePageTable(page_num_t ppn, bool isKernel, uint32 level = 2)
 		}
 	}
 	vm_page* page = vm_lookup_page(ppn);
+	DEBUG_PAGE_ACCESS_START(page);
 	vm_page_set_state(page, PAGE_STATE_FREE);
 }
 
@@ -94,6 +172,7 @@ RISCV64VMTranslationMap::LookupPte(addr_t virtAdr, bool alloc,
 		fPageTable = page->physical_page_number * B_PAGE_SIZE;
 		if (fPageTable == 0)
 			return NULL;
+		DEBUG_PAGE_ACCESS_END(page);
 		fPageTableSize++;
 		if (!fIsKernel) {
 			// Map kernel address space into user address space. Preallocated
@@ -121,6 +200,7 @@ RISCV64VMTranslationMap::LookupPte(addr_t virtAdr, bool alloc,
 			pte->ppn = page->physical_page_number;
 			if (pte->ppn == 0)
 				return NULL;
+			DEBUG_PAGE_ACCESS_END(page);
 			fPageTableSize++;
 			pte->flags |= (1 << pteValid);
 		}
@@ -147,7 +227,9 @@ RISCV64VMTranslationMap::RISCV64VMTranslationMap(bool kernel,
 	phys_addr_t pageTable):
 	fIsKernel(kernel),
 	fPageTable(pageTable),
-	fPageTableSize(GetPageTableSize(pageTable / B_PAGE_SIZE, kernel))
+	fPageTableSize(GetPageTableSize(pageTable / B_PAGE_SIZE, kernel)),
+	fInvalidPagesCount(0),
+	fInvalidCode(false)
 {
 	TRACE("+RISCV64VMTranslationMap(%p, %d, 0x%" B_PRIxADDR ")\n", this,
 		kernel, pageTable);
@@ -235,32 +317,33 @@ RISCV64VMTranslationMap::Map(addr_t virtualAddress, phys_addr_t physicalAddress,
 	if (pte == NULL)
 		panic("can't allocate page table");
 
-	pte->ppn = physicalAddress / B_PAGE_SIZE;
-	pte->flags = 0;
+	Pte newPte;
+	newPte.ppn = physicalAddress / B_PAGE_SIZE;
+	newPte.flags = (1 << pteValid);
+
 	if ((attributes & B_USER_PROTECTION) != 0) {
-		pte->flags |= (1 << pteUser);
+		newPte.flags |= (1 << pteUser);
 		if ((attributes & B_READ_AREA) != 0)
-			pte->flags |= (1 << pteRead);
+			newPte.flags |= (1 << pteRead);
 		if ((attributes & B_WRITE_AREA) != 0)
-			pte->flags |= (1 << pteWrite);
+			newPte.flags |= (1 << pteWrite);
 		if ((attributes & B_EXECUTE_AREA) != 0)
-			pte->flags |= (1 << pteExec);
+			newPte.flags |= (1 << pteExec);
 	} else {
 		if ((attributes & B_KERNEL_READ_AREA) != 0)
-			pte->flags |= (1 << pteRead);
+			newPte.flags |= (1 << pteRead);
 		if ((attributes & B_KERNEL_WRITE_AREA) != 0)
-			pte->flags |= (1 << pteWrite);
-		if ((attributes & B_KERNEL_EXECUTE_AREA) != 0)
-			pte->flags |= (1 << pteExec);
+			newPte.flags |= (1 << pteWrite);
+		if ((attributes & B_KERNEL_EXECUTE_AREA) != 0) {
+			newPte.flags |= (1 << pteExec);
+			fInvalidCode = true;
+		}
 	}
 
-	pte->flags |= (1 << pteValid)
-#ifdef DISABLE_MODIFIED_FLAGS
-		| (1 << pteAccessed) | (1 << pteDirty)
-#endif
-	;
+	*pte = newPte;
 
-	FlushTlbPage(virtualAddress);
+	// Note: We don't need to invalidate the TLB for this address, as previously
+	// the entry was not present and the TLB doesn't cache those entries.
 
 	fMapCount++;
 
@@ -280,9 +363,9 @@ RISCV64VMTranslationMap::Unmap(addr_t start, addr_t end)
 		Pte* pte = LookupPte(page, false, NULL);
 		if (pte != NULL) {
 			fMapCount--;
-			pte->flags = 0;
-			pte->ppn = 0;
-			FlushTlbPage(page);
+			Pte oldPte((uint64)atomic_get_and_set64((int64*)&pte->val, 0));
+			if ((oldPte.flags & (1 << pteAccessed)) != 0)
+				InvalidatePage(page);
 		}
 	}
 	return B_OK;
@@ -327,13 +410,15 @@ RISCV64VMTranslationMap::UnmapPage(VMArea* area, addr_t address,
 
 	RecursiveLocker locker(fLock);
 
-	Pte oldPte = *pte;
-	pte->flags = 0;
-	pte->ppn = 0;
+	Pte oldPte((uint64)atomic_get_and_set64((int64*)&pte->val, 0));
 	fMapCount--;
-	FlushTlbPage(address);
 	pinner.Unlock();
 
+	if ((oldPte.flags & (1 << pteAccessed)) != 0)
+		InvalidatePage(address);
+
+	Flush();
+
 	locker.Detach(); // PageUnmapped takes ownership
 	PageUnmapped(area, oldPte.ppn, ((1 << pteAccessed) & oldPte.flags) != 0,
 		((1 << pteDirty) & oldPte.flags) != 0, updatePageQueue);
@@ -349,8 +434,98 @@ RISCV64VMTranslationMap::UnmapPages(VMArea* area, addr_t base, size_t size,
 		B_PRIxADDR ", 0x%" B_PRIxSIZE ", %d)\n", (addr_t)area,
 		area->name, base, size, updatePageQueue);
 
-	for (addr_t end = base + size; base < end; base += B_PAGE_SIZE)
-		UnmapPage(area, base, updatePageQueue);
+	if (size == 0)
+		return;
+
+	addr_t end = base + size - 1;
+
+	VMAreaMappings queue;
+	RecursiveLocker locker(fLock);
+	ThreadCPUPinner pinner(thread_get_current_thread());
+
+	for (addr_t start = base; start < end; start += B_PAGE_SIZE) {
+		Pte* pte = LookupPte(start, false, NULL);
+		if (pte == NULL)
+			continue;
+
+		Pte oldPte((uint64)atomic_get_and_set64((int64*)&pte->val, 0));
+		if ((oldPte.flags & (1 << pteValid)) == 0)
+			continue;
+
+		fMapCount--;
+
+		if ((oldPte.flags & (1 << pteAccessed)) != 0)
+			InvalidatePage(start);
+
+		if (area->cache_type != CACHE_TYPE_DEVICE) {
+			// get the page
+			vm_page* page = vm_lookup_page(oldPte.ppn);
+			ASSERT(page != NULL);
+			if (false) {
+				WriteVmPage(page); dprintf("\n");
+			}
+
+			DEBUG_PAGE_ACCESS_START(page);
+
+			// transfer the accessed/dirty flags to the page
+			if ((oldPte.flags & (1 << pteAccessed)) != 0)
+				page->accessed = true;
+			if ((oldPte.flags & (1 << pteDirty)) != 0)
+				page->modified = true;
+
+			// remove the mapping object/decrement the wired_count of the
+			// page
+			if (area->wiring == B_NO_LOCK) {
+				vm_page_mapping* mapping = NULL;
+				vm_page_mappings::Iterator iterator
+					= page->mappings.GetIterator();
+				while ((mapping = iterator.Next()) != NULL) {
+					if (mapping->area == area)
+						break;
+				}
+
+				ASSERT(mapping != NULL);
+
+				area->mappings.Remove(mapping);
+				page->mappings.Remove(mapping);
+				queue.Add(mapping);
+			} else
+				page->DecrementWiredCount();
+
+			if (!page->IsMapped()) {
+				atomic_add(&gMappedPagesCount, -1);
+
+				if (updatePageQueue) {
+					if (page->Cache()->temporary)
+						vm_page_set_state(page, PAGE_STATE_INACTIVE);
+					else if (page->modified)
+						vm_page_set_state(page, PAGE_STATE_MODIFIED);
+					else
+						vm_page_set_state(page, PAGE_STATE_CACHED);
+				}
+			}
+
+			DEBUG_PAGE_ACCESS_END(page);
+		}
+
+		// flush explicitly, since we directly use the lock
+		Flush();
+	}
+
+	// TODO: As in UnmapPage() we can lose page dirty flags here. ATM it's not
+	// really critical here, as in all cases this method is used, the unmapped
+	// area range is unmapped for good (resized/cut) and the pages will likely
+	// be freed.
+
+	locker.Unlock();
+
+	// free removed mappings
+	bool isKernelSpace = area->address_space == VMAddressSpace::Kernel();
+	uint32 freeFlags = CACHE_DONT_WAIT_FOR_MEMORY
+		| (isKernelSpace ? CACHE_DONT_LOCK_KERNEL_SPACE : 0);
+
+	while (vm_page_mapping* mapping = queue.RemoveHead())
+		object_cache_free(gPageMappingsObjectCache, mapping, freeFlags);
 }
 
 
@@ -404,9 +579,7 @@ RISCV64VMTranslationMap::UnmapArea(VMArea* area, bool deletingAddressSpace,
 				continue;
 			}
 
-			Pte oldPte = *pte;
-			pte->flags = 0;
-			pte->ppn = 0;
+			Pte oldPte((uint64)atomic_get_and_set64((int64*)&pte->val, 0));
 
 			// transfer the accessed/dirty flags to the page and
 			// invalidate the mapping, if necessary
@@ -414,13 +587,15 @@ RISCV64VMTranslationMap::UnmapArea(VMArea* area, bool deletingAddressSpace,
 				page->accessed = true;
 
 				if (!deletingAddressSpace)
-					FlushTlbPage(address);
+					InvalidatePage(address);
 			}
 
 			if (((1 << pteDirty) & oldPte.flags) != 0)
 				page->modified = true;
 
 			if (pageFullyUnmapped) {
+				DEBUG_PAGE_ACCESS_START(page);
+
 				if (cache->temporary) {
 					vm_page_set_state(page,
 						PAGE_STATE_INACTIVE);
@@ -431,6 +606,8 @@ RISCV64VMTranslationMap::UnmapArea(VMArea* area, bool deletingAddressSpace,
 					vm_page_set_state(page,
 						PAGE_STATE_CACHED);
 				}
+
+				DEBUG_PAGE_ACCESS_END(page);
 			}
 		}
 
@@ -467,29 +644,28 @@ RISCV64VMTranslationMap::Query(addr_t virtualAddress,
 	if (pte == 0)
 		return B_OK;
 
-	*_physicalAddress = pte->ppn * B_PAGE_SIZE;
+	Pte pteVal = *pte;
+	*_physicalAddress = pteVal.ppn * B_PAGE_SIZE;
 
-	if (((1 << pteValid)    & pte->flags) != 0)
+	if (((1 << pteValid)    & pteVal.flags) != 0)
 		*_flags |= PAGE_PRESENT;
-#ifndef DISABLE_MODIFIED_FLAGS
-	if (((1 << pteDirty)    & pte->flags) != 0)
+	if (((1 << pteDirty)    & pteVal.flags) != 0)
 		*_flags |= PAGE_MODIFIED;
-	if (((1 << pteAccessed) & pte->flags) != 0)
+	if (((1 << pteAccessed) & pteVal.flags) != 0)
 		*_flags |= PAGE_ACCESSED;
-#endif
-	if (((1 << pteUser) & pte->flags) != 0) {
-		if (((1 << pteRead)  & pte->flags) != 0)
+	if (((1 << pteUser) & pteVal.flags) != 0) {
+		if (((1 << pteRead)  & pteVal.flags) != 0)
 			*_flags |= B_READ_AREA;
-		if (((1 << pteWrite) & pte->flags) != 0)
+		if (((1 << pteWrite) & pteVal.flags) != 0)
 			*_flags |= B_WRITE_AREA;
-		if (((1 << pteExec)  & pte->flags) != 0)
+		if (((1 << pteExec)  & pteVal.flags) != 0)
 			*_flags |= B_EXECUTE_AREA;
 	} else {
-		if (((1 << pteRead)  & pte->flags) != 0)
+		if (((1 << pteRead)  & pteVal.flags) != 0)
 			*_flags |= B_KERNEL_READ_AREA;
-		if (((1 << pteWrite) & pte->flags) != 0)
+		if (((1 << pteWrite) & pteVal.flags) != 0)
 			*_flags |= B_KERNEL_WRITE_AREA;
-		if (((1 << pteExec)  & pte->flags) != 0)
+		if (((1 << pteExec)  & pteVal.flags) != 0)
 			*_flags |= B_KERNEL_EXECUTE_AREA;
 	}
 
@@ -522,7 +698,8 @@ status_t RISCV64VMTranslationMap::Protect(addr_t base, addr_t top,
 			continue;
 		}
 
-		Pte newPte = *pte;
+		Pte oldPte = *pte;
+		Pte newPte = oldPte;
 		newPte.flags &= (1 << pteValid)
 			| (1 << pteAccessed) | (1 << pteDirty);
 
@@ -532,19 +709,24 @@ status_t RISCV64VMTranslationMap::Protect(addr_t base, addr_t top,
 				newPte.flags |= (1 << pteRead);
 			if ((attributes & B_WRITE_AREA)   != 0)
 				newPte.flags |= (1 << pteWrite);
-			if ((attributes & B_EXECUTE_AREA) != 0)
+			if ((attributes & B_EXECUTE_AREA) != 0) {
 				newPte.flags |= (1 << pteExec);
+				fInvalidCode = true;
+			}
 		} else {
 			if ((attributes & B_KERNEL_READ_AREA)    != 0)
 				newPte.flags |= (1 << pteRead);
 			if ((attributes & B_KERNEL_WRITE_AREA)   != 0)
 				newPte.flags |= (1 << pteWrite);
-			if ((attributes & B_KERNEL_EXECUTE_AREA) != 0)
+			if ((attributes & B_KERNEL_EXECUTE_AREA) != 0) {
 				newPte.flags |= (1 << pteExec);
+				fInvalidCode = true;
+			}
 		}
 		*pte = newPte;
 
-		FlushTlbPage(page);
+		if ((oldPte.flags & (1 << pteAccessed)) != 0)
+			InvalidatePage(page);
 	}
 
 	return B_OK;
@@ -583,9 +765,7 @@ RISCV64VMTranslationMap::SetFlags(addr_t address, uint32 flags)
 	Pte* pte = LookupPte(address, false, NULL);
 	if (pte == NULL || ((1 << pteValid) & pte->flags) == 0)
 		return B_OK;
-#ifndef DISABLE_MODIFIED_FLAGS
 	pte->flags |= ConvertAccessedFlags(flags);
-#endif
 	FlushTlbPage(address);
 	return B_OK;
 }
@@ -600,11 +780,8 @@ RISCV64VMTranslationMap::ClearFlags(addr_t address, uint32 flags)
 	if (pte == NULL || ((1 << pteValid) & pte->flags) == 0)
 		return B_OK;
 
-#ifndef DISABLE_MODIFIED_FLAGS
 	pte->flags &= ~ConvertAccessedFlags(flags);
-#endif
-
-	FlushTlbPage(address);
+	InvalidatePage(address);
 	return B_OK;
 }
 
@@ -624,25 +801,32 @@ RISCV64VMTranslationMap::ClearAccessedAndModified(VMArea* area, addr_t address,
 	if (pte == NULL || ((1 << pteValid) & pte->flags) == 0)
 		return false;
 
-	Pte oldPte = *pte;
-
-#ifndef DISABLE_MODIFIED_FLAGS
+	Pte oldPte;
 	if (unmapIfUnaccessed) {
-		if (((1 << pteAccessed) & pte->flags) != 0) {
-			pte->flags &= ~((1 << pteAccessed) | (1 << pteDirty));
-		} else {
-			pte->flags = 0;
-			pte->ppn = 0;
+		for (;;) {
+			oldPte = *pte;
+			if (((1 << pteValid) & oldPte.flags) == 0)
+				return false;
+
+			if (((1 << pteAccessed) & oldPte.flags) != 0) {
+				oldPte.val = atomic_and64((int64*)&pte->val,
+					~((1 << pteAccessed) | (1 << pteDirty)));
+				break;
+			}
+			if (atomic_test_and_set64((int64*)&pte->val, 0, oldPte.val)
+					== (int64)oldPte.val) {
+				break;
+			}
 		}
 	} else {
-		pte->flags &= ~((1 << pteAccessed) | (1 << pteDirty));
+		oldPte.val = atomic_and64((int64*)&pte->val,
+			~((1 << pteAccessed) | (1 << pteDirty)));
 	}
-#endif
 
 	pinner.Unlock();
 	_modified = ((1 << pteDirty) & oldPte.flags) != 0;
 	if (((1 << pteAccessed) & oldPte.flags) != 0) {
-		FlushTlbPage(address);
+		InvalidatePage(address);
 		Flush();
 		return true;
 	}
@@ -661,7 +845,104 @@ RISCV64VMTranslationMap::ClearAccessedAndModified(VMArea* area, addr_t address,
 void
 RISCV64VMTranslationMap::Flush()
 {
-	//NOT_IMPLEMENTED_PANIC();
+	// copy of X86VMTranslationMap::Flush
+	// TODO: move to common VMTranslationMap class
+
+	if (fInvalidPagesCount <= 0)
+		return;
+/*
+	dprintf("+Flush(%p)\n", this);
+	struct ScopeExit {
+		~ScopeExit()
+		{
+			dprintf("-Flush(%p)\n", this);
+		}
+	} scopeExit;
+*/
+	ThreadCPUPinner pinner(thread_get_current_thread());
+
+	if (fInvalidPagesCount > PAGE_INVALIDATE_CACHE_SIZE) {
+		// invalidate all pages
+		TRACE("flush_tmap: %d pages to invalidate, invalidate all\n",
+			fInvalidPagesCount);
+
+		if (fIsKernel) {
+			arch_cpu_global_TLB_invalidate();
+
+			// dprintf("+smp_send_broadcast_ici\n");
+			smp_send_broadcast_ici(SMP_MSG_GLOBAL_INVALIDATE_PAGES, 0, 0, 0,
+				NULL, SMP_MSG_FLAG_SYNC);
+			// dprintf("-smp_send_broadcast_ici\n");
+
+		} else {
+			cpu_status state = disable_interrupts();
+			arch_cpu_user_TLB_invalidate();
+			restore_interrupts(state);
+
+			int cpu = smp_get_current_cpu();
+			CPUSet cpuMask = fActiveOnCpus;
+			cpuMask.ClearBit(cpu);
+
+			if (!cpuMask.IsEmpty()) {
+				// dprintf("+smp_send_multicast_ici\n");
+				smp_send_multicast_ici(cpuMask, SMP_MSG_USER_INVALIDATE_PAGES,
+					0, 0, 0, NULL, SMP_MSG_FLAG_SYNC);
+				// dprintf("-smp_send_multicast_ici\n");
+			}
+		}
+	} else {
+		TRACE("flush_tmap: %d pages to invalidate, invalidate list\n",
+			fInvalidPagesCount);
+
+		arch_cpu_invalidate_TLB_list(fInvalidPages, fInvalidPagesCount);
+
+		if (fIsKernel) {
+			// dprintf("+smp_send_broadcast_ici\n");
+			smp_send_broadcast_ici(SMP_MSG_INVALIDATE_PAGE_LIST,
+				(addr_t)fInvalidPages, fInvalidPagesCount, 0, NULL,
+				SMP_MSG_FLAG_SYNC);
+			// dprintf("-smp_send_broadcast_ici\n");
+		} else {
+			int cpu = smp_get_current_cpu();
+			CPUSet cpuMask = fActiveOnCpus;
+			cpuMask.ClearBit(cpu);
+
+			if (!cpuMask.IsEmpty()) {
+				// dprintf("+smp_send_multicast_ici\n");
+				smp_send_multicast_ici(cpuMask, SMP_MSG_INVALIDATE_PAGE_LIST,
+					(addr_t)fInvalidPages, fInvalidPagesCount, 0, NULL,
+					SMP_MSG_FLAG_SYNC);
+				// dprintf("-smp_send_multicast_ici\n");
+			}
+		}
+	}
+	fInvalidPagesCount = 0;
+
+	if (fInvalidCode) {
+		FenceI();
+
+		int cpu = smp_get_current_cpu();
+		CPUSet cpuMask = fActiveOnCpus;
+		cpuMask.ClearBit(cpu);
+
+		if (!cpuMask.IsEmpty()) {
+			switch (gPlatform) {
+				case kPlatformSbi: {
+					uint64 hartMask = 0;
+					int32 cpuCount = smp_get_num_cpus();
+					for (int32 i = 0; i < cpuCount; i++) {
+						if (cpuMask.GetBit(i))
+							hartMask |= (uint64)1 << gCPU[i].arch.hartId;
+					}
+					// TODO: handle hart ID >= 64
+					memory_full_barrier();
+					sbi_remote_fence_i(hartMask, 0);
+					break;
+				}
+			}
+		}
+		fInvalidCode = false;
+	}
 }
 
 
@@ -883,11 +1164,7 @@ RISCV64VMPhysicalPageMapper::MemsetPhysical(phys_addr_t address, int value,
 {
 	TRACE("RISCV64VMPhysicalPageMapper::MemsetPhysical(0x%" B_PRIxADDR
 		", 0x%x, 0x%" B_PRIxADDR ")\n", address, value, length);
-	set_ac();
-	memset(VirtFromPhys(address), value, length);
-	clear_ac();
-
-	return B_OK;
+	return user_memset(VirtFromPhys(address), value, length);
 }
 
 
@@ -898,12 +1175,7 @@ RISCV64VMPhysicalPageMapper::MemcpyFromPhysical(void* to, phys_addr_t from,
 	TRACE("RISCV64VMPhysicalPageMapper::MemcpyFromPhysical(0x%" B_PRIxADDR
 		", 0x%" B_PRIxADDR ", %" B_PRIuSIZE ")\n", (addr_t)to,
 		from, length);
-
-	set_ac();
-	memcpy(to, VirtFromPhys(from), length);
-	clear_ac();
-
-	return B_OK;
+	return user_memcpy(to, VirtFromPhys(from), length);
 }
 
 
@@ -914,12 +1186,7 @@ RISCV64VMPhysicalPageMapper::MemcpyToPhysical(phys_addr_t to, const void* from,
 	TRACE("RISCV64VMPhysicalPageMapper::MemcpyToPhysical(0x%" B_PRIxADDR
 		", 0x%" B_PRIxADDR ", %" B_PRIuSIZE ")\n", to, (addr_t)from,
 		length);
-
-	set_ac();
-	memcpy(VirtFromPhys(to), from, length);
-	clear_ac();
-
-	return B_OK;
+	return user_memcpy(VirtFromPhys(to), from, length);
 }
 
 
@@ -929,8 +1196,5 @@ RISCV64VMPhysicalPageMapper::MemcpyPhysicalPage(phys_addr_t to,
 {
 	TRACE("RISCV64VMPhysicalPageMapper::MemcpyPhysicalPage(0x%" B_PRIxADDR
 		", 0x%" B_PRIxADDR ")\n", to, from);
-
-	set_ac();
-	memcpy(VirtFromPhys(to), VirtFromPhys(from), B_PAGE_SIZE);
-	clear_ac();
+	user_memcpy(VirtFromPhys(to), VirtFromPhys(from), B_PAGE_SIZE);
 }

src/system/kernel/arch/riscv64/RISCV64VMTranslationMap.h

@@ -11,6 +11,12 @@
 
 #include <vm/VMTranslationMap.h>
 #include <arch_cpu_defs.h>
+#include <kernel/smp.h>
+
+
+enum {
+	PAGE_INVALIDATE_CACHE_SIZE = 64
+};
 
 
 struct RISCV64VMTranslationMap: public VMTranslationMap {
@@ -87,6 +93,9 @@ struct RISCV64VMTranslationMap: public VMTranslationMap {
 			ssize_t				StrlcpyToMap(addr_t to, const char *from,
 									size_t size);
 
+	inline	CPUSet&				ActiveOnCpus();
+	inline	void				InvalidatePage(addr_t address);
+
 private:
 			Pte*				LookupPte(addr_t virtAdr, bool alloc,
 									vm_page_reservation* reservation);
@@ -94,16 +103,23 @@ private:
 
 			bool				fIsKernel;
 			phys_addr_t			fPageTable;
-			uint64_t			fPageTableSize; // in page units
+			uint64				fPageTableSize; // in page units
+			CPUSet				fActiveOnCpus;
+			int					fInvalidPagesCount;
+			addr_t				fInvalidPages[PAGE_INVALIDATE_CACHE_SIZE];
+			bool				fInvalidCode;
 };
 
 
-inline phys_addr_t RISCV64VMTranslationMap::PageTable()
+inline phys_addr_t
+RISCV64VMTranslationMap::PageTable()
 {
 	return fPageTable;
 }
 
-inline uint64 RISCV64VMTranslationMap::Satp()
+
+inline uint64
+RISCV64VMTranslationMap::Satp()
 {
 	SatpReg satp;
 	satp.ppn = fPageTable / B_PAGE_SIZE;
@@ -113,6 +129,23 @@ inline uint64 RISCV64VMTranslationMap::Satp()
 }
 
 
+CPUSet&
+RISCV64VMTranslationMap::ActiveOnCpus()
+{
+	return fActiveOnCpus;
+}
+
+
+void
+RISCV64VMTranslationMap::InvalidatePage(addr_t address)
+{
+	if (fInvalidPagesCount < PAGE_INVALIDATE_CACHE_SIZE)
+		fInvalidPages[fInvalidPagesCount] = address;
+
+	fInvalidPagesCount++;
+}
+
+
 struct RISCV64VMPhysicalPageMapper: public VMPhysicalPageMapper {
 								RISCV64VMPhysicalPageMapper();
 	virtual						~RISCV64VMPhysicalPageMapper();

src/system/kernel/arch/riscv64/arch_asm.S

@@ -17,7 +17,8 @@ FUNCTION(MSyscall):
 FUNCTION_END(MSyscall)
 
 
-FUNCTION(arch_setjmp):
+FUNCTION(arch_context_switch):
+# save `from` context
 	sd ra,   0*8(a0)
 	sd s0,   1*8(a0)
 	sd s1,   2*8(a0)
@@ -35,34 +36,27 @@ FUNCTION(arch_setjmp):
 	csrr t0, satp
 	sd t0,  14*8(a0)
 
-	li    a0, 0
-	ret
-FUNCTION_END(arch_setjmp)
-
-
-FUNCTION(arch_longjmp):
-	ld ra,   0*8(a0)
-	ld s0,   1*8(a0)
-	ld s1,   2*8(a0)
-	ld s2,   3*8(a0)
-	ld s3,   4*8(a0)
-	ld s4,   5*8(a0)
-	ld s5,   6*8(a0)
-	ld s6,   7*8(a0)
-	ld s7,   8*8(a0)
-	ld s8,   9*8(a0)
-	ld s9,  10*8(a0)
-	ld s10, 11*8(a0)
-	ld s11, 12*8(a0)
-	ld sp,  13*8(a0)
-	ld t0,  14*8(a0)
+# load `to` context
+	ld ra,   0*8(a1)
+	ld s0,   1*8(a1)
+	ld s1,   2*8(a1)
+	ld s2,   3*8(a1)
+	ld s3,   4*8(a1)
+	ld s4,   5*8(a1)
+	ld s5,   6*8(a1)
+	ld s6,   7*8(a1)
+	ld s7,   8*8(a1)
+	ld s8,   9*8(a1)
+	ld s9,  10*8(a1)
+	ld s10, 11*8(a1)
+	ld s11, 12*8(a1)
+	ld sp,  13*8(a1)
+	ld t0,  14*8(a1)
 	csrw satp, t0
 	sfence.vma
 
-	seqz a0, a1
-	add  a0, a0, a1   # a0 = (a1 == 0) ? 1 : a1
 	ret
-FUNCTION_END(arch_longjmp)
+FUNCTION_END(arch_context_switch)
 
 
 FUNCTION(save_fpu):
@@ -147,20 +141,15 @@ FUNCTION_END(restore_fpu)
 
 FUNCTION(arch_thread_entry):
 	mv a0, s2
-	jalr s1
+	jr s1
 FUNCTION_END(arch_thread_entry)
 
 
-FUNCTION(arch_enter_userspace):
-	mv sp, a2
-	sret
-FUNCTION_END(arch_enter_userspace)
-
-
-FUNCTION(arch_longjmp_iframe):
-	mv sp, a0
-	call SVecURet
-FUNCTION_END(arch_longjmp_iframe)
+FUNCTION(arch_load_user_iframe):
+	mv fp, a0
+	mv sp, a1
+	tail SVecURet
+FUNCTION_END(arch_load_user_iframe)
 
 
 FUNCTION(arch_user_thread_exit):

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

@@ -15,12 +15,15 @@
 #include <platform/sbi/sbi_syscalls.h>
 
 
+extern "C" void SVec();
+
 extern uint32 gPlatform;
 
 
 status_t
 arch_cpu_preboot_init_percpu(kernel_args *args, int curr_cpu)
 {
+	// dprintf("arch_cpu_preboot_init_percpu(%" B_PRId32 ")\n", curr_cpu);
 	return B_OK;
 }
 
@@ -28,16 +31,34 @@ arch_cpu_preboot_init_percpu(kernel_args *args, int curr_cpu)
 status_t
 arch_cpu_init_percpu(kernel_args *args, int curr_cpu)
 {
-	//detect_cpu(curr_cpu);
+	SetStvec((uint64)SVec);
+	SstatusReg sstatus(Sstatus());
+	sstatus.ie = 0;
+	sstatus.fs = extStatusInitial; // enable FPU
+	sstatus.xs = extStatusOff;
+	SetSstatus(sstatus.val);
+	SetSie(Sie() | (1 << sTimerInt) | (1 << sSoftInt) | (1 << sExternInt));
 
-	// we only support one anyway...
-	return 0;
+	return B_OK;
 }
 
 
 status_t
 arch_cpu_init(kernel_args *args)
 {
+	for (uint32 curCpu = 0; curCpu < args->num_cpus; curCpu++) {
+		cpu_ent* cpu = &gCPU[curCpu];
+
+		cpu->arch.hartId = args->arch_args.hartIds[curCpu];
+
+		cpu->topology_id[CPU_TOPOLOGY_PACKAGE] = 0;
+		cpu->topology_id[CPU_TOPOLOGY_CORE] = curCpu;
+		cpu->topology_id[CPU_TOPOLOGY_SMT] = 0;
+
+		for (unsigned int i = 0; i < CPU_MAX_CACHE_LEVEL; i++)
+			cpu->cache_id[i] = -1;
+	}
+
 /*
 	uint64 conversionFactor
 		= (1LL << 32) * 1000000LL / args->arch_args.timerFrequency;
@@ -88,24 +109,33 @@ arch_cpu_memory_write_barrier(void)
 void
 arch_cpu_invalidate_TLB_range(addr_t start, addr_t end)
 {
+	int32 numPages = end / B_PAGE_SIZE - start / B_PAGE_SIZE;
+	while (numPages-- >= 0) {
+		FlushTlbPage(start);
+		start += B_PAGE_SIZE;
+	}
 }
 
 
 void
 arch_cpu_invalidate_TLB_list(addr_t pages[], int num_pages)
 {
+	for (int i = 0; i < num_pages; i++)
+		FlushTlbPage(pages[i]);
 }
 
 
 void
 arch_cpu_global_TLB_invalidate(void)
 {
+	FlushTlbAll();
 }
 
 
 void
 arch_cpu_user_TLB_invalidate(void)
 {
+	FlushTlbAll();
 }
 
 

src/system/kernel/arch/riscv64/arch_debug.cpp

@@ -20,6 +20,12 @@ kernel_args *sKernelArgs;
 bool sInitCalled = false;
 
 
+extern "C" void SVecRet();
+extern "C" void SVecURet();
+
+void WriteRegisters(iframe* frame);
+
+
 static void
 WriteImage(preloaded_image* _image)
 {
@@ -120,6 +126,19 @@ FindArea(addr_t adr)
 }
 
 
+static VMArea*
+FindAreaEx(Thread* thread, addr_t adr)
+{
+	if (IS_KERNEL_ADDRESS(adr)) {
+		return VMAddressSpace::Kernel()->LookupArea(adr);
+	}
+	if (IS_USER_ADDRESS(adr)) {
+		return thread->team->address_space->LookupArea(adr);
+	}
+	return NULL;
+}
+
+
 static status_t
 lookup_symbol(Thread* thread, addr_t address, addr_t* _baseAddress,
 	const char** _symbolName, const char** _imageName, bool* _exactMatch)
@@ -172,8 +191,8 @@ WritePCBoot(addr_t pc)
 }
 
 
-void
-WritePC(addr_t pc)
+static void
+WritePCEx(Thread* thread, addr_t pc)
 {
 	dprintf("0x%" B_PRIxADDR " ", pc);
 	if (!sInitCalled) {
@@ -184,7 +203,7 @@ WritePC(addr_t pc)
 	const char* symbolName;
 	const char* imageName;
 	bool exactMatch;
-	if (lookup_symbol(thread_get_current_thread(), pc, &baseAddress,
+	if (lookup_symbol(thread, pc, &baseAddress,
 		&symbolName, &imageName, &exactMatch) >= B_OK) {
 		if (symbolName != NULL) {
 			dprintf("<%s> %s + %" B_PRIdSSIZE, imageName, symbolName,
@@ -195,7 +214,7 @@ WritePC(addr_t pc)
 		return;
 	}
 
-	VMArea* area = FindArea(pc);
+	VMArea* area = FindAreaEx(thread, pc);
 	if (area != NULL) {
 		dprintf("<%s> 0x%" B_PRIxADDR, area->name, pc - area->Base());
 		return;
@@ -205,14 +224,30 @@ WritePC(addr_t pc)
 }
 
 
+void WritePC(addr_t pc)
+{
+	WritePCEx(thread_get_current_thread(), pc);
+}
+
+
+static status_t
+arch_debug_memcpy(void* dst, const void* src, size_t size)
+{
+	if (debug_debugger_running())
+		return debug_memcpy(B_CURRENT_TEAM, dst, src, size);
+
+	return user_memcpy(dst, src, size);
+}
+
+
 static void
 DumpMemory(uint64* adr, size_t len)
 {
 	while (len > 0) {
 		if ((addr_t)adr % 0x10 == 0)
-			dprintf("%08" B_PRIxADDR " ", (addr_t)adr);
+			dprintf("  %08" B_PRIxADDR " ", (addr_t)adr);
 		uint64 val;
-		if (user_memcpy(&val, adr++, sizeof(val)) < B_OK) {
+		if (arch_debug_memcpy(&val, adr++, sizeof(val)) < B_OK) {
 			dprintf(" ????????????????");
 		} else {
 			dprintf(" %016" B_PRIx64, val);
@@ -226,40 +261,56 @@ DumpMemory(uint64* adr, size_t len)
 }
 
 
-void
-DoStackTrace(addr_t fp, addr_t pc)
+static void
+DoStackTraceEx(Thread* thread, addr_t fp, addr_t pc)
 {
 	dprintf("Stack:\n");
 	dprintf("FP: 0x%" B_PRIxADDR, fp);
 	if (pc != 0) {
-		dprintf(", PC: "); WritePC(pc);
+		dprintf(", PC: "); WritePCEx(thread, pc);
 	}
 	dprintf("\n");
 	addr_t oldFp = fp;
-	while (fp != 0) {
+	int i = 0;
+	while (fp != 0 && i < 1000) {
 		if ((pc >= (addr_t)&strcpy && pc < (addr_t)&strcpy + 32)
-				|| (pc >= (addr_t)&memset && pc < (addr_t)&memset + 34)) {
-			if (user_memcpy(&fp, (uint64*)fp - 1, sizeof(pc)) < B_OK)
+				|| (pc >= (addr_t)&memset && pc < (addr_t)&memset + 34)
+				|| (pc >= (addr_t)&memcpy && pc < (addr_t)&memcpy + 186)) {
+			if (arch_debug_memcpy(&fp, (uint64*)fp - 1, sizeof(pc)) < B_OK)
 				break;
 			pc = 0;
 		} else {
-			if (user_memcpy(&pc, (uint64*)fp - 1, sizeof(pc)) < B_OK)
+			if (arch_debug_memcpy(&pc, (uint64*)fp - 1, sizeof(pc)) < B_OK)
 				break;
-			if (user_memcpy(&fp, (uint64*)fp - 2, sizeof(pc)) < B_OK)
+			if (arch_debug_memcpy(&fp, (uint64*)fp - 2, sizeof(pc)) < B_OK)
 				break;
 		}
 		dprintf("FP: 0x%" B_PRIxADDR, fp);
-		dprintf(", PC: "); WritePC((pc == 0) ? 0 : pc - 1);
+		dprintf(", PC: "); WritePCEx(thread, pc);
 		dprintf("\n");
+
+		if (pc == (addr_t)&SVecRet || pc == (addr_t)&SVecURet) {
+			WriteTrapInfo((iframe*)fp - 1);
+		}
 /*
-		if (IS_KERNEL_ADDRESS(oldFp) && IS_KERNEL_ADDRESS(fp))
+		if (IS_KERNEL_ADDRESS(oldFp) != IS_KERNEL_ADDRESS(fp))
+			oldFp = fp;
+		else if (fp != 0)
 			DumpMemory((uint64*)oldFp, (addr_t)fp - (addr_t)oldFp);
 */
 		oldFp = fp;
+		i++;
 	}
 }
 
 
+void
+DoStackTrace(addr_t fp, addr_t pc)
+{
+	DoStackTraceEx(thread_get_current_thread(), fp, pc);
+}
+
+
 static int
 stack_trace(int argc, char **argv)
 {
@@ -272,7 +323,8 @@ stack_trace(int argc, char **argv)
 		}
 		uint64 oldSatp = Satp();
 		SetSatp(thread->arch_info.context.satp);
-		DoStackTrace(thread->arch_info.context.s[0], thread->arch_info.context.ra);
+		DebuggedThreadSetter threadSetter(thread);
+		DoStackTraceEx(thread, thread->arch_info.context.s[0], thread->arch_info.context.ra);
 		SetSatp(oldSatp);
 		return 0;
 	}

src/system/kernel/arch/riscv64/arch_int.cpp

@@ -26,17 +26,12 @@
 #include <algorithm>
 
 
-static uint32 sBootHartId = 0;
-static int32 sPlicContextOfs = 0;
-
-
-extern "C" void SVec();
-extern "C" void SVecU();
+static uint32 sPlicContexts[SMP_MAX_CPUS];
 
 
 //#pragma mark debug output
 
-void
+static void
 WriteMode(int mode)
 {
 	switch (mode) {
@@ -48,7 +43,7 @@ WriteMode(int mode)
 }
 
 
-void
+static void
 WriteModeSet(uint32_t val)
 {
 	bool first = true;
@@ -63,21 +58,20 @@ WriteModeSet(uint32_t val)
 }
 
 
-void
-WriteMstatus(uint64_t val)
+static void
+WriteExt(uint64_t val)
 {
-	MstatusReg status(val);
-	dprintf("(");
-	dprintf("ie: "); WriteModeSet(status.ie);
-	dprintf(", pie: "); WriteModeSet(status.pie);
-	dprintf(", spp: "); WriteMode(status.spp);
-	dprintf(", mpp: "); WriteMode(status.mpp);
-	dprintf(", sum: %d", (int)status.sum);
-	dprintf(")");
+	switch (val) {
+		case 0: dprintf("off"); break;
+		case 1: dprintf("initial"); break;
+		case 2: dprintf("clean"); break;
+		case 3: dprintf("dirty"); break;
+		default: dprintf("%" B_PRId64, val);
+	}
 }
 
 
-void
+static void
 WriteSstatus(uint64_t val)
 {
 	SstatusReg status(val);
@@ -85,12 +79,17 @@ WriteSstatus(uint64_t val)
 	dprintf("ie: "); WriteModeSet(status.ie);
 	dprintf(", pie: "); WriteModeSet(status.pie);
 	dprintf(", spp: "); WriteMode(status.spp);
+	dprintf(", fs: "); WriteExt(status.fs);
+	dprintf(", xs: "); WriteExt(status.xs);
 	dprintf(", sum: %d", (int)status.sum);
+	dprintf(", mxr: %d", (int)status.mxr);
+	dprintf(", uxl: %d", (int)status.uxl);
+	dprintf(", sd: %d", (int)status.sd);
 	dprintf(")");
 }
 
 
-void
+static void
 WriteInterrupt(uint64_t val)
 {
 	switch (val) {
@@ -108,7 +107,7 @@ WriteInterrupt(uint64_t val)
 }
 
 
-void
+static void
 WriteInterruptSet(uint64_t val)
 {
 	bool first = true;
@@ -123,7 +122,7 @@ WriteInterruptSet(uint64_t val)
 }
 
 
-void
+static void
 WriteCause(uint64_t cause)
 {
 	if ((cause & causeInterrupt) == 0) {
@@ -151,18 +150,99 @@ WriteCause(uint64_t cause)
 }
 
 
+const static char* registerNames[] = {
+	" ra", " t6", " sp", " gp",
+	" tp", " t0", " t1", " t2",
+	" t5", " s1", " a0", " a1",
+	" a2", " a3", " a4", " a5",
+	" a6", " a7", " s2", " s3",
+	" s4", " s5", " s6", " s7",
+	" s8", " s9", "s10", "s11",
+	" t3", " t4", " fp", "epc"
+};
+
+
+static void WriteRegisters(iframe* frame)
+{
+	uint64* regs = &frame->ra;
+	for (int i = 0; i < 32; i += 4) {
+		dprintf(
+			"  %s: 0x%016" B_PRIx64
+			"  %s: 0x%016" B_PRIx64
+			"  %s: 0x%016" B_PRIx64
+			"  %s: 0x%016" B_PRIx64 "\n",
+			registerNames[i + 0], regs[i + 0],
+			registerNames[i + 1], regs[i + 1],
+			registerNames[i + 2], regs[i + 2],
+			registerNames[i + 3], regs[i + 3]
+		);
+	}
+}
+
+
+static void
+DumpMemory(uint64* adr, size_t len)
+{
+	while (len > 0) {
+		if ((addr_t)adr % 0x10 == 0)
+			dprintf("%08" B_PRIxADDR " ", (addr_t)adr);
+		uint64 val;
+		if (user_memcpy(&val, adr++, sizeof(val)) < B_OK) {
+			dprintf(" ????????????????");
+		} else {
+			dprintf(" %016" B_PRIx64, val);
+		}
+		if ((addr_t)adr % 0x10 == 0)
+			dprintf("\n");
+		len -= 8;
+	}
+	if ((addr_t)adr % 0x10 != 0)
+		dprintf("\n");
+
+	dprintf("%08" B_PRIxADDR "\n\n", (addr_t)adr);
+}
+
+
 void
-WriteTrapInfo()
+WriteTrapInfo(iframe* frame)
 {
 	InterruptsLocker locker;
-	dprintf("STrap("); WriteCause(Scause()); dprintf(")\n");
-	dprintf("  sstatus: "); WriteSstatus(Sstatus()); dprintf("\n");
-	dprintf("  sie: "); WriteInterruptSet(Sie()); dprintf("\n");
-	dprintf("  sip: "); WriteInterruptSet(Sip()); dprintf("\n");
+	dprintf("STrap("); WriteCause(frame->cause); dprintf(")\n");
+	dprintf("  sstatus: "); WriteSstatus(frame->status); dprintf("\n");
+//	dprintf("  sie: "); WriteInterruptSet(Sie()); dprintf("\n");
+//	dprintf("  sip: "); WriteInterruptSet(Sip()); dprintf("\n");
 	//dprintf("  stval: "); WritePC(Stval()); dprintf("\n");
-	dprintf("  stval: 0x%" B_PRIx64 "\n", Stval());
-	dprintf("  tp: 0x%" B_PRIxADDR "(%s)\n", Tp(),
-		thread_get_current_thread()->name);
+	dprintf("  stval: 0x%" B_PRIx64 "\n", frame->tval);
+//	dprintf("  tp: 0x%" B_PRIxADDR "(%s)\n", Tp(),
+//		thread_get_current_thread()->name);
+
+	WriteRegisters(frame);
+#if 0
+	dprintf("  kernel stack: %#" B_PRIxADDR " - %#" B_PRIxADDR "\n",
+		thread_get_current_thread()->kernel_stack_base,
+		thread_get_current_thread()->kernel_stack_top - 1
+	);
+	dprintf("  user stack: %#" B_PRIxADDR " - %#" B_PRIxADDR "\n",
+		thread_get_current_thread()->user_stack_base,
+		thread_get_current_thread()->user_stack_base +
+		thread_get_current_thread()->user_stack_size - 1
+	);
+	if (thread_get_current_thread()->arch_info.userFrame != NULL) {
+		WriteRegisters(thread_get_current_thread()->arch_info.userFrame);
+
+		dprintf("Stack memory dump:\n");
+		DumpMemory(
+			(uint64*)thread_get_current_thread()->arch_info.userFrame->sp,
+			thread_get_current_thread()->user_stack_base +
+			thread_get_current_thread()->user_stack_size -
+			thread_get_current_thread()->arch_info.userFrame->sp
+		);
+//		if (true) {
+//		} else {
+//			DumpMemory((uint64*)frame->sp, thread_get_current_thread()->kernel_stack_top - frame->sp);
+//		}
+	}
+#endif
 }
 
 
@@ -176,7 +256,6 @@ SendSignal(debug_exception_type type, uint32 signalNumber, int32 signalCode,
 		struct sigaction action;
 		Thread* thread = thread_get_current_thread();
 
-		WriteTrapInfo();
 		DoStackTrace(Fp(), 0);
 
 		enable_interrupts();
@@ -193,7 +272,6 @@ SendSignal(debug_exception_type type, uint32 signalNumber, int32 signalCode,
 			send_signal_to_thread(thread, signal, 0);
 		}
 	} else {
-		WriteTrapInfo();
 		panic("Unexpected exception occurred in kernel mode!");
 	}
 }
@@ -244,6 +322,10 @@ SetAccessedFlags(addr_t addr, bool isWrite)
 	phys_addr_t physAdr;
 	uint32 pageFlags;
 	map->QueryInterrupt(addr, &physAdr, &pageFlags);
+
+	if ((PAGE_PRESENT & pageFlags) == 0)
+		return false;
+
 	if (isWrite) {
 		if (
 			((B_WRITE_AREA | B_KERNEL_WRITE_AREA) & pageFlags) != 0
@@ -272,23 +354,6 @@ SetAccessedFlags(addr_t addr, bool isWrite)
 }
 
 
-static void
-WriteProtection(uint32 flags)
-{
-	dprintf("kernel: {");
-	if (B_KERNEL_READ_AREA & flags) dprintf("R");
-	if (B_KERNEL_WRITE_AREA & flags) dprintf("W");
-	if (B_KERNEL_EXECUTE_AREA & flags) dprintf("X");
-	if (B_KERNEL_STACK_AREA & flags) dprintf("S");
-	dprintf("}, user: {");
-	if (B_READ_AREA & flags) dprintf("R");
-	if (B_WRITE_AREA & flags) dprintf("W");
-	if (B_EXECUTE_AREA & flags) dprintf("X");
-	if (B_STACK_AREA & flags) dprintf("S");
-	dprintf("}");
-}
-
-
 // TODO: needs moved into an arch-agnostic location?
 
 template<typename F>
@@ -328,29 +393,45 @@ STrap(iframe* frame)
 {
 	// dprintf("STrap("); WriteCause(Scause()); dprintf(")\n");
 
-	SstatusReg status(Sstatus());
-	uint64 cause = Scause();
+/*
+	iframe oldFrame = *frame;
+	const auto& frameChangeChecker = MakeScopeExit([&]() {
+			InterruptsLocker locker;
+			bool first = true;
+			for (int i = 0; i < 32; i++) {
+				uint64 oldVal = ((int64*)&oldFrame)[i];
+				uint64 newVal = ((int64*)frame)[i];
+				if (oldVal != newVal) {
+					if (first) {
+						dprintf("FrameChangeChecker, thread: %" B_PRId32 "(%s)\n", thread_get_current_thread()->id, thread_get_current_thread()->name);
+						first = false;
+					}
+					dprintf("  %s: %#" B_PRIxADDR " -> %#" B_PRIxADDR "\n", registerNames[i], oldVal, newVal);
+				}
+			}
 
-	const auto& statusRestorer = MakeScopeExit([&]() {
-		SetSstatus(status.val);
+			if (frame->epc == 0)
+				panic("FrameChangeChecker: EPC = 0");
 	});
-
-	switch (cause) {
+*/
+	switch (frame->cause) {
 		case causeExecPageFault:
 		case causeLoadPageFault:
 		case causeStorePageFault: {
-			if (SetAccessedFlags(Stval(), cause == causeStorePageFault))
+			if (SetAccessedFlags(Stval(), frame->cause == causeStorePageFault))
 				return;
 		}
 	}
 
-	if (status.spp == modeU) {
+	if (SstatusReg(frame->status).spp == modeU) {
 		thread_get_current_thread()->arch_info.userFrame = frame;
+		thread_get_current_thread()->arch_info.oldA0 = frame->a0;
 		thread_at_kernel_entry(system_time());
 	}
 	const auto& kernelExit = MakeScopeExit([&]() {
-		if (status.spp == modeU) {
+		if (SstatusReg(frame->status).spp == modeU) {
 			disable_interrupts();
+			atomic_and(&thread_get_current_thread()->flags, ~THREAD_FLAGS_SYSCALL_RESTARTED);
 			if ((thread_get_current_thread()->flags
 				& (THREAD_FLAGS_SIGNALS_PENDING
 				| THREAD_FLAGS_DEBUG_THREAD
@@ -360,11 +441,18 @@ STrap(iframe* frame)
 			} else {
 				thread_at_kernel_exit_no_signals();
 			}
+			if ((THREAD_FLAGS_RESTART_SYSCALL & thread_get_current_thread()->flags) != 0) {
+				atomic_and(&thread_get_current_thread()->flags, ~THREAD_FLAGS_RESTART_SYSCALL);
+				atomic_or(&thread_get_current_thread()->flags, THREAD_FLAGS_SYSCALL_RESTARTED);
+
+				frame->a0 = thread_get_current_thread()->arch_info.oldA0;
+				frame->epc -= 4;
+			}
 			thread_get_current_thread()->arch_info.userFrame = NULL;
 		}
 	});
 
-	switch (cause) {
+	switch (frame->cause) {
 		case causeIllegalInst: {
 			return SendSignal(B_INVALID_OPCODE_EXCEPTION, SIGILL, ILL_ILLOPC,
 				frame->epc);
@@ -393,7 +481,7 @@ STrap(iframe* frame)
 					cpu_ent* cpu = &gCPU[smp_get_current_cpu()];
 					if (cpu->fault_handler != 0) {
 						debug_set_page_fault_info(stval, frame->epc,
-							(cause == causeStorePageFault)
+							(frame->cause == causeStorePageFault)
 								? DEBUG_PAGE_FAULT_WRITE : 0);
 						frame->epc = cpu->fault_handler;
 						frame->sp = cpu->fault_handler_stack_pointer;
@@ -404,7 +492,7 @@ STrap(iframe* frame)
 						kprintf("ERROR: thread::fault_handler used in kernel "
 							"debugger!\n");
 						debug_set_page_fault_info(stval, frame->epc,
-							cause == causeStorePageFault
+							frame->cause == causeStorePageFault
 								? DEBUG_PAGE_FAULT_WRITE : 0);
 						frame->epc = (addr_t)thread->fault_handler;
 						return;
@@ -416,32 +504,49 @@ STrap(iframe* frame)
 				return;
 			}
 
-			if (status.pie == 0) {
-				WriteTrapInfo();
+			if (SstatusReg(frame->status).pie == 0) {
+				// user_memcpy() failure
+				Thread* thread = thread_get_current_thread();
+				if (thread != NULL && thread->fault_handler != 0) {
+					addr_t handler = (addr_t)(thread->fault_handler);
+					if (frame->epc != handler) {
+						frame->epc = handler;
+						return;
+					}
+				}
 				panic("page fault with interrupts disabled@!dump_virt_page %#" B_PRIx64, stval);
 			}
 
 			addr_t newIP = 0;
 			enable_interrupts();
-			vm_page_fault(stval, frame->epc, cause == causeStorePageFault,
-				cause == causeExecPageFault, status.spp == modeU, &newIP);
+
+			vm_page_fault(stval, frame->epc, frame->cause == causeStorePageFault,
+				frame->cause == causeExecPageFault,
+				SstatusReg(frame->status).spp == modeU, &newIP);
+
 			if (newIP != 0)
 				frame->epc = newIP;
 
 			return;
 		}
+		case causeInterrupt + sSoftInt: {
+			SetSip(Sip() & ~(1 << sSoftInt));
+			// dprintf("sSoftInt(%" B_PRId32 ")\n", smp_get_current_cpu());
+			smp_intercpu_int_handler(smp_get_current_cpu());
+			AfterInterrupt();
+			return;
+		}
 		case causeInterrupt + sTimerInt: {
+			// SetSie(Sie() & ~(1 << sTimerInt));
+			// dprintf("sTimerInt(%" B_PRId32 ")\n", smp_get_current_cpu());
 			timer_interrupt();
 			AfterInterrupt();
 			return;
 		}
 		case causeInterrupt + sExternInt: {
-			// TODO: get PLIC context ID mapping for HARD ID from FDT?
-			uint64 irq = gPlicRegs->contexts[modeS + 2 * sBootHartId
-				+ sPlicContextOfs].claimAndComplete;
+			uint64 irq = gPlicRegs->contexts[sPlicContexts[smp_get_current_cpu()]].claimAndComplete;
 			int_io_interrupt_handler(irq, true);
-			gPlicRegs->contexts[modeS + 2*sBootHartId
-				+ sPlicContextOfs].claimAndComplete = irq;
+			gPlicRegs->contexts[sPlicContexts[smp_get_current_cpu()]].claimAndComplete = irq;
 			AfterInterrupt();
 			return;
 		}
@@ -467,12 +572,12 @@ STrap(iframe* frame)
 			switch (syscall) {
 				case SYSCALL_READ_PORT_ETC:
 				case SYSCALL_WRITE_PORT_ETC:
-					WriteTrapInfo();
 					DoStackTrace(Fp(), 0);
 					break;
 			}
 */
 			// dprintf("syscall: %s\n", kExtendedSyscallInfos[syscall].name);
+
 			enable_interrupts();
 			uint64 returnValue = 0;
 			syscall_dispatcher(syscall, (void*)args, &returnValue);
@@ -480,7 +585,6 @@ STrap(iframe* frame)
 			return;
 		}
 	}
-	WriteTrapInfo();
 	panic("unhandled STrap");
 }
 
@@ -490,22 +594,23 @@ STrap(iframe* frame)
 status_t
 arch_int_init(kernel_args* args)
 {
-	sBootHartId = args->arch_args.bootHart;
-	sPlicContextOfs = (sBootHartId == 0) ? 0 : -1;
+	dprintf("arch_int_init()\n");
 
-	// TODO: Kernel mode FPU handling needs improved?
-	SetStvec((uint64)SVec);
-	SstatusReg sstatus(Sstatus());
-	sstatus.ie = 0;
-	sstatus.fs = extStatusInitial; // enable FPU
-	sstatus.xs = extStatusOff;
-	SetSstatus(sstatus.val);
-	SetSie(Sie() | (1 << sTimerInt) | (1 << sExternInt));
+	for (uint32 i = 0; i < args->num_cpus; i++) {
+		dprintf("  CPU %" B_PRIu32 ":\n", i);
+		dprintf("    hartId: %" B_PRIu32 "\n", args->arch_args.hartIds[i]);
+		dprintf("    plicContext: %" B_PRIu32 "\n", args->arch_args.plicContexts[i]);
+	}
+
+	for (uint32 i = 0; i < args->num_cpus; i++)
+		sPlicContexts[i] = args->arch_args.plicContexts[i];
 
 	// TODO: read from FDT
 	reserve_io_interrupt_vectors(128, 0, INTERRUPT_TYPE_IRQ);
 
-	gPlicRegs->contexts[modeS + 2*sBootHartId + sPlicContextOfs].priorityThreshold = 0;
+	for (uint32 i = 0; i < args->num_cpus; i++)
+		gPlicRegs->contexts[sPlicContexts[i]].priorityThreshold = 0;
+
 	return B_OK;
 }
 
@@ -536,7 +641,7 @@ arch_int_enable_io_interrupt(int irq)
 {
 	dprintf("arch_int_enable_io_interrupt(%d)\n", irq);
 	gPlicRegs->priority[irq] = 1;
-	gPlicRegs->enable[modeS + 2*sBootHartId + sPlicContextOfs][irq / 32] |= 1 << (irq % 32);
+	gPlicRegs->enable[sPlicContexts[0]][irq / 32] |= 1 << (irq % 32);
 }
 
 
@@ -545,7 +650,7 @@ arch_int_disable_io_interrupt(int irq)
 {
 	dprintf("arch_int_disable_io_interrupt(%d)\n", irq);
 	gPlicRegs->priority[irq] = 0;
-	gPlicRegs->enable[modeS + 2*sBootHartId + sPlicContextOfs][irq / 32] &= ~(1 << (irq % 32));
+	gPlicRegs->enable[sPlicContexts[0]][irq / 32] &= ~(1 << (irq % 32));
 }
 
 
@@ -555,3 +660,37 @@ arch_int_assign_to_cpu(int32 irq, int32 cpu)
 	// Not yet supported.
 	return 0;
 }
+
+
+#undef arch_int_enable_interrupts
+#undef arch_int_disable_interrupts
+#undef arch_int_restore_interrupts
+#undef arch_int_are_interrupts_enabled
+
+
+extern "C" void
+arch_int_enable_interrupts()
+{
+	arch_int_enable_interrupts_inline();
+}
+
+
+extern "C" int
+arch_int_disable_interrupts()
+{
+	return arch_int_disable_interrupts_inline();
+}
+
+
+extern "C" void
+arch_int_restore_interrupts(int oldState)
+{
+	arch_int_restore_interrupts_inline(oldState);
+}
+
+
+extern "C" bool
+arch_int_are_interrupts_enabled()
+{
+	return arch_int_are_interrupts_enabled_inline();
+}

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

@@ -17,16 +17,23 @@
 #include <debug.h>
 #include <int.h>
 
+#include <cpu.h>
+#include <platform/sbi/sbi_syscalls.h>
+
+
+extern uint32 gPlatform;
+
 
 status_t
 arch_smp_init(kernel_args *args)
 {
+	dprintf("arch_smp_init()\n");
 	return B_OK;
 }
 
 
 status_t
-arch_smp_per_cpu_init(kernel_args *args, int32 cpu)
+arch_smp_per_cpu_init(kernel_args *args, int32 cpuId)
 {
 	return B_OK;
 }
@@ -35,6 +42,22 @@ arch_smp_per_cpu_init(kernel_args *args, int32 cpu)
 void
 arch_smp_send_multicast_ici(CPUSet& cpuSet)
 {
+	switch (gPlatform) {
+		case kPlatformSbi: {
+			uint64 hartMask = 0;
+			int32 cpuCount = smp_get_num_cpus();
+			for (int32 i = 0; i < cpuCount; i++) {
+				if (cpuSet.GetBit(i) && i != smp_get_current_cpu())
+					hartMask |= (uint64)1 << gCPU[i].arch.hartId;
+			}
+			// TODO: handle hart ID >= 64
+			sbi_send_ipi(hartMask, 0);
+			break;
+		}
+		case kPlatformMNative:
+		default:
+			dprintf("arch_smp_send_multicast_ici: not implemented\n");
+	}
 #if KDEBUG
 	if (are_interrupts_enabled())
 		panic("arch_smp_send_multicast_ici: called with interrupts enabled");
@@ -45,14 +68,28 @@ arch_smp_send_multicast_ici(CPUSet& cpuSet)
 void
 arch_smp_send_ici(int32 target_cpu)
 {
-	panic("called arch_smp_send_ici!\n");
+	switch (gPlatform) {
+	case kPlatformSbi:
+		// dprintf("arch_smp_send_ici(%" B_PRId32 ")\n", target_cpu);
+		sbi_send_ipi((uint64)1 << gCPU[target_cpu].arch.hartId, 0);
+		break;
+	case kPlatformMNative:
+	default:
+		dprintf("arch_smp_send_ici: not implemented\n");
+	}
 }
 
 
 void
 arch_smp_send_broadcast_ici()
 {
-	panic("called arch_smp_send_broadcast_ici\n");
+	switch (gPlatform) {
+	case kPlatformSbi:
+		// dprintf("arch_smp_send_broadcast_ici()\n");
+		sbi_send_ipi(0, -1);
+		break;
+	case kPlatformMNative:
+	default:
+		dprintf("arch_smp_send_broadcast_ici: not implemented\n");
+	}
 }
-
-

src/system/kernel/arch/riscv64/arch_thread.cpp

@@ -22,12 +22,6 @@
 
 extern "C" void SVecU();
 
-extern "C" void RestoreUserRegs()
-{
-	SetSscratch((addr_t)&thread_get_current_thread()->arch_info);
-	SetTp(thread_get_current_thread()->user_local_storage);
-}
-
 
 status_t
 arch_thread_init(struct kernel_args *args)
@@ -50,7 +44,6 @@ arch_team_init_team_struct(Team *team, bool kernel)
 status_t
 arch_thread_init_thread_struct(Thread *thread)
 {
-	thread->arch_info.thread = thread;
 	return B_OK;
 }
 
@@ -70,15 +63,17 @@ void
 arch_thread_init_kthread_stack(Thread* thread, void* _stack, void* _stackTop,
 	void (*function)(void*), const void* data)
 {
-	// dprintf("arch_thread_init_kthread_stack(%p(%s))\n", thread, thread->name);
 	memset(&thread->arch_info.context, 0, sizeof(arch_context));
 	thread->arch_info.context.sp = (addr_t)_stackTop;
 	thread->arch_info.context.s[0] = 0; // fp
 	thread->arch_info.context.s[1] = (addr_t)function;
 	thread->arch_info.context.s[2] = (addr_t)data;
 	thread->arch_info.context.ra = (addr_t)arch_thread_entry;
-	VMTranslationMap* map = GetThreadAddressSpace(thread)->TranslationMap();
-	thread->arch_info.context.satp = ((RISCV64VMTranslationMap*)map)->Satp();
+	RISCV64VMTranslationMap* map = (RISCV64VMTranslationMap*)
+		thread->team->address_space->TranslationMap();
+	thread->arch_info.context.satp = map->Satp();
+
+	memset(&thread->arch_info.fpuContext, 0, sizeof(fpu_context));
 }
 
 
@@ -107,13 +102,22 @@ arch_thread_context_switch(Thread *from, Thread *to)
 	dprintf("arch_thread_context_switch(%p(%s), %p(%s))\n", from, from->name,
 		to, to->name);
 	*/
+
+	RISCV64VMTranslationMap* fromMap = (RISCV64VMTranslationMap*)from->team
+		->address_space->TranslationMap();
+
+	RISCV64VMTranslationMap* toMap = (RISCV64VMTranslationMap*)to->team
+		->address_space->TranslationMap();
+
+	int cpu = to->cpu->cpu_num;
+	toMap->ActiveOnCpus().SetBitAtomic(cpu);
+	fromMap->ActiveOnCpus().ClearBitAtomic(cpu);
+
 	// TODO: save/restore FPU only if needed
 	save_fpu(&from->arch_info.fpuContext);
-	if (arch_setjmp(&from->arch_info.context) == 0) {
-		arch_longjmp(&to->arch_info.context, 1);
-	} else {
-		restore_fpu(&from->arch_info.fpuContext);
-	}
+	restore_fpu(&to->arch_info.fpuContext);
+
+	arch_context_switch(&from->arch_info.context, &to->arch_info.context);
 }
 
 
@@ -138,14 +142,17 @@ arch_thread_enter_userspace(Thread *thread, addr_t entry, void *arg1,
 
 	disable_interrupts();
 
+	arch_stack* stackHeader = (arch_stack*)thread->kernel_stack_top - 1;
+	stackHeader->thread = thread;
+
 	iframe frame;
 	memset(&frame, 0, sizeof(frame));
 
 	SstatusReg status(Sstatus());
 	status.pie = (1 << modeS); // enable interrupts when enter userspace
 	status.spp = modeU;
-	SetSstatus(status.val);
 
+	frame.status = status.val;
 	frame.epc = entry;
 	frame.a0 = (addr_t)arg1;
 	frame.a1 = (addr_t)arg2;
@@ -153,7 +160,7 @@ arch_thread_enter_userspace(Thread *thread, addr_t entry, void *arg1,
 	frame.sp = thread->user_stack_base + thread->user_stack_size;
 	frame.tp = thread->user_local_storage;
 
-	arch_longjmp_iframe(&frame);
+	arch_load_user_iframe(stackHeader, &frame);
 
 	// never return
 	return B_ERROR;
@@ -199,7 +206,7 @@ status_t
 arch_setup_signal_frame(Thread *thread, struct sigaction *sa,
 	struct signal_frame_data *signalFrameData)
 {
-	// dprintf("arch_setup_signal_frame()\n");
+	// dprintf("%s(%" B_PRId32 "(%s))\n", __func__, thread->id, thread->name);
 	iframe* frame = thread->arch_info.userFrame;
 
 	// fill signal context
@@ -250,6 +257,8 @@ arch_setup_signal_frame(Thread *thread, struct sigaction *sa,
 		);
 	}
 */
+	signalFrameData->syscall_restart_return_value = thread->arch_info.oldA0;
+
 	uint8* userStack = get_signal_stack(thread, frame, sa,
 		sizeof(*signalFrameData));
 	// dprintf("  user stack: 0x%" B_PRIxADDR "\n", (addr_t)userStack);
@@ -283,6 +292,9 @@ arch_restore_signal_frame(struct signal_frame_data* signalFrameData)
 	// dprintf("arch_restore_signal_frame()\n");
 	iframe* frame = thread_get_current_thread()->arch_info.userFrame;
 
+	thread_get_current_thread()->arch_info.oldA0
+		= signalFrameData->syscall_restart_return_value;
+
 	frame->ra  = signalFrameData->context.uc_mcontext.x[ 0];
 	frame->sp  = signalFrameData->context.uc_mcontext.x[ 1];
 	frame->gp  = signalFrameData->context.uc_mcontext.x[ 2];
@@ -359,15 +371,19 @@ arch_store_fork_frame(struct arch_fork_arg *arg)
 void
 arch_restore_fork_frame(struct arch_fork_arg *arg)
 {
-	// dprintf("arch_restore_fork_frame(%p)\n", arg);
+	//dprintf("arch_restore_fork_frame(%p)\n", arg);
+	//dprintf("  thread: %" B_PRId32 "(%s))\n", thread_get_current_thread()->id,
+	//	thread_get_current_thread()->name);
+	//dprintf("  kernel SP: %#" B_PRIxADDR "\n", thread_get_current_thread()->kernel_stack_top);
+	//dprintf("  user PC: "); WritePC(arg->frame.epc); dprintf("\n");
+
 	disable_interrupts();
-	if (arch_setjmp(&thread_get_current_thread()->arch_info.context) == 0) {
-		SstatusReg status(Sstatus());
-		status.pie = (1 << modeS); // enable interrupts when enter userspace
-		status.spp = modeU;
-		SetSstatus(status.val);
-		arch_longjmp_iframe(&arg->frame);
-	} else {
-		panic("return from userspace");
-	}
+
+	arch_stack* stackHeader = (arch_stack*)thread_get_current_thread()->kernel_stack_top - 1;
+	stackHeader->thread = thread_get_current_thread();
+	SstatusReg status(Sstatus());
+	status.pie = (1 << modeS); // enable interrupts when enter userspace
+	status.spp = modeU;
+	arg->frame.status = status.val;
+	arch_load_user_iframe(stackHeader, &arg->frame);
 }

src/system/kernel/arch/riscv64/arch_timer.cpp

@@ -17,6 +17,8 @@
 #include <timer.h>
 #include <Clint.h>
 
+#include <smp.h>
+
 
 extern uint32 gPlatform;
 
@@ -24,6 +26,8 @@ extern uint32 gPlatform;
 void
 arch_timer_set_hardware_timer(bigtime_t timeout)
 {
+	// dprintf("arch_timer_set_hardware_timer(%" B_PRIu64 "), cpu: %" B_PRId32 "\n", timeout, smp_get_current_cpu());
+
 	// TODO: Read timer frequency from FDT
 	switch (gPlatform) {
 		case kPlatformMNative:
@@ -37,12 +41,16 @@ arch_timer_set_hardware_timer(bigtime_t timeout)
 		default:
 			;
 	}
+
+	// SetSie(Sie() | (1 << sTimerInt));
 }
 
 
 void
 arch_timer_clear_hardware_timer()
 {
+	// SetSie(Sie() & ~(1 << sTimerInt));
+
 	switch (gPlatform) {
 		case kPlatformMNative:
 			MSyscall(kMSyscallSetTimer, false);

src/system/kernel/arch/riscv64/arch_traps.S

@@ -4,43 +4,58 @@
  */
 
 
+#include <asm_defs.h>
 #include "arch_traps.h"
+#include "asm_offsets.h"
 
 
-.globl SVec
-.type  SVec, @function
 .align 4
-SVec:
-	PushTrapFrame
-	sd fp, 2*8(sp)
+FUNCTION(SVec):
+	PushTrapFrame IFRAME_ra
+	sd fp, IFRAME_sp(sp)
 	csrr t0, sepc
-	sd   t0, 31*8(sp)
+	sd   t0, IFRAME_epc(sp)
+
+	csrr t0, sstatus
+	sd   t0, IFRAME_status(sp)
+	csrr t0, scause
+	sd   t0, IFRAME_cause(sp)
+	csrr t0, stval
+	sd   t0, IFRAME_tval(sp)
 
 	mv a0, sp
 	call STrap
 
-	ld t0, 31*8(sp)
+FUNCTION(SVecRet):
+	ld   t0, IFRAME_status(sp)
+	csrw sstatus, t0
+
+	ld t0, IFRAME_epc(sp)
 	csrw sepc, t0
-	PopTrapFrame
+	PopTrapFrame IFRAME_ra
 	sret
-.size	SVec, .-SVec
+FUNCTION_END(SVec)
 
 
-.globl SVecU
-.type  SVecU, @function
 .align 4
-SVecU:
-	csrrw t0, sscratch, t0   # t0: &arch_thread
-	ld    tp,        0*8(t0) # tp = arch_thread.thread
-	ld    t0, (1 + 13)*8(t0) # t0 = arch_thread.context.sp
-	sd    sp,  2*8 - 256(t0) # save user SP
-	mv    sp, t0             # switch to kernel stack
-	csrr  t0, sscratch
-
-	PushTrapFrame
+FUNCTION(SVecU):
+	# switch to kernel stack, SSCRATCH will hold user SP
+	csrrw sp, sscratch, sp
 
+	PushTrapFrame IFRAME_ra
+	csrr t0, sscratch
+	sd   t0, IFRAME_sp(sp)
 	csrr t0, sepc
-	sd   t0, 31*8(sp)
+	sd   t0, IFRAME_epc(sp)
+
+	csrr t0, sstatus
+	sd   t0, IFRAME_status(sp)
+	csrr t0, scause
+	sd   t0, IFRAME_cause(sp)
+	csrr t0, stval
+	sd   t0, IFRAME_tval(sp)
+
+	ld tp, ARCH_STACK_thread(fp)
 
 	la   t0,    SVec
 	csrw stvec, t0
@@ -48,19 +63,18 @@ SVecU:
 	mv a0, sp
 	call STrap
 
-.globl SVecURet
-.type  SVecURet, @function
-SVecURet:
-	call RestoreUserRegs
-
-	csrr  t0, sscratch
-	sd    fp, (1 + 13)*8(t0) # arch_thread.context.sp = fp
+FUNCTION(SVecURet):
+	csrw sscratch, fp # save kernel SP
 
 	la   t0,    SVecU
 	csrw stvec, t0
 
-	ld t0, 31*8(sp)
+	ld   t0, IFRAME_status(sp)
+	csrw sstatus, t0
+
+	ld tp, IFRAME_tp(sp)
+	ld t0, IFRAME_epc(sp)
 	csrw sepc, t0
-	PopTrapFrame
+	PopTrapFrame IFRAME_ra
 	sret
-.size	SVecU, .-SVecU
+FUNCTION_END(SVecU)

src/system/kernel/arch/riscv64/arch_vm.cpp

@@ -94,91 +94,143 @@ WritePteFlags(uint32 flags)
 }
 
 
-static void
-DumpPageWrite(uint64_t virtAdr, uint64_t physAdr, size_t size, uint64 flags,
-	uint64& firstVirt, uint64& firstPhys, uint64& firstFlags, uint64& len)
+class PageTableDumper
 {
-	if (virtAdr == firstVirt + len && physAdr == firstPhys + len
-			&& flags == firstFlags) {
-		len += size;
-	} else {
-		if (len != 0) {
-			dprintf("  0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR,
-				firstVirt, firstVirt + (len - 1));
-			dprintf(": 0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR ",%#"
-				B_PRIxADDR ", ", firstPhys,
-				firstPhys + (len - 1), len);
-			WritePteFlags(firstFlags); dprintf("\n");
-		}
-		firstVirt = virtAdr;
-		firstPhys = physAdr;
-		firstFlags = flags;
-		len = size;
+private:
+	uint64 firstVirt;
+	uint64 firstPhys;
+	uint64 firstFlags;
+	uint64 len;
+
+public:
+	PageTableDumper()
+		:
+		firstVirt(0),
+		firstPhys(0),
+		firstFlags(0),
+		len(0)
+	{}
+ 
+	~PageTableDumper()
+	{
+		Write(0, 0, 0, 0);
 	}
-}
+
+	void Write(uint64_t virtAdr, uint64_t physAdr, size_t size, uint64 flags) {
+		if (virtAdr == firstVirt + len && physAdr == firstPhys + len && flags == firstFlags) {
+			len += size;
+		} else {
+			if (len != 0) {
+				dprintf("  0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR,
+				firstVirt, firstVirt + (len - 1));
+				dprintf(": 0x%08" B_PRIxADDR " - 0x%08" B_PRIxADDR ", %#" B_PRIxADDR ", ",
+					firstPhys, firstPhys + (len - 1), len);
+				WritePteFlags(firstFlags); dprintf("\n");
+			}
+			firstVirt = virtAdr;
+			firstPhys = physAdr;
+			firstFlags = flags;
+			len = size;
+		}
+	}
+};
 
 
 static void
-DumpPageTableInt(Pte* pte, uint64_t virtAdr, uint32_t level, uint64& firstVirt,
-	uint64& firstPhys, uint64& firstFlags, uint64& len)
+DumpPageTableInt(Pte* pte, uint64_t virtAdr, uint32_t level, PageTableDumper& dumper)
 {
 	for (uint32 i = 0; i < pteCount; i++) {
 		if (((1 << pteValid) & pte[i].flags) != 0) {
 			if ((((1 << pteRead) | (1 << pteWrite)
 					| (1 << pteExec)) & pte[i].flags) == 0) {
 
-				if (level == 0) {
-					kprintf("  internal page table on "
-						"level 0\n");
-				}
+				if (level == 0)
+					kprintf("  internal page table on level 0\n");
 
-				DumpPageTableInt(
-					(Pte*)VirtFromPhys(pageSize*pte[i].ppn),
-					virtAdr + ((uint64_t)i
-						<< (pageBits + pteIdxBits
-							* level)),
-					level - 1, firstVirt, firstPhys,
-						firstFlags, len);
+				DumpPageTableInt((Pte*)VirtFromPhys(pageSize*pte[i].ppn),
+					virtAdr + ((uint64_t)i << (pageBits + pteIdxBits * level)),
+					level - 1, dumper);
 			} else {
-				DumpPageWrite(SignExtendVirtAdr(virtAdr
-					+ ((uint64_t)i << (pageBits
-						+ pteIdxBits*level))),
-					pte[i].ppn * B_PAGE_SIZE,
-					1 << (pageBits + pteIdxBits*level),
-					pte[i].flags, firstVirt, firstPhys,
-					firstFlags, len);
+				dumper.Write(SignExtendVirtAdr(virtAdr
+						+ ((uint64_t)i << (pageBits + pteIdxBits*level))),
+					pte[i].ppn * B_PAGE_SIZE, 1 << (pageBits + pteIdxBits * level),
+					pte[i].flags);
 			}
 		}
 	}
 }
 
 
+static VMArea* LookupArea(area_id id)
+{
+	VMAreaHash::ReadLock();
+	VMArea* area = VMAreaHash::LookupLocked(id);
+	VMAreaHash::ReadUnlock();
+
+	return area;
+}
+
+
 static int
 DumpPageTable(int argc, char** argv)
 {
+	int curArg = 1;
 	SatpReg satp;
-	if (argc >= 2) {
-		team_id id = strtoul(argv[1], NULL, 0);
-		VMAddressSpace* addrSpace = VMAddressSpace::DebugGet(id);
-		if (addrSpace == NULL) {
-			kprintf("could not find team %" B_PRId32 "\n", id);
+	bool isArea = false;
+	addr_t base = 0;
+	size_t size = 0;
+
+	satp.val = Satp();
+	while (curArg < argc && argv[curArg][0] == '-') {
+		if (strcmp(argv[curArg], "-team") == 0) {
+			curArg++;
+			team_id id = strtoul(argv[curArg++], NULL, 0);
+			VMAddressSpace* addrSpace = VMAddressSpace::DebugGet(id);
+			if (addrSpace == NULL) {
+				kprintf("could not find team %" B_PRId32 "\n", id);
+				return 0;
+			}
+			satp.val = ((RISCV64VMTranslationMap*)
+				addrSpace->TranslationMap())->Satp();
+			isArea = false;
+		} else if (strcmp(argv[curArg], "-area") == 0) {
+			curArg++;
+			uint64 areaId;
+			if (!evaluate_debug_expression(argv[curArg++], &areaId, false))
+				return 0;
+			VMArea* area = LookupArea((area_id)areaId);
+			if (area == NULL) {
+				kprintf("could not find area %" B_PRId32 "\n", (area_id)areaId);
+				return 0;
+			}
+			satp.val = ((RISCV64VMTranslationMap*)
+				area->address_space->TranslationMap())->Satp();
+			base = area->Base();
+			size = area->Size();
+			kprintf("area %" B_PRId32 "(%s)\n", area->id, area->name);
+				isArea = true;
+		} else {
+			kprintf("unknown flag \"%s\"\n", argv[curArg]);
 			return 0;
 		}
-		satp.val = ((RISCV64VMTranslationMap*)
-			addrSpace->TranslationMap())->Satp();
-		dprintf("page table for team %" B_PRId32 "\n", id);
-	} else {
-		satp.val = Satp();
-		dprintf("current page table:\n");
 	}
-	Pte* root = (Pte*)VirtFromPhys(satp.ppn * B_PAGE_SIZE);
 
-	uint64 firstVirt = 0;
-	uint64 firstPhys = 0;
-	uint64 firstFlags = 0;
-	uint64 len = 0;
-	DumpPageTableInt(root, 0, 2, firstVirt, firstPhys, firstFlags, len);
-	DumpPageWrite(0, 0, 0, 0, firstVirt, firstPhys, firstFlags, len);
+	kprintf("satp: %#" B_PRIx64 "\n", satp.val);
+
+	PageTableDumper dumper;
+
+	if (!isArea) {
+		Pte* root = (Pte*)VirtFromPhys(satp.ppn * B_PAGE_SIZE);
+		DumpPageTableInt(root, 0, 2, dumper);
+	} else {
+		for (; size > 0; base += B_PAGE_SIZE, size -= B_PAGE_SIZE) {
+			Pte* pte = LookupPte(satp.ppn * B_PAGE_SIZE, base);
+			if (pte == NULL || (pte->flags & (1 << pteValid)) == 0)
+				continue;
+
+			dumper.Write(base, pte->ppn * B_PAGE_SIZE, B_PAGE_SIZE, pte->flags);
+		}
+	}
 
 	return 0;
 }
@@ -191,7 +243,7 @@ DumpVirtPage(int argc, char** argv)
 	SatpReg satp;
 
 	satp.val = Satp();
-	while (argv[curArg][0] == '-') {
+	while (curArg < argc && argv[curArg][0] == '-') {
 		if (strcmp(argv[curArg], "-team") == 0) {
 			curArg++;
 			team_id id = strtoul(argv[curArg++], NULL, 0);
@@ -210,24 +262,20 @@ DumpVirtPage(int argc, char** argv)
 
 	kprintf("satp: %#" B_PRIx64 "\n", satp.val);
 
-	uint64 firstVirt = 0;
-	uint64 firstPhys = 0;
-	uint64 firstFlags = 0;
-	uint64 len = B_PAGE_SIZE;
-	if (!evaluate_debug_expression(argv[curArg++], &firstVirt, false))
+	uint64 virt = 0;
+	if (!evaluate_debug_expression(argv[curArg++], &virt, false))
 		return 0;
 
-	firstVirt = ROUNDDOWN(firstVirt, B_PAGE_SIZE);
+	virt = ROUNDDOWN(virt, B_PAGE_SIZE);
 
-	Pte* pte = LookupPte(satp.ppn * B_PAGE_SIZE, firstVirt);
+	Pte* pte = LookupPte(satp.ppn * B_PAGE_SIZE, virt);
 	if (pte == NULL) {
 		dprintf("not mapped\n");
 		return 0;
 	}
-	firstPhys = pte->ppn * B_PAGE_SIZE;
-	firstFlags = pte->flags;
 
-	DumpPageWrite(0, 0, 0, 0, firstVirt, firstPhys, firstFlags, len);
+	PageTableDumper dumper;
+	dumper.Write(virt, pte->ppn * B_PAGE_SIZE, B_PAGE_SIZE, pte->flags);
 
 	return 0;
 }

src/system/kernel/arch/riscv64/asm_offsets.cpp

@@ -0,0 +1,52 @@
+/*
+ * Copyright 2007-2011, Ingo Weinhold, ingo_weinhold@gmx.de.
+ * Distributed under the terms of the MIT License.
+ */
+
+// This file is used to get C structure offsets into assembly code.
+// The build system assembles the file and processes the output to create
+// a header file with macro definitions, that can be included from assembly
+// code.
+
+
+#include <computed_asm_macros.h>
+
+#include <arch_cpu.h>
+#include <cpu.h>
+#include <ksignal.h>
+#include <ksyscalls.h>
+#include <thread_types.h>
+
+
+#define DEFINE_MACRO(macro, value) DEFINE_COMPUTED_ASM_MACRO(macro, value)
+
+#define DEFINE_OFFSET_MACRO(prefix, structure, member) \
+	DEFINE_MACRO(prefix##_##member, offsetof(struct structure, member));
+
+#define DEFINE_SIZEOF_MACRO(prefix, structure) \
+	DEFINE_MACRO(prefix##_sizeof, sizeof(struct structure));
+
+
+void
+dummy()
+{
+	// struct Thread
+	DEFINE_OFFSET_MACRO(THREAD, Thread, arch_info);
+
+	// struct arch_thread
+	DEFINE_OFFSET_MACRO(ARCH_THREAD, arch_thread, context);
+	DEFINE_OFFSET_MACRO(ARCH_THREAD, arch_thread, fpuContext);
+
+	DEFINE_OFFSET_MACRO(ARCH_CONTEXT, arch_context, sp);
+
+	DEFINE_OFFSET_MACRO(ARCH_STACK, arch_stack, thread);
+
+	DEFINE_SIZEOF_MACRO(IFRAME, iframe);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, status);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, cause);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, tval);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, ra);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, sp);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, tp);
+	DEFINE_OFFSET_MACRO(IFRAME, iframe, epc);
+}

src/system/kernel/int.cpp

@@ -411,6 +411,11 @@ restore_interrupts(cpu_status status)
 static
 uint32 assign_cpu(void)
 {
+// arch_int_assign_to_cpu is not yet implemented for riscv
+#ifdef __riscv
+	return 0;
+#endif
+
 	const cpu_topology_node* node;
 	do {
 		int32 nextID = atomic_add(&sLastCPU, 1);

src/system/kernel/vm/vm.cpp

@@ -4421,8 +4421,8 @@ vm_page_fault(addr_t address, addr_t faultAddress, bool isWrite, bool isExecute,
 
 	if (status < B_OK) {
 		dprintf("vm_page_fault: vm_soft_fault returned error '%s' on fault at "
-			"0x%lx, ip 0x%lx, write %d, user %d, thread 0x%" B_PRIx32 "\n",
-			strerror(status), address, faultAddress, isWrite, isUser,
+			"0x%lx, ip 0x%lx, write %d, user %d, exec %d, thread 0x%" B_PRIx32 "\n",
+			strerror(status), address, faultAddress, isWrite, isUser, isExecute,
 			thread_get_current_thread_id());
 		if (!isUser) {
 			Thread* thread = thread_get_current_thread();