diff --git a/risc_v/ch4/src/asm/boot.S b/risc_v/ch4/src/asm/boot.S
index 7baf809..59cc74d 100644
--- a/risc_v/ch4/src/asm/boot.S
+++ b/risc_v/ch4/src/asm/boot.S
@@ -90,23 +90,23 @@ _start:
 	# 1 << 5    : Supervisor timer interrupt enable (STIE=1 [Enabled])
 	# 1 << 9    : Supervisor external interrupt enable (SEIE=1 [Enabled])
 	# 0xaaa = MEIP/SEIP and MTIP/STIP and MSIP/SSIP
-	li		t2, 0xaaa
-	csrw	mie, t2
+	# Machine's trap vector base address is set to `m_trap_vector`, for
+	# "machine" trap vector.
+	la		t2, m_trap_vector
+	csrw	mtvec, t2
 	# Setting `stvec` (supervisor trap vector) register:
 	# Essentially this is a function pointer, but the last two bits can be 00 or 01
 	# 00        : All exceptions set pc to BASE
 	# 01        : Asynchronous interrupts set pc to BASE + 4 x scause
 	la		t3, s_trap_vector
 	csrw	stvec, t3
-	# Machine's trap vector base address is set to `m_trap_vector`, for
-	# "machine" trap vector.
-	la		t2, m_trap_vector
-	csrw	mtvec, t2
 	# Jump to kmain. We put the MPP = 01 for supervisor mode, so after
 	# mret, we will jump to kmain in supervisor mode.
 	la		t1, kmain
 	csrw	mepc, t1
-
+	li		t2, 0xaaa
+	csrw	mie, t2
+	csrw	sie, t2
 	csrr	a0, mhartid
 	mret
 3:
diff --git a/risc_v/ch4/src/asm/trap.S b/risc_v/ch4/src/asm/trap.S
index 0db19ae..7a0847b 100644
--- a/risc_v/ch4/src/asm/trap.S
+++ b/risc_v/ch4/src/asm/trap.S
@@ -99,6 +99,47 @@ m_trap_vector:
 # of this vector.
 .align 4
 s_trap_vector:
+	csrrw	t6, sscratch, t6
+
+	.set 	i, 1
+	.rept	30
+		save_gp	%i
+		.set	i, i+1
+	.endr
+
+	# Save the actual t6 register, which we swapped into
+	# mscratch
+	mv		t5, t6
+	csrr	t6, sscratch
+	save_gp 31, t5
+
+	# Restore the kernel trap frame into mscratch
+	csrw	sscratch, t5
+
+	# csrw	satp, zero
+
+	# Get ready to go into Rust (trap.rs)
+	# We don't want to write into the user's stack or whomever
+	# messed with us here.
+	csrr	a0, sepc
+	csrr	a1, stval
+	csrr	a2, scause
+	ld		a3, 528(t5)
+	csrr	a4, sstatus
+	mv		a5, t5
+	ld		sp, 520(a5)
+	call	s_trap
+
+	csrw	sepc, a0
+	# Now load the trap frame back into t6
+	csrr	t6, sscratch
+
+	# Restore all GP registers
+	.set	i, 1
+	.rept	31
+		load_gp %i
+		.set	i, i+1
+	.endr
 	sret
 
 
diff --git a/risc_v/ch4/src/cpu.rs b/risc_v/ch4/src/cpu.rs
index 0fb86a2..3a65081 100755
--- a/risc_v/ch4/src/cpu.rs
+++ b/risc_v/ch4/src/cpu.rs
@@ -12,7 +12,8 @@ pub struct KernelTrapFrame {
 	pub regs: [usize; 32],		// 0 - 255
 	pub fregs: [usize; 32],		// 256 - 511
 	pub satp: usize,			// 512 - 519
-	pub trap_stack: *mut u8     // 520
+	pub trap_stack: *mut u8,    // 520
+	pub hartid: usize,			// 528
 }
 
 impl KernelTrapFrame {
@@ -22,6 +23,7 @@ impl KernelTrapFrame {
 			fregs: [0; 32],
 			satp: 0,
 			trap_stack: null_mut(),
+			hartid: 0
 		}
 	}
 }
