diff --git a/Makefile b/Makefile
index 54b292a..7be1e42 100644
--- a/Makefile
+++ b/Makefile
@@ -23,7 +23,7 @@ else
   export LDFLAGS = -Wl,--gc-sections
 endif
 LDFLAGS += $(LIBCBOR)
-CFLAGS = -O2 -fdata-sections -ffunction-sections 
+CFLAGS = -O2 -fdata-sections -ffunction-sections
 
 INCLUDES = -I./tinycbor/src -I./crypto/sha256 -I./crypto/micro-ecc/ -Icrypto/tiny-AES-c/ -I./fido2/ -I./pc -I./fido2/extensions
 
diff --git a/fido2/crypto.c b/fido2/crypto.c
index 9c12921..9248644 100644
--- a/fido2/crypto.c
+++ b/fido2/crypto.c
@@ -104,7 +104,7 @@ void crypto_sha256_hmac_init(uint8_t * key, uint32_t klen, uint8_t * hmac)
 
     if(klen > 64)
     {
-        printf("Error, key size must be <= 64\n");
+        printf2(TAG_ERR,"Error, key size must be <= 64\n");
         exit(1);
     }
 
@@ -134,7 +134,7 @@ void crypto_sha256_hmac_final(uint8_t * key, uint32_t klen, uint8_t * hmac)
 
     if(klen > 64)
     {
-        printf("Error, key size must be <= 64\n");
+        printf2(TAG_ERR,"Error, key size must be <= 64\n");
         exit(1);
     }
     memmove(buf, key, klen);
@@ -168,7 +168,7 @@ void crypto_ecc256_sign(uint8_t * data, int len, uint8_t * sig)
 {
     if ( uECC_sign(_signing_key, data, len, sig, _es256_curve) == 0)
     {
-        printf("error, uECC failed\n");
+        printf2(TAG_ERR,"error, uECC failed\n");
         exit(1);
     }
 }
@@ -205,19 +205,19 @@ void crypto_ecdsa_sign(uint8_t * data, int len, uint8_t * sig, int MBEDTLS_ECP_I
             if (_key_len != 32)  goto fail;
             break;
         default:
-            printf("error, invalid ECDSA alg specifier\n");
+            printf2(TAG_ERR,"error, invalid ECDSA alg specifier\n");
             exit(1);
     }
 
     if ( uECC_sign(_signing_key, data, len, sig, curve) == 0)
     {
-        printf("error, uECC failed\n");
+        printf2(TAG_ERR,"error, uECC failed\n");
         exit(1);
     }
     return;
 
 fail:
-    printf("error, invalid key length\n");
+    printf2(TAG_ERR,"error, invalid key length\n");
     exit(1);
 
 }
@@ -257,7 +257,7 @@ void crypto_ecc256_make_key_pair(uint8_t * pubkey, uint8_t * privkey)
 {
     if (uECC_make_key(pubkey, privkey, _es256_curve) != 1)
     {
-        printf("Error, uECC_make_key failed\n");
+        printf2(TAG_ERR,"Error, uECC_make_key failed\n");
         exit(1);
     }
 }
