diff --git a/hw/application_fpga/Makefile b/hw/application_fpga/Makefile
index 2a6d631..0aa80e4 100644
--- a/hw/application_fpga/Makefile
+++ b/hw/application_fpga/Makefile
@@ -127,7 +127,9 @@ FIRMWARE_OBJS = \
 	$(P)/fw/tk1/lib.o \
 	$(P)/fw/tk1/assert.o \
 	$(P)/fw/tk1/led.o \
-	$(P)/fw/tk1/blake2s/blake2s.o
+	$(P)/fw/tk1/blake2s/blake2s.o \
+	$(P)/fw/tk1/syscall_enable.o \
+	$(P)/fw/tk1/syscall_handler.o
 
 FIRMWARE_SOURCES = \
 	$(P)/fw/tk1/main.c \
@@ -135,7 +137,8 @@ FIRMWARE_SOURCES = \
 	$(P)/fw/tk1/lib.c \
 	$(P)/fw/tk1/assert.c \
 	$(P)/fw/tk1/led.c \
-	$(P)/fw/tk1/blake2s/blake2s.c
+	$(P)/fw/tk1/blake2s/blake2s.c \
+	$(P)/fw/tk1/syscall_handler.c
 
 TESTFW_OBJS = \
 	$(P)/fw/testfw/main.o \
diff --git a/hw/application_fpga/fw/tk1/Makefile b/hw/application_fpga/fw/tk1/Makefile
index 68b8261..34b6da8 100644
--- a/hw/application_fpga/fw/tk1/Makefile
+++ b/hw/application_fpga/fw/tk1/Makefile
@@ -1,5 +1,6 @@
 # Uses ../.clang-format
-FMTFILES=main.c lib.h lib.c proto.h proto.c types.h assert.c assert.h led.c led.h
+FMTFILES=*.[ch]
+
 .PHONY: fmt
 fmt:
 	clang-format --dry-run --ferror-limit=0 $(FMTFILES)
diff --git a/hw/application_fpga/fw/tk1/main.c b/hw/application_fpga/fw/tk1/main.c
index 85ed8ed..69a2376 100644
--- a/hw/application_fpga/fw/tk1/main.c
+++ b/hw/application_fpga/fw/tk1/main.c
@@ -9,6 +9,7 @@
 #include "lib.h"
 #include "proto.h"
 #include "state.h"
+#include "syscall_enable.h"
 #include "types.h"
 
 // clang-format off
@@ -343,13 +344,10 @@ static void run(const struct context *ctx)
 #endif
 	// clang-format on
 
-	// Flip over to application mode
-	*app_mode_ctrl = 1;
-
-	// XXX Firmware stack now no longer available
-	// Don't use any function calls!
+	syscall_enable();
 
 	// Jump to app - doesn't return