diff --git a/risc_v/ch4/src/lib.rs b/risc_v/ch4/src/lib.rs
index 6d357a7..326b9d7 100755
--- a/risc_v/ch4/src/lib.rs
+++ b/risc_v/ch4/src/lib.rs
@@ -257,14 +257,22 @@ extern "C" fn kinit() {
 							as *mut cpu::KernelTrapFrame)
 							as usize,
 		);
+		cpu::sscratch_write(cpu::mscratch_read());
 		cpu::KERNEL_TRAP_FRAME[0].satp = satp_value;
-		cpu::KERNEL_TRAP_FRAME[0].trap_stack = page::zalloc(1);
+		// Move the stack pointer to the very bottom.
+		cpu::KERNEL_TRAP_FRAME[0].trap_stack = page::zalloc(1).add(4096);
 		id_map_range(
 	            &mut root,
+	            cpu::KERNEL_TRAP_FRAME[0].trap_stack as usize - 4096,
 	            cpu::KERNEL_TRAP_FRAME[0].trap_stack as usize,
-	            cpu::KERNEL_TRAP_FRAME[0].trap_stack as usize | 0xfff,
 	            page::EntryBits::ReadWrite.val(),
 		);
+		id_map_range(
+				&mut root,
+				cpu::mscratch_read(),
+				cpu::mscratch_read() | 0xfff,
+				page::EntryBits::ReadWrite.val(),
+		);
 		let p = cpu::KERNEL_TRAP_FRAME[0].trap_stack as usize;
 		let m = page::virt_to_phys(&root, p).unwrap_or(0);
 		println!("Walk 0x{:x} = 0x{:x}", p, m);
@@ -292,6 +300,8 @@ extern "C" fn kinit_hart(hartid: usize) {
 							as *mut cpu::KernelTrapFrame)
 							as usize,
 		);
+		cpu::sscratch_write(cpu::mscratch_read());
+		cpu::KERNEL_TRAP_FRAME[hartid].hartid = hartid;
 		// cpu::KERNEL_TRAP_FRAME[hartid].satp = cpu::KERNEL_TRAP_FRAME[0].satp;
 		// cpu::KERNEL_TRAP_FRAME[hartid].trap_stack = page::zalloc(1);
 	}
@@ -326,7 +336,9 @@ extern "C" fn kmain() {
 	unsafe {
 		let mtimecmp = 0x0200_4000 as *mut u64;
 		let mtime = 0x0200_bff8 as *const u64;
-		mtimecmp.write_volatile(mtime.read_volatile() + 10_000_000);
+		// mtimecmp.write_volatile(mtime.read_volatile() + 10_000_000);
+		let v = 0x0 as *mut u64;
+		v.write_volatile(0);
 	}
 	// If we get here, the Box, vec, and String should all be freed since
 	// they go out of scope. This calls their "Drop" trait.
diff --git a/risc_v/ch4/src/trap.rs b/risc_v/ch4/src/trap.rs
index fd75704..3f66c94 100755
--- a/risc_v/ch4/src/trap.rs
+++ b/risc_v/ch4/src/trap.rs
@@ -8,14 +8,9 @@ use crate::cpu::KernelTrapFrame;
 
 #[no_mangle]
 extern "C"
-fn s_trap(epc: usize, tval: usize, cause: isize) -> usize {
-	println!("STRAP (cause: 0x{:x} @ 0x{:x})", cause, epc);
-	if cause < 0 {
-		epc
-	}
-	else {
-		epc  + 4
-	}
+fn s_trap(epc: usize, tval: usize, cause: usize, hart: usize, stat: usize, frame: &mut KernelTrapFrame) -> usize {
+	println!("STRAP (cause: 0x{:x} @ 0x{:x}) [cpu: {}]", cause, epc, hart);
+	epc  + 4
 }
 
 #[no_mangle]
@@ -31,7 +26,8 @@ fn m_trap(epc: usize, tval: usize, cause: usize, hart: usize, stat: usize, frame
 			// println!("cause: {}", cause & 0xff);
 			// println!("Kernel table = 0x{:x}", KERNEL_TABLE);
 			// asm!("csrw satp, $0" :: "r"(satp) :: "volatile");
-			let mtimecmp = 0x0200_4000 as *mut u64;
+			let addr = 0x0200_4000 + hart * 8;
+			let mtimecmp = addr as *mut u64;
 			let mtime = 0x0200_bff8 as *const u64;
 			mtimecmp.write_volatile(mtime.read_volatile() + 10_000_000);
 			asm!("csrw sip, $0" ::"r"(2));