@@ -266,7 +266,7 @@ void crypto_ecc256_shared_secret(const uint8_t * pubkey, const uint8_t * privkey
 {
     if (uECC_shared_secret(pubkey, privkey, shared_secret, _es256_curve) != 1)
     {
-        printf("Error, uECC_shared_secret failed\n");
+        printf2(TAG_ERR,"Error, uECC_shared_secret failed\n");
         exit(1);
     }
 
diff --git a/fido2/ctaphid.c b/fido2/ctaphid.c
index 2a30b28..86ec845 100644
--- a/fido2/ctaphid.c
+++ b/fido2/ctaphid.c
@@ -616,7 +616,6 @@ uint8_t ctaphid_handle_packet(uint8_t * pkt_raw)
             break;
 #endif
 #ifndef DISABLE_CTAPHID_CBOR
-#error rere
         case CTAPHID_CBOR:
             printf1(TAG_HID,"CTAPHID_CBOR\n");
 
diff --git a/fido2/device.h b/fido2/device.h
index 027e7de..536ab85 100644
--- a/fido2/device.h
+++ b/fido2/device.h
@@ -44,6 +44,7 @@ void usbhid_close();
 void main_loop_delay();
 
 void heartbeat();
+void bootloader_heartbeat();
 
 void authenticator_read_state(AuthenticatorState * );
 
@@ -62,6 +63,9 @@ void device_manage();
 // A timer should be set up to call `ctaphid_update_status`
 void device_set_status(int status);
 
+// Returns if button is currently pressed
+int device_is_button_pressed();
+
 // Test for user presence
 // Return 1 for user is present, 0 user not present, -1 if cancel is requested.
 extern int ctap_user_presence_test();
@@ -93,4 +97,10 @@ void ctap_overwrite_rk(int index,CTAP_residentKey * rk);
 // Boot laoder application
 int bootloader_bridge(uint8_t klen, uint8_t * keyh);
 
+// Trigger software reset
+void device_reboot();
+
+// for bootloader
+int is_authorized_to_boot();
+
 #endif
diff --git a/fido2/extensions/extensions.c b/fido2/extensions/extensions.c
index dd535be..00accea 100644
--- a/fido2/extensions/extensions.c
+++ b/fido2/extensions/extensions.c
@@ -100,7 +100,7 @@ int16_t extend_u2f(struct u2f_request_apdu* req, uint32_t len)
             }
             else
             {
-                rcode =  U2F_SW_WRONG_DATA;
+                rcode =  U2F_SW_WRONG_DATA; 
             }
             printf1(TAG_WALLET,"Ignoring U2F request\n");
             goto end;
diff --git a/fido2/extensions/wallet.c b/fido2/extensions/wallet.c
index 9241a9b..0a4ce9f 100644
--- a/fido2/extensions/wallet.c
+++ b/fido2/extensions/wallet.c
@@ -312,8 +312,6 @@ int16_t bridge_u2f_to_wallet(uint8_t * _chal, uint8_t * _appid, uint8_t klen, ui
             memmove(chksum, args[0] + lens[0] - 4, 4);
             lens[0] -= 4;
 
-            /*printf("chksum: "); dump_hex1(TAG_WALLET, chksum, 4);*/
-
             // perform integrity check
             /*printf1(TAG_WALLET,"shasum on [%d]: ",lens[0]); dump_hex1(TAG_WALLET, args[0], lens[0]);*/
             crypto_sha256_init();
diff --git a/fido2/main.c b/fido2/main.c
index bb09ac0..d864845 100644
--- a/fido2/main.c
+++ b/fido2/main.c
@@ -76,7 +76,7 @@ int main(int argc, char * argv[])
     {
         if (millis() - t1 > 100)
         {
-            /*printf("heartbeat %ld\n", beat++);*/
+            /*printf1(TAG_GEN,"heartbeat %ld\n", beat++);*/
             heartbeat();
             t1 = millis();
         }
diff --git a/fido2/stubs.c b/fido2/stubs.c
index 4897530..c70ac8e 100644
--- a/fido2/stubs.c
+++ b/fido2/stubs.c
@@ -31,18 +31,18 @@
 
 void ctap_init()
 {
-    printf("STUB: ctap_init\n");
+    printf1(TAG_GEN,"STUB: ctap_init\n");
 }
 #endif
 
 #if defined(STUB_CTAPHID)
 void ctaphid_init()
 {
-    printf("STUB: ctaphid_init\n");
+    printf1(TAG_GEN,"STUB: ctaphid_init\n");
 }
 void ctaphid_handle_packet(uint8_t * hidmsg)
 {
-    printf("STUB: ctaphid_handle_packet\n");
+    printf1(TAG_GEN,"STUB: ctaphid_handle_packet\n");
 }
 
 void ctaphid_check_timeouts()
@@ -57,7 +57,7 @@ void ctaphid_check_timeouts()
 
 void ctap_reset_state()
 {
-    printf("STUB: ctap_reset_state\n");
+    printf1(TAG_GEN,"STUB: ctap_reset_state\n");
 }
 
 void ctap_response_init(CTAP_RESPONSE * resp)
@@ -66,12 +66,12 @@ void ctap_response_init(CTAP_RESPONSE * resp)
 
 void u2f_request(struct u2f_request_apdu* req, CTAP_RESPONSE * resp)
 {
-    printf("STUB: u2f_request\n");
+    printf1(TAG_GEN,"STUB: u2f_request\n");
 }
 
 uint8_t ctap_request(uint8_t * pkt_raw, int length, CTAP_RESPONSE * resp)
 {
-    printf("STUB: ctap_request\n");
+    printf1(TAG_GEN,"STUB: ctap_request\n");
     return 0;
 }
 #endif
diff --git a/fido2/util.c b/fido2/util.c
index 5394b74..0d6f9bb 100644
--- a/fido2/util.c
+++ b/fido2/util.c
@@ -22,6 +22,7 @@
 #include <stdint.h>
 #include <stdio.h>
 
+#if DEBUG_LEVEL
 void dump_hex(uint8_t * buf, int size)
 {
     while(size--)
@@ -30,5 +31,9 @@ void dump_hex(uint8_t * buf, int size)
     }
     printf("\n");
 }
+#else
+void dump_hex(uint8_t * buf, int size)
+{
+}
 
-
+#endif
diff --git a/targets/stm32l442/Makefile b/targets/stm32l442/Makefile
index 5c13eac..940d00c 100644
--- a/targets/stm32l442/Makefile
+++ b/targets/stm32l442/Makefile
@@ -71,12 +71,16 @@ bootloader:
 	make -f bootloader.mk
 
 clean:
-	rm -f *.o src/*.o src/*.elf *.elf *.hex $(OBJ)
+	rm -f *.o src/*.o src/*.elf  bootloader/*.o $(OBJ)
 
 flash: $(TARGET).hex
-	STM32_Programmer_CLI -c port=SWD -halt -e all --readunprotect
+	# STM32_Programmer_CLI -c port=SWD -halt -e all --readunprotect
 	STM32_Programmer_CLI -c port=SWD -halt  -d $(TARGET).hex -rst
 
+flashall:
+	# STM32_Programmer_CLI -c port=SWD -halt -e all --readunprotect
+	STM32_Programmer_CLI -c port=SWD -halt  -d all.hex -rst
+
 detach:
 	STM32_Programmer_CLI -c port=usb1 -ob nBOOT0=1
 
diff --git a/targets/stm32l442/bootloader.mk b/targets/stm32l442/bootloader.mk
index 409f58f..b7c2b8b 100644
--- a/targets/stm32l442/bootloader.mk
+++ b/targets/stm32l442/bootloader.mk
@@ -24,11 +24,11 @@ INC = -Ibootloader/ -Isrc/ -Isrc/cmsis/ -Ilib/ -Ilib/usbd/ -I../../fido2/ -I../.
 INC += -I../../tinycbor/src -I../../crypto/sha256 -I../../crypto/micro-ecc
 INC += -I../../crypto/tiny-AES-c
 
-LDSCRIPT=stm32l432xx.ld
+LDSCRIPT=bootloader_stm32l4xx.ld
 
 CFLAGS= $(INC)
 
-TARGET=solo
+TARGET=bootloader
 HW=-mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -mthumb
 
 # Nucleo board
@@ -39,7 +39,7 @@ CHIP=STM32L442xx
 DEFINES = -D$(CHIP) -DAES256=1  -DUSE_FULL_LL_DRIVER -DAPP_CONFIG=\"bootloader.h\"
 # DEFINES += -DTEST_SOLO_STM32 -DTEST -DTEST_FIFO=1
 
-CFLAGS=$(INC) -c $(DEFINES)   -Wall -fdata-sections -ffunction-sections $(HW)
+CFLAGS=$(INC) -c $(DEFINES)   -Wall -fdata-sections -ffunction-sections $(HW) -g
 LDFLAGS_LIB=$(HW) $(SEARCH) -specs=nano.specs  -specs=nosys.specs  -Wl,--gc-sections  -lnosys
 LDFLAGS=$(HW) $(LDFLAGS_LIB) -T$(LDSCRIPT) -Wl,-Map=$(TARGET).map,--cref
 
@@ -65,7 +65,7 @@ all: $(TARGET).elf
 	$(CP) -O ihex $^ $(TARGET).hex
 
 clean:
-	rm -f *.o src/*.o src/*.elf *.elf *.hex $(OBJ)
+	rm -f *.o src/*.o bootloader/*.o src/*.elf  $(OBJ)
 
 flash: $(TARGET).hex
 	STM32_Programmer_CLI -c port=SWD -halt -e all --readunprotect
diff --git a/targets/stm32l442/bootloader/bootloader.h b/targets/stm32l442/bootloader/bootloader.h
index 98d5fb0..bfd78aa 100644
--- a/targets/stm32l442/bootloader/bootloader.h
+++ b/targets/stm32l442/bootloader/bootloader.h
@@ -4,7 +4,7 @@
 
 #define DEBUG_UART      USART1
 
-#define DEBUG_LEVEL        0
+#define DEBUG_LEVEL        1
 
 #define NON_BLOCK_PRINTING 0
 
@@ -16,6 +16,8 @@
 #define ENABLE_U2F_EXTENSIONS
 // #define ENABLE_U2F
 
+#define APPLICATION_JUMP_ADDR               (0x08000000 + 32 * 2048)
+
 #define DISABLE_CTAPHID_PING
 #define DISABLE_CTAPHID_WINK
 #define DISABLE_CTAPHID_CBOR
@@ -26,7 +28,7 @@ void hw_init(void);
 //#define TEST
 //#define TEST_POWER
 
-#define LED_INIT_VALUE			0x001000
+#define LED_INIT_VALUE			0x101000
 
 // Button
 #define SOLO_BUTTON_PORT        GPIOA
diff --git a/targets/stm32l442/bootloader/main.c b/targets/stm32l442/bootloader/main.c
index 9139f14..24b6af1 100644
--- a/targets/stm32l442/bootloader/main.c
+++ b/targets/stm32l442/bootloader/main.c
@@ -32,18 +32,62 @@
 #include "ctap.h"
 #include "app.h"
 
+#include "stm32l4xx.h"
+
 uint8_t REBOOT_FLAG = 0;
 
-#if !defined(TEST)
+
+#if defined ( __CC_ARM   )
+__asm void BOOT_jump(uint32_t sp, uint32_t pc)
+{
+  /* Set new MSP, PSP based on SP (r0)*/
+  msr msp, r0
+  msr psp, r0
+
+  /* Jump to PC (r1)*/
+  bx r1
+}
+#else
+void __attribute__((optimize("O0"))) BOOT_jump(uint32_t sp, uint32_t pc)
+{
+  (void) sp;
+  (void) pc;
+  /* Set new MSP, PSP based on SP (r0)*/
+  __asm("msr msp, r0");
+  __asm("msr psp, r0");
+
+  /* Jump to PC (r1)*/
+  __asm("mov pc, r1");
+}
+#endif
+
+
+void __attribute__((optimize("O0"))) BOOT_boot(void)
+{
+  uint32_t pc, sp;
+
+  uint32_t *bootAddress = (uint32_t *)(APPLICATION_JUMP_ADDR);
+
+  /* Set new vector table */
+  SCB->VTOR = (uint32_t)bootAddress;
+
+  /* Read new SP and PC from vector table */
+  sp = bootAddress[0];
+  pc = bootAddress[1];
+
+  /* Do a jump by loading the PC and SP into the CPU registers */
+  BOOT_jump(sp, pc);
+}
 
 int main(int argc, char * argv[])
 {
     uint8_t hidmsg[64];
     uint32_t t1 = 0;
+    uint32_t boot = 1;
 
     set_logging_mask(
             /*0*/
-           // TAG_GEN|
+            TAG_GEN|
             // TAG_MC |
             // TAG_GA |
             // TAG_WALLET |
@@ -63,11 +107,28 @@ int main(int argc, char * argv[])
     device_init();
     printf1(TAG_GEN,"init device\n");
 
+    t1 = millis();
+    while(device_is_button_pressed())
+    {
+        if ((millis() - t1) > 2000)
+        {
+            boot = 0;
+            break;
+        }
+    }
+
+    if (boot && is_authorized_to_boot())
+    {
+        BOOT_boot();
+    }
+    else
+    {
+        printf1(TAG_RED,"Not authorized to boot\r\n");
+    }
+
     printf1(TAG_GEN,"init ctaphid\n");
     ctaphid_init();
 
-    printf1(TAG_GEN,"init ctap\n");
-    ctap_init();
 
     memset(hidmsg,0,sizeof(hidmsg));
 
@@ -76,10 +137,9 @@ int main(int argc, char * argv[])
 
     while(1)
     {
-        if (millis() - t1 > 100)
+        if (millis() - t1 > 8)
         {
-            /*printf("heartbeat %ld\n", beat++);*/
-            heartbeat();
+            bootloader_heartbeat();
             t1 = millis();
         }
 
@@ -94,6 +154,11 @@ int main(int argc, char * argv[])
         {
         }
         ctaphid_check_timeouts();
+
+        if (REBOOT_FLAG)
+        {
+            device_reboot();
+        }
     }
 
     // Should never get here
@@ -101,5 +166,3 @@ int main(int argc, char * argv[])
     printf1(TAG_GREEN, "done\n");
     return 0;
 }
-
-#endif
diff --git a/targets/stm32l442/bootloader_stm32l4xx.ld b/targets/stm32l442/bootloader_stm32l4xx.ld
new file mode 100644
index 0000000..59a0907
--- /dev/null
+++ b/targets/stm32l442/bootloader_stm32l4xx.ld
@@ -0,0 +1,201 @@
+/*
+*****************************************************************************
+**
+
+**  File        : LinkerScript.ld
+**
+**  Abstract    : Linker script for STM32L432KCUx Device with
+**                256KByte FLASH, 64KByte RAM
+**
+**                Set heap size, stack size and stack location according
+**                to application requirements.
+**
+**                Set memory bank area and size if external memory is used.
+**
+**  Target      : STMicroelectronics STM32
+**
+**
+**  Distribution: The file is distributed as is, without any warranty
+**                of any kind.
+**
+**  (c)Copyright Ac6.
+**  You may use this file as-is or modify it according to the needs of your
+**  project. Distribution of this file (unmodified or modified) is not
+**  permitted. Ac6 permit registered System Workbench for MCU users the
+**  rights to distribute the assembled, compiled & linked contents of this
+**  file as part of an application binary file, provided that it is built
+**  using the System Workbench for MCU toolchain.
+**
+*****************************************************************************
+*/
+
+/* Entry Point */
+ENTRY(Reset_Handler)
+
+/* Highest address of the user mode stack */
+_estack = 0x2000c000;    /* end of RAM */
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0x200;      /* required amount of heap  */
+_Min_Stack_Size = 0x400; /* required amount of stack */
+
+/* Specify the memory areas */
+MEMORY
+{
+FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 32K
+RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 48K
+SRAM2 (rw)      : ORIGIN = 0x10000000, LENGTH = 16K
+}
+
+/* Define output sections */
+SECTIONS
+{
+  /* The startup code goes first into FLASH */
+  .isr_vector :
+  {
+    . = ALIGN(8);
+    KEEP(*(.isr_vector)) /* Startup code */
+    . = ALIGN(8);
+  } >FLASH
+
+  /* The program code and other data goes into FLASH */
+  .text :
+  {
+    . = ALIGN(8);
+    *(.text)           /* .text sections (code) */
+    *(.text*)          /* .text* sections (code) */
+    *(.glue_7)         /* glue arm to thumb code */
+    *(.glue_7t)        /* glue thumb to arm code */
+    *(.eh_frame)
+
+    KEEP (*(.init))
+    KEEP (*(.fini))
+
+    . = ALIGN(8);
+    _etext = .;        /* define a global symbols at end of code */
+  } >FLASH
+
+  /* Constant data goes into FLASH */
+  .rodata :
+  {
+    . = ALIGN(8);
+    *(.rodata)         /* .rodata sections (constants, strings, etc.) */
+    *(.rodata*)        /* .rodata* sections (constants, strings, etc.) */
+    . = ALIGN(8);
+  } >FLASH
+
+  .ARM.extab   :
+  {
+  . = ALIGN(8);
+  *(.ARM.extab* .gnu.linkonce.armextab.*)
+  . = ALIGN(8);
+  } >FLASH
+  .ARM : {
+	. = ALIGN(8);
+    __exidx_start = .;
+    *(.ARM.exidx*)
+    __exidx_end = .;
+	. = ALIGN(8);
+  } >FLASH
+
+  .preinit_array     :
+  {
+	. = ALIGN(8);
+    PROVIDE_HIDDEN (__preinit_array_start = .);
+    KEEP (*(.preinit_array*))
+    PROVIDE_HIDDEN (__preinit_array_end = .);
+	. = ALIGN(8);
+  } >FLASH
+
+  .init_array :
+  {
+	. = ALIGN(8);
+    PROVIDE_HIDDEN (__init_array_start = .);
+    KEEP (*(SORT(.init_array.*)))
+    KEEP (*(.init_array*))
+    PROVIDE_HIDDEN (__init_array_end = .);
+	. = ALIGN(8);
+  } >FLASH
+  .fini_array :
+  {
+	. = ALIGN(8);
+    PROVIDE_HIDDEN (__fini_array_start = .);
+    KEEP (*(SORT(.fini_array.*)))
+    KEEP (*(.fini_array*))
+    PROVIDE_HIDDEN (__fini_array_end = .);
+	. = ALIGN(8);
+  } >FLASH
+
+  /* used by the startup to initialize data */
+  _sidata = LOADADDR(.data);
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data :
+  {
+    . = ALIGN(8);
+    _sdata = .;        /* create a global symbol at data start */
+    *(.data)           /* .data sections */
+    *(.data*)          /* .data* sections */
+
+    . = ALIGN(8);
+    _edata = .;        /* define a global symbol at data end */
+  } >RAM AT> FLASH
+
+  _sisram2 = LOADADDR(.sram2);
+
+  /* CCM-RAM section
+  *
+  * IMPORTANT NOTE!
+  * If initialized variables will be placed in this section,
+  * the startup code needs to be modified to copy the init-values.
+  */
+  .sram2 :
+  {
+    . = ALIGN(8);
+    _ssram2 = .;       /* create a global symbol at sram2 start */
+    *(.sram2)
+    *(.sram2*)
+
+    . = ALIGN(8);
+    _esram2 = .;       /* create a global symbol at sram2 end */
+  } >SRAM2 AT> FLASH
+
+
+  /* Uninitialized data section */
+  . = ALIGN(4);
+  .bss :
+  {
+    /* This is used by the startup in order to initialize the .bss secion */
+    _sbss = .;         /* define a global symbol at bss start */
+    __bss_start__ = _sbss;
+    *(.bss)
+    *(.bss*)
+    *(COMMON)
+
+    . = ALIGN(4);
+    _ebss = .;         /* define a global symbol at bss end */
+    __bss_end__ = _ebss;
+  } >RAM
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  ._user_heap_stack :
+  {
+    . = ALIGN(8);
+    PROVIDE ( end = . );
+    PROVIDE ( _end = . );
+    . = . + _Min_Heap_Size;
+    . = . + _Min_Stack_Size;
+    . = ALIGN(8);
+  } >RAM
+
+
+
+  /* Remove information from the standard libraries */
+  /DISCARD/ :
+  {
+    libc.a ( * )
+    libm.a ( * )
+    libgcc.a ( * )
+  }
+
+  .ARM.attributes 0 : { *(.ARM.attributes) }
+}
diff --git a/targets/stm32l442/lib/usbd/usbd_hid.c b/targets/stm32l442/lib/usbd/usbd_hid.c
index 0cc9512..905480d 100644
--- a/targets/stm32l442/lib/usbd/usbd_hid.c
+++ b/targets/stm32l442/lib/usbd/usbd_hid.c
@@ -76,6 +76,7 @@
 #include "usbd_core.h"
 
 #include "fifo.h"
+#include "log.h"
 
 extern int usbhid_rdy ;
 
@@ -252,7 +253,7 @@ static void dump_pma()
     uint16_t val;
     uint32_t offset = (uint32_t)(USB)->BTABLE;
 
-    printf("btable: %02lx\r\n",offset);
+    printf1(TAG_GREEN,"btable: %02lx\r\n",offset);
 
     for (int i = 0; i < 2; i++)
     {
@@ -261,8 +262,8 @@ static void dump_pma()
         uint16_t addr_rx = pma_ptr[i * 4 + 2];
         uint16_t cnt_rx = pma_ptr[i * 4 + 3];
 
-        printf("EP%d addr_tx == %02x, count_tx == %02x\r\n", i, addr_tx,cnt_tx );
-        printf("EP%d addr_rx == %02x, count_rx == %02x\r\n", i, addr_rx,cnt_rx );
+        printf1(TAG_GREEN,"EP%d addr_tx == %02x, count_tx == %02x\r\n", i, addr_tx,cnt_tx );
+        printf1(TAG_GREEN,"EP%d addr_rx == %02x, count_rx == %02x\r\n", i, addr_rx,cnt_rx );
     }
 
     uint16_t ep1_tx = pma_ptr[1 * 4 + 0];
@@ -270,9 +271,9 @@ static void dump_pma()
     for (int i  = 0; i < 32; i++)
     {
         val = pma_ptr[ep1_tx + i];
-        printf("%04x ",val);
+        printf1(TAG_GREEN,"%04x ",val);
     }
-    printf("\r\n");
+    printf1(TAG_GREEN,"\r\n");
 }
 
 /**
diff --git a/targets/stm32l442/merge_hex.py b/targets/stm32l442/merge_hex.py
new file mode 100644
index 0000000..5bf5522
--- /dev/null
+++ b/targets/stm32l442/merge_hex.py
@@ -0,0 +1,28 @@
+# Merges bootloader and application into 1 file for ST Solo
+#
+# Patches settings in flash so bootloader will boot application.
+
+from intelhex import IntelHex
+import sys
+
+if len(sys.argv) < 3:
+    print('usage: %s <file1.hex> <file2.hex> [...] <output.hex>')
+    sys.exit(1)
+
+def flash_addr(num):
+    return 0x08000000 + num * 2048
+
+PAGES = 128
+APPLICATION_END_PAGE = PAGES - 19
+AUTH_WORD_ADDR       = (flash_addr(APPLICATION_END_PAGE)-8)
+
+first = IntelHex(sys.argv[1])
+for i in range(2, len(sys.argv)-1):
+    first.merge(IntelHex( sys.argv[i] ), overlap = 'replace')
+
+first[AUTH_WORD_ADDR]   = 4
+first[AUTH_WORD_ADDR+1] = 5
+first[AUTH_WORD_ADDR+2] = 6
+first[AUTH_WORD_ADDR+3] = 7
+
+first.tofile(sys.argv[len(sys.argv)-1], format='hex')
diff --git a/targets/stm32l442/requirements.txt b/targets/stm32l442/requirements.txt
new file mode 100644
index 0000000..a7dde12
--- /dev/null
+++ b/targets/stm32l442/requirements.txt
@@ -0,0 +1 @@
+intelhex
diff --git a/targets/stm32l442/src/app.h b/targets/stm32l442/src/app.h
index f506427..a05b461 100644
--- a/targets/stm32l442/src/app.h
+++ b/targets/stm32l442/src/app.h
@@ -4,7 +4,7 @@
 
 #define DEBUG_UART      USART1
 
-#define DEBUG_LEVEL        1
+#define DEBUG_LEVEL        0
 
 #define NON_BLOCK_PRINTING 0
 
diff --git a/targets/stm32l442/src/device.c b/targets/stm32l442/src/device.c
index 30bd620..16decc5 100644
--- a/targets/stm32l442/src/device.c
+++ b/targets/stm32l442/src/device.c
@@ -88,6 +88,10 @@ void device_set_status(int status)
     __device_status = status;
 }
 
+int device_is_button_pressed()
+{
+    return IS_BUTTON_PRESSED();
+}
 
 void delay(uint32_t ms)
 {
@@ -95,7 +99,10 @@ void delay(uint32_t ms)
     while ((millis() - time) < ms)
         ;
 }
+void device_reboot()
+{
 
+}
 void device_init()
 {
     hw_init();
@@ -111,8 +118,10 @@ void device_init()
     printf1(TAG_GEN,"hello solo\r\n");
 }
 
+void usb_init(void);
 void usbhid_init()
 {
+    usb_init();
     printf1(TAG_GEN,"hello solo\r\n");
 }
 int usbhid_recv(uint8_t * msg)
@@ -543,7 +552,7 @@ static void authorize_application()
     ptr = (uint32_t *)AUTH_WORD_ADDR;
     flash_write((uint32_t)ptr, (uint8_t *)&zero, 4);
 }
-static int is_authorized_to_boot()
+int is_authorized_to_boot()
 {
     uint32_t * auth = (uint32_t *)AUTH_WORD_ADDR;
     return *auth == 0;
@@ -558,7 +567,6 @@ int bootloader_bridge(uint8_t klen, uint8_t * keyh)
     uint8_t * pubkey = (uint8_t*)"\x57\xe6\x80\x39\x56\x46\x2f\x0c\x95\xac\x72\x71\xf0\xbc\xe8\x2d\x67\xd0\x59\x29\x2e\x15\x22\x89\x6a\xbd\x3f\x7f\x27\xf3\xc0\xc6\xe2\xd7\x7d\x8a\x9f\xcc\x53\xc5\x91\xb2\x0c\x9c\x3b\x4e\xa4\x87\x31\x67\xb4\xa9\x4b\x0e\x8d\x06\x67\xd8\xc5\xef\x2c\x50\x4a\x55";
     const struct uECC_Curve_t * curve = NULL;
 
-    /*printf("bootloader_bridge\n");*/
     if (req->len > 255-9)
     {
         return CTAP1_ERR_INVALID_LENGTH;
@@ -573,7 +581,6 @@ int bootloader_bridge(uint8_t klen, uint8_t * keyh)
 
     switch(req->op){
         case BootWrite:
-            /*printf("BootWrite 0x%08x\n", addr);*/
             if ((uint32_t)ptr < APPLICATION_START_ADDR || (uint32_t)ptr >= APPLICATION_END_ADDR)
             {
                 return CTAP2_ERR_NOT_ALLOWED;
@@ -592,13 +599,10 @@ int bootloader_bridge(uint8_t klen, uint8_t * keyh)
             flash_write((uint32_t)ptr,payload, req->len + (req->len%4));
             break;
         case BootDone:
-            //            printf("BootDone\n");
             ptr = (uint32_t *)APPLICATION_START_ADDR;
             crypto_sha256_init();
             crypto_sha256_update(ptr, APPLICATION_END_ADDR-APPLICATION_START_ADDR);
             crypto_sha256_final(hash);
-            //            printf("hash: "); dump_hex(hash, 32);
-            //            printf("sig: "); dump_hex(payload, 64);
             curve = uECC_secp256r1();
 
             if (! uECC_verify(pubkey,
@@ -613,11 +617,9 @@ int bootloader_bridge(uint8_t klen, uint8_t * keyh)
             REBOOT_FLAG = 1;
             break;
         case BootCheck:
-            /*printf("BootCheck\n");*/
             return 0;
             break;
         case BootErase:
-            /*printf("BootErase\n");*/
             erase_application();
             return 0;
             break;
@@ -627,6 +629,26 @@ int bootloader_bridge(uint8_t klen, uint8_t * keyh)
     return 0;
 }
 
+void bootloader_heartbeat()
+{
+    static int state = 0;
+    static uint32_t val = 0x10;
+    int but = IS_BUTTON_PRESSED();
 
+    if (state)
+    {
+        val--;
+    }
+    else
+    {
+        val++;
+    }
+
+    if (val > 30 || val < 1)
+    {
+        state = !state;
+    }
+    led_rgb((val * 3)<<8 | (val*10) << 16);
+}
 
 #endif
diff --git a/targets/stm32l442/src/fifo.c b/targets/stm32l442/src/fifo.c
index 645c28b..cb809b5 100644
--- a/targets/stm32l442/src/fifo.c
+++ b/targets/stm32l442/src/fifo.c
@@ -2,6 +2,7 @@
 #include <stdint.h>
 #include <stdio.h>
 #include "fifo.h"
+#include "log.h"
 
 
 FIFO_CREATE(debug,4096,1)
@@ -16,7 +17,7 @@ void fifo_test()
     uint8_t data[10][100];
     uint8_t verif[10][100];
 
-    printf("init\r\n");
+    printf1(TAG_GREEN,"init\r\n");
     for (int i = 0; i < 10; i++)
     {
         memset(data[i],i,100);
@@ -24,43 +25,43 @@ void fifo_test()
 
     for (int i = 0; i < 10; i++)
     {
-        printf("rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
+        printf1(TAG_GREEN,"rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
         ret = fifo_test_add(data[i]);
-        printf("%d\r\n",i);
+        printf1(TAG_GREEN,"%d\r\n",i);
         if (ret != 0)
         {
-            printf("fifo_test_add fail\r\n");
+            printf1(TAG_GREEN,"fifo_test_add fail\r\n");
             goto fail;
         }
     }
 
     for (int i = 0; i < 10; i++)
     {
-        printf("rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
+        printf1(TAG_GREEN,"rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
         ret = fifo_test_take(verif[i]);
-        printf("%d\r\n",i );
+        printf1(TAG_GREEN,"%d\r\n",i );
         if (ret != 0)
         {
-            printf("fifo_test_take fail\r\n");
+            printf1(TAG_GREEN,"fifo_test_take fail\r\n");
             goto fail;
         }
 
         if (memcmp(verif[i], data[i], 100) != 0)
         {
-            printf("fifo_test_take result fail\r\n");
-            dump_hex(data[i],100);
-            dump_hex(verif[i],100);
+            printf1(TAG_GREEN,"fifo_test_take result fail\r\n");
+            dump_hex1(TAG_GREEN,data[i],100);
+            dump_hex1(TAG_GREEN,verif[i],100);
             goto fail;
         }
     }
 
     for (int i = 0; i < 10; i++)
     {
-        printf("rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
+        printf1(TAG_GREEN,"rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
         ret = fifo_test_add(data[i]);
         if (ret != 0)
         {
-            printf("fifo_test_add 2 fail\r\n");
+            printf1(TAG_GREEN,"fifo_test_add 2 fail\r\n");
             goto fail;
         }
     }
@@ -68,7 +69,7 @@ void fifo_test()
     ret = fifo_test_add(data[0]);
     if (ret == 0)
     {
-        printf("fifo_test_add should have failed\r\n");
+        printf1(TAG_GREEN,"fifo_test_add should have failed\r\n");
         goto fail;
     }
 
@@ -76,17 +77,17 @@ void fifo_test()
 
     for (int i = 0; i < 10; i++)
     {
-        printf("rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
+        printf1(TAG_GREEN,"rhead: %d, whead: %d\r\n", fifo_test_rhead(), fifo_test_whead());
         ret = fifo_test_take(verif[i]);
         if (ret != 0)
         {
-            printf("fifo_test_take fail\r\n");
+            printf1(TAG_GREEN,"fifo_test_take fail\r\n");
             goto fail;
         }
 
         if (memcmp(verif[i], data[i], 100) != 0)
         {
-            printf("fifo_test_take result fail\r\n");
+            printf1(TAG_GREEN,"fifo_test_take result fail\r\n");
             goto fail;
         }
     }
@@ -94,11 +95,11 @@ void fifo_test()
     ret = fifo_test_take(verif[0]);
     if (ret == 0)
     {
-        printf("fifo_test_take should have failed\r\n");
+        printf1(TAG_GREEN,"fifo_test_take should have failed\r\n");
         goto fail;
     }
 
-    printf("test pass!\r\n");
+    printf1(TAG_GREEN,"test pass!\r\n");
     return ;
     fail:
     while(1)
diff --git a/targets/stm32l442/src/init.c b/targets/stm32l442/src/init.c
index 904d67e..8cd2d1c 100644
--- a/targets/stm32l442/src/init.c
+++ b/targets/stm32l442/src/init.c
@@ -38,7 +38,6 @@ static void MX_USART1_UART_Init(void);
 static void MX_TIM2_Init(void);
 static void MX_TIM6_Init(void);
 static void MX_RNG_Init(void);
-static void usb_init();
 
 #define Error_Handler() _Error_Handler(__FILE__,__LINE__)
 void _Error_Handler(char *file, int line);
@@ -91,7 +90,7 @@ void hw_init(void)
   __enable_irq();
   NVIC_EnableIRQ(TIM6_IRQn);
 #ifndef TEST_SOLO_STM32
-  usb_init();
+
  #endif
 
 }
@@ -202,7 +201,7 @@ void SystemClock_Config(void)
   NVIC_SetPriority(SysTick_IRQn, NVIC_EncodePriority(NVIC_GetPriorityGrouping(),0, 0));
 }
 
-static void usb_init()
+void usb_init()
 {
     USBD_Init(&Solo_USBD_Device, &Solo_Desc, 0);
     USBD_RegisterClass(&Solo_USBD_Device, &USBD_HID);
diff --git a/targets/stm32l442/stm32l432xx.ld b/targets/stm32l442/stm32l432xx.ld
index 2c44fe9..cbefde8 100644
--- a/targets/stm32l442/stm32l432xx.ld
+++ b/targets/stm32l442/stm32l432xx.ld
@@ -41,7 +41,8 @@ _Min_Stack_Size = 0x400; /* required amount of stack */
 /* Specify the memory areas */
 MEMORY
 {
-FLASH (rx)      : ORIGIN = 0x08000000, LENGTH = 238K /* Leave out 18 Kb for data */
+/* First 32 KB is bootloader */ 
+FLASH (rx)      : ORIGIN = 0x08010000, LENGTH = 206K-8 /* Leave out 18 Kb at end for data */
 RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 48K
 SRAM2 (rw)      : ORIGIN = 0x10000000, LENGTH = 16K
 }