Index: head/sys/riscv/include/pcpu.h
===================================================================
--- head/sys/riscv/include/pcpu.h
+++ head/sys/riscv/include/pcpu.h
@@ -47,8 +47,11 @@
 static inline struct pcpu *
 get_pcpu(void)
 {
+	struct pcpu *pcpu;
 
-	return (pcpup);
+	__asm __volatile("mv %0, gp" : "=&r"(pcpu));
+
+	return (pcpu);
 }
 
 static inline struct thread *
@@ -56,7 +59,7 @@
 {
 	struct thread *td;
 
-	td = (struct thread *)*(uint64_t *)pcpup;
+	__asm __volatile("ld %0, 0(gp)" : "=&r"(td));
 
 	return (td);
 }
Index: head/sys/riscv/riscv/exception.S
===================================================================
--- head/sys/riscv/riscv/exception.S
+++ head/sys/riscv/riscv/exception.S
@@ -41,12 +41,16 @@
 #include <machine/riscvreg.h>
 
 .macro save_registers el
-	addi	sp, sp, -280
+	addi	sp, sp, -(TF_SIZE)
 
 	sd	ra, (TF_RA)(sp)
-	sd	gp, (TF_GP)(sp)
 	sd	tp, (TF_TP)(sp)
 
+.if \el == 0	/* We came from userspace. Load our pcpu */
+	sd	gp, (TF_GP)(sp)
+	ld	gp, (TF_SIZE)(sp)
+.endif
+
 	sd	t0, (TF_T + 0 * 8)(sp)
 	sd	t1, (TF_T + 1 * 8)(sp)
 	sd	t2, (TF_T + 2 * 8)(sp)
@@ -127,13 +131,16 @@
 	csrw	sepc, t0
 
 .if \el == 0
-	/* Load user sp */
+	/* We go to userspace. Load user sp */
 	ld	t0, (TF_SP)(sp)
 	csrw	sscratch, t0
+
+	/* And store our pcpu */
+	sd	gp, (TF_SIZE)(sp)
+	ld	gp, (TF_GP)(sp)
 .endif
 
 	ld	ra, (TF_RA)(sp)
-	ld	gp, (TF_GP)(sp)
 	ld	tp, (TF_TP)(sp)
 
 	ld	t0, (TF_T + 0 * 8)(sp)
@@ -166,7 +173,7 @@
 	ld	a6, (TF_A + 6 * 8)(sp)
 	ld	a7, (TF_A + 7 * 8)(sp)
 
-	addi	sp, sp, 280
+	addi	sp, sp, (TF_SIZE)
 .endm
 
 .macro	do_ast
Index: head/sys/riscv/riscv/genassym.c
===================================================================
--- head/sys/riscv/riscv/genassym.c
+++ head/sys/riscv/riscv/genassym.c
@@ -85,6 +85,7 @@
 ASSYM(TD_MD, offsetof(struct thread, td_md));
 ASSYM(TD_LOCK, offsetof(struct thread, td_lock));
 
+ASSYM(TF_SIZE, sizeof(struct trapframe));
 ASSYM(TF_RA, offsetof(struct trapframe, tf_ra));
 ASSYM(TF_SP, offsetof(struct trapframe, tf_sp));
 ASSYM(TF_GP, offsetof(struct trapframe, tf_gp));
Index: head/sys/riscv/riscv/machdep.c
===================================================================
--- head/sys/riscv/riscv/machdep.c
+++ head/sys/riscv/riscv/machdep.c
@@ -256,7 +256,9 @@
 void
 exec_setregs(struct thread *td, struct image_params *imgp, u_long stack)
 {
-	struct trapframe *tf = td->td_frame;
+	struct trapframe *tf;
+
+	tf = td->td_frame;
 
 	memset(tf, 0, sizeof(struct trapframe));
 
@@ -563,6 +565,7 @@
 static void
 init_proc0(vm_offset_t kstack)
 {
+
 	pcpup = &__pcpu[0];
 
 	proc_linkup0(&proc0, &thread0);
@@ -760,11 +763,7 @@
 	pcpu_init(pcpup, 0, sizeof(struct pcpu));
 
 	/* Set the pcpu pointer */
-#if 0
-	/* SMP TODO: try re-use gp for pcpu pointer */
-	__asm __volatile(
-	    "mv gp, %0" :: "r"(pcpup));
-#endif
+	__asm __volatile("mv gp, %0" :: "r"(pcpup));
 
 	PCPU_SET(curthread, &thread0);
 
Index: head/sys/riscv/riscv/swtch.S
===================================================================
--- head/sys/riscv/riscv/swtch.S
+++ head/sys/riscv/riscv/swtch.S
@@ -74,8 +74,6 @@
 	/* Load registers */
 	ld	ra, (PCB_RA)(x13)
 	ld	sp, (PCB_SP)(x13)
-	ld	gp, (PCB_GP)(x13)
-	ld	tp, (PCB_TP)(x13)
 
 	/* s[0-11] */
 	ld	s0, (PCB_S + 0 * 8)(x13)
@@ -120,8 +118,6 @@
 	/* Store the callee-saved registers */
 	sd	ra, (PCB_RA)(x13)
 	sd	sp, (PCB_SP)(x13)
-	sd	gp, (PCB_GP)(x13)
-	sd	tp, (PCB_TP)(x13)
 
 	/* We use these in fork_trampoline */
 	sd	t0, (PCB_T + 0 * 8)(x13)
@@ -176,8 +172,6 @@
 	/* Restore the registers */
 	ld	ra, (PCB_RA)(x13)
 	ld	sp, (PCB_SP)(x13)
-	ld	gp, (PCB_GP)(x13)
-	ld	tp, (PCB_TP)(x13)
 
 	/* We use these in fork_trampoline */
 	ld	t0, (PCB_T + 0 * 8)(x13)
@@ -254,12 +248,23 @@
 	ld	a6, (TF_A + 6 * 8)(sp)
 	ld	a7, (TF_A + 7 * 8)(sp)
 
+	/* Load user ra and sp */
+	ld	tp, (TF_TP)(sp)
+	ld	ra, (TF_RA)(sp)
+
+	/*
+	 * Store our pcpup on stack, we will load it back
+	 * on kernel mode trap.
+	 */
+	sd	gp, (TF_SIZE)(sp)
+	ld	gp, (TF_GP)(sp)
+
 	/* Save kernel stack so we can use it doing a user trap */
+	addi	sp, sp, TF_SIZE
 	csrw	sscratch, sp
 
-	/* Load user ra and sp */
-	ld	ra, (TF_RA)(sp)
-	ld	sp, (TF_SP)(sp)
+	/* Load user stack */
+	ld	sp, (TF_SP - TF_SIZE)(sp)
 
 	eret
 END(fork_trampoline)
Index: head/sys/riscv/riscv/vm_machdep.c
===================================================================
--- head/sys/riscv/riscv/vm_machdep.c
+++ head/sys/riscv/riscv/vm_machdep.c
@@ -218,7 +218,7 @@
 	td->td_pcb = (struct pcb *)(td->td_kstack +
 	    td->td_kstack_pages * PAGE_SIZE) - 1;
 	td->td_frame = (struct trapframe *)STACKALIGN(
-	    td->td_pcb - 1);
+	    (caddr_t)td->td_pcb - 8 - sizeof(struct trapframe));
 }
 
 void