diff --git a/core/linux/libnx_vmem.cpp b/core/linux/libnx_vmem.cpp
index 0cbfa5cbc..9df38afff 100644
--- a/core/linux/libnx_vmem.cpp
+++ b/core/linux/libnx_vmem.cpp
@@ -266,72 +266,32 @@ void context_switch_aarch64(void* context);
 
 void __libnx_exception_handler(ThreadExceptionDump *ctx)
 {
-   mcontext_t m_ctx;
+	alignas(16) static ucontext_t u_ctx;
+	u_ctx.uc_mcontext.pc = ctx->pc.x;
+	for (int i = 0; i < 29; i++)
+		u_ctx.uc_mcontext.regs[i] = ctx->cpu_gprs[i].x;
 
-   m_ctx.pc = ctx->pc.x;
+	siginfo_t sig_info;
+	sig_info.si_addr = (void*)ctx->far.x;
 
-   for(int i=0; i<29; i++)
-   {
-      // printf("X%d: %p\n", i, ctx->cpu_gprs[i].x);
-      m_ctx.regs[i] = ctx->cpu_gprs[i].x;
-   }
+	fault_handler(0, &sig_info, (void *)&u_ctx);
 
-   /*
-   printf("PC: %p\n", ctx->pc.x);
-   printf("FP: %p\n", ctx->fp.x);
-   printf("LR: %p\n", ctx->lr.x);
-   printf("SP: %p\n", ctx->sp.x);
-   */
+	alignas(16) static uint64_t savedRegisters[66];
+	uint64_t *ptr = savedRegisters;
+	// fpu registers (64 bits)
+	for (int i = 0; i < 32; i++)
+		ptr[i]  = *(u64 *)&ctx->fpu_gprs[i].d;
+	// cpu gp registers
+	for (int i = 0; i < 29; i++)
+		ptr[i + 32]  = u_ctx.uc_mcontext.regs[i];
+	// Special regs
+	ptr[29 + 32] = ctx->fp.x;	// frame pointer
+	ptr[30 + 32] = ctx->lr.x;	// link register
+	ptr[31 + 32] = ctx->pstate;	// sprs
+	ptr[32 + 32] = ctx->sp.x;	// stack pointer
+	ptr[33 + 32] = u_ctx.uc_mcontext.pc; // PC
 
-   ucontext_t u_ctx;
-   u_ctx.uc_mcontext = m_ctx;
-
-   siginfo_t sig_info;
-
-   sig_info.si_addr = (void*)ctx->far.x;
-
-   fault_handler(0, &sig_info, (void*) &u_ctx);
-
-   uint64_t handle[64] = { 0 };
-
-   uint64_t *ptr = (uint64_t*)handle;
-   ptr[0]  = m_ctx.regs[0]; /* x0 0  */
-   ptr[1]  = m_ctx.regs[1]; /* x1 8 */
-   ptr[2]  = m_ctx.regs[2]; /* x2 16 */
-   ptr[3]  = m_ctx.regs[3]; /* x3 24 */
-   ptr[4]  = m_ctx.regs[4]; /* x4 32 */
-   ptr[5]  = m_ctx.regs[5]; /* x5 40 */
-   ptr[6]  = m_ctx.regs[6]; /* x6 48 */
-   ptr[7]  = m_ctx.regs[7]; /* x7 56 */
-   /* Non-volatiles.  */
-   ptr[8]  = m_ctx.regs[8]; /* x8 64 */
-   ptr[9]  = m_ctx.regs[9]; /* x9 72 */
-   ptr[10]  = m_ctx.regs[10]; /* x10 80 */
-   ptr[11]  = m_ctx.regs[11]; /* x11 88 */
-   ptr[12]  = m_ctx.regs[12]; /* x12 96 */
-   ptr[13]  = m_ctx.regs[13]; /* x13 104 */
-   ptr[14]  = m_ctx.regs[14]; /* x14 112 */
-   ptr[15]  = m_ctx.regs[15]; /* x15 120 */
-   ptr[16]  = m_ctx.regs[16]; /* x16 128 */
-   ptr[17]  = m_ctx.regs[17]; /* x17 136 */
-   ptr[18]  = m_ctx.regs[18]; /* x18 144 */
-   ptr[19]  = m_ctx.regs[19]; /* x19 152 */
-   ptr[20] = m_ctx.regs[20]; /* x20 160 */
-   ptr[21] = m_ctx.regs[21]; /* x21 168 */
-   ptr[22] = m_ctx.regs[22]; /* x22 176 */
-   ptr[23] = m_ctx.regs[23]; /* x23 184 */
-   ptr[24] = m_ctx.regs[24]; /* x24 192 */
-   ptr[25] = m_ctx.regs[25]; /* x25 200 */
-   ptr[26] = m_ctx.regs[26]; /* x26 208 */
-   ptr[27] = m_ctx.regs[27]; /* x27 216 */
-   ptr[28] = m_ctx.regs[28]; /* x28 224 */
-   /* Special regs */
-   ptr[29] = ctx->fp.x; /* frame pointer 232 */
-   ptr[30] = ctx->lr.x; /* link register 240 */
-   ptr[31] = ctx->sp.x; /* stack pointer 248 */
-   ptr[32] = (uintptr_t)ctx->pc.x; /* PC 256 */
-
-   context_switch_aarch64(ptr);
+	context_switch_aarch64(ptr);
 }
 }
 #endif	// TARGET_NO_EXCEPTIONS
