diff --git a/Inc/cortexm/stm32/stm32f4.h b/Inc/cortexm/stm32/stm32f4.h
index 0e04fb8..01fa9f2 100644
--- a/Inc/cortexm/stm32/stm32f4.h
+++ b/Inc/cortexm/stm32/stm32f4.h
@@ -13,6 +13,7 @@
 #define STM32F4_FLASH_CR_PG		(1 << 0)
 #define STM32F4_FLASH_CR_SER		(1 << 1)
 #define STM32F4_FLASH_CR_MER		(1 << 2)
+#define STM32F4_FLASH_CR_SNB_SHIFT	(3)
 #define STM32F4_FLASH_CR_PSIZE8		(0 << 8)
 #define STM32F4_FLASH_CR_PSIZE16	(1 << 8)
 #define STM32F4_FLASH_CR_PSIZE32	(2 << 8)
diff --git a/Inc/cortexm/stm32/stm32l4.h b/Inc/cortexm/stm32/stm32l4.h
new file mode 100644
index 0000000..a552d53
--- /dev/null
+++ b/Inc/cortexm/stm32/stm32l4.h
@@ -0,0 +1,79 @@
+#ifndef __STM32L4_H
+#define __STM32L4_H
+
+#define STM32L4_PAGE_SIZE   0x800
+/* Flash Program ad Erase Controller Register Map */
+#define STM32L4_FPEC_BASE			0x40022000
+#define STM32L4_FLASH_ACR			(STM32L4_FPEC_BASE+0x00)
+#define STM32L4_FLASH_KEYR			(STM32L4_FPEC_BASE+0x08)
+#define STM32L4_FLASH_OPTKEYR		(STM32L4_FPEC_BASE+0x0c)
+#define STM32L4_FLASH_SR			(STM32L4_FPEC_BASE+0x10)
+#define STM32L4_FLASH_CR			(STM32L4_FPEC_BASE+0x14)
+#define STM32L4_FLASH_OPTR			(STM32L4_FPEC_BASE+0x20)
+
+#define STM32L4_FLASH_CR_PG			(1 << 0)
+#define STM32L4_FLASH_CR_PER		(1 << 1)
+#define STM32L4_FLASH_CR_MER1		(1 << 2)
+#define STM32L4_FLASH_CR_PAGE_SHIFT	3
+#define STM32L4_FLASH_CR_BKER		(1 << 11)
+#define STM32L4_FLASH_CR_MER2		(1 << 15)
+#define STM32L4_FLASH_CR_STRT		(1 << 16)
+#define STM32L4_FLASH_CR_OPTSTRT	(1 << 17)
+#define STM32L4_FLASH_CR_FSTPG	 	(1 << 18)
+#define STM32L4_FLASH_CR_EOPIE		(1 << 24)
+#define STM32L4_FLASH_CR_ERRIE		(1 << 25)
+#define STM32L4_FLASH_CR_OBL_LAUNCH	(1 << 27)
+#define STM32L4_FLASH_CR_OPTLOCK	(1 << 30)
+#define STM32L4_FLASH_CR_LOCK		(1 << 31)
+
+#define STM32L4_FLASH_SR_EOP		(1 << 0)
+#define STM32L4_FLASH_SR_OPERR		(1 << 1)
+#define STM32L4_FLASH_SR_PROGERR	(1 << 3)
+#define STM32L4_FLASH_SR_WRPERR		(1 << 4)
+#define STM32L4_FLASH_SR_PGAERR		(1 << 5)
+#define STM32L4_FLASH_SR_SIZERR		(1 << 6)
+#define STM32L4_FLASH_SR_PGSERR		(1 << 7)
+#define STM32L4_FLASH_SR_MSERR		(1 << 8)
+#define STM32L4_FLASH_SR_FASTERR	(1 << 9)
+#define STM32L4_FLASH_SR_RDERR		(1 << 14)
+#define STM32L4_FLASH_SR_OPTVERR	(1 << 15)
+#define STM32L4_FLASH_SR_ERROR_MASK	0xC3FA
+#define STM32L4_FLASH_SR_BSY		(1 << 16)
+
+#define STM32L4_KEY1 0x45670123
+#define STM32L4_KEY2 0xCDEF89AB
+
+#define STM32L4_OPTKEY1 0x08192A3B
+#define STM32L4_OPTKEY2 0x4C5D6E7F
+
+#define STM32L4_SR_ERROR_MASK	0xF2
+#define STM32L4_SR_EOP		0x01
+
+#define STM32L4_OR_DUALBANK		(1 << 21)
+
+#define STM32L4_DBGMCU_IDCODE	0xE0042000
+#define STM32L4_FLASH_SIZE_REG 0x1FFF75E0
+
+#define STM32L4_SIZE_OF_ONE_WRITE    0x1000
+#define STM32L4_ERASE_TIME_IN_WRITES 10
+
+/* 
+ * flash errors returned to flash_target_task
+ * 0x0 - 0x1000   - reserved for target (we can use this pool here)
+ * bit 9 (0x200)  - error while flash erasing. [8..0] represent FLASH_SR[8..0]
+ * bit 10 (0x400) - error while flash writing. [8..0] represent FLASH_SR[8..0]
+ */
+#define STM32L4_ERASE_ERROR_BIT            0x200
+#define STM32L4_FLASH_ERROR_BIT            0x400
+#define STM32L4_ERASE_NEVER_END            0x800
+#define STM32L4_ERROR_ON_FLASH_WRITE_SETUP 0x801
+
+typedef struct CORTEXM_s CORTEXM_t;
+
+typedef struct STM32L4_PRIV_s {
+  CORTEXM_t *cortex;
+} STM32L4_PRIV_t;
+
+int stm32l4_probe(CORTEXM_t *cortexm);
+
+#endif //__STM32L4_H
\ No newline at end of file
diff --git a/Inc/jtag/jtag_low_level.h b/Inc/jtag/jtag_low_level.h
index 73a695d..ee519f1 100644
--- a/Inc/jtag/jtag_low_level.h
+++ b/Inc/jtag/jtag_low_level.h
@@ -3,7 +3,9 @@
 
 #include "stm32f4xx_it.h"
 
