diff --git a/bdk/exception_handlers.S b/bdk/exception_handlers.S
index 97d1ec6..2f38bb3 100644
--- a/bdk/exception_handlers.S
+++ b/bdk/exception_handlers.S
@@ -97,6 +97,10 @@ _irq_setup:
 	MSR CPSR, #(MODE_IRQ | IRQ | FIQ) /* IRQ mode, IRQ/FIQ disabled */
 	LDR SP, =0x40040000
 
+	/* Setup FIQ stack pointer */
+	MSR CPSR, #(MODE_FIQ | IRQ | FIQ) /* FIQ mode, IRQ/FIQ disabled */
+	LDR SP, =0x40040000
+
 	/* Setup SYS stack pointer */
 	MSR CPSR, #(MODE_SYS | IRQ | FIQ) /* SYSTEM mode, IRQ/FIQ disabled */
 	LDR SP, =0x4003FF00               /* Will be changed later to DRAM */
@@ -111,7 +115,9 @@ _irq_setup:
 	B ipl_main
 	B .
 
-_reset:
+.globl excp_reset
+.type excp_reset, %function
+excp_reset:
 	LDR R0, =EXCP_EN_ADDR
 	LDR R1, =0x30505645 /* EVP0 */
 	STR R1, [R0]        /* EVP0 in EXCP_EN_ADDR */
@@ -129,25 +135,25 @@ _reset_handler:
 	LDR R0, =EXCP_TYPE_ADDR
 	LDR R1, =0x545352   /* RST */
 	STR R1, [R0]        /* RST in EXCP_TYPE_ADDR */
-	B _reset
+	B excp_reset
 
 _undefined_handler:
 	LDR R0, =EXCP_TYPE_ADDR
 	LDR R1, =0x464455   /* UDF */
 	STR R1, [R0]        /* UDF in EXCP_TYPE_ADDR */
-	B _reset
+	B excp_reset
 
 _prefetch_abort_handler:
 	LDR R0, =EXCP_TYPE_ADDR
 	LDR R1, =0x54424150 /* PABT */
 	STR R1, [R0]        /* PABT in EXCP_TYPE_ADDR */
-	B _reset
+	B excp_reset
 
 _data_abort_handler:
 	LDR R0, =EXCP_TYPE_ADDR
 	LDR R1, =0x54424144 /* DABT */
 	STR R1, [R0]        /* DABT in EXCP_TYPE_ADDR */
-	B _reset
+	B excp_reset
 
 .globl irq_enable_cpu_irq_exceptions
 .type irq_enable_cpu_irq_exceptions, %function
diff --git a/bdk/soc/irq.c b/bdk/soc/irq.c
index c512c87..b44e208 100644
--- a/bdk/soc/irq.c
+++ b/bdk/soc/irq.c
@@ -27,6 +27,7 @@
 //#define DPRINTF(...) gfx_printf(__VA_ARGS__)
 #define DPRINTF(...)
 
+extern void excp_reset();
 extern void irq_disable();
 extern void irq_enable_cpu_irq_exceptions();
 extern void irq_disable_cpu_irq_exceptions();
@@ -271,4 +272,14 @@ void  __attribute__ ((target("arm"), interrupt ("FIQ"))) fiq_handler()
 		len--;
 	}
 */
+#ifdef BDK_WATCHDOG_FIQ_ENABLE
+	// Set watchdog timeout status and disable WDT and its FIQ signal.
+	watchdog_handle();
+
+#ifdef BDK_RESTART_BL_ON_WDT
+	// Restart bootloader.
+	excp_reset();
+#endif
+
+#endif
 }