niietcm4: support for NIIET's Cortex-M4 microcontrollers

This adds docs, example config, flash driver.
Driver is only supports K1921VK01T model for now.

Change-Id: I135259bb055dd2df1a17de99f066e2b24eae1b0f
Signed-off-by: Bogdan Kolbov <kolbov@niiet.ru>
Reviewed-on: http://openocd.zylin.com/3011
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>

7 files changed, 1954 insertions, 2 deletions

diff --git a/README b/README
index d889d2f..8f98e7f 100644
--- a/README
+++ b/README
@@ -128,7 +128,7 @@ Flash drivers
 
 ADUC702x, AT91SAM, AVR, CFI, DSP5680xx, EFM32, EM357, FM3, Kinetis,
 LPC8xx/LPC1xxx/LPC2xxx/LPC541xx, LPC2900, LPCSPIFI, Marvell QSPI,
-Milandr, NuMicro, PIC32mx, PSoC4, SiM3x, Stellaris, STM32, STMSMI,
+Milandr, NIIET, NuMicro, PIC32mx, PSoC4, SiM3x, Stellaris, STM32, STMSMI,
 STR7x, STR9x, nRF51; NAND controllers of AT91SAM9, LPC3180, LPC32xx,
 i.MX31, MXC, NUC910, Orion/Kirkwood, S3C24xx, S3C6400, XMC4xxx.
 
