Merge pull request #251 from riscv/from_upstream

From upstream
2018-05-17 16:47:48 -07:00
parent 3344db3d0a 712d6a5c3a
commit bb86173f37
73 changed files with 1600 additions and 914 deletions
@@ -3117,6 +3117,22 @@ FLASH_BANK_COMMAND_HANDLER(sam3_flash_bank_command)
 	return ERROR_OK;
 }

+/**
+ * Remove all chips from the internal list without distingushing which one
+ * is owned by this bank. This simplification works only for one shot
+ * deallocation like current flash_free_all_banks()
+ */
+void sam3_free_driver_priv(struct flash_bank *bank)
+{
+	struct sam3_chip *chip = all_sam3_chips;
+	while (chip) {
+		struct sam3_chip *next = chip->next;
+		free(chip);
+		chip = next;
+	}
+	all_sam3_chips = NULL;
+}
+
 static int sam3_GetDetails(struct sam3_bank_private *pPrivate)
 {
 	const struct sam3_chip_details *pDetails;
@@ -3771,4 +3787,5 @@ struct flash_driver at91sam3_flash = {
 	.auto_probe = sam3_auto_probe,
 	.erase_check = sam3_erase_check,
 	.protect_check = sam3_protect_check,
+	.free_driver_priv = sam3_free_driver_priv,
 };
@@ -2514,6 +2514,22 @@ FLASH_BANK_COMMAND_HANDLER(sam4_flash_bank_command)
 	return ERROR_OK;
 }