-#define TCKWAIT 10
+// if TCKWAIT == 0 than alternate method of delay is used.
+// if TCKWAIT > 0 than it represent half of TCK cycle in micro seconds
+#define TCKWAIT 0
 #define SIZEOF_IN_BITS(x) (sizeof(x) * 8)
 
 // maximum size of transfer in jtag_tdin function
diff --git a/Makefile b/Makefile
index 392a490..16f0189 100644
--- a/Makefile
+++ b/Makefile
@@ -170,6 +170,7 @@ Src/adiv5/adiv5_jtag.c \
 Src/adiv5/adiv5.c \
 Src/cortexm/cortexm.c \
 Src/cortexm/stm32/stm32f4.c \
+Src/cortexm/stm32/stm32l4.c \
 Src/target.c
 
 # ASM sources
diff --git a/Src/cortexm/cortexm.c b/Src/cortexm/cortexm.c
index 37449ac..05d628a 100644
--- a/Src/cortexm/cortexm.c
+++ b/Src/cortexm/cortexm.c
@@ -3,6 +3,7 @@
 #include "adiv5/adiv5.h"
 #include "cortexm/cortexm.h"
 #include "cortexm/stm32/stm32f4.h"
+#include "cortexm/stm32/stm32l4.h"
 
 inline static uint32_t cortexm_read_word(CORTEXM_PRIV_t *priv, uint32_t addr)
 {
@@ -135,6 +136,10 @@ int probe_cortexm(ADIv5_AP_t *ap)
     return 1;
   }
 
+  if(stm32l4_probe(cortexm)){
+    return 1;
+  }
+
   // Non of the targets successful probed. Cleanup
   vPortFree(cortexm->priv);
   vPortFree(cortexm);
