diff --git a/fido2/device.h b/fido2/device.h
index e75f250..a9ee118 100644
--- a/fido2/device.h
+++ b/fido2/device.h
@@ -86,5 +86,17 @@ void boot_st_bootloader();
 // HID wink command
 void device_wink();
 
+typedef enum {
+    DEVICE_LOW_POWER_IDLE = 0,
+    DEVICE_LOW_POWER_FAST = 1,
+    DEVICE_FAST = 2,
+} DEVICE_CLOCK_RATE;
+
+// Set the clock rate for the device.
+// Three modes are targetted for Solo.
+// 0: Lowest clock rate for NFC.
+// 1: fastest clock rate supported at a low power setting for NFC FIDO.
+// 2: fastest clock rate.  Generally for USB interface.
+void device_set_clock_rate(DEVICE_CLOCK_RATE param);
 
 #endif
diff --git a/fido2/u2f.c b/fido2/u2f.c
index 471e828..9731572 100644
--- a/fido2/u2f.c
+++ b/fido2/u2f.c
@@ -278,7 +278,6 @@ static int16_t u2f_register(struct u2f_register_request * req, bool fromNFC)
     uint8_t i[] = {0x0,U2F_EC_FMT_UNCOMPRESSED};
 
     struct u2f_key_handle key_handle;
-    static uint32_t count = 0;
     uint8_t pubkey[64];
     uint8_t hash[32];
     uint8_t * sig = (uint8_t*)req;
@@ -293,7 +292,7 @@ static int16_t u2f_register(struct u2f_register_request * req, bool fromNFC)
 			return U2F_SW_CONDITIONS_NOT_SATISFIED;
 		}
 	}
-    
+
     if ( u2f_new_keypair(&key_handle, req->app, pubkey) == -1)
     {
         return U2F_SW_INSUFFICIENT_MEMORY;
diff --git a/targets/stm32l432/src/init.c b/targets/stm32l432/src/init.c
index fd4bb61..4834e5d 100644
--- a/targets/stm32l432/src/init.c
+++ b/targets/stm32l432/src/init.c
@@ -34,7 +34,31 @@
 #include APP_CONFIG
 
 // KHz
-#define CLOCK_RATE      16000
+#define MAX_CLOCK_RATE      24000
+
+#define SET_CLOCK_RATE2()        SystemClock_Config()
+
+#if MAX_CLOCK_RATE == 48000
+    #define SET_CLOCK_RATE0()        SystemClock_Config_LF32()
+    #define SET_CLOCK_RATE1()        SystemClock_Config_LF48()
+#elif MAX_CLOCK_RATE == 32000
+    #define SET_CLOCK_RATE0()        SystemClock_Config_LF24()
+    #define SET_CLOCK_RATE1()        SystemClock_Config_LF32()
+#elif MAX_CLOCK_RATE == 28000
+    #define SET_CLOCK_RATE0()        SystemClock_Config_LF24()
+    #define SET_CLOCK_RATE1()        SystemClock_Config_LF28()
+#elif MAX_CLOCK_RATE == 24000
+    #define SET_CLOCK_RATE0()        SystemClock_Config_LF16()
+    #define SET_CLOCK_RATE1()        SystemClock_Config_LF24()
+#elif MAX_CLOCK_RATE == 20000
+    #define SET_CLOCK_RATE0()        SystemClock_Config_LF16()
+    #define SET_CLOCK_RATE1()        SystemClock_Config_LF20()
+#elif MAX_CLOCK_RATE == 16000
+    #define SET_CLOCK_RATE0()        SystemClock_Config_LF8()
+    #define SET_CLOCK_RATE1()        SystemClock_Config_LF16()
+#else
+#error "Invalid clock rate selected"
+#endif
 
 USBD_HandleTypeDef Solo_USBD_Device;
 
@@ -42,6 +66,11 @@ static void LL_Init(void);
 
 #define Error_Handler() _Error_Handler(__FILE__,__LINE__)
 void _Error_Handler(char *file, int line);
+
+void SystemClock_Config(void);
+void SystemClock_Config_LF16(void);
+void SystemClock_Config_LF20(void);
+void SystemClock_Config_LF24(void);
 void SystemClock_Config_LF28(void);
 void SystemClock_Config_LF48(void);
 
@@ -56,23 +85,8 @@ void hw_init(int lowfreq)
 
     if (lowfreq)
     {
-        // Under voltage
-        LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE2);
-#if CLOCK_RATE == 48000
-        SystemClock_Config_LF48();
-#elif CLOCK_RATE == 32000
-        SystemClock_Config_LF32();
-#elif CLOCK_RATE == 28000
-        SystemClock_Config_LF28();
-#elif CLOCK_RATE == 24000
-        SystemClock_Config_LF24();
-#elif CLOCK_RATE == 20000
-        SystemClock_Config_LF20();
-#elif CLOCK_RATE == 16000
-        SystemClock_Config_LF16();
-#else
-#error "Invalid clock rate selected"
-#endif
+        LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE2); // Under voltage
+        device_set_clock_rate(DEVICE_LOW_POWER_IDLE);
         LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE2);
     }
     else