+	// Hardware is responsible for switching to app mode
 	// clang-format off
 #ifndef S_SPLINT_S
 	asm volatile(
diff --git a/hw/application_fpga/fw/tk1/picorv32/README.md b/hw/application_fpga/fw/tk1/picorv32/README.md
new file mode 100644
index 0000000..761be04
--- /dev/null
+++ b/hw/application_fpga/fw/tk1/picorv32/README.md
@@ -0,0 +1,7 @@
+# PicoRV32
+
+## custom_ops.S
+
+Custom PicoRV32 instructions are located in `custom_ops.S`.
+`custom_ops.S` is imported from upstream PicoRV32 commit:
+YosysHQ/picorv32@70f3c33
diff --git a/hw/application_fpga/fw/tk1/picorv32/custom_ops.S b/hw/application_fpga/fw/tk1/picorv32/custom_ops.S
new file mode 100644
index 0000000..71889b9
--- /dev/null
+++ b/hw/application_fpga/fw/tk1/picorv32/custom_ops.S
@@ -0,0 +1,102 @@
+// This is free and unencumbered software released into the public domain.
+//
+// Anyone is free to copy, modify, publish, use, compile, sell, or
+// distribute this software, either in source code form or as a compiled
+// binary, for any purpose, commercial or non-commercial, and by any
+// means.
+
+#define regnum_q0   0
+#define regnum_q1   1
+#define regnum_q2   2
+#define regnum_q3   3
+
+#define regnum_x0   0
+#define regnum_x1   1
+#define regnum_x2   2
+#define regnum_x3   3
+#define regnum_x4   4
+#define regnum_x5   5
+#define regnum_x6   6
+#define regnum_x7   7
+#define regnum_x8   8
+#define regnum_x9   9
+#define regnum_x10 10
+#define regnum_x11 11
+#define regnum_x12 12
+#define regnum_x13 13
+#define regnum_x14 14
+#define regnum_x15 15
+#define regnum_x16 16
+#define regnum_x17 17
+#define regnum_x18 18
+#define regnum_x19 19
+#define regnum_x20 20
+#define regnum_x21 21
+#define regnum_x22 22
+#define regnum_x23 23
+#define regnum_x24 24
+#define regnum_x25 25
+#define regnum_x26 26
+#define regnum_x27 27
+#define regnum_x28 28
+#define regnum_x29 29
+#define regnum_x30 30
+#define regnum_x31 31
+
+#define regnum_zero 0
+#define regnum_ra   1
+#define regnum_sp   2
+#define regnum_gp   3
+#define regnum_tp   4
+#define regnum_t0   5
+#define regnum_t1   6
+#define regnum_t2   7
+#define regnum_s0   8
+#define regnum_s1   9
+#define regnum_a0  10
+#define regnum_a1  11
+#define regnum_a2  12
+#define regnum_a3  13
+#define regnum_a4  14
+#define regnum_a5  15
+#define regnum_a6  16
+#define regnum_a7  17
+#define regnum_s2  18
+#define regnum_s3  19
+#define regnum_s4  20
+#define regnum_s5  21
+#define regnum_s6  22
+#define regnum_s7  23
+#define regnum_s8  24
+#define regnum_s9  25
+#define regnum_s10 26
+#define regnum_s11 27
+#define regnum_t3  28
+#define regnum_t4  29
+#define regnum_t5  30
+#define regnum_t6  31
+
+// x8 is s0 and also fp
+#define regnum_fp   8
+
+#define r_type_insn(_f7, _rs2, _rs1, _f3, _rd, _opc) \
+.word (((_f7) << 25) | ((_rs2) << 20) | ((_rs1) << 15) | ((_f3) << 12) | ((_rd) << 7) | ((_opc) << 0))
+
+#define picorv32_getq_insn(_rd, _qs) \
+r_type_insn(0b0000000, 0, regnum_ ## _qs, 0b100, regnum_ ## _rd, 0b0001011)
+
+#define picorv32_setq_insn(_qd, _rs) \
+r_type_insn(0b0000001, 0, regnum_ ## _rs, 0b010, regnum_ ## _qd, 0b0001011)
+
+#define picorv32_retirq_insn() \
+r_type_insn(0b0000010, 0, 0, 0b000, 0, 0b0001011)
+
+#define picorv32_maskirq_insn(_rd, _rs) \
+r_type_insn(0b0000011, 0, regnum_ ## _rs, 0b110, regnum_ ## _rd, 0b0001011)
+
+#define picorv32_waitirq_insn(_rd) \
+r_type_insn(0b0000100, 0, 0, 0b100, regnum_ ## _rd, 0b0001011)
+
+#define picorv32_timer_insn(_rd, _rs) \
+r_type_insn(0b0000101, 0, regnum_ ## _rs, 0b110, regnum_ ## _rd, 0b0001011)
+
diff --git a/hw/application_fpga/fw/tk1/start.S b/hw/application_fpga/fw/tk1/start.S
index 8f09a97..53c058a 100644
--- a/hw/application_fpga/fw/tk1/start.S
+++ b/hw/application_fpga/fw/tk1/start.S
@@ -1,11 +1,111 @@
 /*
- * Copyright (C) 2022, 2023 - Tillitis AB
+ * Copyright (C) 2022-2025 - Tillitis AB
  * SPDX-License-Identifier: GPL-2.0-only
 */
 
+#include "../tk1_mem.h"
+#include "picorv32/custom_ops.S"  // PicoRV32 custom instructions
+
+#define illegal_insn() .word 0
+
+	// Variables in bss
+	.lcomm irq_ret_addr, 4
+	.lcomm app_sp, 4
+
 	.section ".text.init"
 	.globl _start
 _start:
+	j init
+
+/*
+ * IRQ handler
+*/
+	.=0x10
+irq_handler:
+	// PicoRV32 stores the IRQ bitmask in x4.
+	// If bit 31 is 1: IRQ31 was triggered.
+	li t4, (1 << 31)
+	beq x4, t4, irq_source_ok
+unexpected_irq_source:
+	illegal_insn()
+	j unexpected_irq_source
+irq_source_ok:
+
+	// Save interrupt return address (x3)
+	la t0, irq_ret_addr
+	sw x3, 0(t0)
+
+	// Save app stack pointer. App is responsible for saving the rest of
+	// the registers.
+	la t0, app_sp
+	sw sp, 0(t0)
+
+	// Setup firmware stack pointer
+	la sp, _estack
+
+	// Run syscall handler
+	call syscall_handler
+
+	// Restore app stack pointer
+	la t0, app_sp
+	lw sp, 0(t0)
+
+	// Restore interrupt return address (x3)
+	la t0, irq_ret_addr
+	lw x3, 0(t0)
+
+	// Verify that interrupt return address (x3) is in app RAM
+	li t0, TK1_RAM_BASE // 0x40000000
+	blt x3, t0, x3_invalid
+	li t0, TK1_RAM_BASE + TK1_RAM_SIZE // 0x40020000
+	bge x3, t0, x3_invalid
+	j x3_valid
+x3_invalid:
+	illegal_insn()
+	j x3_invalid
+x3_valid:
+
+	// Remove data left over from the syscall handling
+	mv x0, zero
+	mv x1, zero
+	// x2 (sp) is assumed to be preserved by the interrupt handler
+	// x3 (interrupt return address) need to be preserved
+	mv x4, zero
+	mv x5, zero
+	mv x6, zero
+	mv x7, zero
+	mv x8, zero
+	mv x9, zero
+	// x10 (a0) contains syscall return value. And should not be destroyed.
+	mv x11, zero
+	mv x12, zero
+	mv x13, zero
+	mv x14, zero
+	mv x15, zero
+	mv x16, zero
+	mv x17, zero
+	mv x18, zero
+	mv x19, zero
+	mv x20, zero
+	mv x21, zero
+	mv x22, zero
+	mv x23, zero
+	mv x24, zero
+	mv x25, zero
+	mv x26, zero
+	mv x27, zero
+	mv x28, zero
+	mv x29, zero
+	mv x30, zero
+	mv x31, zero
+
+	picorv32_retirq_insn() // Return from interrupt
+
+/*
+ * Init
+*/
+	.=0x100
+init:
 	li x1, 0
 	li x2, 0
 	li x3, 0
diff --git a/hw/application_fpga/fw/tk1/syscall_enable.S b/hw/application_fpga/fw/tk1/syscall_enable.S
new file mode 100644
index 0000000..2bcd357
--- /dev/null
+++ b/hw/application_fpga/fw/tk1/syscall_enable.S
@@ -0,0 +1,12 @@
+#include "../tk1/picorv32/custom_ops.S"
+
+	.section ".text"
+	.globl syscall_enable
+
+
+syscall_enable:
+	/* Enable syscall IRQ */
+	li t0, 0x7fffffff // IRQ31 mask
+	picorv32_maskirq_insn(zero, t0) // Enable IRQs
+
+	ret
diff --git a/hw/application_fpga/fw/tk1/syscall_enable.h b/hw/application_fpga/fw/tk1/syscall_enable.h
new file mode 100644
index 0000000..4377d0d
--- /dev/null
+++ b/hw/application_fpga/fw/tk1/syscall_enable.h
@@ -0,0 +1,9 @@
+// Copyright (C) 2025 - Tillitis AB
+// SPDX-License-Identifier: GPL-2.0-only
+
+#ifndef TKEY_SYSCALL_ENABLE_H
+#define TKEY_SYSCALL_ENABLE_H
+
+void syscall_enable(void);
+
+#endif
diff --git a/hw/application_fpga/fw/tk1/syscall_handler.c b/hw/application_fpga/fw/tk1/syscall_handler.c
new file mode 100644
index 0000000..ce5bba3
--- /dev/null
+++ b/hw/application_fpga/fw/tk1/syscall_handler.c
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2025 - Tillitis AB
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+
+#include "../tk1/assert.h"
+#include "../tk1/led.h"
+#include "../tk1/syscall_num.h"
+#include "../tk1/types.h"
+
+// clang-format off
+static volatile uint32_t *system_reset = (volatile uint32_t *)TK1_MMIO_TK1_SYSTEM_RESET;
+// clang-format on
+
+int32_t syscall_handler(uint32_t number, uint32_t arg1)
+{
+	switch (number) {
+	case TK1_SYSCALL_RESET:
+		*system_reset = 1;
+		return 0;
+	case TK1_SYSCALL_SET_LED:
+		set_led(arg1);
+		return 0;
+	default:
+		assert(1 == 2);
+	}
+
+	assert(1 == 2);
+	return -1; // This should never run
+}
diff --git a/hw/application_fpga/fw/tk1/syscall_num.h b/hw/application_fpga/fw/tk1/syscall_num.h
new file mode 100644
index 0000000..609afdc
--- /dev/null
+++ b/hw/application_fpga/fw/tk1/syscall_num.h
@@ -0,0 +1,12 @@
+// Copyright (C) 2025 - Tillitis AB
+// SPDX-License-Identifier: GPL-2.0-only
+
+#ifndef TKEY_SYSCALL_NUM_H
+#define TKEY_SYSCALL_NUM_H
+
+enum syscall_num {
+	TK1_SYSCALL_RESET = 1,
+	TK1_SYSCALL_SET_LED = 10,
+};
+
+#endif