diff --git a/Src/admin_vendor.c b/Src/admin_vendor.c
index 043dc9e..2bf9714 100644
--- a/Src/admin_vendor.c
+++ b/Src/admin_vendor.c
@@ -56,6 +56,36 @@ int admin_vendor_version(const CAPDU *capdu, RAPDU *rapdu) {
   return 0;
 }
 
+int admin_vendor_nfc_enable(const CAPDU *capdu, RAPDU *rapdu) {
+  if (P1 != 0x00 && P1 != 0x01) EXCEPT(SW_WRONG_P1P2);
+  if (P2 != 0x00) EXCEPT(SW_WRONG_P1P2);
+  if (LC != 0x00) EXCEPT(SW_WRONG_LENGTH);
+
+  uint32_t magic = P1 * 0x50 + 0x100;
+  FLASH_OBProgramInitTypeDef cfg = {
+    .OptionType = OPTIONBYTE_PCROP,
+    .PCROPConfig = FLASH_BANK_1,
+    .PCROPStartAddr = FLASH_BASE + magic, // Reuse this option word as NFC switch
+    .PCROPEndAddr = FLASH_BASE + 0xF, // Fixed value
+  };
+  DBG_MSG("Unlock OB\n");
+  HAL_FLASH_Unlock();
+  HAL_FLASH_OB_Unlock();
+  int ret = HAL_FLASHEx_OBProgram(&cfg);
+  HAL_FLASH_OB_Lock();
+  HAL_FLASH_Lock();
+  HAL_FLASHEx_OBGetConfig(&cfg);
+  uint32_t *flash_loc = (uint32_t*) 0x1FFF7808U;
+  DBG_MSG("HAL_FLASHEx_OBGetConfig: %d %x %x %x\n",
+    ret, cfg.PCROPStartAddr, cfg.PCROPEndAddr, *flash_loc);
+  // DBG_MSG("value= %x %x\n", *(uint32_t*)FLASH_BASE, *(uint32_t*)cfg.PCROPStartAddr);
+
+  if (ret != HAL_OK) return -1;
+
+  return 0;
+}
+
+
 extern uint32_t _stack_boundary;
 
 static int stack_test(const CAPDU *capdu, RAPDU *rapdu) {
diff --git a/Src/device.c b/Src/device.c
index f8c71a6..4793945 100644
--- a/Src/device.c
+++ b/Src/device.c
@@ -14,7 +14,9 @@
 
 const uint32_t UNTOUCHED_MAX_VAL = 40; /* Suitable for 56K pull-down resistor */
 const uint32_t CALI_TIMES = 4;
-const uint32_t TOUCH_GAP_TIME = 1500; /* Gap period (in ms) between two consecutive touch events */
+const uint32_t TOUCH_GAP_TIME = 800; /* Gap period (in ms) between two consecutive touch events */
+const uint32_t MIN_LONG_TOUCH_TIME = 500;
+const uint32_t MIN_TOUCH_TIME = 20;
 
 extern TIM_HandleTypeDef htim6;
 extern SPI_HandleTypeDef FM_SPI;
@@ -127,25 +129,27 @@ void device_periodic_task(void) {
     if (LL_USART_IsActiveFlag_RXNE(DBG_UART.Instance)) {
       int data = LL_USART_ReceiveData8(DBG_UART.Instance);
       DBG_MSG("UART: %x\n", data);
-      if ('T' == data) {
-        set_touch_result(TOUCH_SHORT);
+      if ('T' == data || 'L' == data) {
+        set_touch_result('T' == data ? TOUCH_SHORT : TOUCH_LONG);
         fsm = TOUCH_STATE_ASSERT;
         event_tick = tick;
       }
     }
 #endif
     if(GPIO_Touched()) {
+      measure_touch = 0;
       fsm = TOUCH_STATE_DOWN;
       event_tick = tick;
     }
     break;
   case TOUCH_STATE_DOWN:
-    if(!GPIO_Touched()) {
-      fsm = TOUCH_STATE_IDLE;
-    } else if (tick - event_tick > 50) {
-      set_touch_result(TOUCH_SHORT);
-      fsm = TOUCH_STATE_ASSERT;
-      event_tick = tick;
+    if(!GPIO_Touched() || tick - event_tick > MIN_LONG_TOUCH_TIME) {
+      if (tick - event_tick > MIN_TOUCH_TIME) {
+        set_touch_result(tick - event_tick > MIN_LONG_TOUCH_TIME ? TOUCH_LONG : TOUCH_SHORT);
+        fsm = TOUCH_STATE_ASSERT;
+        event_tick = tick;
+      } else
+        fsm = TOUCH_STATE_IDLE;
     }
     break;
   case TOUCH_STATE_ASSERT:
@@ -232,12 +236,9 @@ void usb_resources_alloc(void) {
   IFACE_TABLE.ccid = iface++;
   EP_SIZE_TABLE.ccid = 64;
 
-  if (cfg_is_kbd_interface_enable() && ep <= EP_ADDR_MSK) {
-    DBG_MSG("Keyboard interface enabled, Iface %u\n", iface);
-    EP_TABLE.kbd_hid = ep;
-    IFACE_TABLE.kbd_hid = iface;
-    EP_SIZE_TABLE.kbd_hid = 8;
-  }
+  EP_TABLE.kbd_hid = ep;
+  IFACE_TABLE.kbd_hid = iface;
+  EP_SIZE_TABLE.kbd_hid = 8;
 }
 
 int device_atomic_compare_and_swap(volatile uint32_t *var, uint32_t expect, uint32_t update) {
diff --git a/Src/lfs_init.c b/Src/lfs_init.c
index 57fe5af..0d3b208 100644
--- a/Src/lfs_init.c
+++ b/Src/lfs_init.c
@@ -7,7 +7,6 @@
 
 #define CHIP_FLASH_SIZE 0x40000
 #define LOOKAHEAD_SIZE 16
-#define CACHE_SIZE 128
 #define WRITE_SIZE 8
 #define READ_SIZE 1
 #define FS_BASE (&_lfs_begin)
@@ -16,8 +15,9 @@
 #define FLASH_ADDR2BLOCK(a) (((a) & ~0x8000000u) / FLASH_PAGE_SIZE)
 
 static struct lfs_config config;
-static uint8_t read_buffer[CACHE_SIZE];
-static uint8_t prog_buffer[CACHE_SIZE];
+static uint8_t read_buffer[LFS_CACHE_SIZE];
+static alignas(4) uint8_t prog_buffer[LFS_CACHE_SIZE];
+// uint8_t file_buffer[LFS_LFS_CACHE_SIZE];
 static alignas(4) uint8_t lookahead_buffer[LOOKAHEAD_SIZE];
 extern uint8_t _lfs_begin;
 
@@ -30,13 +30,29 @@ int block_read(const struct lfs_config *c, lfs_block_t block, lfs_off_t off, voi
 
 static int program_space(uint32_t paddr, const void *buffer, lfs_size_t size) {
   int ret = 0;
-  for (lfs_size_t i = 0; i < size; i += WRITE_SIZE) {
-    if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, paddr + i, *(const uint64_t *)((uintptr_t)buffer + i)) !=
+  uint32_t typ;
+  for (lfs_size_t i = 0; size;) {
+    // DBG_MSG("%d\n", i);
+    // if (size >= 512) {
+    //   typ = FLASH_TYPEPROGRAM_FAST;
+    // } else if (size >= 256) {
+    //   typ = FLASH_TYPEPROGRAM_FAST_AND_LAST;
+    // } else {
+      typ = FLASH_TYPEPROGRAM_DOUBLEWORD;
+    // }
+    if (HAL_FLASH_Program(typ, paddr + i, *(const uint64_t *)((uintptr_t)buffer + i)) !=
         HAL_OK) {
-      ERR_MSG("Flash prog fail @%#lx", paddr + i);
+      ERR_MSG("Flash prog failed @%#lx\n", paddr + i);
       ret = LFS_ERR_CORRUPT;
       break;
     }
+    if (typ == FLASH_TYPEPROGRAM_DOUBLEWORD) {
+      i += 8;
+      size -= 8;
+    } else {
+      i += 256;
+      size -= 256;
+    }
   }
 
   return ret;
@@ -51,18 +67,24 @@ int block_prog(const struct lfs_config *c, lfs_block_t block, lfs_off_t off, con
   int ret;
 
   // DBG_MSG("blk %d @ %p len %u buf %p\r\n", block, (void*)paddr, size, buffer);
-
+  // for (size_t i = 0; i < size; i++)
+  // {
+  //   if(*(uint8_t*)(paddr+i) != 0xFF) {
+  //     DBG_MSG("blank check: %p = %x\n", paddr+i, *(uint8_t*)(paddr+i));
+  //   }
+  // }
+  
   HAL_FLASH_Unlock();
   ret = program_space(paddr, buffer, size);
   HAL_FLASH_Lock();
 
   // Invalidate cache
-  __HAL_FLASH_DATA_CACHE_DISABLE();
-  // __HAL_FLASH_INSTRUCTION_CACHE_DISABLE();
-  __HAL_FLASH_DATA_CACHE_RESET();
-  // __HAL_FLASH_INSTRUCTION_CACHE_RESET();
-  // __HAL_FLASH_INSTRUCTION_CACHE_ENABLE();
-  __HAL_FLASH_DATA_CACHE_ENABLE();
+  // __HAL_FLASH_DATA_CACHE_DISABLE();
+  // // __HAL_FLASH_INSTRUCTION_CACHE_DISABLE();
+  // __HAL_FLASH_DATA_CACHE_RESET();
+  // // __HAL_FLASH_INSTRUCTION_CACHE_RESET();
+  // // __HAL_FLASH_INSTRUCTION_CACHE_ENABLE();
+  // __HAL_FLASH_DATA_CACHE_ENABLE();
 
   // DBG_MSG("verify %d\n", memcmp(buffer, (const void *)FLASH_ADDR(block, off), size));
 
@@ -78,26 +100,27 @@ int block_erase(const struct lfs_config *c, lfs_block_t block) {
   EraseInitStruct.Banks = FS_BANK;
   EraseInitStruct.Page = block + FLASH_ADDR2BLOCK((uintptr_t)FS_BASE);
   EraseInitStruct.NbPages = 1;
-  DBG_MSG("block 0x%x\r\n", EraseInitStruct.Page);
+  // DBG_MSG("block 0x%x\r\n", EraseInitStruct.Page);
 
   HAL_FLASH_Unlock();
 
   if (HAL_FLASHEx_Erase(&EraseInitStruct, &PageError) != HAL_OK) {
     ret = LFS_ERR_IO;
-    ERR_MSG("HAL_FLASHEx_Erase %#x failed", (unsigned int)PageError);
+    ERR_MSG("HAL_FLASHEx_Erase %#x failed\n", (unsigned int)PageError);
     goto erase_fail;
   }
 
   // Invalidate cache
-  __HAL_FLASH_DATA_CACHE_DISABLE();
-  // __HAL_FLASH_INSTRUCTION_CACHE_DISABLE();
-  __HAL_FLASH_DATA_CACHE_RESET();
-  // __HAL_FLASH_INSTRUCTION_CACHE_RESET();
-  // __HAL_FLASH_INSTRUCTION_CACHE_ENABLE();
-  __HAL_FLASH_DATA_CACHE_ENABLE();
+  // __HAL_FLASH_DATA_CACHE_DISABLE();
+  // // __HAL_FLASH_INSTRUCTION_CACHE_DISABLE();
+  // __HAL_FLASH_DATA_CACHE_RESET();
+  // // __HAL_FLASH_INSTRUCTION_CACHE_RESET();
+  // // __HAL_FLASH_INSTRUCTION_CACHE_ENABLE();
+  // __HAL_FLASH_DATA_CACHE_ENABLE();
 
 erase_fail:
   HAL_FLASH_Lock();
+  // DBG_MSG("done\n");
 
   return ret;
 }
@@ -121,7 +144,7 @@ void littlefs_init() {
   config.block_size = FLASH_PAGE_SIZE;
   config.block_count = CHIP_FLASH_SIZE / FLASH_PAGE_SIZE - FLASH_ADDR2BLOCK((uintptr_t)FS_BASE);
   config.block_cycles = 100000;
-  config.cache_size = CACHE_SIZE;
+  config.cache_size = LFS_CACHE_SIZE;
   config.lookahead_size = LOOKAHEAD_SIZE;
   config.read_buffer = read_buffer;
   config.prog_buffer = prog_buffer;
diff --git a/Src/main.c b/Src/main.c
index 4df37d5..74483e5 100644
--- a/Src/main.c
+++ b/Src/main.c
@@ -61,7 +61,7 @@ UART_HandleTypeDef huart2;
 
 /* USER CODE BEGIN PV */
 extern uint32_t _stack_boundary;
-uint32_t device_loop_enable;
+uint8_t device_loop_enable, usb_init_done;
 /* USER CODE END PV */
 
 /* Private function prototypes -----------------------------------------------*/
@@ -274,6 +274,20 @@ static void config_usb_mode(void) {
   // enable the device_periodic_task, which controls LED and Touch sensing
   device_loop_enable = 1;
 }
+
+static int check_is_nfc_en(void) {
+  uint32_t *flash_loc = (uint32_t*) 0x1FFF7808U;
+  uint32_t val = *flash_loc; //FLASH->PCROP1SR;
+  DBG_MSG("%x\n", val);
+  return val == 0xFFFFFFFFU || // ST production default value
+          val == 0xFFFF802a; // magic written by admin_vendor_nfc_enable()
+}
+
+// Called by core library
+void USBD_LL_Init_Done(void)
+{
+  usb_init_done = 1;
+}
 /* USER CODE END 0 */
 
 /**
@@ -311,7 +325,7 @@ int main(void) {
   MX_USART2_UART_Init();
   SetupMPU(); // comment out this line during on-chip debugging
   /* USER CODE BEGIN 2 */
-  in_nfc_mode = 1; // boot in NFC mode by default
+  in_nfc_mode = check_is_nfc_en(); // boot in NFC mode by default
   nfc_init();
   set_nfc_state(in_nfc_mode);
 
@@ -322,6 +336,10 @@ int main(void) {
   applets_install();
   init_apdu_buffer();
 
+  if (!in_nfc_mode) {
+    while (!detect_usb());
+    config_usb_mode();
+  }
   DBG_MSG("Main Loop\n");
   /* USER CODE END 2 */
 
@@ -343,7 +361,8 @@ int main(void) {
         DBG_MSG("Touch calibrating...\n");
         GPIO_Touch_Calibrate();
       }
-      device_loop(1);
+      if (usb_init_done)
+        device_loop(1);
       ++i;
     }
   }
diff --git a/Src/usbd_conf.c b/Src/usbd_conf.c
index ff4c81c..0f1868b 100644
--- a/Src/usbd_conf.c
+++ b/Src/usbd_conf.c
@@ -792,6 +792,7 @@ static void SystemClockConfig_Resume(void)
 {
   SystemClock_CustomConfig(false, true);
 }
+
 /* USER CODE END 5 */
 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
diff --git a/canokey-core b/canokey-core
index a4bba30..0afb6fc 160000
--- a/canokey-core
+++ b/canokey-core
@@ -1 +1 @@
-Subproject commit a4bba3032bbf1a9c235b6e8f19f38aa445da90f7
+Subproject commit 0afb6fcf6edc0aafd39290715c721983daee9bfa