@@ -123,6 +137,22 @@ static void LL_Init(void)
 
 }
 
+void device_set_clock_rate(DEVICE_CLOCK_RATE param)
+{
+    switch(param)
+    {
+        case DEVICE_LOW_POWER_IDLE:
+            SET_CLOCK_RATE0();
+        break;
+        case DEVICE_LOW_POWER_FAST:
+            SET_CLOCK_RATE1();
+        break;
+        case DEVICE_FAST:
+            SET_CLOCK_RATE2();
+        break;
+    }
+}
+
 /**
   * @brief System Clock Configuration
   * @retval None
@@ -847,7 +877,7 @@ void init_millisecond_timer(int lf)
     if (!lf)
         TIM_InitStruct.Prescaler = 48000;
     else
-        TIM_InitStruct.Prescaler = CLOCK_RATE;
+        TIM_InitStruct.Prescaler = MAX_CLOCK_RATE;
 
     TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
     TIM_InitStruct.Autoreload = 90;
diff --git a/targets/stm32l432/src/init.h b/targets/stm32l432/src/init.h
index a9920ad..729e898 100644
--- a/targets/stm32l432/src/init.h
+++ b/targets/stm32l432/src/init.h
@@ -22,10 +22,6 @@
 #ifndef _INIT_H_
 #define _INIT_H_
 
-void SystemClock_Config(void);
-void SystemClock_Config_LF(void);
-void SystemClock_Config_LF16(void);
-
 void init_usb();
 void init_gpio(void);
 void init_debug_uart(void);
diff --git a/targets/stm32l432/src/nfc.c b/targets/stm32l432/src/nfc.c
index 0299d13..cf526df 100644
--- a/targets/stm32l432/src/nfc.c
+++ b/targets/stm32l432/src/nfc.c
@@ -396,12 +396,8 @@ void nfc_process_iblock(uint8_t * buf, int len)
     uint8_t * payload = buf + 1 + 5;
     uint8_t plen = apdu->lc;
     int selected;
-    uint8_t res[32];
-	uint32_t t1;
-    int l1;
     CTAP_RESPONSE ctap_resp;
     int status;
-    struct u2f_register_request * u2freq = (struct u2f_register_request *)payload;
 
     printf1(TAG_NFC,"Iblock: ");
 	dump_hex1(TAG_NFC, buf, len);
@@ -479,19 +475,19 @@ void nfc_process_iblock(uint8_t * buf, int len)
 				return;
 			}
 
-			t1 = millis();
+			timestamp();
 
 
 			// WTX_on(WTX_TIME_DEFAULT);
             // SystemClock_Config_LF32();
             // delay(300);
-            SystemClock_Config_LF24();
+            device_set_clock_rate(DEVICE_LOW_POWER_FAST);;
 			u2f_request_nfc(&buf[1], len, &ctap_resp);
-            SystemClock_Config_LF16();
+            device_set_clock_rate(DEVICE_LOW_POWER_IDLE);;
 			// if (!WTX_off())
 			// 	return;
 
-            printf1(TAG_NFC,"U2F Register P2 took %d\r\n", millis() - t1);
+            printf1(TAG_NFC,"U2F Register P2 took %d\r\n", timestamp());
             nfc_write_response_chaining(buf[0], ctap_resp.data, ctap_resp.length, 0 );
 
 			// printf1(TAG_NFC, "U2F resp len: %d\r\n", ctap_resp.length);
@@ -518,16 +514,16 @@ void nfc_process_iblock(uint8_t * buf, int len)
 				return;
 			}
 
-			t1 = millis();
+			timestamp();
 			// WTX_on(WTX_TIME_DEFAULT);
 			u2f_request_nfc(&buf[1], len, &ctap_resp);
 			// if (!WTX_off())
 			// 	return;
 
 			printf1(TAG_NFC, "U2F resp len: %d\r\n", ctap_resp.length);
-            printf1(TAG_NFC,"U2F Authenticate processing %d (took %d)\r\n", millis(), millis() - t1);
+            printf1(TAG_NFC,"U2F Authenticate processing %d (took %d)\r\n", millis(), timestamp());
 			nfc_write_response_chaining(buf[0], ctap_resp.data, ctap_resp.length, 0);
-            printf1(TAG_NFC,"U2F Authenticate answered %d (took %d)\r\n", millis(), millis() - t1);
+            printf1(TAG_NFC,"U2F Authenticate answered %d (took %d)\r\n", millis(), timestamp);
         break;
 
         case APDU_FIDO_NFCCTAP_MSG:
@@ -536,7 +532,7 @@ void nfc_process_iblock(uint8_t * buf, int len)
 				break;
 			}
 
-			t1 = millis();
+			timestamp();
 			printf1(TAG_NFC, "FIDO2 CTAP message. %d\r\n", t1);
 
 			WTX_on(WTX_TIME_DEFAULT);
@@ -558,9 +554,9 @@ void nfc_process_iblock(uint8_t * buf, int len)
 			ctap_resp.data[ctap_resp.length - 2] = SW_SUCCESS >> 8;
 			ctap_resp.data[ctap_resp.length - 1] = SW_SUCCESS & 0xff;
 
-            printf1(TAG_NFC,"CTAP processing %d (took %d)\r\n", millis(), millis() - t1);
+            printf1(TAG_NFC,"CTAP processing %d (took %d)\r\n", millis(), timestamp());
 			nfc_write_response_chaining(buf[0], ctap_resp.data, ctap_resp.length, 0);
-            printf1(TAG_NFC,"CTAP answered %d (took %d)\r\n", millis(), millis() - t1);
+            printf1(TAG_NFC,"CTAP answered %d (took %d)\r\n", millis(), timestamp());
         break;
 
         case APDU_INS_READ_BINARY:
@@ -613,7 +609,7 @@ void clear_ibuf()
 	memset(ibuf, 0, sizeof(ibuf));
 }
 
-void nfc_process_block(uint8_t * buf, int len)
+void nfc_process_block(uint8_t * buf, unsigned int len)
 {
 
 	if (!len)
@@ -706,8 +702,6 @@ void nfc_process_block(uint8_t * buf, int len)
 
 void nfc_loop()
 {
-    static uint32_t t1 = 0;
-    static uint32_t t2 = 0;
     uint8_t buf[32];
     AMS_DEVICE ams;
     int len = 0;
@@ -732,7 +726,6 @@ void nfc_loop()
         if (ams.regs.int0 & AMS_INT_INIT)
         {
             nfc_state_init();
-            t1 = millis();
         }
         if (ams.regs.int1)
         {
@@ -771,14 +764,12 @@ void nfc_loop()
                     printf1(TAG_NFC, "HLTA/Halt\r\n");
                 break;
                 case NFC_CMD_RATS:
-                    t2 = millis();
 
                     answer_rats(buf[1]);
 
                     NFC_STATE.block_num = 1;
     				clear_ibuf();
     				WTX_clear();
-                    printf1(TAG_NFC,"RATS answered %d (took %d)\r\n",millis(), millis() - t1);
                 break;
                 default:
 
diff --git a/targets/stm32l432/src/system_stm32l4xx.c b/targets/stm32l432/src/system_stm32l4xx.c
index 262e883..a903c1c 100644
--- a/targets/stm32l432/src/system_stm32l4xx.c
+++ b/targets/stm32l432/src/system_stm32l4xx.c
@@ -106,6 +106,7 @@
   */
 
 #include "stm32l4xx.h"
+#include "device.h"
 #include "init.h"
 
 #if !defined  (HSE_VALUE)
@@ -220,7 +221,7 @@ void SystemInit(void)
   /* Disable all interrupts */
   RCC->CIER = 0x00000000U;
 
-  SystemClock_Config_LF16();
+  device_set_clock_rate(DEVICE_LOW_POWER_IDLE);
 
 }