diff --git a/libsel4platsupport/src/common.c b/libsel4platsupport/src/common.c
index eac1e4d5..ab82b6aa 100644
--- a/libsel4platsupport/src/common.c
+++ b/libsel4platsupport/src/common.c
@@ -32,8 +32,8 @@
 #include <simple-default/simple-default.h>
 #include <utils/util.h>
 
-#if defined(CONFIG_LIB_SEL4_PLAT_SUPPORT_USE_SEL4_DEBUG_PUTCHAR) && defined(CONFIG_DEBUG_BUILD)
-#define USE_DEBUG_PUTCHAR
+#if !(defined(CONFIG_LIB_SEL4_PLAT_SUPPORT_USE_SEL4_DEBUG_PUTCHAR) || defined(CONFIG_DEBUG_BUILD))
+#define USE_DEBUG_HARDWARE
 #endif
 
 enum serial_setup_status {
@@ -45,13 +45,15 @@ enum serial_setup_status {
 
 typedef struct {
     enum serial_setup_status setup_status;
-#ifndef USE_DEBUG_PUTCHAR
+#if USE_DEBUG_HARDWARE
     seL4_CPtr device_cap;
+    ps_io_ops_t io_ops;
     vspace_t *vspace;
-    simple_t *simple;
     vka_t *vka;
-#endif /* not USE_DEBUG_PUTCHAR */
-
+    /* To keep failsafe setup we need actual memory for a simple and a vka */
+    simple_t simple_mem;
+    vka_t vka_mem;
+#endif /* USE_DEBUG_HARDWARE */
 } ctx_t;
 
 /* Some globals for tracking initialisation variables. This is currently just to avoid
@@ -61,13 +63,9 @@ static ctx_t ctx = {
     .setup_status = NOT_INITIALIZED
 };
 
-/* To keep failsafe setup we need actual memory for a simple and a vka */
-static simple_t _simple_mem;
-static vka_t _vka_mem;
-
 extern char __executable_start[];
 
-#ifndef USE_DEBUG_PUTCHAR
+#if USE_DEBUG_HARDWARE
 
 static void *__map_device_page(void *cookie, uintptr_t paddr, size_t size,
                                int cached, ps_mem_flags_t flags)
@@ -121,14 +119,18 @@ static void *__map_device_page(void *cookie, uintptr_t paddr, size_t size,
     abort();
 }
 
-static ps_io_ops_t io_ops = {
-    .io_mapper = {
-        .io_map_fn = &__map_device_page,
-        .io_unmap_fn = NULL,
-    },
-};
+static int __setup_io_ops(
+    simple_t *simple __attribute__((unused)))
+{
+    ctx.io_ops.io_map_fn = &__map_device_page;
 
-#endif /* !USE_DEBUG_PUTCHAR */
+#ifdef CONFIG_ARCH_X86
+    sel4platsupport_get_io_port_ops(&ctx.io_ops.io_port_ops, simple, ctx.vka);
+#endif
+    return platsupport_serial_setup_io_ops(&ctx.io_ops);
+}
+
+#endif /* USE_DEBUG_HARDWARE */
 
 /*
  * This function is designed to be called when creating a new cspace/vspace,
@@ -138,7 +140,7 @@ void platsupport_undo_serial_setup(void)
 {
     /* Re-initialise some structures. */
     ctx.setup_status = NOT_INITIALIZED;
-#ifndef USE_DEBUG_PUTCHAR
+#if USE_DEBUG_HARDWARE
     if (ctx.device_cap) {
         cspacepath_t path;
         seL4_ARCH_Page_Unmap(ctx.device_cap);
@@ -148,7 +150,7 @@ void platsupport_undo_serial_setup(void)
         ctx.device_cap = 0;
     }
     ctx.vka = NULL;
-#endif /* not USE_DEBUG_PUTCHAR */    
+#endif /* USE_DEBUG_HARDWARE */
 }
 
 /* Initialise serial input interrupt. */
@@ -171,62 +173,58 @@ int platsupport_serial_setup_io_ops(ps_io_ops_t *io_ops)
 
 int platsupport_serial_setup_bootinfo_failsafe(void)
 {
-    int err = 0;
     if (ctx.setup_status == SETUP_COMPLETE) {
         return 0;
     }
-    memset(&_simple_mem, 0, sizeof(simple_t));
-    memset(&_vka_mem, 0, sizeof(vka_t));
-#ifdef USE_DEBUG_PUTCHAR
+
+#ifdef USE_DEBUG_HARDWARE
+    ctx.setup_status = START_FAILSAFE_SETUP;
+
+    simple_t *simple = &(ctx.simple_mem);
+    memset(simple, 0, sizeof(*simple));
+
+    vka_t *vka = &(ctx.vka_mem);
+    memset(vka, 0, sizeof(*vka));
+
+    simple_default_init_bootinfo(simple, platsupport_get_bootinfo());
+    simple_make_vka(simple, vka);
+    ctx.vka = vka;
+
+    return __setup_io_ops(simple);
+#else /* not USE_DEBUG_HARDWARE */
     /* only support putchar on a debug kernel */
     ctx.setup_status = SETUP_COMPLETE;
-#else /* not USE_DEBUG_PUTCHAR */
-    ctx.setup_status = START_FAILSAFE_SETUP;
-    simple_default_init_bootinfo(&_simple_mem, platsupport_get_bootinfo());
-    ctx.simple = &_simple_mem;
-    ctx.vka = &_vka_mem;
-    simple_make_vka(ctx.simple, ctx.vka);
-#ifdef CONFIG_ARCH_X86
-    sel4platsupport_get_io_port_ops(&io_ops.io_port_ops, ctx.simple, ctx.vka);
-#endif
-    err = platsupport_serial_setup_io_ops(&io_ops);
-#endif /* [not] USE_DEBUG_PUTCHAR */
-    return err;
+    return 0;
+#endif /* [not] USE_DEBUG_HARDWARE */
 }
 
 int platsupport_serial_setup_simple(
-    vspace_t *_vspace __attribute__((unused)),
-    simple_t *_simple __attribute__((unused)),
-    vka_t *_vka __attribute__((unused)))
+    vspace_t *vspace __attribute__((unused)),
+    simple_t *simple __attribute__((unused)),
+    vka_t *vka __attribute__((unused)))
 {
-    int err = 0;
-    if (ctx.setup_status == SETUP_COMPLETE) {
+    switch (ctx.setup_status) {
+    case SETUP_COMPLETE:
         return 0;
-    }
-    if (ctx.setup_status != NOT_INITIALIZED) {
+    case NOT_INITIALIZED:
+        break;
+    default:
         ZF_LOGE("Trying to initialise a partially initialised serial. Current setup status is %d\n", ctx.setup_status);
         assert(!"You cannot recover");
         return -1;
     }
-#ifdef USE_DEBUG_PUTCHAR
-    /* only support putchar on a debug kernel */
-    ctx.setup_status = SETUP_COMPLETE;
-#else /* not USE_DEBUG_PUTCHAR */
+
+#ifdef USE_DEBUG_HARDWARE
     /* start setup */
     ctx.setup_status = START_REGULAR_SETUP;
-    ctx.vspace = _vspace;
-    ctx.simple = _simple;
-    ctx.vka = _vka;
-#ifdef CONFIG_ARCH_X86
-    sel4platsupport_get_io_port_ops(&io_ops.io_port_ops, ctx.simple, ctx.vka);
-#endif
-    err = platsupport_serial_setup_io_ops(&io_ops);
-    /* done */
-    ctx.vspace = NULL;
-    ctx.simple = NULL;
-    /* Don't reset vka here */
-#endif /* [not] USE_DEBUG_PUTCHAR */
-    return err;
+    ctx.vspace = vspace;
+    ctx.vka = vka;
+    return __setup_io_ops(simple); /* uses global variables simple and vka */
+#else /* not USE_DEBUG_HARDWARE */
+    /* only support putchar on a debug kernel */
+    ctx.setup_status = SETUP_COMPLETE;
+    return 0;
+#endif /* [not] USE_DEBUG_HARDWARE */
 }
 
 /* This function is called if an attempt for serial I/O is done before it has