diff --git a/sw/device/bootrom/bootrom.c b/sw/device/bootrom/bootrom.c
index 0d8958674..aef1edc54 100644
--- a/sw/device/bootrom/bootrom.c
+++ b/sw/device/bootrom/bootrom.c
@@ -15,7 +15,7 @@
 #include <stdint.h>
 
 #define MAJOR "00"
-#define MINOR "01"
+#define MINOR "02"
 #define PATCH "00"
 
 const uintptr_t boot_slots[] = { 0x10000000, 0x80000000 };
@@ -56,6 +56,8 @@ int boot_main(void)
 
     timer_init(boot_ctx.timer);
     timer_enable_write(boot_ctx.timer, true);
+    led_init(boot_ctx.gpio);
+
     if (bootstrap_requested(&boot_ctx)) {
         uprintf(boot_ctx.console, "Entering SPI bootstrap\n");
         clear_slots(); // Cleaning slots from previeous boot.
@@ -107,8 +109,6 @@ bool get_boot_addr(uint32_t *addr)
 
 bool spi_boot_strap(struct boot_context *ctx)
 {
-    led_init(ctx->gpio);
-
     spi_device_t spid = mocha_system_spi_device();
     spi_device_init(spid);
     spi_device_enable_set(spid, true);
@@ -223,9 +223,37 @@ bool bootstrap_requested(struct boot_context *ctx)
     return false;
 }
 
-// TODO: Catch exceptions properly.
-void _trap_handler(struct trap_registers *registers, struct trap_context *context)
+static void _exception_handler(struct trap_registers *registers, struct trap_context *context)
 {
     (void)registers;
-    (void)context;
+
+    uart_t console = mocha_system_uart();
+    timer_t timer = mocha_system_timer();
+    gpio_t gpio = mocha_system_gpio();
+    uart_puts(console, "Exception!\n");
+    uprintf(console, "\tepc:   %lx\n", (unsigned long)context->epc);
+    uprintf(console, "\tcause: %lx\n", context->cause);
+    uprintf(console, "\ttval:  %lx\n", context->tval);
+    uprintf(console, "\ttval2: %lx\n", context->tval2);
+
+    uint32_t pattern = 0x55;
+
+    while (true) {
+        gpio_write(gpio, pattern);
+        pattern ^= 0xff;
+        uint64_t timeout = timer_value_read_us(timer) + led_animation_period_us / 2;
+        while (timer_value_read_us(timer) < timeout) {
+        };
+    };
+}
+void _trap_handler(struct trap_registers *registers, struct trap_context *context)
+{
+    if (context->cause & (1ul << 63)) {
+        /* trap cause is interrupt */
+        while (true) {
+        };
+    } else {
+        /* trap cause is exception */
+        _exception_handler(registers, context);
+    }
 }
diff --git a/sw/device/lib/hal/gpio.c b/sw/device/lib/hal/gpio.c
index 44f9dd8e1..99d66d675 100644
--- a/sw/device/lib/hal/gpio.c
+++ b/sw/device/lib/hal/gpio.c
@@ -20,10 +20,15 @@ bool gpio_read_pin(gpio_t gpio, uint32_t pin)
 void gpio_write_pin(gpio_t gpio, uint32_t pin, bool state)
 {
     uint32_t reg = pin < 16 ? GPIO_REG_MASKED_OUT_LOWER : GPIO_REG_MASKED_OUT_UPPER;
-    uint32_t mask = 1u << (pin & 0xFu);
+    uint32_t mask = 1u << ((pin % 16) & 0xFu);
     DEV_WRITE(gpio + reg, (mask << 16) | (state ? mask : 0u));
 }
 
+void gpio_write(gpio_t gpio, uint32_t val)
+{
+    DEV_WRITE(gpio + GPIO_REG_DIRECT_OUT, val);
+}
+
 void gpio_set_oe_pin(gpio_t gpio, uint32_t pin, bool output)
 {
     uint32_t reg = pin < 16 ? GPIO_REG_MASKED_OE_LOWER : GPIO_REG_MASKED_OE_UPPER;
diff --git a/sw/device/lib/hal/gpio.h b/sw/device/lib/hal/gpio.h
index e1511d93e..d3e4d0645 100644
--- a/sw/device/lib/hal/gpio.h
+++ b/sw/device/lib/hal/gpio.h
@@ -25,5 +25,8 @@ bool gpio_read_pin(gpio_t gpio, uint32_t pin);
 // Set the value of one output pin
 void gpio_write_pin(gpio_t gpio, uint32_t pin, bool state);
 
+// Write the value to the output port.
+void gpio_write(gpio_t gpio, uint32_t val);
+
 // Set the output enable state of one pin
 void gpio_set_oe_pin(gpio_t gpio, uint32_t pin, bool output);