diff --git a/shell/switch/context_switch.S b/shell/switch/context_switch.S
index 1e1c72d6b..77878bcbf 100644
--- a/shell/switch/context_switch.S
+++ b/shell/switch/context_switch.S
@@ -1,47 +1,46 @@
 .hidden context_switch_aarch64
 .globl context_switch_aarch64
 context_switch_aarch64:
-    // Setup SP on the Buffer
-    MOV SP, X0
+    // Restore FPU regs
+    ldp d0, d1, [x0, #0x00]
+    ldp d2, d3, [x0, #0x10]
+    ldp d4, d5, [x0, #0x20]
+    ldp d6, d7, [x0, #0x30]
+    ldp d8, d9, [x0, #0x40]
+    ldp d10, d11, [x0, #0x50]
+    ldp d12, d13, [x0, #0x60]
+    ldp d14, d15, [x0, #0x70]
+    ldp d16, d17, [x0, #0x80]
+    ldp d18, d19, [x0, #0x90]
+    ldp d20, d21, [x0, #0xa0]
+    ldp d22, d23, [x0, #0xb0]
+    ldp d24, d25, [x0, #0xc0]
+    ldp d26, d27, [x0, #0xd0]
+    ldp d28, d29, [x0, #0xe0]
+    ldp d30, d31, [x0, #0xf0]
 
-    // Restore Regs
-    LDR X0, [SP, #0]
-    LDR X1, [SP, #8]
-    LDR X2, [SP, #16]
-    LDR X3, [SP, #24]
-    LDR X4, [SP, #32]
-    LDR X5, [SP, #40]
-    LDR X6, [SP, #48]
-    LDR X7, [SP, #56]
-    LDR X8, [SP, #64]
-    LDR X9, [SP, #72]
-    LDR X10, [SP, #80]
-    LDR X11, [SP, #88]
-    LDR X12, [SP, #96]
-    LDR X13, [SP, #104]
-    LDR X14, [SP, #112]
-    LDR X15, [SP, #120]
-    LDR X16, [SP, #128]
-    LDR X17, [SP, #136]
-    LDR X18, [SP, #144]
-    LDR X19, [SP, #152]
-    LDR X20, [SP, #160]
-    LDR X21, [SP, #168]
-    LDR X22, [SP, #176]
-    LDR X23, [SP, #184]
-    LDR X24, [SP, #192]
-    LDR X25, [SP, #200]
-    LDR X26, [SP, #208]
-    LDR X27, [SP, #216]
-    LDR X28, [SP, #224]
-
-    // Restore FP, LR, SP
-    LDR FP, [SP, #232]
-    LDR LR, [SP, #240]
-    MOV X15, SP
-    LDR X17, [X15, #248]
-    MOV SP, X17
-
-    // Branch to PC
-    LDR X17, [X15, #256]
-    BR X17
+    // Restore GPRS regs
+    ldr x1, [x0, #0x108]
+    ldp x2, x3, [x0, #0x110]
+    ldp x4, x5, [x0, #0x120]
+    ldp x6, x7, [x0, #0x130]
+    ldp x8, x9, [x0, #0x140]
+    ldp x10, x11, [x0, #0x150]
+    ldp x12, x13, [x0, #0x160]
+    ldp x14, x15, [x0, #0x170]
+    ldp x16, x17, [x0, #0x180]
+    ldp x18, x19, [x0, #0x190]
+    ldp x20, x21, [x0, #0x1a0]
+    ldp x22, x23, [x0, #0x1b0]
+    ldp x24, x25, [x0, #0x1c0]
+    ldp x26, x27, [x0, #0x1d0]
+    ldp x28, x29, [x0, #0x1e0]	// x29 is FP
+    // Restore special regs and jump
+    // x16 and x17 are safe to use?
+    ldp x30, x17, [x0, #0x1f0]	// LR, pstate
+    msr nzcv, x17
+    ldr x17, [x0, #0x200]		// SP
+    mov sp, x17
+    ldr x17, [x0, #0x208]		// PC
+    ldr x0, [x0, #0x100]
+    br x17