diff --git a/Src/cortexm/stm32/flash_stubs/stm32l4_flash_stub.S b/Src/cortexm/stm32/flash_stubs/stm32l4_flash_stub.S
new file mode 100644
index 0000000..0a65346
--- /dev/null
+++ b/Src/cortexm/stm32/flash_stubs/stm32l4_flash_stub.S
@@ -0,0 +1,44 @@
+  .text
+  .syntax unified
+  .cpu cortex-m4
+  .thumb
+
+
+#define STM32_FLASH_CR_OFFSET	0x14	/* offset of CR register in FLASH struct */
+#define STM32_FLASH_SR_OFFSET	0x10	/* offset of SR register in FLASH struct */
+
+_start:
+  ldr r0, _flashbase
+  ldr r1, _addr
+  adr r2, _data
+  ldr r3, _size
+  ldr r5, _cr
+_next:
+  cbz r3, _done
+  str r5, [r0, #STM32_FLASH_CR_OFFSET]
+  ldr r4, [r2]
+  str r4, [r1]
+_wait:
+  ldr  r4, [r0, #STM32_FLASH_SR_OFFSET]
+  tst  r6, #0x10000   /* BSY (bit16) == 1 => operation in progress */
+  bne _wait
+  tst r6, #0xfa       /* PGSERR | PGPERR | PGAERR | WRPERR | PROGERR*/
+  bne _done           /* fail... */
+
+  subs r3, #4
+  adds r1, #4
+  adds r2, #4
+  b _next
+_done:
+  bkpt
+
+_cr:
+  .word 0x00000001 /*(Value to write to FLASH_CR) PG*/
+_flashbase:
+  .word 0x40022000 /* (FPEC_BASE) */
+_addr:
+  .word 0x0
+_size:
+  .word 0x0
+_data:
+  .word 0x0
diff --git a/Src/cortexm/stm32/flash_stubs/stm32l4_flash_stub.o b/Src/cortexm/stm32/flash_stubs/stm32l4_flash_stub.o
new file mode 100644
index 0000000..639ce66
Binary files /dev/null and b/Src/cortexm/stm32/flash_stubs/stm32l4_flash_stub.o differ
diff --git a/Src/cortexm/stm32/stm32f4.c b/Src/cortexm/stm32/stm32f4.c
index c5ffe53..e8686e8 100644
--- a/Src/cortexm/stm32/stm32f4.c
+++ b/Src/cortexm/stm32/stm32f4.c
@@ -61,7 +61,6 @@ static int stm32f4_erase_all_flash(STM32F4_PRIV_t *priv, int *progress, int prog
   int time = 0;
   int progress_step = progress_end/10;
 
-  printf("Erase Flash\n");
   /* Flash mass erase start instruction */
   priv->cortex->ops->write_word(priv->cortex->priv, STM32F4_FLASH_CR, STM32F4_FLASH_CR_MER);
   priv->cortex->ops->write_word(priv->cortex->priv, STM32F4_FLASH_CR, STM32F4_FLASH_CR_STRT | STM32F4_FLASH_CR_MER | STM32F4_FLASH_CR_EOPIE);
@@ -73,7 +72,7 @@ static int stm32f4_erase_all_flash(STM32F4_PRIV_t *priv, int *progress, int prog
       printf("Error while waiting for erase end\n");
       return STM32F4_ERASE_NEVER_END;
     }
-    osDelay(1);
+    osDelay(10);
     time++;
     // after each 100 loops add one to progress, we asume that whole erase will take 1000 loops
     if(time > 100) {
@@ -98,6 +97,84 @@ static int stm32f4_erase_all_flash(STM32F4_PRIV_t *priv, int *progress, int prog
   return 0;
 }
 
+// This function return how many sectors is needed in bank 1 of flash
+// to save len bytes. If len is bigger then 7 sectors, return bigger number than number of sectors
+static uint8_t stm32f4_sectors_in_bank1(int len)
+{
+  uint8_t sector = 0;
+  while(len > 0) {
+    /*
+     * Secotr sizes from:
+     * RM0090 -- STM32F4xx reference manual, chapter 3.3
+     */
+    switch (sector) {
+      case 0:
+      case 1:
+      case 2:
+      case 3:
+        len -= 0x4000;
+        break;
+      case 4:
+        len -= 0x10000;
+        break;
+      case 5:
+      case 6:
+      case 7:
+        len -= 0x20000;
+        break;
+      case 8:
+        return 255;
+    }
+    sector++;
+  }
+  return sector;
+}
+
+static int stm32f4_erase_flash(STM32F4_PRIV_t *priv, int len, int *progress, int progress_end)
+{
+  uint16_t sr;
+  uint32_t cr;
+  uint8_t  last_sector_to_flash = stm32f4_sectors_in_bank1(len);
+  int progress_step = (progress_end + 31)/32;
+
+  printf("Erase Flash\n");
+  // TODO: For simplicity only when writing less than 512KB optimal sector erase
+  //       algorithm is used. First 7 sectors are the same size which is independent
+  //       of single and dual bank mode and size of flash memory.
+  if(last_sector_to_flash > 7) {
+    return stm32f4_erase_all_flash(priv, progress, progress_end);
+  }
+
+  for(uint8_t sector = 0; sector <= last_sector_to_flash; sector++) {
+    cr = STM32F4_FLASH_CR_EOPIE | STM32F4_FLASH_CR_ERRIE | STM32F4_FLASH_CR_SER;
+    cr |= sector << STM32F4_FLASH_CR_SNB_SHIFT;
+    /* Flash page erase instruction */
+    priv->cortex->ops->write_word(priv->cortex->priv, STM32F4_FLASH_CR, cr);
+    /* write address to FMA */
+    priv->cortex->ops->write_word(priv->cortex->priv, STM32F4_FLASH_CR, cr | STM32F4_FLASH_CR_STRT);
+    /* Read FLASH_SR to poll for BSY bit */
+    while(priv->cortex->ops->read_word(priv->cortex->priv, STM32F4_FLASH_SR) & STM32F4_FLASH_SR_BSY) {
+      if(priv->cortex->ops->check_error(priv->cortex->priv)) {
+        // TODO: handle error
+        printf("Error while waiting for erase end\n");
+        return STM32F4_ERASE_NEVER_END;
+      }
+    }
+    *progress += progress_step;
+    printf("Flash progress %d\n", *progress);
+  }
+
+  /* Check for error */
+  sr = priv->cortex->ops->read_word(priv->cortex->priv, STM32F4_FLASH_SR);
+  if ((sr & STM32F4_SR_ERROR_MASK) || !(sr & STM32F4_SR_EOP)) {
+    // TODO: handle error
+    printf("Error after erase 0x%x\n", sr);
+    return sr | STM32F4_ERASE_ERROR_BIT;
+  }
+
+  return 0;
+}
+
 static int stm32f4_flash_write(STM32F4_PRIV_t *priv, uint32_t dest, const uint32_t *src, int len)
 {
   uint32_t start_of_ram = 0x20000000;
@@ -141,25 +218,28 @@ static int stm32f4_program(void *priv_void, FIL *file, int *progress)
   UINT br;
   uint8_t unaligned;
   uint32_t addr = 0x8000000; // start of flash memory
-  uint32_t *data = pvPortMalloc(STM32F4_SIZE_OF_ONE_WRITE/sizeof(uint32_t));
+  uint32_t *data = pvPortMalloc(STM32F4_SIZE_OF_ONE_WRITE);
   STM32F4_PRIV_t *priv = priv_void;
   uint16_t result;
+  uint32_t file_len = f_size(file);
 
   // these variables are only needed to show progress
-  int number_of_writes = (f_size(file) + STM32F4_SIZE_OF_ONE_WRITE - 1)/STM32F4_SIZE_OF_ONE_WRITE + STM32F4_ERASE_TIME_IN_WRITES;
-  float progress_as_float = 100 * STM32F4_ERASE_TIME_IN_WRITES/number_of_writes;
-  float progress_on_one_write = 100.0/number_of_writes;
+  int number_of_writes = (file_len + STM32F4_SIZE_OF_ONE_WRITE - 1)/STM32F4_SIZE_OF_ONE_WRITE;
+  float progress_as_float = 100 * STM32F4_ERASE_TIME_IN_WRITES/(number_of_writes + STM32F4_ERASE_TIME_IN_WRITES);
+  float progress_on_one_write;
 
   printf("Start flashing STM32F4x\n");
 
   priv->cortex->ops->halt_request(priv->cortex->priv);
   stm32f4_flash_unlock(priv);
-  result = stm32f4_erase_all_flash(priv, progress, progress_as_float);
+  result = stm32f4_erase_flash(priv, file_len, progress, progress_as_float);
   if(result) {
     vPortFree(data);
     return result;
   }
 
+  progress_as_float = *progress;
+  progress_on_one_write = (100 - *progress)/number_of_writes;
 
   do {
     f_read(file, data, STM32F4_SIZE_OF_ONE_WRITE, &br);
diff --git a/Src/cortexm/stm32/stm32l4.c b/Src/cortexm/stm32/stm32l4.c
new file mode 100644
index 0000000..88cf2f8
--- /dev/null
+++ b/Src/cortexm/stm32/stm32l4.c
@@ -0,0 +1,307 @@
+#include "stm32f4xx_it.h"
+#include "cmsis_os.h"
+#include "cortexm/cortexm.h"
+#include "cortexm/stm32/stm32l4.h"
+#include "fatfs.h"
+#include "target.h"
+
+/* This routine is uses word access. */
+static uint16_t stm32l4_flash_write_stub[] = {
+// 00000000 <_start>:
+  0xf8df, 0x002c,     // ldr.w	r0, [pc, #44]	; 30 <_flashbase>
+  0x490b,             // ldr	r1, [pc, #44]	; (34 <_addr>)
+  0xa20d,             // add	r2, pc, #52	; (adr r2, 3c <_data>)
+  0x4b0b,             // ldr	r3, [pc, #44]	; (38 <_size>)
+  0x4d08,             // ldr	r5, [pc, #32]	; (2c <_cr>)
+
+// 0000000c <_next>:
+  0xb16b,             // cbz	r3, 2a <_done>
+  0x6145,             // str	r5, [r0, #20]
+  0x6814,             // ldr	r4, [r2, #0]
+  0x600c,             // str	r4, [r1, #0]
+
+//00000014 <_wait>:
+  0x6904,             // ldr	r4, [r0, #16]
+  0xf416, 0x3f80,     // tst.w	r6, #65536	; 0x10000
+  0xd1fb,             // bne.n	14 <_wait>
+  0xf016, 0x0ffa,     // tst.w	r6, #250	; 0xfa
+  0xd103,             // bne.n	2a <_done>
+  0x3b04,             // subs	r3, #4
+  0x3104,             // adds	r1, #4
+  0x3204,             // adds	r2, #4
+  0xe7f0,             // b.n	c <_next>
+
+//0000002a <_done>:
+  0xbe00,             // bkpt	0x0000
+
+//0000002c <_cr>:
+  0x0001, 0x0000,     //.word	0x00000001
+
+//00000030 <_flashbase>:
+  0x2000, 0x4002,     //.word	0x40022000
+
+//00000034 <_addr>:
+  0x0000, 0x0000,     //.word	0x00000000
+
+//00000038 <_size>:
+  0x0000, 0x0000      //.word	0x00000000
+
+//  0000003c <_data>:
+};
+
+static void stm32l4_flash_unlock(STM32L4_PRIV_t *priv)
+{
+  printf("Flash unlock\n");
+  if (priv->cortex->ops->read_word(priv->cortex->priv, STM32L4_FLASH_CR) & STM32L4_FLASH_CR_LOCK) {
+    /* Enable FPEC controller access */
+    priv->cortex->ops->write_word(priv->cortex->priv, STM32L4_FLASH_KEYR, STM32L4_KEY1);
+    priv->cortex->ops->write_word(priv->cortex->priv, STM32L4_FLASH_KEYR, STM32L4_KEY2);
+  }
+}
+
+static int stm32l4_erase_all_flash(STM32L4_PRIV_t *priv, int *progress, int progress_end)
+{
+  uint16_t sr;
+  int time = 0;
+  int progress_step = progress_end/10;
+
+  /* Flash mass erase start instruction */
+  priv->cortex->ops->write_word(priv->cortex->priv, STM32L4_FLASH_CR, STM32L4_FLASH_CR_MER1 | STM32L4_FLASH_CR_MER2);
+  priv->cortex->ops->write_word(priv->cortex->priv, STM32L4_FLASH_CR, STM32L4_FLASH_CR_STRT | STM32L4_FLASH_CR_MER1 | STM32L4_FLASH_CR_MER2 | STM32L4_FLASH_CR_EOPIE);
+
+  /* Read FLASH_SR to poll for BSY bit */
+  while(priv->cortex->ops->read_word(priv->cortex->priv, STM32L4_FLASH_SR) & STM32L4_FLASH_SR_BSY) {
+    if(priv->cortex->ops->check_error(priv->cortex->priv)) {
+      // TODO: handle error
+      printf("Error while waiting for erase end\n");
+      return STM32L4_ERASE_NEVER_END;
+    }
+    osDelay(10);
+    time++;
+    // after each 100 loops add one to progress, we asume that whole erase will take 1000 loops
+    if(time > 100) {
+      time = 0;
+      if(*progress < progress_end - progress_step) {
+        *progress += progress_step;
+        printf("Flash progress %d\n", *progress);
+      }
+    }
+  }
+
+  /* Check for error */
+  sr = priv->cortex->ops->read_word(priv->cortex->priv, STM32L4_FLASH_SR);
+  if ((sr & STM32L4_SR_ERROR_MASK) || !(sr & STM32L4_SR_EOP)) {
+    // TODO: handle error
+    printf("Error after erase 0x%x\n", sr);
+    return sr | STM32L4_ERASE_ERROR_BIT;
+  }
+
+  // End of erase, update progress
+  *progress = progress_end;
+  return 0;
+}
+
+// This function return how many sectors is needed to save len bytes.
+// If len is bigger then 64 sectors, return bigger number than number of sectors
+static uint16_t stm32l4_last_sector_to_erase(int len)
+{
+  uint16_t sectors = len/STM32L4_PAGE_SIZE;
+  return sectors <= 63 ? sectors : 1024;
+}
+
+static int stm32l4_erase_flash(STM32L4_PRIV_t *priv, int len, int *progress, int progress_end)
+{
+  uint16_t sr;
+  uint32_t cr;
+  uint16_t last_sector_to_flash = stm32l4_last_sector_to_erase(len);
+  int progress_step = (progress_end + 511)/512;
+
+  printf("Erase Flash\n");
+  // TODO: For simplicity only when writing less than 64 sectors (2KB each) optimal sector erase
+  //       algorithm is used. First 64 sectors are always present which is independent
+  //       of size of flash memory.
+  if(last_sector_to_flash > 63) {
+    return stm32l4_erase_all_flash(priv, progress, progress_end);
+  }
+
+  for(uint8_t sector = 0; sector <= last_sector_to_flash; sector++) {
+    cr = STM32L4_FLASH_CR_EOPIE | STM32L4_FLASH_CR_ERRIE | STM32L4_FLASH_CR_PER;
+    cr |= sector << STM32L4_FLASH_CR_PAGE_SHIFT;
+    /* Flash page erase instruction */
+    priv->cortex->ops->write_word(priv->cortex->priv, STM32L4_FLASH_CR, cr);
+    /* write address to FMA */
+    priv->cortex->ops->write_word(priv->cortex->priv, STM32L4_FLASH_CR, cr | STM32L4_FLASH_CR_STRT);
+    /* Read FLASH_SR to poll for BSY bit */
+    while(priv->cortex->ops->read_word(priv->cortex->priv, STM32L4_FLASH_SR) & STM32L4_FLASH_SR_BSY) {
+      if(priv->cortex->ops->check_error(priv->cortex->priv)) {
+        // TODO: handle error
+        printf("Error while waiting for erase end\n");
+        return STM32L4_ERASE_NEVER_END;
+      }
+    }
+    *progress += progress_step;
+    printf("Flash progress %d\n", *progress);
+  }
+
+  /* Check for error */
+  sr = priv->cortex->ops->read_word(priv->cortex->priv, STM32L4_FLASH_SR);
+  if ((sr & STM32L4_SR_ERROR_MASK) || !(sr & STM32L4_SR_EOP)) {
+    // TODO: handle error
+    printf("Error after erase 0x%x\n", sr);
+    return sr | STM32L4_ERASE_ERROR_BIT;
+  }
+
+  return 0;
+}
+
+static int stm32l4_flash_write(STM32L4_PRIV_t *priv, uint32_t dest, const uint32_t *src, int len)
+{
+  uint32_t start_of_ram = 0x20000000;
+  uint32_t stub_len = 0x3c;
+  uint16_t sr;
+
+  /* Fill stm32l4_flash_write_stub with address and size */
+	*(uint32_t *)&(stm32l4_flash_write_stub[26]) = dest;
+  *(uint32_t *)&(stm32l4_flash_write_stub[28]) = len;
+
+  /* Write stub and data to target ram and set PC */
+  priv->cortex->ops->write_words(priv->cortex->priv, start_of_ram, (void*)stm32l4_flash_write_stub, stub_len);
+  priv->cortex->ops->write_words(priv->cortex->priv, start_of_ram + stub_len, src, len);
+  priv->cortex->ops->pc_write(priv->cortex->priv, start_of_ram);
+  if(priv->cortex->ops->check_error(priv->cortex->priv)) {
+    // TODO: handle error
+    printf("ERROR: Filed to setup write operation\n");
+    return STM32L4_ERROR_ON_FLASH_WRITE_SETUP;
+  }
+
+  /* Execute the stub */
+  priv->cortex->ops->halt_resume(priv->cortex->priv);
+  while(!priv->cortex->ops->halt_wait(priv->cortex->priv)) {
+    // Don't be greedy about CPU, allow another task
+    osDelay(10);
+  }
+
+  /* Check for error */
+  sr = priv->cortex->ops->read_word(priv->cortex->priv, STM32L4_FLASH_SR);
+  if (sr & STM32L4_SR_ERROR_MASK) {
+    // TODO: handle error
+    printf("ERROR: writing ended with error 0x%x\n", sr);
+    return sr | STM32L4_FLASH_ERROR_BIT;
+  }
+
+  return 0;
+}
+
+static int stm32l4_program(void *priv_void, FIL *file, int *progress)
+{
+  UINT br;
+  uint8_t unaligned;
+  uint32_t addr = 0x8000000; // start of flash memory
+  uint32_t *data = pvPortMalloc(STM32L4_SIZE_OF_ONE_WRITE);
+  STM32L4_PRIV_t *priv = priv_void;
+  uint16_t result;
+  uint32_t file_len = f_size(file);
+
+  // these variables are only needed to show progress
+  int number_of_writes = (file_len + STM32L4_SIZE_OF_ONE_WRITE - 1)/STM32L4_SIZE_OF_ONE_WRITE;
+  float progress_as_float = 100 * STM32L4_ERASE_TIME_IN_WRITES/(number_of_writes + STM32L4_ERASE_TIME_IN_WRITES);
+  float progress_on_one_write;
+
+  printf("Start flashing STM32L4x\n");
+
+  priv->cortex->ops->halt_request(priv->cortex->priv);
+  stm32l4_flash_unlock(priv);
+  result = stm32l4_erase_flash(priv, file_len, progress, progress_as_float);
+  if(result) {
+    vPortFree(data);
+    return result;
+  }
+
+  progress_as_float = *progress;
+  progress_on_one_write = (100 - *progress)/number_of_writes;
+
+  do {
+    f_read(file, data, STM32L4_SIZE_OF_ONE_WRITE, &br);
+    printf("flash 0x%x bytes on 0x%lx\n", br, addr);
+    unaligned = br & 0x3;
+    if (unaligned) {
+      // If number of readed bytes % sizeof(uint32_t) != 0, last readed bytes are unaligned.
+      // Fill all unreaded bytes in last 4 bytes as 0xFF. This way flash is not damaged
+      // by writing additional (1..3) bytes.
+      br >>= 2;
+      data[br] |= ((~0) >> (unaligned << 0x3));
+      // add modified bytes to bytes that will be written
+      br++;
+      br <<= 2;
+      result = stm32l4_flash_write(priv, addr, data, br);
+      if(result) {
+        vPortFree(data);
+        return result;
+      }
+      // Unaligned read is always smaller then SIZE_OF_ONE_WRITE.
+      // This is EOF so we have done here.
+      break;
+    }
+    result = stm32l4_flash_write(priv, addr, data, br);
+    if(result) {
+      vPortFree(data);
+      return 1;
+    }
+    addr += br;
+
+    progress_as_float += progress_on_one_write;
+    *progress = (int)progress_as_float;
+    printf("Flash progress %d\n", *progress);
+
+    // EOF is when readed less bytes than requested
+  } while(br == STM32L4_SIZE_OF_ONE_WRITE);
+
+  vPortFree(data);
+
+  printf("Device flashed\nReset device\n");
+  priv->cortex->ops->restart(priv->cortex->priv);
+
+  *progress = 100;
+  return 0;
+}
+
+static void stm32l4_restart(void *priv)
+{
+  ((STM32L4_PRIV_t*)priv)->cortex->ops->restart(((STM32L4_PRIV_t*)priv)->cortex->priv);
+}
+
+static void stm32l4_free_priv(void *priv){
+  ((STM32L4_PRIV_t*)priv)->cortex->ops->free(((STM32L4_PRIV_t*)priv)->cortex);
+  vPortFree(priv);
+}
+
+static TARGET_OPS_t stm32l4_ops = {
+  .flash_target = stm32l4_program,
+  .reset_target = stm32l4_restart,
+
+  .free_priv = stm32l4_free_priv
+};
+
+static char stm32l4_name[] = "STM32L4x";
+
+int stm32l4_probe(CORTEXM_t *cortexm)
+{
+  uint32_t idcode;
+  STM32L4_PRIV_t *priv;
+
+  idcode = cortexm->ops->read_word(cortexm->priv, STM32L4_DBGMCU_IDCODE);
+  printf("STM32L4 probed id code: 0x%lx\n", idcode);
+
+  switch (idcode & 0xFFF) {
+    case 0x461: /* L496/RM0351 */
+    case 0x415: /* L471/RM0392, L475/RM0395, L476/RM0351 */
+    case 0x462: /* L45x L46x / RM0394  */
+    case 0x435: /* L43x L44x / RM0394  */
+      priv = pvPortMalloc(sizeof(STM32L4_PRIV_t));
+      priv->cortex = cortexm;
+      register_target(priv, &stm32l4_ops, stm32l4_name);
+      return 1;
+  }
+
+  return 0;
+}
diff --git a/Src/jtag/jtag_low_level.c b/Src/jtag/jtag_low_level.c
index 965f91d..e4e8ebf 100644
--- a/Src/jtag/jtag_low_level.c
+++ b/Src/jtag/jtag_low_level.c
@@ -5,7 +5,10 @@
 #include "jtag/jtag_low_level.h"
 
 uint32_t getUs(void) {
-  uint32_t usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
+  static uint32_t usTicks = 0;
+  if (usTicks == 0) {
+    usTicks = HAL_RCC_GetSysClockFreq() / 1000000;
+  }
   register uint32_t ms, cycle_cnt;
   do {
     ms = HAL_GetTick();
@@ -23,15 +26,38 @@ void delayUs(uint16_t micros)
   }
 }
 
+#if TCKWAIT == 0
+// current implementation of JTAG low level routines allow us to get ~1MHz if there
+// is no additional delay.
+static inline void short_delay(){
+  asm("nop");
+  asm("nop");
+  asm("nop");
+  asm("nop");
+  asm("nop");
+  asm("nop");
+  asm("nop");
+  asm("nop");
+}
+#endif
+
 void jtag_tclk_up()
 {
+#if TCKWAIT == 0
+  short_delay();
+#else
   delayUs(TCKWAIT);
+#endif
   HAL_GPIO_WritePin(JTAG_TCLK_GPIO_Port, JTAG_TCLK_Pin, GPIO_PIN_SET);
 }
 
 void jtag_tclk_down()
 {
+#if TCKWAIT == 0
+  short_delay();
+#else
   delayUs(TCKWAIT);
+#endif
   HAL_GPIO_WritePin(JTAG_TCLK_GPIO_Port, JTAG_TCLK_Pin, GPIO_PIN_RESET);
 }