diff --git a/contrib/loaders/flash/k1921vk01t.S b/contrib/loaders/flash/k1921vk01t.S
new file mode 100644
index 0000000..b8f0b53
--- /dev/null
+++ b/contrib/loaders/flash/k1921vk01t.S
@@ -0,0 +1,112 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by Bogdan Kolbov                                   *
+ *   kolbov@niiet.ru                                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.                                        *
+ ***************************************************************************/
+
+	.text
+	.syntax unified
+	.cpu cortex-m4
+	.thumb
+	.thumb_func
+
+/* K1921VK01T has 128-bitwidth flash, so it`s able to load 4x32-bit words at the time.
+ * And only after all words loaded we can start write
+ */
+
+/* Registers addresses */
+#define FLASH_FMA	0x00		/* Address reg */
+#define FLASH_FMD1	0x04		/* Data1 reg */
+#define FLASH_FMC	0x08		/* Command reg */
+#define FLASH_FCIS	0x0C		/* Operation Status reg */
+#define FLASH_FCIC	0x14		/* Operation Status Clear reg */
+#define FLASH_FMD2	0x50		/* Data2 reg */
+#define FLASH_FMD3	0x54		/* Data3 reg */
+#define FLASH_FMD4	0x58		/* Data4 reg*/
+
+	/* Params:
+	 * r0 - write cmd (in), status (out)
+	 * r1 - count
+	 * r2 - workarea start
+	 * r3 - workarea end
+	 * r4 - target address
+	 * Clobbered:
+	 * r5 - rp
+	 * r6 - wp, tmp
+	 * r7 - flash base
+	 */
+
+ldr     r7, =#0xA001C000  /* Flash reg base*/
+
+wait_fifo:
+	ldr		r6, [r2, #0]	/* read wp */
+	cmp		r6, #0			/* abort if wp == 0 */
+	beq		exit
+	ldr		r5, [r2, #4]	/* read rp */
+	cmp		r5, r6			/* wait until rp != wp */
+	beq		wait_fifo
+
+
+load_data:
+	ldr r6, [r5]			/* read data1 */
+	str r6, [r7, #FLASH_FMD1]
+	adds	r5, #4
+
+	ldr r6, [r5]			/* read data2 */
+	str r6, [r7, #FLASH_FMD2]
+	adds	r5, #4
+
+	ldr r6, [r5]			/* read data3 */
+	str r6, [r7, #FLASH_FMD3]
+	adds	r5, #4
+
+	ldr r6, [r5]			/* read data4 */
+	str r6, [r7, #FLASH_FMD4]
+	adds	r5, #4
+
+start_write:
+	str r4, [r7, #FLASH_FMA]		/* set addr */
+	adds	r4, #16
+	str r0, [r7, #FLASH_FMC]		/* write cmd */
+
+busy:
+	ldr		r6, [r7, #FLASH_FCIS]	/* wait until flag set */
+	cmp		r6, #0x0
+	beq		busy
+
+	cmp		r6, #2			/* check the error bit */
+	beq		error
+
+	movs	r6, #1			/* clear flags */
+	str r6, [r7, #FLASH_FCIC]
+
+	cmp 	r5, r3			/* wrap rp at end of buffer */
+	bcc	no_wrap
+	mov	r5, r2
+	adds	r5, #8
+no_wrap:
+	str 	r5, [r2, #4]	/* store rp */
+	subs	r1, r1, #1		/* decrement 16-byte block count */
+	cmp     r1, #0
+	beq     exit		/* loop if not done */
+	b	wait_fifo
+
+error:
+	movs	r0, #0
+	str		r0, [r2, #4]	/* set rp = 0 on error */
+exit:
+	mov		r0, r6			/* return status in r0 */
+	bkpt	#0
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 14835a9..81a45d1 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -5380,6 +5380,66 @@ if @{ [info exists IMEMORY] && [string equal $IMEMORY true] @} @{
 @end example
 @end deffn
 
+@deffn {Flash Driver} niietcm4
+This drivers handles the integrated NOR flash on NIIET Cortex-M4
+based controllers. Flash size and sector layout are auto-configured by the driver.
+Main flash memory is called "Bootflash" and has main region and info region.
+Info region is NOT memory mapped by default,
+but it can replace first part of main region if needed.
+Full erase, single and block writes are supported for both main and info regions.
+There is additional not memory mapped flash called "Userflash", which
+also have division into regions: main and info.
+Purpose of userflash - to store system and user settings.
+Driver has special commands to perform operations with this memmory.
+
+@example
+flash bank $_FLASHNAME niietcm4 0 0 0 0 $_TARGETNAME
+@end example
+
+Some niietcm4-specific commands are defined:
+
+@deffn Command {niietcm4 uflash_read_byte} bank ('main'|'info') address
+Read byte from main or info userflash region.
+@end deffn
+
+@deffn Command {niietcm4 uflash_write_byte} bank ('main'|'info') address value
+Write byte to main or info userflash region.
+@end deffn
+
+@deffn Command {niietcm4 uflash_full_erase} bank
+Erase all userflash including info region.
+@end deffn
+
+@deffn Command {niietcm4 uflash_erase} bank ('main'|'info') first_sector last_sector
+Erase sectors of main or info userflash region, starting at sector first up to and including last.
+@end deffn
+
+@deffn Command {niietcm4 uflash_protect_check} bank ('main'|'info')
+Check sectors protect.
+@end deffn
+
+@deffn Command {niietcm4 uflash_protect} bank ('main'|'info') first_sector last_sector ('on'|'off')
+Protect sectors of main or info userflash region, starting at sector first up to and including last.
+@end deffn
+
+@deffn Command {niietcm4 bflash_info_remap} bank ('on'|'off')
+Enable remapping bootflash info region to 0x00000000 (or 0x40000000 if external memory boot used).
+@end deffn
+
+@deffn Command {niietcm4 extmem_cfg} bank ('gpioa'|'gpiob'|'gpioc'|'gpiod'|'gpioe'|'gpiof'|'gpiog'|'gpioh') pin_num ('func1'|'func3')
+Configure external memory interface for boot.
+@end deffn
+
+@deffn Command {niietcm4 service_mode_erase} bank
+Perform emergency erase of all flash (bootflash and userflash).
+@end deffn
+
+@deffn Command {niietcm4 driver_info} bank
+Show information about flash driver.
+@end deffn
+
+@end deffn
+
 @deffn {Flash Driver} nrf51
 All members of the nRF51 microcontroller families from Nordic Semiconductor
 include internal flash and use ARM Cortex-M0 core.
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index eabf6f9..834e4d4 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -50,7 +50,9 @@ NOR_DRIVERS = \
 	mrvlqspi.c \
 	psoc4.c \
 	sim3x.c \
-	xmc4xxx.c
+	xmc4xxx.c \
+	niietcm4.c
+
 
 noinst_HEADERS = \
 	core.h \
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 24bd306..48d584e 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -63,6 +63,7 @@ extern struct flash_driver psoc4_flash;
 extern struct flash_driver sim3x_flash;
 extern struct flash_driver jtagspi_flash;
 extern struct flash_driver xmc4xxx_flash;
+extern struct flash_driver niietcm4_flash;
 
 /**
  * The list of built-in flash drivers.
@@ -110,6 +111,7 @@ static struct flash_driver *flash_drivers[] = {
 	&sim3x_flash,
 	&jtagspi_flash,
 	&xmc4xxx_flash,
+	&niietcm4_flash,
 	NULL,
 };
 
diff --git a/src/flash/nor/niietcm4.c b/src/flash/nor/niietcm4.c
new file mode 100644
index 0000000..78467c5
--- /dev/null
+++ b/src/flash/nor/niietcm4.c
@@ -0,0 +1,1721 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by Bogdan Kolbov                                   *
+ *   kolbov@niiet.ru                                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.                                        *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "imp.h"
+#include <helper/binarybuffer.h>
+#include <target/algorithm.h>
+#include <target/armv7m.h>
+
+#define FLASH_DRIVER_VER			0x00010000
+#define CHIPID_ADDR					0xF0000000
+#define K1921VK01T_ID				0x00000000
+
+/*==============================================================================
+ *							FLASH CONTROL REGS
+ *==============================================================================
+ */
+
+#define MAIN_MEM_TYPE				0
+#define INFO_MEM_TYPE				1
+#define SERVICE_MODE_ERASE_ADDR		0x80030164
+#define MAGIC_KEY					0xA442
+
+/*-- BOOTFLASH ---------------------------------------------------------------*/
+#define BOOTFLASH_BASE				0xA001C000
+#define FMA							(BOOTFLASH_BASE + 0x00)
+#define FMD1						(BOOTFLASH_BASE + 0x04)
+#define FMC							(BOOTFLASH_BASE + 0x08)
+#define FCIS						(BOOTFLASH_BASE + 0x0C)
+#define FCIM						(BOOTFLASH_BASE + 0x10)
+#define FCIC						(BOOTFLASH_BASE + 0x14)
+#define FMD2						(BOOTFLASH_BASE + 0x50)
+#define FMD3						(BOOTFLASH_BASE + 0x54)
+#define FMD4						(BOOTFLASH_BASE + 0x58)
+
+
+/*---- FMC: Command register */
+#define FMC_WRITE					(1<<0)				/* Writing in main region */
+#define FMC_PAGE_ERASE				(1<<1)				/* Page erase the main region */
+#define FMC_FULL_ERASE				(1<<2)				/* Erase full flash */
+#define FMC_WRITE_IFB				(1<<4)				/* Writing in info region */
+#define FMC_PAGEERASE_IFB			(1<<5)				/* Erase page of info region */
+#define FMC_MAGIC_KEY				(MAGIC_KEY<<16)		/* Operation run command */
+
+/*---- FCIS: Status register */
+#define FCIS_OP_CMLT				(1<<0)				/* Completion flag operation */
+#define FCIS_OP_ERROR				(1<<1)				/* Flag operation error */
+
+/*---- FCIC: CLear status register */
+#define FCIC_CLR_OPCMLT				(1<<0)				/* Cleare completion flag in register FCIS */
+#define FCIC_CLR_OPERROR			(1<<1)				/* Cleare error flag in register FCIS */
+
+/*-- USERFLASH ---------------------------------------------------------------*/
+#define USERFLASH_PAGE_SIZE			256
+#define USERFLASH_PAGE_TOTALNUM		256
+
+#define USERFLASH_BASE				0xA0022000
+#define UFMA						(USERFLASH_BASE + 0x00)
+#define UFMD						(USERFLASH_BASE + 0x04)
+#define UFMC						(USERFLASH_BASE + 0x08)
+#define UFCIS						(USERFLASH_BASE + 0x0C)
+#define UFCIM						(USERFLASH_BASE + 0x10)
+#define UFCIC						(USERFLASH_BASE + 0x14)
+
+/*---- UFMC: Command register */
+#define UFMC_WRITE					(1<<0)				/* Writing in main region */
+#define UFMC_PAGE_ERASE				(1<<1)				/* Paged erase the main region */
+#define UFMC_FULL_ERASE				(1<<2)				/* Erase full flash */
+#define UFMC_READ					(1<<3)				/* Reading from main region */
+#define UFMC_WRITE_IFB				(1<<4)				/* Writing in info region */
+#define UFMC_PAGEERASE_IFB			(1<<5)				/* Erase page of info region */
+#define UFMC_READ_IFB				(1<<6)				/* Reading from info region */
+#define UFMC_MAGIC_KEY				(MAGIC_KEY<<16)		/* Operation run command */
+
+/*---- UFCIS: Status register */
+#define UFCIS_OP_CMLT				(1<<0)				/* Completion flag operation */
+#define UFCIS_OP_ERROR				(1<<1)				/* Flag operation error */
+
+/*---- UFCIC: CLear status register */
+#define UFCIC_CLR_OPCMLT			(1<<0)				/* Cleared completion flag in register FCIS */
+#define UFCIC_CLR_OPERROR			(1<<1)				/* Cleared error flag in register FCIS */
+
+/*---- In info userflash address space */
+#define INFOWORD0_ADDR				0x00
+#define INFOWORD0_BOOTFROM_IFB		(1<<0)				/* Boot from bootflash or bootflash_ifb */
+#define INFOWORD0_EN_GPIO			(1<<1)				/* Remap to 0x00000000 extmem or bootflash */
+#define INFOWORD0_BOOTFROM_IFB_POS	0
+#define INFOWORD0_EN_GPIO_POS		1
+#define INFOWORD0_EXTMEM_SEL_POS	3					/* Choose altfunc of gpio to work with extmem */
+
+#define INFOWORD1_ADDR				0x01
+#define INFOWORD1_PINNUM_POS		0					/* Choose gpio pin number to control extmem boot */
+#define INFOWORD1_PORTNUM_POS		4					/* Choose gpio port to control extmem boot */
+
+#define INFOWORD2_ADDR				0x02
+#define INFOWORD2_LOCK_IFB_BF		(1<<0)				/* Protect info part of bootflash */
+
+#define INFOWORD3_ADDR				0x03
+#define INFOWORD3_LOCK_IFB_UF		(1<<0)				/* Protect info part of userflash */
+
+#define BF_LOCK_ADDR				0x40
+#define UF_LOCK_ADDR				0x80
+
+/**
+ * Private data for flash driver.
+ */
+struct niietcm4_flash_bank {
+	/* target params */
+	bool probed;
+	uint32_t chipid;
+	char *chip_name;
+	char chip_brief[4096];
+	/* not mapped userflash params */
+	uint32_t uflash_width;
+	uint32_t uflash_size;
+	uint32_t uflash_pagetotal;
+	uint32_t uflash_info_size;
+	uint32_t uflash_info_pagetotal;
+	/* boot params */
+	bool bflash_info_remap;
+	char *extmem_boot_port;
+	uint32_t extmem_boot_pin;
+	uint32_t extmem_boot_altfunc;
+	bool extmem_boot;
+};
+
+/*==============================================================================
+ *							HELPER FUNCTIONS
+ *==============================================================================
+ */
+
+/**
+ * Wait while operation with bootflash being performed and check result status
+ */
+static int niietcm4_opstatus_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	int retval;
+	int timeout = 100;
+
+	uint32_t flash_status;
+	retval = target_read_u32(target, FCIS, &flash_status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	while (flash_status == 0x00) {
+		retval = target_read_u32(target, FCIS, &flash_status);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeout-- <= 0) {
+			LOG_ERROR("Bootflash operation timeout");
+			return ERROR_FLASH_OPERATION_FAILED;
+			}
+		busy_sleep(1);	/* can use busy sleep for short times. */
+	}
+	if (flash_status == FCIS_OP_ERROR) {
+		LOG_ERROR("Bootflash operation error");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	/* clear status */
+	uint32_t flash_cmd = FCIC_CLR_OPCMLT | FCIC_CLR_OPERROR;
+	retval = target_write_u32(target, FCIC, flash_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+/**
+ * Wait while operation with userflash being performed and check result status
+ */
+static int niietcm4_uopstatus_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	int retval;
+	int timeout = 100;
+
+	uint32_t uflash_status;
+	retval = target_read_u32(target, UFCIS, &uflash_status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	while (uflash_status == 0x00) {
+		retval = target_read_u32(target, UFCIS, &uflash_status);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeout-- <= 0) {
+			LOG_ERROR("Userflash operation timeout");
+			return ERROR_FLASH_OPERATION_FAILED;
+			}
+		busy_sleep(1);	/* can use busy sleep for short times. */
+	}
+	if (uflash_status == UFCIS_OP_ERROR) {
+		LOG_ERROR("Userflash operation error");
+		return ERROR_FLASH_OPERATION_FAILED;
+	}
+	/* clear status */
+	uint32_t uflash_cmd = UFCIC_CLR_OPCMLT | UFCIC_CLR_OPERROR;
+	retval = target_write_u32(target, UFCIC, uflash_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+/**
+ * Dump page of userflash region.
+ * If we want to change some settings, we have to dump it full, because userflash is flash(not EEPROM).
+ * And correct write to flash can be performed only after erase.
+ * So without dump, changing one registers will clear others.
+ */
+static int niietcm4_dump_uflash_page(struct flash_bank *bank, uint32_t *dump, int page_num, int mem_type)
+{
+	struct target *target = bank->target;
+	int i, retval;
+
+	uint32_t uflash_cmd;
+	if (mem_type == INFO_MEM_TYPE)
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+	else
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ;
+
+	int first = page_num*USERFLASH_PAGE_SIZE;
+	int last = first + USERFLASH_PAGE_SIZE;
+
+	for (i = first; i < last; i++) {
+		retval = target_write_u32(target, UFMA, i);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_write_u32(target, UFMC, uflash_cmd);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = niietcm4_uopstatus_check(bank);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_read_u32(target, UFMD, &dump[i]);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+/**
+ * Load modified page dump to userflash region page.
+ */
+static int niietcm4_load_uflash_page(struct flash_bank *bank, uint32_t *dump, int page_num, int mem_type)
+{
+	struct target *target = bank->target;
+	int i, retval;
+
+	uint32_t uflash_cmd;
+	if (mem_type == INFO_MEM_TYPE)
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_WRITE_IFB;
+	else
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_WRITE;
+
+	int first = page_num*USERFLASH_PAGE_SIZE;
+	int last = first + USERFLASH_PAGE_SIZE;
+
+	for (i = first; i < last; i++) {
+		retval = target_write_u32(target, UFMA, i);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_write_u32(target, UFMD, dump[i]);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_write_u32(target, UFMC, uflash_cmd);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = niietcm4_uopstatus_check(bank);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+/**
+ * Erase one page of userflash info or main region
+ */
+static int niietcm4_uflash_page_erase(struct flash_bank *bank, int page_num, int mem_type)
+{
+	struct target *target = bank->target;
+	int retval;
+
+	uint32_t uflash_cmd;
+	if (mem_type == INFO_MEM_TYPE)
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_PAGEERASE_IFB;
+	else
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_PAGE_ERASE;
+
+	retval = target_write_u32(target, UFMA, page_num*USERFLASH_PAGE_SIZE);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, UFMD, 0xFF);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, UFMC, uflash_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+	/* status check */
+	retval = niietcm4_uopstatus_check(bank);
+	if (retval != ERROR_OK)
+			return retval;
+
+	return retval;
+}
+
+/**
+ * Enable or disable protection of userflash pages
+ */
+static int niietcm4_uflash_protect(struct flash_bank *bank, int mem_type, int set, int first, int last)
+{
+	int retval;
+	if (mem_type == INFO_MEM_TYPE) {
+		/* read dump */
+		uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+		retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* modify dump */
+		if (set)
+			uflash_dump[INFOWORD2_ADDR] &= ~INFOWORD3_LOCK_IFB_UF;
+		else
+			uflash_dump[INFOWORD2_ADDR] |= INFOWORD3_LOCK_IFB_UF;
+		/* erase page 0 userflash */
+		retval = niietcm4_uflash_page_erase(bank, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* write dump to userflash */
+		retval = niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+	} else {
+		/* read dump */
+		uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+		retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* modify dump */
+		for (int i = first; i <= last; i++)	{
+			uint32_t reg_num = i/8;
+			uint32_t bit_num = i%8;
+			if (set)
+				uflash_dump[UF_LOCK_ADDR+reg_num] &= ~(1<<bit_num);
+			else
+				uflash_dump[UF_LOCK_ADDR+reg_num] |= (1<<bit_num);
+		}
+		/* erase page 0 info userflash */
+		retval = niietcm4_uflash_page_erase(bank, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* write dump to userflash */
+		retval = niietcm4_load_uflash_page(bank, uflash_dump,  0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+/*==============================================================================
+ *							FLASH COMMANDS
+ *==============================================================================
+ */
+COMMAND_HANDLER(niietcm4_handle_uflash_read_byte_command)
+{
+	if (CMD_ARGC < 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	uint32_t uflash_addr;
+	uint32_t uflash_cmd;
+	uint32_t uflash_data;
+
+	if (strcmp("info", CMD_ARGV[0]) == 0)
+			uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+	else if (strcmp("main", CMD_ARGV[0]) == 0)
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], uflash_addr);
+
+	retval = target_write_u32(target, UFMA, uflash_addr);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, UFMC, uflash_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+	/* status check */
+	retval = niietcm4_uopstatus_check(bank);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_read_u32(target, UFMD, &uflash_data);
+	if (retval != ERROR_OK)
+		return retval;
+	command_print(CMD_CTX,  "Read userflash %s region:\n"
+							"address = 0x%04x,\n"
+							"value   = 0x%02x.", CMD_ARGV[0], uflash_addr, uflash_data);
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_uflash_write_byte_command)
+{
+	if (CMD_ARGC < 4)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	uint32_t uflash_addr;
+	uint32_t uflash_data;
+	int mem_type;
+
+	if (strcmp("info", CMD_ARGV[0]) == 0)
+		mem_type = 1;
+	else if (strcmp("main", CMD_ARGV[0]) == 0)
+		mem_type = 0;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], uflash_addr);
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[2], uflash_data);
+
+	int page_num = uflash_addr/USERFLASH_PAGE_SIZE;
+
+	command_print(CMD_CTX, "Write userflash %s region:\n"
+						   "address = 0x%04x,\n"
+						   "value   = 0x%02x.\n"
+						   "Please wait ... ", CMD_ARGV[0], uflash_addr, uflash_data);
+	/* dump */
+	uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+	niietcm4_dump_uflash_page(bank, uflash_dump, page_num, mem_type);
+
+	/* modify dump */
+	uflash_dump[uflash_addr%USERFLASH_PAGE_SIZE] = uflash_data;
+
+	/* erase page userflash */
+	niietcm4_uflash_page_erase(bank, page_num, mem_type);
+
+	/* write dump to userflash */
+	niietcm4_load_uflash_page(bank, uflash_dump, page_num, mem_type);
+	command_print(CMD_CTX, "done!");
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_uflash_full_erase_command)
+{
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	uint32_t uflash_addr = 0;
+	uint32_t uflash_data = 0xFF;
+	uint32_t uflash_cmd = UFMC_MAGIC_KEY | UFMC_FULL_ERASE;
+
+	retval = target_write_u32(target, UFMA, uflash_addr);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, UFMD, uflash_data);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = target_write_u32(target, UFMC, uflash_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+	/* status check */
+	retval = niietcm4_uopstatus_check(bank);
+	if (retval != ERROR_OK)
+		return retval;
+	command_print(CMD_CTX, "Userflash full erase done!");
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_uflash_erase_command)
+{
+	if (CMD_ARGC < 4)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	unsigned int first, last;
+	int mem_type;
+
+	if (strcmp("info", CMD_ARGV[0]) == 0)
+			mem_type = 1;
+	else if (strcmp("main", CMD_ARGV[0]) == 0)
+		mem_type = 0;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], first);
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[2], last);
+	for (unsigned int i = first; i <= last; i++) {
+		retval = niietcm4_uflash_page_erase(bank, i, mem_type);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	command_print(CMD_CTX, "Erase %s userflash pages %d through %d done!", CMD_ARGV[0], first, last);
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_uflash_protect_check_command)
+{
+	if (CMD_ARGC < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	struct target *target = bank->target;
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	int mem_type;
+	if (strcmp("info", CMD_ARGV[0]) == 0)
+		mem_type = 1;
+	else if (strcmp("main", CMD_ARGV[0]) == 0)
+		mem_type = 0;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	int i, j;
+	uint32_t uflash_addr;
+	uint32_t uflash_cmd;
+	uint32_t uflash_data;
+
+	/* chose between main userflash and info userflash */
+	if (mem_type == INFO_MEM_TYPE) {
+		uflash_addr = INFOWORD3_ADDR;
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+		retval = target_write_u32(target, UFMA, uflash_addr);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_write_u32(target, UFMC, uflash_cmd);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* status check */
+		retval = niietcm4_uopstatus_check(bank);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_read_u32(target, UFMD, &uflash_data);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (uflash_data & INFOWORD3_LOCK_IFB_UF)
+			command_print(CMD_CTX, "All sectors of info userflash are not protected!");
+		else
+			command_print(CMD_CTX, "All sectors of info userflash are protected!");
+	} else {
+		uflash_addr = UF_LOCK_ADDR;
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+		for (i = 0; i < USERFLASH_PAGE_TOTALNUM/8; i++) {
+			retval = target_write_u32(target, UFMA, uflash_addr);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_write_u32(target, UFMC, uflash_cmd);
+			if (retval != ERROR_OK)
+				return retval;
+
+			/* status check */
+			retval = niietcm4_uopstatus_check(bank);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_read_u32(target, UFMD, &uflash_data);
+			if (retval != ERROR_OK)
+				return retval;
+
+			for (j = 0; j < 8; j++) {
+				if (uflash_data & 0x1)
+					command_print(CMD_CTX, "Userflash sector #%03d: 0x%04x (0x100) is not protected!",
+											i*8+j, (i*8+j)*USERFLASH_PAGE_SIZE);
+				else
+					command_print(CMD_CTX, "Userflash sector #%03d: 0x%04x (0x100) is protected!",
+											i*8+j, (i*8+j)*USERFLASH_PAGE_SIZE);
+				uflash_data = uflash_data >> 1;
+			}
+			uflash_addr++;
+		}
+	}
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_uflash_protect_command)
+{
+	if (CMD_ARGC < 5)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	int mem_type;
+	if (strcmp("info", CMD_ARGV[0]) == 0)
+		mem_type = 1;
+	else if (strcmp("main", CMD_ARGV[0]) == 0)
+		mem_type = 0;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	unsigned int first, last;
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], first);
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[2], last);
+
+	int set;
+	if (strcmp("on", CMD_ARGV[3]) == 0) {
+		command_print(CMD_CTX, "Try to enable %s userflash sectors %d through %d protection. Please wait ... ",
+								CMD_ARGV[0], first, last);
+		set = 1;
+	} else if (strcmp("off", CMD_ARGV[3]) == 0) {
+		command_print(CMD_CTX, "Try to disable %s userflash sectors %d through %d protection. Please wait ... ",
+								CMD_ARGV[0], first, last);
+		set = 0;
+	} else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	retval = niietcm4_uflash_protect(bank, mem_type, set, first, last);
+		if (retval != ERROR_OK)
+			return retval;
+
+	command_print(CMD_CTX, "done!");
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_bflash_info_remap_command)
+{
+	if (CMD_ARGC < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	int set;
+	if (strcmp("on", CMD_ARGV[0]) == 0) {
+		command_print(CMD_CTX, "Try to enable bootflash info region remap. Please wait ...");
+		set = 1;
+	} else if (strcmp("off", CMD_ARGV[0]) == 0) {
+		command_print(CMD_CTX, "Try to disable bootflash info region remap. Please wait ...");
+		set = 0;
+	} else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* dump */
+	uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+	niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+
+	/* modify dump */
+	if (set)
+		uflash_dump[INFOWORD0_ADDR] &= ~INFOWORD0_BOOTFROM_IFB;
+	else
+		uflash_dump[INFOWORD0_ADDR] |= INFOWORD0_BOOTFROM_IFB;
+
+	/* erase page userflash */
+	niietcm4_uflash_page_erase(bank, 0, 1);
+
+	/* write dump to userflash */
+	niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
+	command_print(CMD_CTX, "done!");
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_extmem_cfg_command)
+{
+	if (CMD_ARGC < 4)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	uint32_t port;
+	if (strcmp("gpioa", CMD_ARGV[0]) == 0)
+		port = 8;
+	else if (strcmp("gpiob", CMD_ARGV[0]) == 0)
+		port = 9;
+	else if (strcmp("gpioc", CMD_ARGV[0]) == 0)
+		port = 10;
+	else if (strcmp("gpiod", CMD_ARGV[0]) == 0)
+		port = 11;
+	else if (strcmp("gpioe", CMD_ARGV[0]) == 0)
+		port = 12;
+	else if (strcmp("gpiof", CMD_ARGV[0]) == 0)
+		port = 13;
+	else if (strcmp("gpiog", CMD_ARGV[0]) == 0)
+		port = 14;
+	else if (strcmp("gpioh", CMD_ARGV[0]) == 0)
+		port = 15;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	uint32_t pin;
+	COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], pin);
+	if (pin > 15)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	uint32_t func;
+	if (strcmp("func1", CMD_ARGV[2]) == 0)
+		func = 0;
+	else if (strcmp("func3", CMD_ARGV[2]) == 0)
+		func = 3;
+	else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	command_print(CMD_CTX,  "Try to configure external memory boot interface:\n"
+							"port = %s\n"
+							"pin  = %s\n"
+							"func = %s\n"
+							"Please wait ...", CMD_ARGV[0], CMD_ARGV[1], CMD_ARGV[2]);
+	/* dump */
+	uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+	niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+
+	/* modify dump */
+	uflash_dump[INFOWORD0_ADDR] &= ~(3<<INFOWORD0_EXTMEM_SEL_POS);
+	uflash_dump[INFOWORD0_ADDR] |= func<<INFOWORD0_EXTMEM_SEL_POS;
+	uflash_dump[INFOWORD1_ADDR] = (port<<INFOWORD1_PORTNUM_POS) | (pin<<INFOWORD1_PINNUM_POS);
+
+	/* erase page userflash */
+	niietcm4_uflash_page_erase(bank, 0, 1);
+
+	/* write dump to userflash */
+	niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
+	command_print(CMD_CTX, "done!");
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_extmem_boot_command)
+{
+	if (CMD_ARGC < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* skip over flash bank */
+	CMD_ARGC--;
+	CMD_ARGV++;
+
+	int set;
+
+	if (strcmp("on", CMD_ARGV[0]) == 0) {
+		command_print(CMD_CTX, "Try to enable boot from external memory. Please wait ...");
+		set = 1;
+	} else if (strcmp("off", CMD_ARGV[0]) == 0) {
+		command_print(CMD_CTX, "Try to disable boot from external memory. Please wait ...");
+		set = 0;
+	} else
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* dump */
+	uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+	niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+
+	/* modify dump */
+	if (set)
+		uflash_dump[INFOWORD0_ADDR] &= ~INFOWORD0_EN_GPIO;
+	else
+		uflash_dump[INFOWORD0_ADDR] |= INFOWORD0_EN_GPIO;
+
+	/* erase page userflash */
+	niietcm4_uflash_page_erase(bank, 0, 1);
+
+	/* write dump to userflash */
+	niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
+	command_print(CMD_CTX, "done!");
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_service_mode_erase_command)
+{
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+	struct target *target = bank->target;
+
+	command_print(CMD_CTX, "Try to perform service mode erase. Please wait ...");
+
+	retval = target_write_u32(target, SERVICE_MODE_ERASE_ADDR, 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	int timeout = 500;
+	uint32_t status;
+
+	retval = target_read_u32(target, SERVICE_MODE_ERASE_ADDR, &status);
+	if (retval != ERROR_OK)
+		return retval;
+
+	while (status != 0x03) {
+		retval = target_read_u32(target, SERVICE_MODE_ERASE_ADDR, &status);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeout-- <= 0) {
+			LOG_ERROR("Service mode erase timeout");
+			return ERROR_FLASH_OPERATION_FAILED;
+			}
+		busy_sleep(1);	/* can use busy sleep for short times. */
+	}
+	command_print(CMD_CTX, "done! All data erased.");
+
+	return retval;
+}
+
+COMMAND_HANDLER(niietcm4_handle_driver_info_command)
+{
+	if (CMD_ARGC < 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	command_print(CMD_CTX, "niietcm4 flash driver\n"
+						   "version: %d.%d\n"
+						   "author: Bogdan Kolbov\n"
+						   "mail: kolbov@niiet.ru",
+						   FLASH_DRIVER_VER>>16,
+						   FLASH_DRIVER_VER&0xFFFF);
+
+	return retval;
+}
+
+static const struct command_registration niietcm4_exec_command_handlers[] = {
+	{
+		.name = "uflash_read_byte",
+		.handler = niietcm4_handle_uflash_read_byte_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('main'|'info') address",
+		.help = "Read byte from main or info userflash region",
+	},
+	{
+		.name = "uflash_write_byte",
+		.handler = niietcm4_handle_uflash_write_byte_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('main'|'info') address value",
+		.help = "Write byte to main or info userflash region",
+	},
+	{
+		.name = "uflash_full_erase",
+		.handler = niietcm4_handle_uflash_full_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Erase all userflash including info region",
+	},
+	{
+		.name = "uflash_erase",
+		.handler = niietcm4_handle_uflash_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('main'|'info') first_sector_num last_sector_num",
+		.help = "Erase sectors of main or info userflash region, starting at sector first up to and including last.",
+	},
+	{
+		.name = "uflash_protect_check",
+		.handler = niietcm4_handle_uflash_protect_check_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('main'|'info')",
+		.help = "Check sectors protect.",
+	},
+	{
+		.name = "uflash_protect",
+		.handler = niietcm4_handle_uflash_protect_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('main'|'info') first_sector_num last_sector_num ('on'|'off')",
+		.help = "Protect sectors of main or info userflash region, starting at sector first up to and including last.",
+	},
+	{
+		.name = "bflash_info_remap",
+		.handler = niietcm4_handle_bflash_info_remap_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('on'|'off')",
+		.help = "Enable remapping bootflash info region to 0x00000000 (or 0x40000000 if external memory boot used).",
+	},
+	{
+		.name = "extmem_cfg",
+		.handler = niietcm4_handle_extmem_cfg_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('gpioa'|'gpiob'|'gpioc'|'gpiod'|'gpioe'|'gpiof'|'gpiog'|'gpioh') pin_num ('func1'|'func3')",
+		.help = "Configure external memory interface for boot.",
+	},
+	{
+		.name = "extmem_boot",
+		.handler = niietcm4_handle_extmem_boot_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id ('on'|'off')",
+		.help = "Enable boot from external memory.",
+	},
+	{
+		.name = "service_mode_erase",
+		.handler = niietcm4_handle_service_mode_erase_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Perform emergency erase of all flash (bootflash and userflash).",
+	},
+	{
+		.name = "driver_info",
+		.handler = niietcm4_handle_driver_info_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Show information about flash driver.",
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration niietcm4_command_handlers[] = {
+	{
+		.name = "niietcm4",
+		.mode = COMMAND_ANY,
+		.help = "niietcm4 flash command group",
+		.usage = "",
+		.chain = niietcm4_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+/*==============================================================================
+ *							FLASH INTERFACE
+ *==============================================================================
+ */
+
+FLASH_BANK_COMMAND_HANDLER(niietcm4_flash_bank_command)
+{
+	struct niietcm4_flash_bank *niietcm4_info;
+
+	if (CMD_ARGC < 7)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	niietcm4_info = malloc(sizeof(struct niietcm4_flash_bank));
+
+	bank->driver_priv = niietcm4_info;
+
+	/* information will be updated by probing */
+	niietcm4_info->probed = false;
+	niietcm4_info->chipid = 0;
+	niietcm4_info->chip_name = NULL;
+	niietcm4_info->uflash_width = 0;
+	niietcm4_info->uflash_size = 0;
+	niietcm4_info->uflash_pagetotal = 0;
+	niietcm4_info->uflash_info_size = 0;
+	niietcm4_info->uflash_info_pagetotal = 0;
+	niietcm4_info->bflash_info_remap = false;
+	niietcm4_info->extmem_boot_port = NULL;
+	niietcm4_info->extmem_boot_pin = 0;
+	niietcm4_info->extmem_boot_altfunc = 0;
+	niietcm4_info->extmem_boot = false;
+
+	return ERROR_OK;
+}
+
+static int niietcm4_protect_check(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+
+	int retval = ERROR_FLASH_OPERATION_FAILED;
+	int set;
+	uint32_t uflash_addr;
+	uint32_t uflash_cmd;
+	uint32_t uflash_data;
+	/* chose between main bootflash and info bootflash  */
+	if (niietcm4_info->bflash_info_remap) {
+		uflash_addr = INFOWORD2_ADDR;
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+		retval = target_write_u32(target, UFMA, uflash_addr);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_write_u32(target, UFMC, uflash_cmd);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* status check */
+		retval = niietcm4_uopstatus_check(bank);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_read_u32(target, UFMD, &uflash_data);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (uflash_data & INFOWORD2_LOCK_IFB_BF)
+			set = 0;
+		else
+			set = 1;
+		bank->sectors[0].is_protected = set;
+	} else {
+		uflash_addr = BF_LOCK_ADDR;
+		uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+		for (int i = 0; i < bank->num_sectors/8; i++) {
+			retval = target_write_u32(target, UFMA, uflash_addr);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_write_u32(target, UFMC, uflash_cmd);
+			if (retval != ERROR_OK)
+				return retval;
+
+			/* status check */
+			retval = niietcm4_uopstatus_check(bank);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_read_u32(target, UFMD, &uflash_data);
+			if (retval != ERROR_OK)
+				return retval;
+
+			for (int j = 0; j < 8; j++) {
+				if (uflash_data & 0x1)
+					set = 0;
+				else
+					set = 1;
+				bank->sectors[i*8+j].is_protected = set;
+				uflash_data = uflash_data >> 1;
+			}
+			uflash_addr++;
+		}
+	}
+
+	return retval;
+}
+
+static int niietcm4_mass_erase(struct flash_bank *bank)
+{
+	struct target *target = bank->target;
+
+	int retval;
+	uint32_t flash_cmd;
+
+	/* start mass erase */
+	flash_cmd = FMC_MAGIC_KEY | FMC_FULL_ERASE;
+	retval = target_write_u32(target, FMC, flash_cmd);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* status check */
+	retval = niietcm4_opstatus_check(bank);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval;
+}
+
+static int niietcm4_erase(struct flash_bank *bank, int first, int last)
+{
+	struct target *target = bank->target;
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+	int retval = ERROR_FLASH_OPERATION_FAILED;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if ((first == 0) && (last == (bank->num_sectors - 1))) {
+		retval = niietcm4_mass_erase(bank);
+		return retval;
+	}
+
+	/* chose between main bootflash and info bootflash */
+	uint32_t flash_cmd, flash_addr;
+	if (niietcm4_info->bflash_info_remap)
+		flash_cmd = FMC_MAGIC_KEY | FMC_PAGEERASE_IFB;
+	else
+		flash_cmd = FMC_MAGIC_KEY | FMC_PAGE_ERASE;
+
+	/* erasing pages */
+	unsigned int page_size = bank->size / bank->num_sectors;
+	for (int i = first; i <= last; i++) {
+		/* current page addr */
+		flash_addr = i*page_size;
+		retval = target_write_u32(target, FMA, flash_addr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* start erase */
+		retval = target_write_u32(target, FMC, flash_cmd);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* status check */
+		retval = niietcm4_opstatus_check(bank);
+		if (retval != ERROR_OK)
+			return retval;
+
+		bank->sectors[i].is_erased = 1;
+	}
+
+	return retval;
+}
+
+static int niietcm4_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	struct target *target = bank->target;
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+
+	int retval;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	LOG_INFO("Plese wait ..."); /* it`s quite a long process */
+	/* chose between main bootflash and info bootflash */
+	if (niietcm4_info->bflash_info_remap) {
+		/* dump */
+		uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+		retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* modify dump */
+		if (set)
+			uflash_dump[INFOWORD2_ADDR] &= ~INFOWORD2_LOCK_IFB_BF;
+		else
+			uflash_dump[INFOWORD2_ADDR] |= INFOWORD2_LOCK_IFB_BF;
+		/* erase page 0 userflash */
+		retval = niietcm4_uflash_page_erase(bank, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* write dump to userflash */
+		retval = niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+	} else {
+		/* read dump*/
+		uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
+		retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* modify dump */
+		for (int i = first; i <= last; i++)	{
+			uint32_t reg_num = i/8;
+			uint32_t bit_num = i%8;
+			if (set)
+				uflash_dump[BF_LOCK_ADDR+reg_num] &= ~(1<<bit_num);
+			else
+				uflash_dump[BF_LOCK_ADDR+reg_num] |= (1<<bit_num);
+		}
+		/* erase page 0 info userflash */
+		retval = niietcm4_uflash_page_erase(bank, 0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+		/* write dump to userflash */
+		retval = niietcm4_load_uflash_page(bank, uflash_dump,  0, 1);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int niietcm4_write_block(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+	uint32_t buffer_size = 32768 + 8; /* 8 bytes for rp and wp */
+	struct working_area *write_algorithm;
+	struct working_area *source;
+	uint32_t address = bank->base + offset;
+	struct reg_param reg_params[5];
+	struct armv7m_algorithm armv7m_info;
+	int retval = ERROR_OK;
+
+	/* see contrib/loaders/flash/k1921vk01t.S for src */
+	static const uint8_t niietcm4_flash_write_code[] = {
+		0x14, 0x4f, 0x16, 0x68, 0x00, 0x2e, 0x23, 0xd0, 0x55, 0x68, 0xb5, 0x42, 0xf9, 0xd0, 0x2e, 0x68,
+		0x7e, 0x60, 0x04, 0x35, 0x2e, 0x68, 0x3e, 0x65, 0x04, 0x35, 0x2e, 0x68, 0x7e, 0x65, 0x04, 0x35,
+		0x2e, 0x68, 0xbe, 0x65, 0x04, 0x35, 0x3c, 0x60, 0x10, 0x34, 0xb8, 0x60, 0xfe, 0x68, 0x00, 0x2e,
+		0xfc, 0xd0, 0x02, 0x2e, 0x0a, 0xd0, 0x01, 0x26, 0x7e, 0x61, 0x9d, 0x42, 0x01, 0xd3, 0x15, 0x46,
+		0x08, 0x35, 0x55, 0x60, 0x01, 0x39, 0x00, 0x29, 0x02, 0xd0, 0xda, 0xe7, 0x00, 0x20, 0x50, 0x60,
+		0x30, 0x46, 0x00, 0xbe, 0x00, 0xc0, 0x01, 0xa0
+	};
+
+	/* flash write code */
+	if (target_alloc_working_area(target, sizeof(niietcm4_flash_write_code),
+			&write_algorithm) != ERROR_OK) {
+		LOG_WARNING("no working area available, can't do block memory writes");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	};
+
+	retval = target_write_buffer(target, write_algorithm->address,
+			sizeof(niietcm4_flash_write_code), niietcm4_flash_write_code);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* memory buffer */
+	while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
+		buffer_size /= 2;
+		buffer_size &= ~15UL; /* Make sure it's 16 byte aligned */
+		buffer_size += 8; /* And 8 bytes for WP and RP */
+		if (buffer_size <= 256) {
+			/* we already allocated the writing code, but failed to get a
+			 * buffer, free the algorithm */
+			target_free_working_area(target, write_algorithm);
+
+			LOG_WARNING("no large enough working area available, can't do block memory writes");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+	};
+
+	init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT);	/* write_cmd base (in), status (out) */
+	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);	/* count (128bit) */
+	init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);	/* buffer start */
+	init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);	/* buffer end */
+	init_reg_param(&reg_params[4], "r4", 32, PARAM_IN_OUT);	/* target address */
+
+	uint32_t flash_cmd;
+	if (niietcm4_info->bflash_info_remap)
+		flash_cmd = FMC_MAGIC_KEY | FMC_WRITE_IFB;
+	else
+		flash_cmd = FMC_MAGIC_KEY | FMC_WRITE;
+
+	buf_set_u32(reg_params[0].value, 0, 32, flash_cmd);
+	buf_set_u32(reg_params[1].value, 0, 32, count);
+	buf_set_u32(reg_params[2].value, 0, 32, source->address);
+	buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
+	buf_set_u32(reg_params[4].value, 0, 32, address);
+
+	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	retval = target_run_flash_async_algorithm(target, buffer, count, 16,
+			0, NULL,
+			5, reg_params,
+			source->address, source->size,
+			write_algorithm->address, 0,
+			&armv7m_info);
+
+	if (retval == ERROR_FLASH_OPERATION_FAILED)
+		LOG_ERROR("flash write failed at address 0x%"PRIx32,
+				buf_get_u32(reg_params[4].value, 0, 32));
+
+	target_free_working_area(target, source);
+	target_free_working_area(target, write_algorithm);
+
+	destroy_reg_param(&reg_params[0]);
+	destroy_reg_param(&reg_params[1]);
+	destroy_reg_param(&reg_params[2]);
+	destroy_reg_param(&reg_params[3]);
+	destroy_reg_param(&reg_params[4]);
+
+	return retval;
+}
+
+static int niietcm4_write(struct flash_bank *bank, const uint8_t *buffer,
+		uint32_t offset, uint32_t count)
+{
+	struct target *target = bank->target;
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+
+	if (bank->target->state != TARGET_HALTED) {
+		LOG_ERROR("Target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	if (offset & 0x3) {
+		LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-byte alignment", offset);
+		return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+	}
+
+	int retval;
+
+	/* try using block write */
+	retval = niietcm4_write_block(bank, buffer, offset, count/16);
+	uint32_t flash_addr, flash_cmd, flash_data;
+
+	if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+		/* if block write failed (no sufficient working area),
+		 * we use normal (slow) single halfword accesses */
+		LOG_WARNING("Can't use block writes, falling back to single memory accesses");
+		LOG_INFO("Plese wait ..."); /* it`s quite a long process */
+
+		/* chose between main bootflash and info bootflash */
+		if (niietcm4_info->bflash_info_remap)
+			flash_cmd = FMC_MAGIC_KEY | FMC_WRITE_IFB;
+		else
+			flash_cmd = FMC_MAGIC_KEY | FMC_WRITE;
+
+		/* write 16 bytes per try */
+		for (unsigned int i = 0; i < count; i += 16) {
+			/* current addr */
+			LOG_INFO("%d byte of %d", i, count);
+			flash_addr = offset + i;
+			retval = target_write_u32(target, FMA, flash_addr);
+			if (retval != ERROR_OK)
+				return retval;
+
+			/* if there's an odd number of bytes, the data has to be padded */
+			uint8_t padding[16] = { 0xff, 0xff, 0xff, 0xff,
+									0xff, 0xff, 0xff, 0xff,
+									0xff, 0xff, 0xff, 0xff,
+									0xff, 0xff, 0xff, 0xff};
+			memcpy(padding, buffer + i, MIN(16, count-i));
+
+			/* place in reg 16 bytes of data */
+			flash_data = (padding[3]<<24) | (padding[2]<<16) | (padding[1]<<8) | padding[0];
+			retval = target_write_u32(target, FMD1, flash_data);
+			if (retval != ERROR_OK)
+				return retval;
+			flash_data = (padding[7]<<24) | (padding[6]<<16) | (padding[5]<<8) | padding[4];
+			retval = target_write_u32(target, FMD2, flash_data);
+			if (retval != ERROR_OK)
+				return retval;
+			flash_data = (padding[11]<<24) | (padding[10]<<16) | (padding[9]<<8) | padding[8];
+			retval = target_write_u32(target, FMD3, flash_data);
+			if (retval != ERROR_OK)
+				return retval;
+			flash_data = (padding[15]<<24) | (padding[14]<<16) | (padding[13]<<8) | padding[12];
+			retval = target_write_u32(target, FMD4, flash_data);
+			if (retval != ERROR_OK)
+				return retval;
+
+			/* write start */
+			retval = target_write_u32(target, FMC, flash_cmd);
+			if (retval != ERROR_OK)
+				return retval;
+
+			/* status check */
+			retval = niietcm4_opstatus_check(bank);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+	}
+
+	return retval;
+}
+
+static int niietcm4_probe_k1921vk01t(struct flash_bank *bank)
+{
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+	struct target *target = bank->target;
+	int retval;
+
+	niietcm4_info->chip_name = "K1921VK01T";
+
+	/* check if we in service mode */
+	uint32_t service_mode;
+	retval = target_read_u32(target, 0x80017000, &service_mode);
+	if (retval != ERROR_OK)
+		return retval;
+	service_mode = (service_mode>>2) & 0x1;
+
+	if (!service_mode) {
+		niietcm4_info->uflash_width = 8;
+		niietcm4_info->uflash_size = 0x10000;
+		niietcm4_info->uflash_pagetotal = 256;
+		niietcm4_info->uflash_info_size = 0x200;
+		niietcm4_info->uflash_info_pagetotal = 2;
+
+		uint32_t uflash_data[2];
+		uint32_t uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
+		for (int i = 0; i < 2; i++) {
+			retval = target_write_u32(target, UFMA, i);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_write_u32(target, UFMC, uflash_cmd);
+			if (retval != ERROR_OK)
+				return retval;
+			/* status check */
+			retval = niietcm4_uopstatus_check(bank);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = target_read_u32(target, UFMD, &uflash_data[i]);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+
+		int boot_from_ifb = (uflash_data[0]>>INFOWORD0_BOOTFROM_IFB_POS) & 0x1;
+		int en_gpio = (uflash_data[0]>>INFOWORD0_EN_GPIO_POS) & 0x1;
+		int extmem_sel = (uflash_data[0]>>INFOWORD0_EXTMEM_SEL_POS) & 0x3;
+		int pinnum = (uflash_data[1]>>INFOWORD1_PINNUM_POS) & 0xF;
+		int portnum = (uflash_data[1]>>INFOWORD1_PORTNUM_POS) & 0x7;
+
+		if (boot_from_ifb)
+			niietcm4_info->bflash_info_remap = false;
+		else
+			niietcm4_info->bflash_info_remap = true;
+		if (extmem_sel == 0x2)
+			niietcm4_info->extmem_boot_altfunc = 3;
+		else
+			niietcm4_info->extmem_boot_altfunc = 1;
+		if (portnum == 0x0)
+			niietcm4_info->extmem_boot_port = "GPIOA";
+		else if (portnum == 0x1)
+			niietcm4_info->extmem_boot_port = "GPIOB";
+		else if (portnum == 0x2)
+			niietcm4_info->extmem_boot_port = "GPIOC";
+		else if (portnum == 0x3)
+			niietcm4_info->extmem_boot_port = "GPIOD";
+		else if (portnum == 0x4)
+			niietcm4_info->extmem_boot_port = "GPIOE";
+		else if (portnum == 0x5)
+			niietcm4_info->extmem_boot_port = "GPIOF";
+		else if (portnum == 0x6)
+			niietcm4_info->extmem_boot_port = "GPIOG";
+		else if (portnum == 0x7)
+			niietcm4_info->extmem_boot_port = "GPIOH";
+		else
+			niietcm4_info->extmem_boot_port = "not defined";
+		if (en_gpio)
+			niietcm4_info->extmem_boot = false;
+		else
+			niietcm4_info->extmem_boot = true;
+		niietcm4_info->extmem_boot_pin = pinnum;
+
+		/* check state of extmem boot en pin, if "high", extmem remapped to 0x00000000 */
+		uint32_t extmem_boot_port_data;
+		retval = target_read_u32(target, 0x80010000 + 0x1000*portnum, &extmem_boot_port_data);
+		if (retval != ERROR_OK)
+			return retval;
+		int extmem_boot_pin_data = (extmem_boot_port_data>>pinnum) & 0x1;
+
+		uint32_t extmem_base;
+		uint32_t bflash_base;
+		if (extmem_boot_pin_data && niietcm4_info->extmem_boot) {
+			extmem_base = 0x00000000;
+			bflash_base = 0x40000000;
+		} else {
+			extmem_base = 0x40000000;
+			bflash_base = 0x00000000;
+		}
+
+		uint32_t bflash_size = 0x100000;
+		uint32_t bflash_pages = 128;
+		uint32_t bflash_info_size = 0x2000;
+		uint32_t bflash_info_pages = 1;
+		if (niietcm4_info->bflash_info_remap) {
+			bflash_base += 0x2000;
+			bflash_size -= 0x2000;
+			bflash_pages--;
+			bank->size = bflash_info_size;
+			bank->num_sectors = bflash_info_pages;
+		} else {
+			bank->size = bflash_size;
+			bank->num_sectors = bflash_pages;
+		}
+
+		char info_bootflash_addr_str[64];
+		if (niietcm4_info->bflash_info_remap)
+			snprintf(info_bootflash_addr_str, sizeof(info_bootflash_addr_str), "0x%08x base adress", bank->base);
+		else
+			snprintf(info_bootflash_addr_str, sizeof(info_bootflash_addr_str), "not maped to global adress space");
+
+		snprintf(niietcm4_info->chip_brief,
+				sizeof(niietcm4_info->chip_brief),
+				"\n"
+				"MEMORY CONFIGURATION\n"
+				"Bootflash :\n"
+				"    %d kB total\n"
+				"    %d pages %d kB each\n"
+				"    0x%08x base adress\n"
+				"%s"
+				"Info bootflash :\n"
+				"    %d kB total\n"
+				"    %d pages %d kB each\n"
+				"    %s\n"
+				"%s"
+				"Userflash :\n"
+				"    %d kB total\n"
+				"    %d pages %d B each\n"
+				"    %d bit cells\n"
+				"    not maped to global adress space\n"
+				"Info userflash :\n"
+				"    %d B total\n"
+				"    %d pages of %d B each\n"
+				"    %d bit cells\n"
+				"    not maped to global adress space\n"
+				"RAM :\n"
+				"    192 kB total\n"
+				"    0x20000000 base adress\n"
+				"External memory :\n"
+				"    8/16 bit address space\n"
+				"    0x%08x base adress\n"
+				"\n"
+				"INFOWORD STATUS\n"
+				"Bootflash info region remap :\n"
+				"    %s\n"
+				"External memory boot port :\n"
+				"    %s\n"
+				"External memory boot pin :\n"
+				"    %d\n"
+				"External memory interface alternative function :\n"
+				"    %d\n"
+				"Option boot from external memory :\n"
+				"    %s\n",
+				bflash_size/1024,
+				bflash_pages,
+				(bflash_size/bflash_pages)/1024,
+				bflash_base,
+				niietcm4_info->bflash_info_remap ? "" : "    this flash will be used for debugging, writing and etc\n",
+				bflash_info_size/1024,
+				bflash_info_pages,
+				(bflash_info_size/bflash_info_pages)/1024,
+				info_bootflash_addr_str,
+				niietcm4_info->bflash_info_remap ? "    this flash will be used for debugging, writing and etc\n" : "",
+				niietcm4_info->uflash_size/1024,
+				niietcm4_info->uflash_pagetotal,
+				niietcm4_info->uflash_size/niietcm4_info->uflash_pagetotal,
+				niietcm4_info->uflash_width,
+				niietcm4_info->uflash_info_size,
+				niietcm4_info->uflash_info_pagetotal,
+				niietcm4_info->uflash_info_size/niietcm4_info->uflash_info_pagetotal,
+				niietcm4_info->uflash_width,
+				extmem_base,
+				niietcm4_info->bflash_info_remap ? "enable" : "disable",
+				niietcm4_info->extmem_boot_port,
+				niietcm4_info->extmem_boot_pin,
+				niietcm4_info->extmem_boot_altfunc,
+				niietcm4_info->extmem_boot ? "enable" : "disable");
+	} else{
+		bank->size = 0x100000;
+		bank->num_sectors = 128;
+
+		sprintf(niietcm4_info->chip_brief,
+				"\n"
+				"H[2] was HIGH while startup. Device entered service mode.\n"
+				"All flashes were locked.\n"
+				"If you want to perform emergency erase (erase all flashes),\n"
+				"please use \"service_mode_erase\" command and reset device.\n"
+				"Do not forget to pull H[2] down while reset for returning to normal operation mode.\n"
+				);
+	}
+
+	return retval;
+}
+
+static int niietcm4_probe(struct flash_bank *bank)
+{
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+	struct target *target = bank->target;
+
+	if (bank->sectors) {
+		free(bank->sectors);
+		bank->sectors = NULL;
+	}
+	uint32_t retval;
+	uint32_t chipid;
+
+	retval = target_read_u32(target, CHIPID_ADDR, &chipid);
+	if (retval != ERROR_OK) {
+		chipid = K1921VK01T_ID;
+		LOG_INFO("unknown chipid, assuming K1921VK01T");
+	}
+
+	if (chipid == K1921VK01T_ID)
+		niietcm4_probe_k1921vk01t(bank);
+
+	int page_total = bank->num_sectors;
+	int page_size = bank->size / page_total;
+
+	bank->sectors = malloc(sizeof(struct flash_sector) * page_total);
+
+	for (int i = 0; i < page_total; i++) {
+		bank->sectors[i].offset = i * page_size;
+		bank->sectors[i].size = page_size;
+		bank->sectors[i].is_erased = -1;
+		bank->sectors[i].is_protected = -1;
+	}
+
+	niietcm4_info->probed = true;
+
+	return ERROR_OK;
+}
+
+static int niietcm4_auto_probe(struct flash_bank *bank)
+{
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+	if (niietcm4_info->probed)
+		return ERROR_OK;
+	return niietcm4_probe(bank);
+}
+
+static int get_niietcm4_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
+	LOG_INFO("\nNIIET Cortex M4F %s\n%s", niietcm4_info->chip_name, niietcm4_info->chip_brief);
+	snprintf(buf, buf_size, " ");
+
+	return ERROR_OK;
+}
+
+
+struct flash_driver niietcm4_flash = {
+	.name = "niietcm4",
+	.usage = "flash bank <name> niietcm4 <base> <size> 0 0 <target#>",
+	.commands = niietcm4_command_handlers,
+	.flash_bank_command = niietcm4_flash_bank_command,
+	.erase = niietcm4_erase,
+	.protect = niietcm4_protect,
+	.write = niietcm4_write,
+	.read = default_flash_read,
+	.probe = niietcm4_probe,
+	.auto_probe = niietcm4_auto_probe,
+	.erase_check = default_flash_blank_check,
+	.protect_check = niietcm4_protect_check,
+	.info = get_niietcm4_info,
+};
diff --git a/tcl/target/k1921vk01t.cfg b/tcl/target/k1921vk01t.cfg
new file mode 100755
index 0000000..131bdad
--- /dev/null
+++ b/tcl/target/k1921vk01t.cfg
@@ -0,0 +1,55 @@
+# K1921VK01T
+# http://niiet.ru/chips/nis?id=354
+
+source [find target/swj-dp.tcl]
+source [find mem_helper.tcl]
+
+if { [info exists CHIPNAME] } {
+   set _CHIPNAME $CHIPNAME
+} else {
+   set _CHIPNAME k1921vk01t
+}
+
+set _ENDIAN little
+
+# Work-area is a space in RAM used for flash programming
+if { [info exists WORKAREASIZE] } {
+   set _WORKAREASIZE $WORKAREASIZE
+} else {
+   set _WORKAREASIZE 0x10000
+}
+
+#jtag scan chain
+if { [info exists CPUTAPID] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+   if { [using_jtag] } {
+      set _CPUTAPID 0x410fc241
+   } {
+      # SWD IDCODE
+      set _CPUTAPID 0x2ba01477
+   }
+}
+swj_newdap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m -endian $_ENDIAN -chain-position $_TARGETNAME
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+flash bank $_CHIPNAME.flash niietcm4 0 0 0 0 $_TARGETNAME
+
+adapter_khz 2000
+
+adapter_nsrst_delay 100
+if {[using_jtag]} {
+   jtag_ntrst_delay 100
+}
+
+reset_config srst_nogate
+
+if {![using_hla]} {
+   # if srst is not fitted use SYSRESETREQ to
+   # perform a soft reset
+   cortex_m reset_config sysresetreq
+}