diff --git a/targets/stm32l442/.ycm_extra_conf.py b/targets/stm32l442/.ycm_extra_conf.py
index 6ac4088..c9e1ec7 100644
--- a/targets/stm32l442/.ycm_extra_conf.py
+++ b/targets/stm32l442/.ycm_extra_conf.py
@@ -90,6 +90,7 @@ get_python_inc(),
     '-std=c99',
     '-Ilib/',
     '-Isrc/cmsis',
+    '-I../../fido2',
 ]
 
 # Clang automatically sets the '-std=' flag to 'c++14' for MSVC 2015 or later,
diff --git a/targets/stm32l442/Makefile b/targets/stm32l442/Makefile
index 81993e0..baa0697 100644
--- a/targets/stm32l442/Makefile
+++ b/targets/stm32l442/Makefile
@@ -2,11 +2,13 @@ CC=arm-none-eabi-gcc
 CP=arm-none-eabi-objcopy
 SZ=arm-none-eabi-size
 
-SRC=src/main.c src/init.c src/redirect.c src/flash.c src/rng.c \
+SRC=src/main.c src/init.c src/redirect.c src/flash.c src/rng.c src/led.c src/device.c \
+../../fido2/util.c \
 src/startup_stm32l432xx.s src/system_stm32l4xx.c $(wildcard lib/*c)
 
-OBJ=$(SRC:.c=.o)
-INC=-Isrc/ -Isrc/cmsis -Ilib/
+OBJ1=$(SRC:.c=.o)
+OBJ=$(OBJ1:.s=.o)
+INC=-Isrc/ -Isrc/cmsis -Ilib/ -I../../fido2
 LDSCRIPT=stm32l432xx.ld
 
 CFLAGS= $(INC)
@@ -41,7 +43,7 @@ all: $(TARGET).elf
 	$(CP) -O ihex $^ $(TARGET).hex
 
 clean:
-	rm -f *.o src/*.o src/*.elf *.elf *.hex
+	rm -f *.o src/*.o src/*.elf *.elf *.hex $(OBJ)
 
 flash: $(TARGET).hex
 	STM32_Programmer_CLI -c port=SWD -halt -d $(TARGET).hex -rst
diff --git a/targets/stm32l442/src/app.h b/targets/stm32l442/src/app.h
index 69af8f9..cdd2a14 100644
--- a/targets/stm32l442/src/app.h
+++ b/targets/stm32l442/src/app.h
@@ -4,8 +4,7 @@
 
 #define DEBUG_UART      USART1
 
-extern uint32_t __65_seconds;
 
-#define millis()    (((uint32_t)TIM6->CNT) | (__65_seconds<<16))
+void hw_init(void);
 
 #endif
diff --git a/targets/stm32l442/src/device.c b/targets/stm32l442/src/device.c
new file mode 100644
index 0000000..d10549d
--- /dev/null
+++ b/targets/stm32l442/src/device.c
@@ -0,0 +1,20 @@
+
+#include "device.h"
+
+uint32_t __65_seconds = 0;
+void TIM6_DAC_IRQHandler()
+{
+    // timer is only 16 bits, so roll it over here
+    TIM6->SR = 0;
+    __65_seconds += 1;
+}
+
+
+void delay(uint32_t ms)
+{
+    uint32_t time = millis();
+    while ((millis() - time) < ms)
+        ;
+}
+
+
diff --git a/targets/stm32l442/src/device.h b/targets/stm32l442/src/device.h
new file mode 100644
index 0000000..ee9219a
--- /dev/null
+++ b/targets/stm32l442/src/device.h
@@ -0,0 +1,12 @@
+#ifndef _DEVICE_H_
+#define _DEVICE_H_
+
+#include <stdint.h>
+#include "stm32l4xx_ll_tim.h"
+
+void delay(uint32_t ms);
+
+#define millis()    (((uint32_t)TIM6->CNT) | (__65_seconds<<16))
+extern uint32_t __65_seconds;
+
+#endif
diff --git a/targets/stm32l442/src/flash.c b/targets/stm32l442/src/flash.c
index e5a9a2f..500d6ad 100644
--- a/targets/stm32l442/src/flash.c
+++ b/targets/stm32l442/src/flash.c
@@ -33,7 +33,7 @@ void flash_erase_page(uint8_t page)
 
     if(FLASH->SR & (1<<1))
     {
-        printf("erase NOT successful %x\r\n", FLASH->SR);
+        printf("erase NOT successful %lx\r\n", FLASH->SR);
     }
 
     FLASH->CR &= ~(0x7);
@@ -58,7 +58,7 @@ void flash_write_dword(uint32_t addr, uint64_t data)
 
     if(FLASH->SR & (1<<1))
     {
-        printf("program NOT successful %x\r\n", FLASH->SR);
+        printf("program NOT successful %lx\r\n", FLASH->SR);
     }
 
     FLASH->SR = (1<<0);
@@ -68,7 +68,7 @@ void flash_write_dword(uint32_t addr, uint64_t data)
 
 void flash_write(uint32_t addr, uint8_t * data, size_t sz)
 {
-    int i,j;
+    int i;
     uint8_t buf[8];
 
     // dword align
diff --git a/targets/stm32l442/src/led.c b/targets/stm32l442/src/led.c
new file mode 100644
index 0000000..f8373aa
--- /dev/null
+++ b/targets/stm32l442/src/led.c
@@ -0,0 +1,109 @@
+#include <stdint.h>
+#include <stdio.h>
+
+#include "stm32l4xx_ll_gpio.h"
+#include "stm32l4xx_ll_tim.h"
+
+#include "led.h"
+#include "device.h"
+
+void led_rgb(uint32_t hex)
+{
+    uint32_t r = hex >> 16;
+    uint32_t g = (hex >> 8)&0xff;
+    uint32_t b = hex & 0xff;
+
+    // CCR2 == blue
+    // CCR3 == red
+    // CCR4 == green
+
+    // map and scale colors
+    TIM2->CCR2 = 1000 - (b * 1000)/(256);
+    TIM2->CCR3 = 1000 - (r * 1000)/(256*6);
+    TIM2->CCR4 = 1000 - (g * 1000)/(256);
+}
+
+void led_test_colors()
+{
+    // Should produce pulsing of various colors
+    int i = 0;
+    int j = 0;
+    int inc = 1;
+    uint32_t time = 0;
+#define update() do {\
+        i += inc;\
+        if (i > 254)\
+        {\
+            inc *= -1;\
+        }\
+        else if (i == 0)\
+        {\
+            inc *= -1;\
+        }\
+        delay(2);\
+        }while(0);
+
+    while(1)
+    {
+
+        printf("%d: %lu\r\n", j++, millis());
+
+        printf("white pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb(i | (i << 8) | (i << 16));
+        }
+
+        printf("blue pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb(i);
+        }
+
+        printf("green pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb(i<<8);
+        }
+
+        printf("red pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb(i<<16);
+        }
+
+        printf("purple pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb((i<<16) | i);
+        }
+
+        printf("orange pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb((i<<16) | (i<<8));
+        }
+
+        printf("yellow pulse\r\n");
+        time = millis();
+        while((millis() - time) < 5000)
+        {
+            update();
+            led_rgb((i<<8) | (i<<0));
+        }
+    }
+}
+
+
diff --git a/targets/stm32l442/src/led.h b/targets/stm32l442/src/led.h
new file mode 100644
index 0000000..d63b54c
--- /dev/null
+++ b/targets/stm32l442/src/led.h
@@ -0,0 +1,14 @@
+#ifndef _LED_H_
+#define _LED_H_
+
+#include <stdint.h>
+
+void led_rgb(uint32_t hex);
+void led_test_colors();
+
+#define LED_PIN_G     LL_GPIO_PIN_0
+#define LED_PIN_B     LL_GPIO_PIN_1
+#define LED_PIN_R     LL_GPIO_PIN_2
+#define LED_PORT      GPIOA
+
+#endif
diff --git a/targets/stm32l442/src/main.c b/targets/stm32l442/src/main.c
index 7dd5797..8ff22e1 100644
--- a/targets/stm32l442/src/main.c
+++ b/targets/stm32l442/src/main.c
@@ -11,144 +11,15 @@
 #include "stm32l4xx_ll_gpio.h"
 #include "stm32l4xx_ll_usart.h"
 #include "stm32l4xx_ll_bus.h"
-#include "stm32l4xx_ll_tim.h"
 
 #include "app.h"
 #include "flash.h"
 #include "rng.h"
+#include "led.h"
+#include "device.h"
 
 #define Error_Handler() _Error_Handler(__FILE__,__LINE__)
 
-#define LED_PIN_G     LL_GPIO_PIN_0
-#define LED_PIN_B     LL_GPIO_PIN_1
-#define LED_PIN_R     LL_GPIO_PIN_2
-#define LED_PORT      GPIOA
-
-void hw_init(void);
-
-void delay(uint32_t ms)
-{
-    uint32_t time = millis();
-    while ((millis() - time) < ms)
-        ;
-}
-
-void rgb(uint32_t hex)
-{
-    uint32_t r = hex >> 16;
-    uint32_t g = (hex >> 8)&0xff;
-    uint32_t b = hex & 0xff;
-
-    // CCR2 == blue
-    // CCR3 == red
-    // CCR4 == green
-
-    // map and scale colors
-    TIM2->CCR2 = 1000 - (b * 1000)/(256);
-    TIM2->CCR3 = 1000 - (r * 1000)/(256*6);
-    TIM2->CCR4 = 1000 - (g * 1000)/(256);
-}
-
-void test_colors()
-{
-    // Should produce pulsing of various colors
-    int i = 0;
-    int j = 0;
-    int inc = 1;
-    uint32_t time = 0;
-#define update() do {\
-        i += inc;\
-        if (i > 254)\
-        {\
-            inc *= -1;\
-        }\
-        else if (i == 0)\
-        {\
-            inc *= -1;\
-        }\
-        delay(2);\
-        }while(0);
-
-    while(1)
-    {
-
-        printf("%d: %lu\r\n", j++, millis());
-
-        printf("white pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb(i | (i << 8) | (i << 16));
-        }
-
-        printf("blue pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb(i);
-        }
-
-        printf("green pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb(i<<8);
-        }
-
-        printf("red pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb(i<<16);
-        }
-
-        printf("purple pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb((i<<16) | i);
-        }
-
-        printf("orange pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb((i<<16) | (i<<8));
-        }
-
-        printf("yellow pulse\r\n");
-        time = millis();
-        while((millis() - time) < 5000)
-        {
-            update();
-            rgb((i<<8) | (i<<0));
-        }
-    }
-}
-
-
-uint32_t __65_seconds = 0;
-void TIM6_DAC_IRQHandler()
-{
-    // timer is only 16 bits, so roll it over here
-    TIM6->SR = 0;
-    __65_seconds += 1;
-}
-
-void dump_hex(uint8_t * b, int len)
-{
-    while(len--)
-    {
-        printf("%02x ", *b++);
-    }
-    printf("\r\n");
-}
 
 int main(void)
 {
@@ -157,18 +28,11 @@ int main(void)
     uint32_t i = 0;
     float ent;
     float test = 1235.889944f;
+
     hw_init();
+
     printf("hello solo\r\n");
 
-
-    /*LL_GPIO_SetPinMode(LED_PORT, LED_PIN_R, LL_GPIO_MODE_OUTPUT);*/
-    /*LL_GPIO_SetPinMode(LED_PORT, LED_PIN_G, LL_GPIO_MODE_OUTPUT);*/
-    /*LL_GPIO_SetPinMode(LED_PORT, LED_PIN_B, LL_GPIO_MODE_OUTPUT);*/
-
-    /*LL_GPIO_SetOutputPin(LED_PORT, LED_PIN_R);*/
-    /*LL_GPIO_SetOutputPin(LED_PORT, LED_PIN_G);*/
-    /*LL_GPIO_SetOutputPin(LED_PORT, LED_PIN_B);*/
-
     // Test flash
     flash_erase_page(60);
     flash_write(flash_addr(60), str, sizeof(str));
@@ -181,18 +45,18 @@ int main(void)
     uint32_t t2 = millis();
     printf("100 ms delay (%lu)\r\n",t2-t1);
 
-
+    // test rng
     ent = rng_test(64 * 1024);
 
     printf("entropy of 64KB from RNG: %.6f\r\n", ent);
     printf("test float: %.2f\r\n", test);
 
     // Test PWM + weighting of RGB
-    test_colors();
+    led_test_colors();
 
     while (1)
     {
-        rgb(i | (i << 8) | (i << 16));
+        led_rgb(i | (i << 8) | (i << 16));
 
         delay(1000);
         printf("%lu: %lu\r\n", i+=50, millis());
@@ -203,6 +67,7 @@ int main(void)
 void _Error_Handler(char *file, int line)
 {
 
+    printf("Error: %s: %d\r\n", file, line);
     while(1)
     {
     }