+/**
+ * Remove all chips from the internal list without distingushing which one
+ * is owned by this bank. This simplification works only for one shot
+ * deallocation like current flash_free_all_banks()
+ */
+static void sam4_free_driver_priv(struct flash_bank *bank)
+{
+	struct sam4_chip *chip = all_sam4_chips;
+	while (chip) {
+		struct sam4_chip *next = chip->next;
+		free(chip);
+		chip = next;
+	}
+	all_sam4_chips = NULL;
+}
+
 static int sam4_GetDetails(struct sam4_bank_private *pPrivate)
 {
 	const struct sam4_chip_details *pDetails;
@@ -3194,4 +3210,5 @@ struct flash_driver at91sam4_flash = {
 	.auto_probe = sam4_auto_probe,
 	.erase_check = default_flash_blank_check,
 	.protect_check = sam4_protect_check,
+	.free_driver_priv = sam4_free_driver_priv,
 };
@@ -129,10 +129,8 @@ struct sam4l_info {

 	bool probed;
 	struct target *target;
-	struct sam4l_info *next;
 };

-static struct sam4l_info *sam4l_chips;

 static int sam4l_flash_wait_until_ready(struct target *target)
 {
@@ -204,30 +202,6 @@ static int sam4l_flash_command(struct target *target, uint8_t cmd, int page)

 FLASH_BANK_COMMAND_HANDLER(sam4l_flash_bank_command)
 {
-	struct sam4l_info *chip = sam4l_chips;
-
-	while (chip) {
-		if (chip->target == bank->target)
-			break;
-		chip = chip->next;
-	}
-
-	if (!chip) {
-		/* Create a new chip */
-		chip = calloc(1, sizeof(*chip));
-		if (!chip)
-			return ERROR_FAIL;
-
-		chip->target = bank->target;
-		chip->probed = false;
-
-		bank->driver_priv = chip;
-
-		/* Insert it into the chips list (at head) */
-		chip->next = sam4l_chips;
-		sam4l_chips = chip;
-	}
-
 	if (bank->base != SAM4L_FLASH) {
 		LOG_ERROR("Address 0x%08" PRIx32 " invalid bank address (try 0x%08" PRIx32
 				"[at91sam4l series] )",
@@ -235,6 +209,18 @@ FLASH_BANK_COMMAND_HANDLER(sam4l_flash_bank_command)
 		return ERROR_FAIL;
 	}

+	struct sam4l_info *chip;
+	chip = calloc(1, sizeof(*chip));
+	if (!chip) {
+		LOG_ERROR("No memory for flash bank chip info");
+		return ERROR_FAIL;
+	}
+
+	chip->target = bank->target;
+	chip->probed = false;
+
+	bank->driver_priv = chip;
+
 	return ERROR_OK;
 }

@@ -396,7 +382,7 @@ static int sam4l_protect_check(struct flash_bank *bank)

 static int sam4l_protect(struct flash_bank *bank, int set, int first, int last)
 {
-	struct sam4l_info *chip = sam4l_chips;
+	struct sam4l_info *chip = (struct sam4l_info *)bank->driver_priv;

 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
@@ -709,4 +695,5 @@ struct flash_driver at91sam4l_flash = {
 	.auto_probe = sam4l_probe,
 	.erase_check = default_flash_blank_check,
 	.protect_check = sam4l_protect_check,
+	.free_driver_priv = default_flash_free_driver_priv,
 };
@@ -639,14 +639,6 @@ static int at91sam7_read_part_info(struct flash_bank *bank)

 static int at91sam7_erase_check(struct flash_bank *bank)
 {
-	struct target *target = bank->target;
-	uint16_t retval;
-	uint32_t blank;
-	uint16_t fast_check;
-	uint8_t *buffer;
-	uint16_t nSector;
-	uint16_t nByte;
-
 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -656,45 +648,7 @@ static int at91sam7_erase_check(struct flash_bank *bank)
 	at91sam7_read_clock_info(bank);
 	at91sam7_set_flash_mode(bank, FMR_TIMING_FLASH);

-	fast_check = 1;
-	for (nSector = 0; nSector < bank->num_sectors; nSector++) {
-		retval = target_blank_check_memory(target,
-				bank->base + bank->sectors[nSector].offset,
-				bank->sectors[nSector].size,
-				&blank, bank->erased_value);
-		if (retval != ERROR_OK) {
-			fast_check = 0;
-			break;
-		}
-		if (blank == 0xFF)
-			bank->sectors[nSector].is_erased = 1;
-		else
-			bank->sectors[nSector].is_erased = 0;
-	}
-
-	if (fast_check)
-		return ERROR_OK;
-
-	LOG_USER("Running slow fallback erase check - add working memory");
-
-	buffer = malloc(bank->sectors[0].size);
-	for (nSector = 0; nSector < bank->num_sectors; nSector++) {
-		bank->sectors[nSector].is_erased = 1;
-		retval = target_read_memory(target, bank->base + bank->sectors[nSector].offset, 4,
-				bank->sectors[nSector].size/4, buffer);
-		if (retval != ERROR_OK)
-			return retval;
-
-		for (nByte = 0; nByte < bank->sectors[nSector].size; nByte++) {
-			if (buffer[nByte] != 0xFF) {
-				bank->sectors[nSector].is_erased = 0;
-				break;
-			}
-		}
-	}
-	free(buffer);
-
-	return ERROR_OK;
+	return default_flash_blank_check(bank);
 }

 static int at91sam7_protect_check(struct flash_bank *bank)
@@ -304,10 +304,8 @@ struct samd_info {

 	bool probed;
 	struct target *target;
-	struct samd_info *next;
 };

-static struct samd_info *samd_chips;

 /**
 * Gives the family structure to specific device id.
@@ -876,30 +874,6 @@ free_pb:

 FLASH_BANK_COMMAND_HANDLER(samd_flash_bank_command)
 {
-	struct samd_info *chip = samd_chips;
-
-	while (chip) {
-		if (chip->target == bank->target)
-			break;
-		chip = chip->next;
-	}
-
-	if (!chip) {
-		/* Create a new chip */
-		chip = calloc(1, sizeof(*chip));
-		if (!chip)
-			return ERROR_FAIL;
-
-		chip->target = bank->target;
-		chip->probed = false;
-
-		bank->driver_priv = chip;
-
-		/* Insert it into the chips list (at head) */
-		chip->next = samd_chips;
-		samd_chips = chip;
-	}
-
 	if (bank->base != SAMD_FLASH) {
 		LOG_ERROR("Address 0x%08" PRIx32 " invalid bank address (try 0x%08" PRIx32
 				"[at91samd series] )",
@@ -907,6 +881,18 @@ FLASH_BANK_COMMAND_HANDLER(samd_flash_bank_command)
 		return ERROR_FAIL;
 	}

+	struct samd_info *chip;
+	chip = calloc(1, sizeof(*chip));
+	if (!chip) {
+		LOG_ERROR("No memory for flash bank chip info");
+		return ERROR_FAIL;
+	}
+
+	chip->target = bank->target;
+	chip->probed = false;
+
+	bank->driver_priv = chip;
+
 	return ERROR_OK;
 }

@@ -1281,4 +1267,5 @@ struct flash_driver at91samd_flash = {
 	.auto_probe = samd_probe,
 	.erase_check = default_flash_blank_check,
 	.protect_check = samd_protect_check,
+	.free_driver_priv = default_flash_free_driver_priv,
 };
@@ -4,6 +4,7 @@
 *   Copyright (C) 2008 by Spencer Oliver <spen@spen-soft.co.uk>           *
 *   Copyright (C) 2009 Zachary T Welch <zw@superlucidity.net>             *
 *   Copyright (C) 2010 by Antonio Borneo <borneo.antonio@gmail.com>       *
+ *   Copyright (C) 2017-2018 Tomas Vanek <vanekt@fbl.cz>                   *
 *                                                                         *
 *   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  *
@@ -339,36 +340,49 @@ int default_flash_blank_check(struct flash_bank *bank)
 	struct target *target = bank->target;
 	int i;
 	int retval;
-	int fast_check = 0;
-	uint32_t blank;

 	if (bank->target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}

-	for (i = 0; i < bank->num_sectors; i++) {
-		uint32_t address = bank->base + bank->sectors[i].offset;
-		uint32_t size = bank->sectors[i].size;
+	struct target_memory_check_block *block_array;
+	block_array = malloc(bank->num_sectors * sizeof(struct target_memory_check_block));
+	if (block_array == NULL)
+		return default_flash_mem_blank_check(bank);

-		retval = target_blank_check_memory(target, address, size, &blank, bank->erased_value);
-		if (retval != ERROR_OK) {
-			fast_check = 0;
+	for (i = 0; i < bank->num_sectors; i++) {
+		block_array[i].address = bank->base + bank->sectors[i].offset;
+		block_array[i].size = bank->sectors[i].size;
+		block_array[i].result = UINT32_MAX; /* erase state unknown */
+	}
+
+	bool fast_check = true;
+	for (i = 0; i < bank->num_sectors; ) {
+		retval = target_blank_check_memory(target,
+				block_array + i, bank->num_sectors - i,
+				bank->erased_value);
+		if (retval < 1) {
+			/* Run slow fallback if the first run gives no result
+			 * otherwise use possibly incomplete results */
+			if (i == 0)
+				fast_check = false;
 			break;
 		}
-		if (blank == bank->erased_value)
-			bank->sectors[i].is_erased = 1;
-		else
-			bank->sectors[i].is_erased = 0;
-		fast_check = 1;
+		i += retval; /* add number of blocks done this round */
 	}

-	if (!fast_check) {
+	if (fast_check) {
+		for (i = 0; i < bank->num_sectors; i++)
+			bank->sectors[i].is_erased = block_array[i].result;
+		retval = ERROR_OK;
+	} else {
 		LOG_USER("Running slow fallback erase check - add working memory");
-		return default_flash_mem_blank_check(bank);
+		retval = default_flash_mem_blank_check(bank);
 	}
+	free(block_array);

-	return ERROR_OK;
+	return retval;
 }

 /* Manipulate given flash region, selecting the bank according to target
@@ -587,6 +601,87 @@ static int compare_section(const void *a, const void *b)
 		return -1;
 }

+/**
+ * Get aligned start address of a flash write region
+ */
+target_addr_t flash_write_align_start(struct flash_bank *bank, target_addr_t addr)
+{
+	if (addr < bank->base || addr >= bank->base + bank->size
+			|| bank->write_start_alignment <= 1)
+		return addr;
+
+	if (bank->write_start_alignment == FLASH_WRITE_ALIGN_SECTOR) {
+		uint32_t offset = addr - bank->base;
+		uint32_t aligned = 0;
+		int sect;
+		for (sect = 0; sect < bank->num_sectors; sect++) {
+			if (bank->sectors[sect].offset > offset)
+				break;
+
+			aligned = bank->sectors[sect].offset;
+		}
+		return bank->base + aligned;
+	}
+
+	return addr & ~(bank->write_start_alignment - 1);
+}
+
+/**
+ * Get aligned end address of a flash write region
+ */
+target_addr_t flash_write_align_end(struct flash_bank *bank, target_addr_t addr)
+{
+	if (addr < bank->base || addr >= bank->base + bank->size
+			|| bank->write_end_alignment <= 1)
+		return addr;
+
+	if (bank->write_end_alignment == FLASH_WRITE_ALIGN_SECTOR) {
+		uint32_t offset = addr - bank->base;
+		uint32_t aligned = 0;
+		int sect;
+		for (sect = 0; sect < bank->num_sectors; sect++) {
+			aligned = bank->sectors[sect].offset + bank->sectors[sect].size - 1;
+			if (aligned >= offset)
+				break;
+		}
+		return bank->base + aligned;
+	}
+
+	return addr | (bank->write_end_alignment - 1);
+}
+
+/**
+ * Check if gap between sections is bigger than minimum required to discontinue flash write
+ */
+static bool flash_write_check_gap(struct flash_bank *bank,
+				target_addr_t addr1, target_addr_t addr2)
+{
+	if (bank->minimal_write_gap == FLASH_WRITE_CONTINUOUS
+			|| addr1 < bank->base || addr1 >= bank->base + bank->size
+			|| addr2 < bank->base || addr2 >= bank->base + bank->size)
+		return false;
+
+	if (bank->minimal_write_gap == FLASH_WRITE_GAP_SECTOR) {
+		int sect;
+		uint32_t offset1 = addr1 - bank->base;
+		/* find the sector following the one containing addr1 */
+		for (sect = 0; sect < bank->num_sectors; sect++) {
+			if (bank->sectors[sect].offset > offset1)
+				break;
+		}
+		if (sect >= bank->num_sectors)
+			return false;
+
+		uint32_t offset2 = addr2 - bank->base;
+		return bank->sectors[sect].offset + bank->sectors[sect].size <= offset2;
+	}
+
+	target_addr_t aligned1 = flash_write_align_end(bank, addr1);
+	target_addr_t aligned2 = flash_write_align_start(bank, addr2);
+	return aligned1 + bank->minimal_write_gap < aligned2;
+}
+
+
 int flash_write_unlock(struct target *target, struct image *image,
 	uint32_t *written, int erase, bool unlock)
 {
@@ -626,7 +721,7 @@ int flash_write_unlock(struct target *target, struct image *image,

 	/* loop until we reach end of the image */
 	while (section < image->num_sections) {
-		uint32_t buffer_size;
+		uint32_t buffer_idx;
 		uint8_t *buffer;
 		int section_last;
 		target_addr_t run_address = sections[section]->base_address + section_offset;
@@ -663,43 +758,37 @@ int flash_write_unlock(struct target *target, struct image *image,
 				break;
 			}

-			/* FIXME This needlessly touches sectors BETWEEN the
-			 * sections it's writing.  Without auto erase, it just
-			 * writes ones.  That WILL INVALIDATE data in cases
-			 * like Stellaris Tempest chips, corrupting internal
-			 * ECC codes; and at least FreeScale suggests issues
-			 * with that approach (in HC11 documentation).
-			 *
-			 * With auto erase enabled, data in those sectors will
-			 * be needlessly destroyed; and some of the limited
-			 * number of flash erase cycles will be wasted...
-			 *
-			 * In both cases, the extra writes slow things down.
-			 */
-
 			/* if we have multiple sections within our image,
 			 * flash programming could fail due to alignment issues
 			 * attempt to rebuild a consecutive buffer for the flash loader */
 			target_addr_t run_next_addr = run_address + run_size;
-			if (sections[section_last + 1]->base_address < run_next_addr) {
+			target_addr_t next_section_base = sections[section_last + 1]->base_address;
+			if (next_section_base < run_next_addr) {
 				LOG_ERROR("Section at " TARGET_ADDR_FMT
 					" overlaps section ending at " TARGET_ADDR_FMT,
-					sections[section_last + 1]->base_address,
-					run_next_addr);
+					next_section_base, run_next_addr);
 				LOG_ERROR("Flash write aborted.");
 				retval = ERROR_FAIL;
 				goto done;
 			}

-			pad_bytes = sections[section_last + 1]->base_address - run_next_addr;
-			padding[section_last] = pad_bytes;
-			run_size += sections[++section_last]->size;
-			run_size += pad_bytes;
-
+			pad_bytes = next_section_base - run_next_addr;
+			if (pad_bytes) {
+				if (flash_write_check_gap(c, run_next_addr - 1, next_section_base)) {
+					LOG_INFO("Flash write discontinued at " TARGET_ADDR_FMT
+						", next section at " TARGET_ADDR_FMT,
+						run_next_addr, next_section_base);
+					break;
+				}
+			}
 			if (pad_bytes > 0)
-				LOG_INFO("Padding image section %d with %d bytes",
-					section_last-1,
-					pad_bytes);
+				LOG_INFO("Padding image section %d at " TARGET_ADDR_FMT
+					" with %d bytes",
+					section_last, run_next_addr, pad_bytes);
+
+			padding[section_last] = pad_bytes;
+			run_size += pad_bytes;
+			run_size += sections[++section_last]->size;
 		}

 		if (run_address + run_size - 1 > c->base + c->size - 1) {
@@ -712,10 +801,38 @@ int flash_write_unlock(struct target *target, struct image *image,
 			assert(run_size > 0);
 		}

-		/* If we're applying any sector automagic, then pad this
-		 * (maybe-combined) segment to the end of its last sector.
-		 */
-		if (unlock || erase) {
+		uint32_t padding_at_start = 0;
+		if (c->write_start_alignment || c->write_end_alignment) {
+			/* align write region according to bank requirements */
+			target_addr_t aligned_start = flash_write_align_start(c, run_address);
+			padding_at_start = run_address - aligned_start;
+			if (padding_at_start > 0) {
+				LOG_WARNING("Section start address " TARGET_ADDR_FMT
+					" breaks the required alignment of flash bank %s",
+					run_address, c->name);
+				LOG_WARNING("Padding %d bytes from " TARGET_ADDR_FMT,
+					padding_at_start, aligned_start);
+
+				run_address -= padding_at_start;
+				run_size += padding_at_start;
+			}
+
+			target_addr_t run_end = run_address + run_size - 1;
+			target_addr_t aligned_end = flash_write_align_end(c, run_end);
+			pad_bytes = aligned_end - run_end;
+			if (pad_bytes > 0) {
+				LOG_INFO("Padding image section %d at " TARGET_ADDR_FMT
+					" with %d bytes (bank write end alignment)",
+					section_last, run_end + 1, pad_bytes);
+
+				padding[section_last] += pad_bytes;
+				run_size += pad_bytes;
+			}
+
+		} else if (unlock || erase) {
+			/* If we're applying any sector automagic, then pad this
+			 * (maybe-combined) segment to the end of its last sector.
+			 */
 			int sector;
 			uint32_t offset_start = run_address - c->base;
 			uint32_t offset_end = offset_start + run_size;
@@ -740,13 +857,17 @@ int flash_write_unlock(struct target *target, struct image *image,
 			retval = ERROR_FAIL;
 			goto done;
 		}
-		buffer_size = 0;
+
+		if (padding_at_start)
+			memset(buffer, c->default_padded_value, padding_at_start);
+
+		buffer_idx = padding_at_start;

 		/* read sections to the buffer */
-		while (buffer_size < run_size) {
+		while (buffer_idx < run_size) {
 			size_t size_read;

-			size_read = run_size - buffer_size;
+			size_read = run_size - buffer_idx;
 			if (size_read > sections[section]->size - section_offset)
 				size_read = sections[section]->size - section_offset;

@@ -759,23 +880,25 @@ int flash_write_unlock(struct target *target, struct image *image,
 			int t_section_num = diff / sizeof(struct imagesection);

 			LOG_DEBUG("image_read_section: section = %d, t_section_num = %d, "
-					"section_offset = %d, buffer_size = %d, size_read = %d",
-				(int)section, (int)t_section_num, (int)section_offset,
-				(int)buffer_size, (int)size_read);
+					"section_offset = %"PRIu32", buffer_idx = %"PRIu32", size_read = %zu",
+				section, t_section_num, section_offset,
+				buffer_idx, size_read);
 			retval = image_read_section(image, t_section_num, section_offset,
-					size_read, buffer + buffer_size, &size_read);
+					size_read, buffer + buffer_idx, &size_read);
 			if (retval != ERROR_OK || size_read == 0) {
 				free(buffer);
 				goto done;
 			}

-			/* see if we need to pad the section */
-			while (padding[section]--)
-				(buffer + buffer_size)[size_read++] = c->default_padded_value;
-
-			buffer_size += size_read;
+			buffer_idx += size_read;
 			section_offset += size_read;

+			/* see if we need to pad the section */
+			if (padding[section]) {
+				memset(buffer + buffer_idx, c->default_padded_value, padding[section]);
+				buffer_idx += padding[section];
+			}
+
 			if (section_offset >= sections[section]->size) {
 				section++;
 				section_offset = 0;
@@ -65,6 +65,13 @@ struct flash_sector {
 	int is_protected;
 };

+/** Special value for write_start_alignment and write_end_alignment field */
+#define FLASH_WRITE_ALIGN_SECTOR	UINT32_MAX
+
+/** Special values for minimal_write_gap field */
+#define FLASH_WRITE_CONTINUOUS		0
+#define FLASH_WRITE_GAP_SECTOR		UINT32_MAX
+
 /**
 * Provides details of a flash bank, available either on-chip or through
 * a major interface.
@@ -97,6 +104,18 @@ struct flash_bank {
 	 * erased value. Defaults to 0xFF. */
 	uint8_t default_padded_value;

+	/** Required alignment of flash write start address.
+	 * Default 0, no alignment. Can be any power of two or FLASH_WRITE_ALIGN_SECTOR */
+	uint32_t write_start_alignment;
+	/** Required alignment of flash write end address.
+	 * Default 0, no alignment. Can be any power of two or FLASH_WRITE_ALIGN_SECTOR */
+	uint32_t write_end_alignment;
+	/** Minimal gap between sections to discontinue flash write
+	 * Default FLASH_WRITE_GAP_SECTOR splits the write if one or more untouched
+	 * sectors in between.
+     * Can be size in bytes or FLASH_WRITE_CONTINUOUS */
+	uint32_t minimal_write_gap;
+
 	/**
 	 * The number of sectors on this chip.  This value will
 	 * be set intially to 0, and the flash driver must set this to
@@ -135,6 +154,22 @@ int flash_erase_address_range(struct target *target,
 int flash_unlock_address_range(struct target *target, uint32_t addr,
 		uint32_t length);

+/**
+ * Align start address of a flash write region according to bank requirements.
+ * @param bank Pointer to bank descriptor structure
+ * @param addr Address to align
+ * @returns Aligned address
+*/
+target_addr_t flash_write_align_start(struct flash_bank *bank, target_addr_t addr);
+/**
+ * Align end address of a flash write region according to bank requirements.
+ * Note: Use address of the last byte to write, not the next after the region.
+ * @param bank Pointer to bank descriptor structure
+ * @param addr Address to align (address of the last byte to write)
+ * @returns Aligned address (address of the last byte of padded region)
+*/
+target_addr_t flash_write_align_end(struct flash_bank *bank, target_addr_t addr);
+
 /**
 * Writes @a image into the @a target flash.  The @a written parameter
 * will contain the
@@ -915,6 +915,22 @@ FLASH_BANK_COMMAND_HANDLER(kinetis_flash_bank_command)
 }


+static void kinetis_free_driver_priv(struct flash_bank *bank)
+{
+	struct kinetis_flash_bank *k_bank = bank->driver_priv;
+	if (k_bank == NULL)
+		return;
+
+	struct kinetis_chip *k_chip = k_bank->k_chip;
+	if (k_chip == NULL)
+		return;
+
+	k_chip->num_banks--;
+	if (k_chip->num_banks == 0)
+		free(k_chip);
+}
+
+
 static int kinetis_create_missing_banks(struct kinetis_chip *k_chip)
 {
 	unsigned bank_idx;
@@ -939,7 +955,7 @@ static int kinetis_create_missing_banks(struct kinetis_chip *k_chip)
 			if (k_chip->num_pflash_blocks > 1) {
 				/* rename first bank if numbering is needed */
 				snprintf(name, sizeof(name), "%s.pflash0", base_name);
-				free((void *)bank->name);
+				free(bank->name);
 				bank->name = strdup(name);
 			}
 		}
@@ -3132,4 +3148,5 @@ struct flash_driver kinetis_flash = {
 	.erase_check = kinetis_blank_check,
 	.protect_check = kinetis_protect_check,
 	.info = kinetis_info,
+	.free_driver_priv = kinetis_free_driver_priv,
 };
@@ -1,6 +1,6 @@
 /***************************************************************************
 *                                                                         *
- *   Copyright (C) 2017 by Bohdan Tymkiv                                   *
+ *   Copyright (C) 2018 by Bohdan Tymkiv                                   *
 *   bohdan.tymkiv@cypress.com bohdan200@gmail.com                         *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
@@ -101,7 +101,7 @@ struct row_region {
 	size_t size;
 };

-static struct row_region safe_sflash_regions[] = {
+static const struct row_region safe_sflash_regions[] = {
 	{0x16000800, 0x800},	/* SFLASH: User Data */
 	{0x16001A00, 0x200},	/* SFLASH: NAR */
 	{0x16005A00, 0xC00},	/* SFLASH: Public Key */
@@ -111,8 +111,12 @@ static struct row_region safe_sflash_regions[] = {
 #define SFLASH_NUM_REGIONS (sizeof(safe_sflash_regions) / sizeof(safe_sflash_regions[0]))

 static struct working_area *g_stack_area;
-/**************************************************************************************************
- * Initializes timeout_s structure with given timeout in milliseconds
+static struct armv7m_algorithm g_armv7m_info;
+
+/** ***********************************************************************************************
+ * @brief Initializes `struct timeout` structure with given timeout value
+ * @param to pointer to `struct timeout` structure
+ * @param timeout_ms timeout, in milliseconds
 *************************************************************************************************/
 static void timeout_init(struct timeout *to, long timeout_ms)
 {
@@ -120,17 +124,23 @@ static void timeout_init(struct timeout *to, long timeout_ms)
 	to->timeout_ms = timeout_ms;
 }

-/**************************************************************************************************
- * Returns true if given timeout_s object has expired
+/** ***********************************************************************************************
+ * @brief Returns true if given `struct timeout` structure has expired
+ * @param to pointer to `struct timeout` structure
+ * @return true if timeout expired
 *************************************************************************************************/
 static bool timeout_expired(struct timeout *to)
 {
 	return (timeval_ms() - to->start_time) > to->timeout_ms;
 }

-/**************************************************************************************************
- * Prepares PSoC6 for running pseudo flash algorithm. This function allocates Working Area for
- * the algorithm and for CPU Stack.
+/** ***********************************************************************************************
+ * @brief Starts pseudo flash algorithm and leaves it running. Function allocates working area for
+ * algorithm code and CPU stack, adjusts stack pointer, uploads and starts the algorithm.
+ * Algorithm (a basic infinite loop) runs asynchronously while driver performs Flash operations.
+ *
+ * @param target target for the algorithm
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int sromalgo_prepare(struct target *target)
 {
@@ -141,21 +151,42 @@ static int sromalgo_prepare(struct target *target)
 	if (hr != ERROR_OK)
 		return hr;

+	/* Restore THUMB bit in xPSR register */
+	const struct armv7m_common *cm = target_to_armv7m(target);
+	hr = cm->store_core_reg_u32(target, ARMV7M_xPSR, 0x01000000);
+	if (hr != ERROR_OK)
+		return hr;
+
 	/* Allocate Working Area for Stack and Flash algorithm */
 	hr = target_alloc_working_area(target, RAM_STACK_WA_SIZE, &g_stack_area);
 	if (hr != ERROR_OK)
 		return hr;

-	/* Restore THUMB bit in xPSR register */
-	const struct armv7m_common *cm = target_to_armv7m(target);
-	hr = cm->store_core_reg_u32(target, ARMV7M_xPSR, 0x01000000);
+	g_armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+	g_armv7m_info.core_mode = ARM_MODE_THREAD;
+
+	struct reg_param reg_params;
+	init_reg_param(&reg_params, "sp", 32, PARAM_OUT);
+	buf_set_u32(reg_params.value, 0, 32, g_stack_area->address + g_stack_area->size);
+
+	/* Write basic infinite loop algorithm to target RAM */
+	hr = target_write_u32(target, g_stack_area->address, 0xFEE7FEE7);
 	if (hr != ERROR_OK)
-		goto exit_free_wa;
+		goto destroy_rp_free_wa;

-	return ERROR_OK;
+	hr = target_start_algorithm(target, 0, NULL, 1, &reg_params, g_stack_area->address,
+			0, &g_armv7m_info);
+	if (hr != ERROR_OK)
+		goto destroy_rp_free_wa;
+
+	destroy_reg_param(&reg_params);
+
+	return hr;
+
+destroy_rp_free_wa:
+	/* Something went wrong, do some cleanup */
+	destroy_reg_param(&reg_params);

-exit_free_wa:
-	/* Something went wrong, free allocated area */
 	if (g_stack_area) {
 		target_free_working_area(target, g_stack_area);
 		g_stack_area = NULL;
@@ -164,65 +195,48 @@ exit_free_wa:
 	return hr;
 }

-/**************************************************************************************************
- * Releases working area
+/** ***********************************************************************************************
+ * @brief Stops running flash algorithm and releases associated resources.
+ * This function is also used for cleanup in case of errors so g_stack_area may be NULL.
+ * These cases have to be handled gracefully.
+ *
+ * @param target current target
 *************************************************************************************************/
-static int sromalgo_release(struct target *target)
+static void sromalgo_release(struct target *target)
 {
 	int hr = ERROR_OK;

-	/* Free Stack/Flash algorithm working area */
 	if (g_stack_area) {
-		hr = target_free_working_area(target, g_stack_area);
+		/* Stop flash algorithm if it is running */
+		if (target->running_alg) {
+			hr = target_halt(target);
+			if (hr != ERROR_OK)
+				goto exit_free_wa;
+
+			hr = target_wait_algorithm(target, 0, NULL, 0, NULL, 0,
+					IPC_TIMEOUT_MS, &g_armv7m_info);
+			if (hr != ERROR_OK)
+				goto exit_free_wa;
+		}
+
+exit_free_wa:
+		/* Free Stack/Flash algorithm working area */
+		target_free_working_area(target, g_stack_area);
 		g_stack_area = NULL;
 	}
-
-	return hr;
 }

-/**************************************************************************************************
- * Runs pseudo flash algorithm. Algorithm itself consist of couple of NOPs followed by BKPT
- * instruction. The trick here is that NMI has already been posted to CM0 via IPC structure
- * prior to calling this function. CM0 will immediately jump to NMI handler and execute
- * SROM API code.
- * This approach is borrowed from PSoC4 Flash Driver.
- *************************************************************************************************/
-static int sromalgo_run(struct target *target)
-{
-	int hr;
-
-	struct armv7m_algorithm armv7m_info;
-	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
-	armv7m_info.core_mode = ARM_MODE_THREAD;
-
-	struct reg_param reg_params;
-	init_reg_param(&reg_params, "sp", 32, PARAM_OUT);
-	buf_set_u32(reg_params.value, 0, 32, g_stack_area->address + g_stack_area->size);
-
-	/* mov r8, r8; mov r8, r8 */
-	hr = target_write_u32(target, g_stack_area->address + 0, 0x46C046C0);
-	if (hr != ERROR_OK)
-		return hr;
-
-	/* mov r8, r8; bkpt #0    */
-	hr = target_write_u32(target, g_stack_area->address + 4, 0xBE0046C0);
-	if (hr != ERROR_OK)
-		return hr;
-
-	hr = target_run_algorithm(target, 0, NULL, 1, &reg_params, g_stack_area->address,
-			0, SROMAPI_CALL_TIMEOUT_MS, &armv7m_info);
-
-	destroy_reg_param(&reg_params);
-
-	return hr;
-}
-
-/**************************************************************************************************
- * Waits for expected IPC lock status.
- * PSoC6 uses IPC structures for inter-core communication. Same IPCs are used to invoke SROM API.
- * IPC structure must be locked prior to invoking any SROM API. This ensures nothing else in the
- * system will use same IPC thus corrupting our data. Locking is performed by ipc_acquire(), this
- * function ensures that IPC is actually in expected state
+/** ***********************************************************************************************
+ * @brief Waits for expected IPC lock status. PSoC6 uses IPC structures for inter-core
+ * communication. Same IPCs are used to invoke SROM API. IPC structure must be locked prior to
+ * invoking any SROM API. This ensures nothing else in the system will use same IPC thus corrupting
+ * our data. Locking is performed by ipc_acquire(), this function ensures that IPC is actually
+ * in expected state
+ *
+ * @param target current target
+ * @param ipc_id IPC index to poll. IPC #2 is dedicated for DAP access
+ * @param lock_expected expected lock status
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int ipc_poll_lock_stat(struct target *target, uint32_t ipc_id, bool lock_expected)
 {
@@ -258,11 +272,15 @@ static int ipc_poll_lock_stat(struct target *target, uint32_t ipc_id, bool lock_
 	return ERROR_TARGET_TIMEOUT;
 }

-/**************************************************************************************************
- * Acquires IPC structure
- * PSoC6 uses IPC structures for inter-core communication. Same IPCs are used to invoke SROM API.
- * IPC structure must be locked prior to invoking any SROM API. This ensures nothing else in the
- * system will use same IPC thus corrupting our data. This function locks the IPC.
+/** ***********************************************************************************************
+ * @brief Acquires IPC structure. PSoC6 uses IPC structures for inter-core communication.
+ * Same IPCs are used to invoke SROM API. IPC structure must be locked prior to invoking any SROM API.
+ * This ensures nothing else in the system will use same IPC thus corrupting our data.
+ * This function locks the IPC.
+ *
+ * @param target current target
+ * @param ipc_id ipc_id IPC index to acquire. IPC #2 is dedicated for DAP access
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int ipc_acquire(struct target *target, char ipc_id)
 {
@@ -303,8 +321,14 @@ static int ipc_acquire(struct target *target, char ipc_id)
 	return hr;
 }

-/**************************************************************************************************
- * Invokes SROM API functions which are responsible for Flash operations
+/** ***********************************************************************************************
+ * @brief Invokes SROM API functions which are responsible for Flash operations
+ *
+ * @param target current target
+ * @param req_and_params requect id of the function to invoke
+ * @param working_area address of memory buffer in target's memory space for SROM API parameters
+ * @param data_out pointer to variable which will be populated with execution status
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int call_sromapi(struct target *target,
 	uint32_t req_and_params,
@@ -336,10 +360,6 @@ static int call_sromapi(struct target *target,
 	if (hr != ERROR_OK)
 		return hr;

-	hr = sromalgo_run(target);
-	if (hr != ERROR_OK)
-		return hr;
-
 	/* Poll lock status */
 	hr = ipc_poll_lock_stat(target, IPC_ID, false);
 	if (hr != ERROR_OK)
@@ -365,8 +385,12 @@ static int call_sromapi(struct target *target,
 	return ERROR_OK;
 }

-/**************************************************************************************************
- * Retrieves SiliconID and Protection status of the target device
+/** ***********************************************************************************************
+ * @brief Retrieves SiliconID and Protection status of the target device
+ * @param target current target
+ * @param si_id pointer to variable, will be populated with SiliconID
+ * @param protection pointer to variable, will be populated with protection status
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int get_silicon_id(struct target *target, uint32_t *si_id, uint8_t *protection)
 {
@@ -375,17 +399,17 @@ static int get_silicon_id(struct target *target, uint32_t *si_id, uint8_t *prote

 	hr = sromalgo_prepare(target);
 	if (hr != ERROR_OK)
-		return hr;
+		goto exit;

 	/* Read FamilyID and Revision */
 	hr = call_sromapi(target, SROMAPI_SIID_REQ_FAMILY_REVISION, 0, &family_rev);
 	if (hr != ERROR_OK)
-		return hr;
+		goto exit;

 	/* Read SiliconID and Protection */
 	hr = call_sromapi(target, SROMAPI_SIID_REQ_SIID_PROTECTION, 0, &siid_prot);
 	if (hr != ERROR_OK)
-		return hr;
+		goto exit;

 	*si_id  = (siid_prot & 0x0000FFFF) << 16;
 	*si_id |= (family_rev & 0x00FF0000) >> 8;
@@ -393,12 +417,15 @@ static int get_silicon_id(struct target *target, uint32_t *si_id, uint8_t *prote

 	*protection = (siid_prot & 0x000F0000) >> 0x10;

-	hr = sromalgo_release(target);
-	return hr;
+exit:
+	sromalgo_release(target);
+	return ERROR_OK;
 }

-/**************************************************************************************************
- * Translates Protection status to openocd-friendly boolean value
+/** ***********************************************************************************************
+ * @brief Translates Protection status to openocd-friendly boolean value
+ * @param bank current flash bank
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_protect_check(struct flash_bank *bank)
 {
@@ -429,8 +456,9 @@ static int psoc6_protect_check(struct flash_bank *bank)
 	return ERROR_OK;
 }

-/**************************************************************************************************
- * Life Cycle transition is not currently supported
+/** ***********************************************************************************************
+ * @brief Dummy function, Life Cycle transition is not currently supported
+ * @return ERROR_OK always
 *************************************************************************************************/
 static int psoc6_protect(struct flash_bank *bank, int set, int first, int last)
 {
@@ -443,8 +471,10 @@ static int psoc6_protect(struct flash_bank *bank, int set, int first, int last)
 	return ERROR_OK;
 }

-/**************************************************************************************************
- * Translates Protection status to string
+/** ***********************************************************************************************
+ * @brief Translates Protection status to string
+ * @param protection protection value
+ * @return pointer to const string describintg protection status
 *************************************************************************************************/
 static const char *protection_to_str(uint8_t protection)
 {
@@ -468,8 +498,12 @@ static const char *protection_to_str(uint8_t protection)
 	}
 }

-/**************************************************************************************************
- * Displays human-readable information about acquired device
+/** ***********************************************************************************************
+ * @brief psoc6_get_info Displays human-readable information about acquired device
+ * @param bank current flash bank
+ * @param buf pointer to buffer for human-readable text
+ * @param buf_size size of the buffer
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_get_info(struct flash_bank *bank, char *buf, int buf_size)
 {
@@ -494,8 +528,10 @@ static int psoc6_get_info(struct flash_bank *bank, char *buf, int buf_size)
 	return ERROR_OK;
 }

-/**************************************************************************************************
- * Returns true if flash bank name represents Supervisory Flash
+/** ***********************************************************************************************
+ * @brief Checks if given flash bank belongs to Supervisory Flash
+ * @param bank current flash bank
+ * @return true if flash bank belongs to Supervisory Flash
 *************************************************************************************************/
 static bool is_sflash_bank(struct flash_bank *bank)
 {
@@ -507,27 +543,33 @@ static bool is_sflash_bank(struct flash_bank *bank)
 	return false;
 }

-/**************************************************************************************************
- * Returns true if flash bank name represents Work Flash
+/** ***********************************************************************************************
+ * @brief Checks if given flash bank belongs to Work Flash
+ * @param bank current flash bank
+ * @return true if flash bank belongs to Work Flash
 *************************************************************************************************/
 static inline bool is_wflash_bank(struct flash_bank *bank)
 {
 	return (bank->base == MEM_BASE_WFLASH);
 }

-/**************************************************************************************************
- * Returns true if flash bank name represents Main Flash
+/** ***********************************************************************************************
+ * @brief Checks if given flash bank belongs to Main Flash
+ * @param bank current flash bank
+ * @return true if flash bank belongs to Main Flash
 *************************************************************************************************/
 static inline bool is_mflash_bank(struct flash_bank *bank)
 {
 	return (bank->base == MEM_BASE_MFLASH);
 }

-/**************************************************************************************************
- * Probes the device and populates related data structures with target flash geometry data.
+/** ***********************************************************************************************
+ * @brief Probes the device and populates related data structures with target flash geometry data.
 * This is done in non-intrusive way, no SROM API calls are involved so GDB can safely attach to a
- * running target.
- * Function assumes that size of Work Flash is 32kB (true for all current part numbers)
+ * running target. Function assumes that size of Work Flash is 32kB (true for all current part numbers)
+ *
+ * @param bank current flash bank
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_probe(struct flash_bank *bank)
 {
@@ -595,8 +637,10 @@ static int psoc6_probe(struct flash_bank *bank)
 	return hr;
 }

-/**************************************************************************************************
- * Probes target device only if it hasn't been probed yet
+/** ***********************************************************************************************
+ * @brief Probes target device only if it hasn't been probed yet
+ * @param bank current flash bank
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_auto_probe(struct flash_bank *bank)
 {
@@ -611,8 +655,12 @@ static int psoc6_auto_probe(struct flash_bank *bank)
 	return hr;
 }

-/**************************************************************************************************
- * Erases single sector (256k) on target device
+/** ***********************************************************************************************
+ * @brief Erases single sector (256k) on target device
+ * @param bank current flash bank
+ * @param wa working area for SROM API parameters
+ * @param addr starting address of the sector
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_erase_sector(struct flash_bank *bank, struct working_area *wa, uint32_t addr)
 {
@@ -636,8 +684,12 @@ static int psoc6_erase_sector(struct flash_bank *bank, struct working_area *wa,
 	return hr;
 }

-/**************************************************************************************************
- * Erases single row (512b) on target device
+/** ***********************************************************************************************
+ * @brief Erases single row (512b) on target device
+ * @param bank current flash bank
+ * @param wa working area for SROM API parameters
+ * @param addr starting address of the flash row
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_erase_row(struct flash_bank *bank, struct working_area *wa, uint32_t addr)
 {
@@ -661,9 +713,14 @@ static int psoc6_erase_row(struct flash_bank *bank, struct working_area *wa, uin
 	return hr;
 }

-/**************************************************************************************************
- * Performs Erase operation.
- * Function will try to use biggest erase block possible to speedup the operation
+/** ***********************************************************************************************
+ * @brief Performs Erase operation. Function will try to use biggest erase block possible to
+ * speedup the operation.
+ *
+ * @param bank current flash bank
+ * @param first first sector to erase
+ * @param last last sector to erase
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_erase(struct flash_bank *bank, int first, int last)
 {
@@ -681,7 +738,7 @@ static int psoc6_erase(struct flash_bank *bank, int first, int last)

 	hr = sromalgo_prepare(target);
 	if (hr != ERROR_OK)
-		return hr;
+		goto exit;

 	hr = target_alloc_working_area(target, psoc6_info->row_sz + 32, &wa);
 	if (hr != ERROR_OK)
@@ -720,9 +777,13 @@ exit:
 	return hr;
 }

-
-/**************************************************************************************************
- * Programs single Flash Row
+/** ***********************************************************************************************
+ * @brief Programs single Flash Row
+ * @param bank current flash bank
+ * @param addr address of the flash row
+ * @param buffer pointer to the buffer with data
+ * @param is_sflash true if current flash bank belongs to Supervisory Flash
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_program_row(struct flash_bank *bank,
 	uint32_t addr,
@@ -773,9 +834,13 @@ exit:
 	return hr;
 }

-
-/**************************************************************************************************
- * Programs set of Rows
+/** ***********************************************************************************************
+ * @brief Performs Program operation
+ * @param bank current flash bank
+ * @param buffer pointer to the buffer with data
+ * @param offset starting offset in falsh bank
+ * @param count number of bytes in buffer
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 static int psoc6_program(struct flash_bank *bank,
 	const uint8_t *buffer,
@@ -787,11 +852,11 @@ static int psoc6_program(struct flash_bank *bank,
 	const bool is_sflash = is_sflash_bank(bank);
 	int hr;

+	uint8_t page_buf[psoc6_info->row_sz];
+
 	hr = sromalgo_prepare(target);
 	if (hr != ERROR_OK)
-		return hr;
-
-	uint8_t page_buf[psoc6_info->row_sz];
+		goto exit;

 	while (count) {
 		uint32_t row_offset = offset % psoc6_info->row_sz;
@@ -804,7 +869,7 @@ static int psoc6_program(struct flash_bank *bank,
 		hr = psoc6_program_row(bank, aligned_addr, page_buf, is_sflash);
 		if (hr != ERROR_OK) {
 			LOG_ERROR("Failed to program Flash at address 0x%08X", aligned_addr);
-			break;
+			goto exit;
 		}

 		buffer += row_bytes;
@@ -812,13 +877,15 @@ static int psoc6_program(struct flash_bank *bank,
 		count -= row_bytes;
 	}

-	hr = sromalgo_release(target);
+exit:
+	sromalgo_release(target);
 	return hr;
 }

-/**************************************************************************************************
- * Performs Mass Erase of given flash bank
- * Syntax: psoc6 mass_erase bank_id
+/** ***********************************************************************************************
+ * @brief Performs Mass Erase operation
+ * @param bank flash bank index to erase
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 COMMAND_HANDLER(psoc6_handle_mass_erase_command)
 {
@@ -835,13 +902,16 @@ COMMAND_HANDLER(psoc6_handle_mass_erase_command)
 	return hr;
 }

-/**************************************************************************************************
- * Simulates broken Vector Catch
+/** ***********************************************************************************************
+ * @brief Simulates broken Vector Catch
 * Function will try to determine entry point of user application. If it succeeds it will set HW
 * breakpoint at that address, issue SW Reset and remove the breakpoint afterwards.
 * In case of CM0, SYSRESETREQ is used. This allows to reset all peripherals. Boot code will
 * reset CM4 anyway, so using SYSRESETREQ is safe here.
 * In case of CM4, VECTRESET is used instead of SYSRESETREQ to not disturb CM0 core.
+ *
+ * @param target current target
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
 *************************************************************************************************/
 int handle_reset_halt(struct target *target)
 {
@@ -889,33 +959,42 @@ int handle_reset_halt(struct target *target)

 	const struct armv7m_common *cm = target_to_armv7m(target);

+	/* PSoC6 reboots immediatelly after issuing SYSRESETREQ / VECTRESET
+	 * this disables SWD/JTAG pins momentarily and may break communication
+	 * Ignoring return value of mem_ap_write_atomic_u32 seems to be ok here */
 	if (is_cm0) {
 		/* Reset the CM0 by asserting SYSRESETREQ. This will also reset CM4 */
 		LOG_INFO("psoc6.cm0: bkpt @0x%08X, issuing SYSRESETREQ", reset_addr);
-		hr = mem_ap_write_atomic_u32(cm->debug_ap,
-				NVIC_AIRCR,
-				AIRCR_VECTKEY | AIRCR_SYSRESETREQ);
-
-		/* Wait for bootcode and initialize DAP */
-		usleep(3000);
-		dap_dp_init(cm->debug_ap->dap);
+		mem_ap_write_atomic_u32(cm->debug_ap, NVIC_AIRCR,
+			AIRCR_VECTKEY | AIRCR_SYSRESETREQ);
 	} else {
 		LOG_INFO("psoc6.cm4: bkpt @0x%08X, issuing VECTRESET", reset_addr);
-		hr = mem_ap_write_atomic_u32(cm->debug_ap,
-				NVIC_AIRCR,
-				AIRCR_VECTKEY | AIRCR_VECTRESET);
-		if (hr != ERROR_OK)
-			return hr;
+		mem_ap_write_atomic_u32(cm->debug_ap, NVIC_AIRCR,
+			AIRCR_VECTKEY | AIRCR_VECTRESET);
 	}

+	/* Wait 100ms for bootcode and reinitialize DAP */
+	usleep(100000);
+	dap_dp_init(cm->debug_ap->dap);
+
 	target_wait_state(target, TARGET_HALTED, IPC_TIMEOUT_MS);

 	/* Remove the break point */
 	breakpoint_remove(target, reset_addr);

-	return hr;
+	return ERROR_OK;
 }

+/** ***********************************************************************************************
+ * @brief Simulates broken Vector Catch
+ * Function will try to determine entry point of user application. If it succeeds it will set HW
+ * breakpoint at that address, issue SW Reset and remove the breakpoint afterwards.
+ * In case of CM0, SYSRESETREQ is used. This allows to reset all peripherals. Boot code will
+ * reset CM4 anyway, so using SYSRESETREQ is safe here.
+ * In case of CM4, VECTRESET is used instead of SYSRESETREQ to not disturb CM0 core.
+ *
+ * @return ERROR_OK in case of success, ERROR_XXX code otherwise
+ *************************************************************************************************/
 COMMAND_HANDLER(psoc6_handle_reset_halt)
 {
 	if (CMD_ARGC)
@@ -945,7 +1024,7 @@ static const struct command_registration psoc6_exec_command_handlers[] = {
 		.name = "mass_erase",
 		.handler = psoc6_handle_mass_erase_command,
 		.mode = COMMAND_EXEC,
-		.usage = NULL,
+		.usage = "bank",
 		.help = "Erases entire Main Flash",
 	},
 	{
@@ -572,45 +572,8 @@ static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;

-	/* see contrib/loaders/flash/stm32f1x.S for src */
-
 	static const uint8_t stm32x_flash_write_code[] = {
-		/* #define STM32_FLASH_SR_OFFSET 0x0C */
-		/* wait_fifo: */
-			0x16, 0x68,   /* ldr   r6, [r2, #0] */
-			0x00, 0x2e,   /* cmp   r6, #0 */
-			0x18, 0xd0,   /* beq   exit */
-			0x55, 0x68,   /* ldr   r5, [r2, #4] */
-			0xb5, 0x42,   /* cmp   r5, r6 */
-			0xf9, 0xd0,   /* beq   wait_fifo */
-			0x2e, 0x88,   /* ldrh  r6, [r5, #0] */
-			0x26, 0x80,   /* strh  r6, [r4, #0] */
-			0x02, 0x35,   /* adds  r5, #2 */
-			0x02, 0x34,   /* adds  r4, #2 */
-		/* busy: */
-			0xc6, 0x68,   /* ldr   r6, [r0, #STM32_FLASH_SR_OFFSET] */
-			0x01, 0x27,   /* movs  r7, #1 */
-			0x3e, 0x42,   /* tst   r6, r7 */
-			0xfb, 0xd1,   /* bne   busy */
-			0x14, 0x27,   /* movs  r7, #0x14 */
-			0x3e, 0x42,   /* tst   r6, r7 */
-			0x08, 0xd1,   /* bne   error */
-			0x9d, 0x42,   /* cmp   r5, r3 */
-			0x01, 0xd3,   /* bcc   no_wrap */
-			0x15, 0x46,   /* mov   r5, r2 */
-			0x08, 0x35,   /* adds  r5, #8 */
-		/* no_wrap: */
-			0x55, 0x60,   /* str   r5, [r2, #4] */
-			0x01, 0x39,   /* subs  r1, r1, #1 */
-			0x00, 0x29,   /* cmp   r1, #0 */
-			0x02, 0xd0,   /* beq   exit */
-			0xe5, 0xe7,   /* b     wait_fifo */
-		/* error: */
-			0x00, 0x20,   /* movs  r0, #0 */
-			0x50, 0x60,   /* str   r0, [r2, #4] */
-		/* exit: */
-			0x30, 0x46,   /* mov   r0, r6 */
-			0x00, 0xbe,   /* bkpt  #0 */
+#include "../../../contrib/loaders/flash/stm32/stm32f1x.inc"
 	};

 	/* flash write code */
@@ -584,45 +584,8 @@ static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;

-	/* see contrib/loaders/flash/stm32f2x.S for src */
-
 	static const uint8_t stm32x_flash_write_code[] = {
-									/* wait_fifo: */
-		0xD0, 0xF8, 0x00, 0x80,		/* ldr		r8, [r0, #0] */
-		0xB8, 0xF1, 0x00, 0x0F,		/* cmp		r8, #0 */
-		0x1A, 0xD0,					/* beq		exit */
-		0x47, 0x68,					/* ldr		r7, [r0, #4] */
-		0x47, 0x45,					/* cmp		r7, r8 */
-		0xF7, 0xD0,					/* beq		wait_fifo */
-
-		0xDF, 0xF8, 0x34, 0x60,		/* ldr		r6, STM32_PROG16 */
-		0x26, 0x61,					/* str		r6, [r4, #STM32_FLASH_CR_OFFSET] */
-		0x37, 0xF8, 0x02, 0x6B,		/* ldrh		r6, [r7], #0x02 */
-		0x22, 0xF8, 0x02, 0x6B,		/* strh		r6, [r2], #0x02 */
-		0xBF, 0xF3, 0x4F, 0x8F,		/* dsb		sy */
-									/* busy: */
-		0xE6, 0x68,					/* ldr		r6, [r4, #STM32_FLASH_SR_OFFSET] */
-		0x16, 0xF4, 0x80, 0x3F,		/* tst		r6, #0x10000 */
-		0xFB, 0xD1,					/* bne		busy */
-		0x16, 0xF0, 0xF0, 0x0F,		/* tst		r6, #0xf0 */
-		0x07, 0xD1,					/* bne		error */
-
-		0x8F, 0x42,					/* cmp		r7, r1 */
-		0x28, 0xBF,					/* it		cs */
-		0x00, 0xF1, 0x08, 0x07,		/* addcs	r7, r0, #8 */
-		0x47, 0x60,					/* str		r7, [r0, #4] */
-		0x01, 0x3B,					/* subs		r3, r3, #1 */
-		0x13, 0xB1,					/* cbz		r3, exit */
-		0xDF, 0xE7,					/* b		wait_fifo */
-									/* error: */
-		0x00, 0x21,					/* movs		r1, #0 */
-		0x41, 0x60,					/* str		r1, [r0, #4] */
-									/* exit: */
-		0x30, 0x46,					/* mov		r0, r6 */
-		0x00, 0xBE,					/* bkpt		#0x00 */
-
-		/* <STM32_PROG16>: */
-		0x01, 0x01, 0x00, 0x00,		/* .word	0x00000101 */
+#include "../../../contrib/loaders/flash/stm32/stm32f2x.inc"
 	};

 	if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code),
@@ -568,51 +568,8 @@ static int stm32x_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	struct stm32h7x_flash_bank *stm32x_info = bank->driver_priv;
 	int retval = ERROR_OK;

-	/* see contrib/loaders/flash/smt32h7x.S for src */
 	static const uint8_t stm32x_flash_write_code[] = {
-								/* <code>: */
-		0x45, 0x68,				/*		ldr		r5, [r0, #4] */
-								/* <wait_fifo>: */
-		0x06, 0x68,				/*		ldr		r6, [r0, #0] */
-		0x26, 0xb3,				/*		cbz		r6, <exit> */
-		0x76, 0x1b,				/*		subs	r6, r6, r5 */
-		0x42, 0xbf,				/*		ittt	mi */
-		0x76, 0x18,				/*		addmi	r6, r6, r1 */
-		0x36, 0x1a,				/*		submi	r6, r6, r0 */
-		0x08, 0x3e,				/*		submi	r6, #8 */
-		0x20, 0x2e,				/*		cmp		r6, #32 */
-		0xf6, 0xd3,				/*		bcc.n	<wait_fifo> */
-		0x4f, 0xf0, 0x32, 0x06,	/*		mov.w	r6, #STM32_PROG */
-		0xe6, 0x60,				/*		str		r6, [r4, #STM32_FLASH_CR_OFFSET] */
-		0x4f, 0xf0, 0x08, 0x07,	/*		mov.w	r7, #8 */
-								/* <write_flash>: */
-		0x55, 0xf8, 0x04, 0x6b,	/*		ldr.w	r6, [r5], #4 */
-		0x42, 0xf8, 0x04, 0x6b,	/*		str.w	r6, [r2], #4 */
-		0xbf, 0xf3, 0x4f, 0x8f,	/*		dsb		sy */
-		0x8d, 0x42,				/*		cmp		r5, r1 */
-		0x28, 0xbf,				/*		it		cs */
-		0x00, 0xf1, 0x08, 0x05,	/*		addcs.w	r5, r0, #8 */
-		0x01, 0x3f,				/*		subs	r7, #1 */
-		0xf3, 0xd1,				/*		bne.n	<write_flash> */
-								/* <busy>: */
-		0x26, 0x69,				/*		ldr		r6, [r4, #STM32_FLASH_SR_OFFSET] */
-		0x16, 0xf0, 0x01, 0x0f,	/*		tst.w	r6, #STM32_SR_BUSY_MASK */
-		0xfb, 0xd1,				/*		bne.n	<busy> */
-		0x05, 0x4f,				/*		ldr		r7, [pc, #20] ; (<stm32_sr_error_mask>) */
-		0x3e, 0x42,				/*		tst		r6, r7 */
-		0x03, 0xd1,				/*		bne.n	<error> */
-		0x45, 0x60,				/*		str		r5, [r0, #4] */
-		0x01, 0x3b,				/*		subs	r3, #1 */
-		0xdb, 0xd1,				/*		bne.n	<wait_fifo> */
-		0x01, 0xe0,				/*		b.n		<exit> */
-								/* <error>: */
-		0x00, 0x27,				/*		movs	r7, #0 */
-		0x47, 0x60,				/*		str		r7, [r0, #4] */
-								/* <exit>: */
-		0x30, 0x46,				/*		mov		r0, r6 */
-		0x00, 0xbe,				/*		bkpt	0x0000 */
-								/* <stm32_sr_error_mask>: */
-		0x00, 0x00, 0xee, 0x03	/*		.word	0x03ee0000 ; (STM32_SR_ERROR_MASK) */
+#include "../../../contrib/loaders/flash/stm32/stm32h7x.inc"
 	};

 	if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code),
@@ -461,19 +461,8 @@ static int stm32l4_write_block(struct flash_bank *bank, const uint8_t *buffer,
 	struct armv7m_algorithm armv7m_info;
 	int retval = ERROR_OK;

-	/* See contrib/loaders/flash/stm32l4x.S for source and
-	 * hints how to generate the data!
-	 */
-
 	static const uint8_t stm32l4_flash_write_code[] = {
-		0xd0, 0xf8, 0x00, 0x80, 0xb8, 0xf1, 0x00, 0x0f, 0x21, 0xd0, 0x45, 0x68,
-		0xb8, 0xeb, 0x05, 0x06, 0x44, 0xbf, 0x76, 0x18, 0x36, 0x1a, 0x08, 0x2e,
-		0xf2, 0xd3, 0xdf, 0xf8, 0x36, 0x60, 0x66, 0x61, 0xf5, 0xe8, 0x02, 0x67,
-		0xe2, 0xe8, 0x02, 0x67, 0xbf, 0xf3, 0x4f, 0x8f, 0x26, 0x69, 0x16, 0xf4,
-		0x80, 0x3f, 0xfb, 0xd1, 0x16, 0xf0, 0xfa, 0x0f, 0x07, 0xd1, 0x8d, 0x42,
-		0x28, 0xbf, 0x00, 0xf1, 0x08, 0x05, 0x45, 0x60, 0x01, 0x3b, 0x13, 0xb1,
-		0xda, 0xe7, 0x00, 0x21, 0x41, 0x60, 0x30, 0x46, 0x00, 0xbe, 0x01, 0x00,
-		0x00, 0x00
+#include "../../../contrib/loaders/flash/stm32/stm32l4x.inc"
 	};

 	if (target_alloc_working_area(target, sizeof(stm32l4_flash_write_code),
@@ -448,10 +448,8 @@ static int stm32lx_write_half_pages(struct flash_bank *bank, const uint8_t *buff

 	int retval = ERROR_OK;

-	/* see contib/loaders/flash/stm32lx.S for src */
-
 	static const uint8_t stm32lx_flash_write_code[] = {
-			0x92, 0x00, 0x8A, 0x18, 0x01, 0xE0, 0x08, 0xC9, 0x08, 0xC0, 0x91, 0x42, 0xFB, 0xD1, 0x00, 0xBE
+#include "../../../contrib/loaders/flash/stm32/stm32lx.inc"
 	};

 	/* Make sure we're performing a half-page aligned write. */
@@ -3,6 +3,7 @@
 *   Copyright (C) 2007,2008 Øyvind Harboe <oyvind.harboe@zylin.com>       *
 *   Copyright (C) 2008 by Spencer Oliver <spen@spen-soft.co.uk>           *
 *   Copyright (C) 2009 Zachary T Welch <zw@superlucidity.net>             *
+ *   Copyright (C) 2017-2018 Tomas Vanek <vanekt@fbl.cz>                   *
 *                                                                         *
 *   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  *
@@ -460,42 +461,29 @@ COMMAND_HANDLER(handle_flash_write_image_command)

 COMMAND_HANDLER(handle_flash_fill_command)
 {
-	int err = ERROR_OK;
-	uint32_t address;
+	target_addr_t address;
 	uint32_t pattern;
 	uint32_t count;
-	uint32_t wrote = 0;
-	uint32_t cur_size = 0;
-	uint32_t chunk_count;
 	struct target *target = get_current_target(CMD_CTX);
 	unsigned i;
 	uint32_t wordsize;
-	int retval = ERROR_OK;
+	int retval;

-	static size_t const chunksize = 1024;
-	uint8_t *chunk = NULL, *readback = NULL;
-
-	if (CMD_ARGC != 3) {
-		retval = ERROR_COMMAND_SYNTAX_ERROR;
-		goto done;
-	}
+	if (CMD_ARGC != 3)
+		return ERROR_COMMAND_SYNTAX_ERROR;

+#if BUILD_TARGET64
+	COMMAND_PARSE_NUMBER(u64, CMD_ARGV[0], address);
+#else
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+#endif
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], pattern);
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], count);

-	chunk = malloc(chunksize);
-	if (chunk == NULL)
-		return ERROR_FAIL;
-
-	readback = malloc(chunksize);
-	if (readback == NULL) {
-		free(chunk);
-		return ERROR_FAIL;
-	}
-
-	if (count == 0)
-		goto done;
+	struct flash_bank *bank;
+	retval = get_flash_bank_by_addr(target, address, true, &bank);
+	if (retval != ERROR_OK)
+		return retval;

 	switch (CMD_NAME[4]) {
 		case 'w':
@@ -508,73 +496,109 @@ COMMAND_HANDLER(handle_flash_fill_command)
 			wordsize = 1;
 			break;
 		default:
-			retval = ERROR_COMMAND_SYNTAX_ERROR;
-			goto done;
+			return ERROR_COMMAND_SYNTAX_ERROR;
 	}

-	chunk_count = MIN(count, (chunksize / wordsize));
+	if (count == 0)
+		return ERROR_OK;
+
+	if (address + count >= bank->base + bank->size) {
+		LOG_ERROR("Cannot cross flash bank borders");
+		return ERROR_FAIL;
+	}
+
+	uint32_t size_bytes = count * wordsize;
+	target_addr_t aligned_start = flash_write_align_start(bank, address);
+	target_addr_t end_addr = address + size_bytes - 1;
+	target_addr_t aligned_end = flash_write_align_end(bank, end_addr);
+	uint32_t aligned_size = aligned_end + 1 - aligned_start;
+	uint32_t padding_at_start = address - aligned_start;
+	uint32_t padding_at_end = aligned_end - end_addr;
+
+	uint8_t *buffer = malloc(aligned_size);
+	if (buffer == NULL)
+		return ERROR_FAIL;
+
+	if (padding_at_start) {
+		memset(buffer, bank->default_padded_value, padding_at_start);
+		LOG_WARNING("Start address " TARGET_ADDR_FMT
+			" breaks the required alignment of flash bank %s",
+			address, bank->name);
+		LOG_WARNING("Padding %" PRId32 " bytes from " TARGET_ADDR_FMT,
+		    padding_at_start, aligned_start);
+	}
+
+	uint8_t *ptr = buffer + padding_at_start;
+
 	switch (wordsize) {
 		case 4:
-			for (i = 0; i < chunk_count; i++)
-				target_buffer_set_u32(target, chunk + i * wordsize, pattern);
+			for (i = 0; i < count; i++, ptr += wordsize)
+				target_buffer_set_u32(target, ptr, pattern);
 			break;
 		case 2:
-			for (i = 0; i < chunk_count; i++)
-				target_buffer_set_u16(target, chunk + i * wordsize, pattern);
+			for (i = 0; i < count; i++, ptr += wordsize)
+				target_buffer_set_u16(target, ptr, pattern);
 			break;
 		case 1:
-			memset(chunk, pattern, chunk_count);
+			memset(ptr, pattern, count);
+			ptr += count;
 			break;
 		default:
 			LOG_ERROR("BUG: can't happen");
 			exit(-1);
 	}

+	if (padding_at_end) {
+		memset(ptr, bank->default_padded_value, padding_at_end);
+		LOG_INFO("Padding at " TARGET_ADDR_FMT " with %" PRId32
+			" bytes (bank write end alignment)",
+			end_addr + 1, padding_at_end);
+	}
+
 	struct duration bench;
 	duration_start(&bench);

-	for (wrote = 0; wrote < (count*wordsize); wrote += cur_size) {
-		struct flash_bank *bank;
+	retval = flash_driver_write(bank, buffer, aligned_start - bank->base, aligned_size);
+	if (retval != ERROR_OK)
+		goto done;

-		retval = get_flash_bank_by_addr(target, address, true, &bank);
-		if (retval != ERROR_OK)
-			goto done;
+	retval = flash_driver_read(bank, buffer, address - bank->base, size_bytes);
+	if (retval != ERROR_OK)
+		goto done;

-		cur_size = MIN((count * wordsize - wrote), chunksize);
-		err = flash_driver_write(bank, chunk, address - bank->base + wrote, cur_size);
-		if (err != ERROR_OK) {
-			retval = err;
+	for (i = 0, ptr = buffer; i < count; i++) {
+		uint32_t readback = 0;
+
+		switch (wordsize) {
+			case 4:
+				readback = target_buffer_get_u32(target, ptr);
+				break;
+			case 2:
+				readback = target_buffer_get_u16(target, ptr);
+				break;
+			case 1:
+				readback = *ptr;
+				break;
+		}
+		if (readback != pattern) {
+			LOG_ERROR(
+				"Verification error address " TARGET_ADDR_FMT
+				", read back 0x%02" PRIx32 ", expected 0x%02" PRIx32,
+				address + i * wordsize, readback, pattern);
+			retval = ERROR_FAIL;
 			goto done;
 		}
-
-		err = flash_driver_read(bank, readback, address - bank->base + wrote, cur_size);
-		if (err != ERROR_OK) {
-			retval = err;
-			goto done;
-		}
-
-		for (i = 0; i < cur_size; i++) {
-			if (readback[i] != chunk[i]) {
-				LOG_ERROR(
-					"Verification error address 0x%08" PRIx32 ", read back 0x%02x, expected 0x%02x",
-					address + wrote + i,
-					readback[i],
-					chunk[i]);
-				retval = ERROR_FAIL;
-				goto done;
-			}
-		}
+		ptr += wordsize;
 	}

 	if ((retval == ERROR_OK) && (duration_measure(&bench) == ERROR_OK)) {
-		command_print(CMD_CTX, "wrote %" PRIu32 " bytes to 0x%8.8" PRIx32
-			" in %fs (%0.3f KiB/s)", wrote, address,
-			duration_elapsed(&bench), duration_kbps(&bench, wrote));
+		command_print(CMD_CTX, "wrote %" PRIu32 " bytes to " TARGET_ADDR_FMT
+			" in %fs (%0.3f KiB/s)", size_bytes, address,
+			duration_elapsed(&bench), duration_kbps(&bench, size_bytes));
 	}

 done:
-	free(readback);
-	free(chunk);
+	free(buffer);

 	return retval;
 }
@@ -592,8 +616,8 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 	struct duration bench;
 	duration_start(&bench);

-	struct flash_bank *p;
-	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &p);
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 	if (ERROR_OK != retval)
 		return retval;

@@ -602,7 +626,7 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 	if (CMD_ARGC > 2)
 		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], offset);

-	if (offset > p->size) {
+	if (offset > bank->size) {
 		LOG_ERROR("Offset 0x%8.8" PRIx32 " is out of range of the flash bank",
 			offset);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
@@ -618,7 +642,7 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 		return retval;
 	}

-	length = MIN(filesize, p->size - offset);
+	length = MIN(filesize, bank->size - offset);

 	if (!length) {
 		LOG_INFO("Nothing to write to flash bank");
@@ -630,14 +654,33 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 		LOG_INFO("File content exceeds flash bank size. Only writing the "
 			"first %zu bytes of the file", length);

-	buffer = malloc(length);
+	target_addr_t start_addr = bank->base + offset;
+	target_addr_t aligned_start = flash_write_align_start(bank, start_addr);
+	target_addr_t end_addr = start_addr + length - 1;
+	target_addr_t aligned_end = flash_write_align_end(bank, end_addr);
+	uint32_t aligned_size = aligned_end + 1 - aligned_start;
+	uint32_t padding_at_start = start_addr - aligned_start;
+	uint32_t padding_at_end = aligned_end - end_addr;
+
+	buffer = malloc(aligned_size);
 	if (buffer == NULL) {
 		fileio_close(fileio);
 		LOG_ERROR("Out of memory");
 		return ERROR_FAIL;
 	}
+
+	if (padding_at_start) {
+		memset(buffer, bank->default_padded_value, padding_at_start);
+		LOG_WARNING("Start offset 0x%08" PRIx32
+			" breaks the required alignment of flash bank %s",
+			offset, bank->name);
+		LOG_WARNING("Padding %" PRId32 " bytes from " TARGET_ADDR_FMT,
+		    padding_at_start, aligned_start);
+	}
+
+	uint8_t *ptr = buffer + padding_at_start;
 	size_t buf_cnt;
-	if (fileio_read(fileio, length, buffer, &buf_cnt) != ERROR_OK) {
+	if (fileio_read(fileio, length, ptr, &buf_cnt) != ERROR_OK) {
 		free(buffer);
 		fileio_close(fileio);
 		return ERROR_FAIL;
@@ -649,15 +692,23 @@ COMMAND_HANDLER(handle_flash_write_bank_command)
 		return ERROR_FAIL;
 	}

-	retval = flash_driver_write(p, buffer, offset, length);
+	ptr += length;
+
+	if (padding_at_end) {
+		memset(ptr, bank->default_padded_value, padding_at_end);
+		LOG_INFO("Padding at " TARGET_ADDR_FMT " with %" PRId32
+			" bytes (bank write end alignment)",
+			end_addr + 1, padding_at_end);
+	}
+
+	retval = flash_driver_write(bank, buffer, aligned_start - bank->base, aligned_size);

 	free(buffer);
-	buffer = NULL;

 	if ((ERROR_OK == retval) && (duration_measure(&bench) == ERROR_OK)) {
 		command_print(CMD_CTX, "wrote %zu bytes from file %s to flash bank %u"
 			" at offset 0x%8.8" PRIx32 " in %fs (%0.3f KiB/s)",
-			length, CMD_ARGV[1], p->bank_number, offset,
+			length, CMD_ARGV[1], bank->bank_number, offset,
 			duration_elapsed(&bench), duration_kbps(&bench, length));
 	}

@@ -1071,21 +1122,16 @@ COMMAND_HANDLER(handle_flash_bank_command)
 		}
 	}

-	struct flash_bank *c = malloc(sizeof(*c));
+	struct flash_bank *c = calloc(1, sizeof(*c));
 	c->name = strdup(bank_name);
 	c->target = target;
 	c->driver = driver;
-	c->driver_priv = NULL;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], c->base);
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], c->size);
 	COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], c->chip_width);
 	COMMAND_PARSE_NUMBER(int, CMD_ARGV[4], c->bus_width);
 	c->default_padded_value = c->erased_value = 0xff;
-	c->num_sectors = 0;
-	c->sectors = NULL;
-	c->num_prot_blocks = 0;
-	c->prot_blocks = NULL;
-	c->next = NULL;
+	c->minimal_write_gap = FLASH_WRITE_GAP_SECTOR;

 	int retval;
 	retval = CALL_COMMAND_HANDLER(driver->flash_bank_command, c);
@@ -149,14 +149,14 @@ COMMAND_HANDLER(handle_interface_command)

 		jtag_interface = jtag_interfaces[i];

-	/* LEGACY SUPPORT ... adapter drivers  must declare what
-	 * transports they allow.  Until they all do so, assume
-	 * the legacy drivers are JTAG-only
-	 */
-	if (!jtag_interface->transports)
-		LOG_WARNING("Adapter driver '%s' did not declare "
-			"which transports it allows; assuming "
-			"legacy JTAG-only", jtag_interface->name);
+		/* LEGACY SUPPORT ... adapter drivers  must declare what
+		 * transports they allow.  Until they all do so, assume
+		 * the legacy drivers are JTAG-only
+		 */
+		if (!jtag_interface->transports)
+			LOG_WARNING("Adapter driver '%s' did not declare "
+				"which transports it allows; assuming "
+				"legacy JTAG-only", jtag_interface->name);
 		retval = allow_transports(CMD_CTX, jtag_interface->transports
 						? jtag_interface->transports : jtag_only);
 			if (ERROR_OK != retval)
@@ -1494,8 +1494,6 @@ int adapter_quit(void)
 		t = n;
 	}

-	dap_cleanup_all();
-
 	return ERROR_OK;
 }

@@ -359,6 +359,10 @@ int openocd_main(int argc, char *argv[])

 	unregister_all_commands(cmd_ctx, NULL);

+	/* free all DAP and CTI objects */
+	dap_cleanup_all();
+	arm_cti_cleanup_all();
+
 	adapter_quit();

 	/* Shutdown commandline interface */
@@ -69,10 +69,9 @@ struct ChibiOS_chdebug {
 /**
 * @brief ChibiOS thread states.
 */
-static const char * const ChibiOS_thread_states[] = {
-	"READY", "CURRENT", "SUSPENDED", "WTSEM", "WTMTX", "WTCOND", "SLEEPING",
-	"WTEXIT", "WTOREVT", "WTANDEVT", "SNDMSGQ", "SNDMSG", "WTMSG", "WTQUEUE",
-	"FINAL"
+static const char * const ChibiOS_thread_states[] = { "READY", "CURRENT",
+"WTSTART", "SUSPENDED", "QUEUED", "WTSEM", "WTMTX", "WTCOND", "SLEEPING",
+"WTEXIT", "WTOREVT", "WTANDEVT", "SNDMSGQ", "SNDMSG", "WTMSG", "FINAL"
 };

 #define CHIBIOS_NUM_STATES (sizeof(ChibiOS_thread_states)/sizeof(char *))
@@ -229,6 +229,25 @@ static int64_t rtos_standard_Cortex_M3_stack_align(struct target *target,
 		stack_ptr, XPSR_OFFSET);
 }

+static int64_t rtos_standard_Cortex_M4F_stack_align(struct target *target,
+	const uint8_t *stack_data, const struct rtos_register_stacking *stacking,
+	int64_t stack_ptr)
+{
+	const int XPSR_OFFSET = 0x40;
+	return rtos_Cortex_M_stack_align(target, stack_data, stacking,
+		stack_ptr, XPSR_OFFSET);
+}
+
+static int64_t rtos_standard_Cortex_M4F_FPU_stack_align(struct target *target,
+	const uint8_t *stack_data, const struct rtos_register_stacking *stacking,
+	int64_t stack_ptr)
+{
+	const int XPSR_OFFSET = 0x80;
+	return rtos_Cortex_M_stack_align(target, stack_data, stacking,
+		stack_ptr, XPSR_OFFSET);
+}
+
+
 const struct rtos_register_stacking rtos_standard_Cortex_M3_stacking = {
 	0x40,					/* stack_registers_size */
 	-1,						/* stack_growth_direction */
@@ -241,7 +260,7 @@ const struct rtos_register_stacking rtos_standard_Cortex_M4F_stacking = {
 	0x44,					/* stack_registers_size 4 more for LR*/
 	-1,						/* stack_growth_direction */
 	ARMV7M_NUM_CORE_REGS,	/* num_output_registers */
-	rtos_standard_Cortex_M3_stack_align,	/* stack_alignment */
+	rtos_standard_Cortex_M4F_stack_align,	/* stack_alignment */
 	rtos_standard_Cortex_M4F_stack_offsets	/* register_offsets */
 };

@@ -249,7 +268,7 @@ const struct rtos_register_stacking rtos_standard_Cortex_M4F_FPU_stacking = {
 	0xcc,					/* stack_registers_size 4 more for LR + 48 more for FPU S0-S15 register*/
 	-1,						/* stack_growth_direction */
 	ARMV7M_NUM_CORE_REGS,	/* num_output_registers */
-	rtos_standard_Cortex_M3_stack_align,	/* stack_alignment */
+	rtos_standard_Cortex_M4F_FPU_stack_align,	/* stack_alignment */
 	rtos_standard_Cortex_M4F_FPU_stack_offsets	/* register_offsets */
 };

@@ -1806,7 +1806,7 @@ static int gdb_memory_map(struct connection *connection,
 	int offset;
 	int length;
 	char *separator;
-	uint32_t ram_start = 0;
+	target_addr_t ram_start = 0;
 	int i;
 	int target_flash_banks = 0;

@@ -1821,9 +1821,6 @@ static int gdb_memory_map(struct connection *connection,
 	/* Sort banks in ascending order.  We need to report non-flash
 	 * memory as ram (or rather read/write) by default for GDB, since
 	 * it has no concept of non-cacheable read/write memory (i/o etc).
-	 *
-	 * FIXME Most non-flash addresses are *NOT* RAM!  Don't lie.
-	 * Current versions of GDB assume unlisted addresses are RAM...
 	 */
 	banks = malloc(sizeof(struct flash_bank *)*flash_get_bank_count());

@@ -1846,14 +1843,13 @@ static int gdb_memory_map(struct connection *connection,
 	for (i = 0; i < target_flash_banks; i++) {
 		int j;
 		unsigned sector_size = 0;
-		uint32_t start;
+		unsigned group_len = 0;

 		p = banks[i];
-		start = p->base;

 		if (ram_start < p->base)
 			xml_printf(&retval, &xml, &pos, &size,
-				"<memory type=\"ram\" start=\"0x%x\" "
+				"<memory type=\"ram\" start=\"" TARGET_ADDR_FMT "\" "
 				"length=\"0x%x\"/>\n",
 				ram_start, p->base - ram_start);

@@ -1864,27 +1860,35 @@ static int gdb_memory_map(struct connection *connection,
 		 * regions with 8KB, 32KB, and 64KB sectors; etc.
 		 */
 		for (j = 0; j < p->num_sectors; j++) {
-			unsigned group_len;

 			/* Maybe start a new group of sectors. */
 			if (sector_size == 0) {
+				if (p->sectors[j].offset + p->sectors[j].size > p->size) {
+					LOG_WARNING("The flash sector at offset 0x%08" PRIx32
+						" overflows the end of %s bank.",
+						p->sectors[j].offset, p->name);
+					LOG_WARNING("The rest of bank will not show in gdb memory map.");
+					break;
+				}
+				target_addr_t start;
 				start = p->base + p->sectors[j].offset;
 				xml_printf(&retval, &xml, &pos, &size,
 					"<memory type=\"flash\" "
-					"start=\"0x%x\" ",
+					"start=\"" TARGET_ADDR_FMT "\" ",
 					start);
 				sector_size = p->sectors[j].size;
+				group_len = sector_size;
+			} else {
+				group_len += sector_size; /* equal to p->sectors[j].size */
 			}

 			/* Does this finish a group of sectors?
 			 * If not, continue an already-started group.
 			 */
-			if (j == p->num_sectors - 1)
-				group_len = (p->base + p->size) - start;
-			else if (p->sectors[j + 1].size != sector_size)
-				group_len = p->base + p->sectors[j + 1].offset
-					- start;
-			else
+			if (j < p->num_sectors - 1
+					&& p->sectors[j + 1].size == sector_size
+					&& p->sectors[j + 1].offset == p->sectors[j].offset + sector_size
+					&& p->sectors[j + 1].offset + p->sectors[j + 1].size <= p->size)
 				continue;

 			xml_printf(&retval, &xml, &pos, &size,
@@ -1902,7 +1906,7 @@ static int gdb_memory_map(struct connection *connection,

 	if (ram_start != 0)
 		xml_printf(&retval, &xml, &pos, &size,
-			"<memory type=\"ram\" start=\"0x%x\" "
+			"<memory type=\"ram\" start=\"" TARGET_ADDR_FMT "\" "
 			"length=\"0x%x\"/>\n",
 			ram_start, 0-ram_start);
 	/* ELSE a flash chip could be at the very end of the 32 bit address
@@ -1910,11 +1914,11 @@ static int gdb_memory_map(struct connection *connection,
 	 */

 	free(banks);
-	banks = NULL;

 	xml_printf(&retval, &xml, &pos, &size, "</memory-map>\n");

 	if (retval != ERROR_OK) {
+		free(xml);
 		gdb_error(connection, retval);
 		return retval;
 	}
@@ -2386,6 +2386,20 @@ static int aarch64_target_create(struct target *target, Jim_Interp *interp)
 	return aarch64_init_arch_info(target, aarch64, pc->adiv5_config.dap);
 }

+static void aarch64_deinit_target(struct target *target)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct arm_dpm *dpm = &armv8->dpm;
+
+	armv8_free_reg_cache(target);
+	free(aarch64->brp_list);
+	free(dpm->dbp);
+	free(dpm->dwp);
+	free(target->private_config);
+	free(aarch64);
+}
+
 static int aarch64_mmu(struct target *target, int *enabled)
 {
 	if (target->state != TARGET_HALTED) {
@@ -2658,6 +2672,7 @@ struct target_type aarch64_target = {
 	.target_create = aarch64_target_create,
 	.target_jim_configure = aarch64_jim_configure,
 	.init_target = aarch64_init_target,
+	.deinit_target = aarch64_deinit_target,
 	.examine = aarch64_examine,

 	.read_phys_memory = aarch64_read_phys_memory,
@@ -553,7 +553,7 @@ static int jtagdp_overrun_check(struct adiv5_dap *dap)
 static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 {
 	int retval;
-	uint32_t ctrlstat;
+	uint32_t ctrlstat, pwrmask;

 	/* too expensive to call keep_alive() here */

@@ -571,8 +571,10 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 	if (ctrlstat & SSTICKYERR) {
 		LOG_DEBUG("jtag-dp: CTRL/STAT 0x%" PRIx32, ctrlstat);
 		/* Check power to debug regions */
-		if ((ctrlstat & (CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) !=
-						(CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) {
+		pwrmask = CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ;
+		if (!dap->ignore_syspwrupack)
+			pwrmask |= CSYSPWRUPACK;
+		if ((ctrlstat & pwrmask) != pwrmask) {
 			LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
 			dap->do_reconnect = true;
 		}
@@ -308,7 +308,7 @@ int armv4_5_run_algorithm_inner(struct target *target,
 int arm_checksum_memory(struct target *target,
 		target_addr_t address, uint32_t count, uint32_t *checksum);
 int arm_blank_check_memory(struct target *target,
-		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
+		struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value);

 void arm_set_cpsr(struct arm *arm, uint32_t cpsr);
 struct reg *arm_reg_current(struct arm *arm, unsigned regnum);
@@ -97,8 +97,7 @@ static uint32_t max_tar_block_size(uint32_t tar_autoincr_block, uint32_t address

 static int mem_ap_setup_csw(struct adiv5_ap *ap, uint32_t csw)
 {
-	csw = csw | CSW_DBGSWENABLE | CSW_MASTER_DEBUG | CSW_HPROT |
-		ap->csw_default;
+	csw |= ap->csw_default;

 	if (csw != ap->csw_value) {
 		/* LOG_DEBUG("DAP: Set CSW %x",csw); */
@@ -676,12 +675,14 @@ int dap_dp_init(struct adiv5_dap *dap)
 	if (retval != ERROR_OK)
 		return retval;

-	LOG_DEBUG("DAP: wait CSYSPWRUPACK");
-	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
-				      CSYSPWRUPACK, CSYSPWRUPACK,
-				      DAP_POWER_DOMAIN_TIMEOUT);
-	if (retval != ERROR_OK)
-		return retval;
+	if (!dap->ignore_syspwrupack) {
+		LOG_DEBUG("DAP: wait CSYSPWRUPACK");
+		retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+					      CSYSPWRUPACK, CSYSPWRUPACK,
+					      DAP_POWER_DOMAIN_TIMEOUT);
+		if (retval != ERROR_OK)
+			return retval;
+	}

 	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
 	if (retval != ERROR_OK)
@@ -1645,22 +1646,33 @@ COMMAND_HANDLER(dap_apcsw_command)
 {
 	struct adiv5_dap *dap = adiv5_get_dap(CMD_DATA);
 	uint32_t apcsw = dap->ap[dap->apsel].csw_default;
-	uint32_t sprot = 0;
+	uint32_t csw_val, csw_mask;

 	switch (CMD_ARGC) {
 	case 0:
-		command_print(CMD_CTX, "apsel %" PRIi32 " selected, csw 0x%8.8" PRIx32,
-			(dap->apsel), apcsw);
-		break;
+		command_print(CMD_CTX, "ap %" PRIi32 " selected, csw 0x%8.8" PRIx32,
+			dap->apsel, apcsw);
+		return ERROR_OK;
 	case 1:
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], sprot);
-		/* AP address is in bits 31:24 of DP_SELECT */
-		if (sprot > 1)
-			return ERROR_COMMAND_SYNTAX_ERROR;
-		if (sprot)
-			apcsw |= CSW_SPROT;
+		if (strcmp(CMD_ARGV[0], "default") == 0)
+			csw_val = CSW_DEFAULT;
 		else
-			apcsw &= ~CSW_SPROT;
+			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], csw_val);
+
+		if (csw_val & (CSW_SIZE_MASK | CSW_ADDRINC_MASK)) {
+			LOG_ERROR("CSW value cannot include 'Size' and 'AddrInc' bit-fields");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+		apcsw = csw_val;
+		break;
+	case 2:
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], csw_val);
+		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], csw_mask);
+		if (csw_mask & (CSW_SIZE_MASK | CSW_ADDRINC_MASK)) {
+			LOG_ERROR("CSW mask cannot include 'Size' and 'AddrInc' bit-fields");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+		apcsw = (apcsw & ~csw_mask) | (csw_val & csw_mask);
 		break;
 	default:
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1784,8 +1796,8 @@ const struct command_registration dap_instance_commands[] = {
 		.name = "apcsw",
 		.handler = dap_apcsw_command,
 		.mode = COMMAND_EXEC,
-		.help = "Set csw access bit ",
-		.usage = "[sprot]",
+		.help = "Set CSW default bits",
+		.usage = "[value [mask]]",
 	},

 	{
@@ -112,13 +112,16 @@
 #define CSW_ADDRINC_PACKED  (2UL << 4)
 #define CSW_DEVICE_EN       (1UL << 6)
 #define CSW_TRIN_PROG       (1UL << 7)
+/* all fields in bits 12 and above are implementation-defined! */
 #define CSW_SPIDEN          (1UL << 23)
-/* 30:24 - implementation-defined! */
-#define CSW_HPROT           (1UL << 25) /* ? */
-#define CSW_MASTER_DEBUG    (1UL << 29) /* ? */
+#define CSW_HPROT1          (1UL << 25) /* AHB: Privileged */
+#define CSW_MASTER_DEBUG    (1UL << 29) /* AHB: set HMASTER signals to AHB-AP ID */
 #define CSW_SPROT           (1UL << 30)
 #define CSW_DBGSWENABLE     (1UL << 31)

+/* initial value of csw_default used for MEM-AP transfers */
+#define CSW_DEFAULT			(CSW_HPROT1 | CSW_MASTER_DEBUG | CSW_DBGSWENABLE)
+
 /* Fields of the MEM-AP's IDR register */
 #define IDR_REV     (0xFUL << 28)
 #define IDR_JEP106  (0x7FFUL << 17)
@@ -244,6 +247,10 @@ struct adiv5_dap {
 	 * should be performed before the next access.
 	 */
 	bool do_reconnect;
+
+	/** Flag saying whether to ignore the syspwrupack flag in DAP. Some devices
+	 *  do not set this bit until later in the bringup sequence */
+	bool ignore_syspwrupack;
 };

 /**
@@ -219,6 +219,18 @@ static int cti_find_reg_offset(const char *name)
 	return -1;
 }

+int arm_cti_cleanup_all(void)
+{
+	struct arm_cti_object *obj, *tmp;
+
+	list_for_each_entry_safe(obj, tmp, &all_cti, lh) {
+		free(obj->name);
+		free(obj);
+	}
+
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(handle_cti_dump)
 {
 	struct arm_cti_object *obj = CMD_DATA;
@@ -73,7 +73,7 @@ extern int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *va
 extern int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel);
 extern int arm_cti_set_channel(struct arm_cti *self, uint32_t channel);
 extern int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel);
-
+extern int arm_cti_cleanup_all(void);
 extern int cti_register_commands(struct command_context *cmd_ctx);

 #endif /* OPENOCD_TARGET_ARM_CTI_H */
@@ -55,6 +55,8 @@ static void dap_instance_init(struct adiv5_dap *dap)
 		dap->ap[i].memaccess_tck = 255;
 		/* Number of bits for tar autoincrement, impl. dep. at least 10 */
 		dap->ap[i].tar_autoincr_block = (1<<10);
+		/* default CSW value */
+		dap->ap[i].csw_default = CSW_DEFAULT;
 	}
 	INIT_LIST_HEAD(&dap->cmd_journal);
 }
@@ -141,10 +143,12 @@ int dap_cleanup_all(void)

 enum dap_cfg_param {
 	CFG_CHAIN_POSITION,
+	CFG_IGNORE_SYSPWRUPACK,
 };

 static const Jim_Nvp nvp_config_opts[] = {
 	{ .name = "-chain-position",   .value = CFG_CHAIN_POSITION },
+	{ .name = "-ignore-syspwrupack", .value = CFG_IGNORE_SYSPWRUPACK },
 	{ .name = NULL, .value = -1 }
 };

@@ -177,6 +181,9 @@ static int dap_configure(Jim_GetOptInfo *goi, struct arm_dap_object *dap)
 			/* loop for more */
 			break;
 		}
+		case CFG_IGNORE_SYSPWRUPACK:
+			dap->dap.ignore_syspwrupack = true;
+			break;
 		default:
 			break;
 		}
@@ -587,11 +587,13 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	arm->pc->dirty = false;

-	/* flush R0 -- it's *very* dirty by now */
-	retval = dpm_write_reg(dpm, &cache->reg_list[0], 0);
-	if (retval != ERROR_OK)
-		goto done;
-	cache->reg_list[0].dirty = false;
+	/* flush R0 and R1 (our scratch registers) */
+	for (unsigned i = 0; i < 2; i++) {
+		retval = dpm_write_reg(dpm, &cache->reg_list[i], i);
+		if (retval != ERROR_OK)
+			goto done;
+		cache->reg_list[i].dirty = false;
+	}

 	/* (void) */ dpm->finish(dpm);
 done:
@@ -1663,7 +1663,7 @@ cleanup:
 *
 */
 int arm_blank_check_memory(struct target *target,
-	target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+	struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value)
 {
 	struct working_area *check_algorithm;
 	struct reg_param reg_params[3];
@@ -1706,10 +1706,10 @@ int arm_blank_check_memory(struct target *target,
 	arm_algo.core_state = ARM_STATE_ARM;

 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
-	buf_set_u32(reg_params[0].value, 0, 32, address);
+	buf_set_u32(reg_params[0].value, 0, 32, blocks[0].address);

 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
-	buf_set_u32(reg_params[1].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, blocks[0].size);

 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
@@ -1724,7 +1724,7 @@ int arm_blank_check_memory(struct target *target,
 			10000, &arm_algo);

 	if (retval == ERROR_OK)
-		*blank = buf_get_u32(reg_params[2].value, 0, 32);
+		blocks[0].result = buf_get_u32(reg_params[2].value, 0, 32);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
@@ -1733,7 +1733,10 @@ int arm_blank_check_memory(struct target *target,
 cleanup:
 	target_free_working_area(target, check_algorithm);

-	return retval;
+	if (retval != ERROR_OK)
+		return retval;
+
+	return 1;       /* only one block has been checked */
 }

 static int arm_full_context(struct target *target)
@@ -731,34 +731,23 @@ cleanup:
 	return retval;
 }

-/** Checks whether a memory region is erased. */
+/** Checks an array of memory regions whether they are erased. */
 int armv7m_blank_check_memory(struct target *target,
-	target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+	struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
-	struct reg_param reg_params[3];
+	struct working_area *erase_check_params;
+	struct reg_param reg_params[2];
 	struct armv7m_algorithm armv7m_info;
-	const uint8_t *code;
-	uint32_t code_size;
 	int retval;

+	static bool timed_out;
+
 	static const uint8_t erase_check_code[] = {
 #include "../../contrib/loaders/erase_check/armv7m_erase_check.inc"
 	};
-	static const uint8_t zero_erase_check_code[] = {
-#include "../../contrib/loaders/erase_check/armv7m_0_erase_check.inc"
-	};

-	switch (erased_value) {
-	case 0x00:
-		code = zero_erase_check_code;
-		code_size = sizeof(zero_erase_check_code);
-		break;
-	case 0xff:
-	default:
-		code = erase_check_code;
-		code_size = sizeof(erase_check_code);
-	}
+	const uint32_t code_size = sizeof(erase_check_code);

 	/* make sure we have a working area */
 	if (target_alloc_working_area(target, code_size,
@@ -766,40 +755,110 @@ int armv7m_blank_check_memory(struct target *target,
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;

 	retval = target_write_buffer(target, erase_check_algorithm->address,
-			code_size, code);
+			code_size, erase_check_code);
 	if (retval != ERROR_OK)
-		goto cleanup;
+		goto cleanup1;
+
+	/* prepare blocks array for algo */
+	struct algo_block {
+		union {
+			uint32_t size;
+			uint32_t result;
+		};
+		uint32_t address;
+	};
+
+	uint32_t avail = target_get_working_area_avail(target);
+	int blocks_to_check = avail / sizeof(struct algo_block) - 1;
+	if (num_blocks < blocks_to_check)
+		blocks_to_check = num_blocks;
+
+	struct algo_block *params = malloc((blocks_to_check+1)*sizeof(struct algo_block));
+	if (params == NULL) {
+		retval = ERROR_FAIL;
+		goto cleanup1;
+	}
+
+	int i;
+	uint32_t total_size = 0;
+	for (i = 0; i < blocks_to_check; i++) {
+		total_size += blocks[i].size;
+		target_buffer_set_u32(target, (uint8_t *)&(params[i].size),
+						blocks[i].size / sizeof(uint32_t));
+		target_buffer_set_u32(target, (uint8_t *)&(params[i].address),
+						blocks[i].address);
+	}
+	target_buffer_set_u32(target, (uint8_t *)&(params[blocks_to_check].size), 0);
+
+	uint32_t param_size = (blocks_to_check + 1) * sizeof(struct algo_block);
+	if (target_alloc_working_area(target, param_size,
+			&erase_check_params) != ERROR_OK) {
+		retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		goto cleanup2;
+	}
+
+	retval = target_write_buffer(target, erase_check_params->address,
+				param_size, (uint8_t *)params);
+	if (retval != ERROR_OK)
+		goto cleanup3;
+
+	uint32_t erased_word = erased_value | (erased_value << 8)
+			       | (erased_value << 16) | (erased_value << 24);
+
+	LOG_DEBUG("Starting erase check of %d blocks, parameters@"
+		 TARGET_ADDR_FMT, blocks_to_check, erase_check_params->address);

 	armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 	armv7m_info.core_mode = ARM_MODE_THREAD;

 	init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
-	buf_set_u32(reg_params[0].value, 0, 32, address);
+	buf_set_u32(reg_params[0].value, 0, 32, erase_check_params->address);

 	init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
-	buf_set_u32(reg_params[1].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, erased_word);

-	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
+	/* assume CPU clk at least 1 MHz */
+	int timeout = (timed_out ? 30000 : 2000) + total_size * 3 / 1000;

 	retval = target_run_algorithm(target,
-			0,
-			NULL,
-			3,
-			reg_params,
-			erase_check_algorithm->address,
-			erase_check_algorithm->address + (code_size - 2),
-			10000,
-			&armv7m_info);
+				0, NULL,
+				ARRAY_SIZE(reg_params), reg_params,
+				erase_check_algorithm->address,
+				erase_check_algorithm->address + (code_size - 2),
+				timeout,
+				&armv7m_info);

-	if (retval == ERROR_OK)
-		*blank = buf_get_u32(reg_params[2].value, 0, 32);
+	timed_out = retval == ERROR_TARGET_TIMEOUT;
+	if (retval != ERROR_OK && !timed_out)
+		goto cleanup4;

+	retval = target_read_buffer(target, erase_check_params->address,
+				param_size, (uint8_t *)params);
+	if (retval != ERROR_OK)
+		goto cleanup4;
+
+	for (i = 0; i < blocks_to_check; i++) {
+		uint32_t result = target_buffer_get_u32(target,
+					(uint8_t *)&(params[i].result));
+		if (result != 0 && result != 1)
+			break;
+
+		blocks[i].result = result;
+	}
+	if (i && timed_out)
+		LOG_INFO("Slow CPU clock: %d blocks checked, %d remain. Continuing...", i, num_blocks-i);
+
+	retval = i;		/* return number of blocks really checked */
+
+cleanup4:
 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
-	destroy_reg_param(&reg_params[2]);

-cleanup:
+cleanup3:
+	target_free_working_area(target, erase_check_params);
+cleanup2:
+	free(params);
+cleanup1:
 	target_free_working_area(target, erase_check_algorithm);

 	return retval;
@@ -225,7 +225,7 @@ int armv7m_restore_context(struct target *target);
 int armv7m_checksum_memory(struct target *target,
 		target_addr_t address, uint32_t count, uint32_t *checksum);
 int armv7m_blank_check_memory(struct target *target,
-		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
+		struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value);

 int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found);

@@ -1547,15 +1547,14 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 		} else
 			LOG_ERROR("unable to allocate feature list");

-		if (armv8_regs[i].data_type == NULL) {
-			reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
-			if (reg_list[i].reg_data_type)
+		reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
+		if (reg_list[i].reg_data_type) {
+			if (armv8_regs[i].data_type == NULL)
 				reg_list[i].reg_data_type->type = armv8_regs[i].type;
 			else
-				LOG_ERROR("unable to allocate reg type list");
+				*reg_list[i].reg_data_type = *armv8_regs[i].data_type;
 		} else
-			reg_list[i].reg_data_type =	armv8_regs[i].data_type;
-
+			LOG_ERROR("unable to allocate reg type list");
 	}

 	arm->cpsr = reg_list + ARMV8_xPSR;
@@ -1608,6 +1607,41 @@ struct reg *armv8_reg_current(struct arm *arm, unsigned regnum)
 	return r;
 }

+static void armv8_free_cache(struct reg_cache *cache, bool regs32)
+{
+	struct reg *reg;
+	unsigned int i;
+
+	if (!cache)
+		return;
+
+	for (i = 0; i < cache->num_regs; i++) {
+		reg = &cache->reg_list[i];
+
+		free(reg->feature);
+		free(reg->reg_data_type);
+	}
+
+	if (!regs32)
+		free(cache->reg_list[0].arch_info);
+	free(cache->reg_list);
+	free(cache);
+}
+
+void armv8_free_reg_cache(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	struct reg_cache *cache = NULL, *cache32 = NULL;
+
+	cache = arm->core_cache;
+	if (cache != NULL)
+		cache32 = cache->next;
+	armv8_free_cache(cache32, true);
+	armv8_free_cache(cache, false);
+	arm->core_cache = NULL;
+}
+
 const struct command_registration armv8_command_handlers[] = {
 	COMMAND_REGISTRATION_DONE
 };
@@ -318,6 +318,8 @@ static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
 void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
 int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value);

+extern void armv8_free_reg_cache(struct target *target);
+
 extern const struct command_registration armv8_command_handlers[];

 #endif /* OPENOCD_TARGET_ARMV8_H */
@@ -51,11 +51,6 @@
 * any longer.
 */

-/**
- * Returns the type of a break point required by address location
- */
-#define BKPT_TYPE_BY_ADDR(addr) ((addr) < 0x20000000 ? BKPT_HARD : BKPT_SOFT)
-
 /* forward declarations */
 static int cortex_m_store_core_reg_u32(struct target *target,
 		uint32_t num, uint32_t value);
@@ -868,7 +863,7 @@ static int cortex_m_step(struct target *target, int current,
 				if (breakpoint)
 					retval = cortex_m_set_breakpoint(target, breakpoint);
 				else
-					retval = breakpoint_add(target, pc_value, 2, BKPT_TYPE_BY_ADDR(pc_value));
+					retval = breakpoint_add(target, pc_value, 2, BKPT_HARD);
 				bool tmp_bp_set = (retval == ERROR_OK);

 				/* No more breakpoints left, just do a step */
@@ -1131,9 +1126,6 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 		return ERROR_OK;
 	}

-	if (cortex_m->auto_bp_type)
-		breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
-
 	if (breakpoint->type == BKPT_HARD) {
 		uint32_t fpcr_value;
 		while (comparator_list[fp_num].used && (fp_num < cortex_m->fp_num_code))
@@ -1253,21 +1245,6 @@ int cortex_m_add_breakpoint(struct target *target, struct breakpoint *breakpoint
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);

-	if (cortex_m->auto_bp_type)
-		breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
-
-	if (breakpoint->type != BKPT_TYPE_BY_ADDR(breakpoint->address)) {
-		if (breakpoint->type == BKPT_HARD) {
-			LOG_INFO("flash patch comparator requested outside code memory region");
-			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-		}
-
-		if (breakpoint->type == BKPT_SOFT) {
-			LOG_INFO("soft breakpoint requested in code (flash) memory region");
-			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-		}
-	}
-
 	if ((breakpoint->type == BKPT_HARD) && (cortex_m->fp_code_available < 1)) {
 		LOG_INFO("no flash patch comparator unit available for hardware breakpoint");
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
@@ -1299,9 +1276,6 @@ int cortex_m_remove_breakpoint(struct target *target, struct breakpoint *breakpo
 		return ERROR_TARGET_NOT_HALTED;
 	}

-	if (cortex_m->auto_bp_type)
-		breakpoint->type = BKPT_TYPE_BY_ADDR(breakpoint->address);
-
 	if (breakpoint->set)
 		cortex_m_unset_breakpoint(target, breakpoint);

@@ -2111,7 +2085,6 @@ int cortex_m_examine(struct target *target)

 		/* Setup FPB */
 		target_read_u32(target, FP_CTRL, &fpcr);
-		cortex_m->auto_bp_type = 1;
 		/* bits [14:12] and [7:4] */
 		cortex_m->fp_num_code = ((fpcr >> 8) & 0x70) | ((fpcr >> 4) & 0xF);
 		cortex_m->fp_num_lit = (fpcr >> 8) & 0xF;
@@ -175,7 +175,6 @@ struct cortex_m_common {
 	int fp_code_available;
 	int fp_rev;
 	int fpb_enabled;
-	int auto_bp_type;
 	struct cortex_m_fp_comparator *fp_comparator_list;

 	/* Data Watchpoint and Trace (DWT) */
@@ -827,7 +827,8 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,

 /** Checks whether a memory region is erased. */
 int mips32_blank_check_memory(struct target *target,
-		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+		struct target_memory_check_block *blocks, int num_blocks,
+		uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
 	struct reg_param reg_params[3];
@@ -866,16 +867,16 @@ int mips32_blank_check_memory(struct target *target,
 	int retval = target_write_buffer(target, erase_check_algorithm->address,
 						sizeof(erase_check_code), erase_check_code_8);
 	if (retval != ERROR_OK)
-		return retval;
+		goto cleanup;

 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
 	mips32_info.isa_mode = isa ? MIPS32_ISA_MMIPS32 : MIPS32_ISA_MIPS32;

 	init_reg_param(&reg_params[0], "r4", 32, PARAM_OUT);
-	buf_set_u32(reg_params[0].value, 0, 32, address);
+	buf_set_u32(reg_params[0].value, 0, 32, blocks[0].address);

 	init_reg_param(&reg_params[1], "r5", 32, PARAM_OUT);
-	buf_set_u32(reg_params[1].value, 0, 32, count);
+	buf_set_u32(reg_params[1].value, 0, 32, blocks[0].size);

 	init_reg_param(&reg_params[2], "r6", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
@@ -884,15 +885,19 @@ int mips32_blank_check_memory(struct target *target,
 			erase_check_algorithm->address + (sizeof(erase_check_code) - 4), 10000, &mips32_info);

 	if (retval == ERROR_OK)
-		*blank = buf_get_u32(reg_params[2].value, 0, 32);
+		blocks[0].result = buf_get_u32(reg_params[2].value, 0, 32);

 	destroy_reg_param(&reg_params[0]);
 	destroy_reg_param(&reg_params[1]);
 	destroy_reg_param(&reg_params[2]);

+cleanup:
 	target_free_working_area(target, erase_check_algorithm);

-	return retval;
+	if (retval != ERROR_OK)
+		return retval;
+
+	return 1;       /* only one block has been checked */
 }

 static int mips32_verify_pointer(struct command_context *cmd_ctx,
@@ -428,6 +428,6 @@ int mips32_get_gdb_reg_list(struct target *target,
 int mips32_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum);
 int mips32_blank_check_memory(struct target *target,
-		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
+		struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value);

 #endif /* OPENOCD_TARGET_MIPS32_H */
@@ -953,22 +953,6 @@ static int riscv_checksum_memory(struct target *target,
 	return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 }

-/* Should run code on the target to check whether a memory
-block holds all-ones (because this is generally called on
-NOR flash which is 1 when "blank")
-Not yet implemented.
-*/
-int riscv_blank_check_memory(struct target *target,
-				target_addr_t address,
-				uint32_t count,
-				uint32_t *blank,
-				uint8_t erased_value)
-{
-	*blank = 0;
-
-	return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-}
-
 /*** OpenOCD Helper Functions ***/

 enum riscv_poll_hart {
@@ -1549,7 +1533,6 @@ struct target_type riscv_target = {
 	.read_memory = riscv_read_memory,
 	.write_memory = riscv_write_memory,

-	.blank_check_memory = riscv_blank_check_memory,
 	.checksum_memory = riscv_checksum_memory,

 	.get_gdb_reg_list = riscv_get_gdb_reg_list,
@@ -1750,7 +1750,7 @@ static int stm8_examine(struct target *target)

 /** Checks whether a memory region is erased. */
 static int stm8_blank_check_memory(struct target *target,
-		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+		struct target_memory_check_block *blocks, int num_blocks, uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
 	struct reg_param reg_params[2];
@@ -1778,10 +1778,10 @@ static int stm8_blank_check_memory(struct target *target,
 	stm8_info.common_magic = STM8_COMMON_MAGIC;

 	init_mem_param(&mem_params[0], 0x0, 3, PARAM_OUT);
-	buf_set_u32(mem_params[0].value, 0, 24, address);
+	buf_set_u32(mem_params[0].value, 0, 24, blocks[0].address);

 	init_mem_param(&mem_params[1], 0x3, 3, PARAM_OUT);
-	buf_set_u32(mem_params[1].value, 0, 24, count);
+	buf_set_u32(mem_params[1].value, 0, 24, blocks[0].size);

 	init_reg_param(&reg_params[0], "a", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[0].value, 0, 32, erased_value);
@@ -1795,7 +1795,7 @@ static int stm8_blank_check_memory(struct target *target,
 			10000, &stm8_info);

 	if (retval == ERROR_OK)
-		*blank = (*(reg_params[0].value) == 0xff);
+		blocks[0].result = (*(reg_params[0].value) == 0xff);

 	destroy_mem_param(&mem_params[0]);
 	destroy_mem_param(&mem_params[1]);
@@ -1803,7 +1803,10 @@ static int stm8_blank_check_memory(struct target *target,

 	target_free_working_area(target, erase_check_algorithm);

-	return retval;
+	if (retval != ERROR_OK)
+		return retval;
+
+	return 1;	/* only one block has been checked */
 }

 static int stm8_checksum_memory(struct target *target, target_addr_t address,
@@ -1912,6 +1912,18 @@ static void target_destroy(struct target *target)
 		free(target->working_areas);
 	}

+	/* release the targets SMP list */
+	if (target->smp) {
+		struct target_list *head = target->head;
+		while (head != NULL) {
+			struct target_list *pos = head->next;
+			head->target->smp = 0;
+			free(head);
+			head = pos;
+		}
+		target->smp = 0;
+	}
+
 	free(target->type);
 	free(target->trace_info);
 	free(target->fileio_info);
@@ -2241,21 +2253,19 @@ int target_checksum_memory(struct target *target, target_addr_t address, uint32_
 	return retval;
 }

-int target_blank_check_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t* blank,
+int target_blank_check_memory(struct target *target,
+	struct target_memory_check_block *blocks, int num_blocks,
 	uint8_t erased_value)
 {
-	int retval;
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
 		return ERROR_FAIL;
 	}

-	if (target->type->blank_check_memory == 0)
+	if (target->type->blank_check_memory == NULL)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;

-	retval = target->type->blank_check_memory(target, address, size, blank, erased_value);
-
-	return retval;
+	return target->type->blank_check_memory(target, blocks, num_blocks, erased_value);
 }

 int target_read_u64(struct target *target, target_addr_t address, uint64_t *value)
@@ -312,6 +312,12 @@ struct target_timer_callback {
 	struct target_timer_callback *next;
 };

+struct target_memory_check_block {
+	target_addr_t address;
+	uint32_t size;
+	uint32_t result;
+};
+
 int target_register_commands(struct command_context *cmd_ctx);
 int target_examine(void);

@@ -585,7 +591,8 @@ int target_read_buffer(struct target *target,
 int target_checksum_memory(struct target *target,
 		target_addr_t address, uint32_t size, uint32_t *crc);
 int target_blank_check_memory(struct target *target,
-		target_addr_t address, uint32_t size, uint32_t *blank, uint8_t erased_value);
+		struct target_memory_check_block *blocks, int num_blocks,
+		uint8_t erased_value);
 int target_wait_state(struct target *target, enum target_state state, int ms);

 /**
@@ -130,8 +130,9 @@ struct target_type {

 	int (*checksum_memory)(struct target *target, target_addr_t address,
 			uint32_t count, uint32_t *checksum);
-	int (*blank_check_memory)(struct target *target, target_addr_t address,
-			uint32_t count, uint32_t *blank, uint8_t erased_value);
+	int (*blank_check_memory)(struct target *target,
+			struct target_memory_check_block *blocks, int num_blocks,
+			uint8_t erased_value);

 	/*
 	 * target break-/watchpoint control
@@ -97,10 +97,7 @@ bool transports_are_declared(void);
 bool transport_is_jtag(void);
 bool transport_is_swd(void);

-/* FIXME: ZY1000 test build on jenkins is configured with enabled hla adapters
- * but jtag/hla/hla_*.c files are not compiled. To workaround the problem we assume hla
- * is broken if BUILD_ZY1000 is set */
-#if BUILD_HLADAPTER && !BUILD_ZY1000
+#if BUILD_HLADAPTER
 bool transport_is_hla(void);
 #else
 static inline bool transport_is_hla(void)