From bb976e3c387bc82e20ab7304f0cfac3e5eede3a1 Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Sun, 9 Apr 2017 10:59:57 +0200
Subject: [PATCH 01/65] jtag/drivers/cmsis-dap: fix connect under reset

Commit ef02b69b14d133b061217a91add5a028a77e86bc included
a call to cmsis_dap_cmd_DAP_Connect() before calling
cmsis_dap_cmd_DAP_SWJ_Sequence(). According to comment
it is necessary for at least Keil ULINK-ME.

Commit 72c3464be42088dc75245cf2fcc8f5c6e6959b4b added
a cmsis_dap_cmd_DAP_Disconnect() before connect call to pair
connection/disconnection. It solves some problems on Atmel EDBG.

Unfortunately calling either of cmsis_dap_cmd_DAP_Connect()
or cmsis_dap_cmd_DAP_Disconnect() deasserts reset signal.
So these workarounds break ability to connect under reset.

Use cmsis_dap_cmd_DAP_Disconnect() and cmsis_dap_cmd_DAP_Connect()
pair only if both SRST and TRST are deasserted.

Change-Id: I0914dae0a1360b8c7fe48231ff3867caedfb2dbe
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reported-by: Leonardo Sabino dos Santos <leonardo.sabino@gmail.com>
Reviewed-on: http://openocd.zylin.com/4100
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/jtag/drivers/cmsis_dap_usb.c | 30 +++++++++++++++++++-----------
 1 file changed, 19 insertions(+), 11 deletions(-)

diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 345c1fd7e..b8181d6ae 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -206,6 +206,8 @@ static uint8_t queued_seq_buf[1024]; /* TODO: make dynamic / move into cmsis obj
 
 static int queued_retval;
 
+static uint8_t output_pins = SWJ_PIN_SRST | SWJ_PIN_TRST;
+
 static struct cmsis_dap *cmsis_dap_handle;
 
 static int cmsis_dap_usb_open(void)
@@ -790,15 +792,21 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 	unsigned int s_len;
 	int retval;
 
-	/* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
-	cmsis_dap_cmd_DAP_Disconnect();
+	if ((output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST)) == (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
+		/* Following workaround deasserts reset on most adapters.
+		 * Do not reconnect if a reset line is active!
+		 * Reconnecting would break connecting under reset. */
 
-	/* When we are reconnecting, DAP_Connect needs to be rerun, at
-	 * least on Keil ULINK-ME */
-	retval = cmsis_dap_cmd_DAP_Connect(seq == LINE_RESET || seq == JTAG_TO_SWD ?
+		/* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
+		cmsis_dap_cmd_DAP_Disconnect();
+
+		/* When we are reconnecting, DAP_Connect needs to be rerun, at
+		 * least on Keil ULINK-ME */
+		retval = cmsis_dap_cmd_DAP_Connect(seq == LINE_RESET || seq == JTAG_TO_SWD ?
 					   CONNECT_SWD : CONNECT_JTAG);
-	if (retval != ERROR_OK)
-		return retval;
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
 	switch (seq) {
 	case LINE_RESET:
@@ -1010,14 +1018,14 @@ static void cmsis_dap_execute_reset(struct jtag_command *cmd)
 {
 	/* Set both TRST and SRST even if they're not enabled as
 	 * there's no way to tristate them */
-	uint8_t pins = 0;
 
+	output_pins = 0;
 	if (!cmd->cmd.reset->srst)
-		pins |= SWJ_PIN_SRST;
+		output_pins |= SWJ_PIN_SRST;
 	if (!cmd->cmd.reset->trst)
-		pins |= SWJ_PIN_TRST;
+		output_pins |= SWJ_PIN_TRST;
 
-	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(pins,
+	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(output_pins,
 			SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
 	if (retval != ERROR_OK)
 		LOG_ERROR("CMSIS-DAP: Interface reset failed");

From ada631cc5ff09e20a245fdca1afd4064be5eaa3d Mon Sep 17 00:00:00 2001
From: Bas Vermeulen <bas@daedalean.ai>
Date: Sun, 26 Nov 2017 22:31:55 +0100
Subject: [PATCH 02/65] target aarch64: rework memory read/write to use 8/16/32
 bit operations

The existing code only used Memory Access mode to read memory,
which uses 32 bit operations only.
Rework the code to check the alignment/size of the read/write operation,
and use the Memory Access mode to read aligned 32 bit memory.
When using unaligned access, or 8 or 16 bit reads, use LDR{BHW} and STR{BHW}
instead.

The exception handling is still the same as it was before (meaning it breaks
when things go wrong), but I can now read an 8 bit register correctly.

Change-Id: I739a5ee825c0226ed4a89c32895cc2a047b8dc15
Signed-off-by: Bas Vermeulen <bas@daedalean.ai>
Reviewed-on: http://openocd.zylin.com/4301
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c       | 445 +++++++++++++++++++++----------------
 src/target/armv8_opcodes.c |  12 +
 src/target/armv8_opcodes.h |  14 ++
 3 files changed, 284 insertions(+), 187 deletions(-)

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 5e5d3fc7f..8354d3ac0 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -54,7 +54,7 @@ static int aarch64_unset_breakpoint(struct target *target,
 static int aarch64_mmu(struct target *target, int *enabled);
 static int aarch64_virt2phys(struct target *target,
 	target_addr_t virt, target_addr_t *phys);
-static int aarch64_read_apb_ap_memory(struct target *target,
+static int aarch64_read_cpu_memory(struct target *target,
 	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 
 #define foreach_smp_target(pos, head) \
@@ -1668,7 +1668,99 @@ static int aarch64_deassert_reset(struct target *target)
 	return aarch64_init_debug_access(target);
 }
 
-static int aarch64_write_apb_ap_memory(struct target *target,
+static int aarch64_write_cpu_memory_slow(struct target *target,
+	uint32_t size, uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int retval;
+
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	/* change DCC to normal mode if necessary */
+	if (*dscr & DSCR_MA) {
+		*dscr &= ~DSCR_MA;
+		retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	while (count) {
+		uint32_t data, opcode;
+
+		/* write the data to store into DTRRX */
+		if (size == 1)
+			data = *buffer;
+		else if (size == 2)
+			data = target_buffer_get_u16(target, buffer);
+		else
+			data = target_buffer_get_u32(target, buffer);
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRRX, data);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (arm->core_state == ARM_STATE_AARCH64)
+			retval = dpm->instr_execute(dpm, ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 1));
+		else
+			retval = dpm->instr_execute(dpm, ARMV4_5_MRC(14, 0, 1, 0, 5, 0));
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (size == 1)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_STRB_IP);
+		else if (size == 2)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_STRH_IP);
+		else
+			opcode = armv8_opcode(armv8, ARMV8_OPC_STRW_IP);
+		retval = dpm->instr_execute(dpm, opcode);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Advance */
+		buffer += size;
+		--count;
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_write_cpu_memory_fast(struct target *target,
+	uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int retval;
+
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	/* Step 1.d   - Change DCC to memory mode */
+	*dscr |= DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+
+	/* Step 2.a   - Do the write */
+	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
+					buffer, 4, count, armv8->debug_base + CPUV8_DBG_DTRRX);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Step 3.a   - Switch DTR mode back to Normal mode */
+	*dscr &= ~DSCR_MA;
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int aarch64_write_cpu_memory(struct target *target,
 	uint64_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
@@ -1677,71 +1769,27 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = &armv8->dpm;
 	struct arm *arm = &armv8->arm;
-	int total_bytes = count * size;
-	int total_u32;
-	int start_byte = address & 0x3;
-	int end_byte   = (address + total_bytes) & 0x3;
-	struct reg *reg;
 	uint32_t dscr;
-	uint8_t *tmp_buff = NULL;
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
-
-	/* Mark register R0 as dirty, as it will be used
+	/* Mark register X0 as dirty, as it will be used
 	 * for transferring the data.
 	 * It will be restored automatically when exiting
 	 * debug mode
 	 */
-	reg = armv8_reg_current(arm, 1);
-	reg->dirty = true;
-
-	reg = armv8_reg_current(arm, 0);
-	reg->dirty = true;
+	armv8_reg_current(arm, 0)->dirty = true;
 
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
-	/* The algorithm only copies 32 bit words, so the buffer
-	 * should be expanded to include the words at either end.
-	 * The first and last words will be read first to avoid
-	 * corruption if needed.
-	 */
-	tmp_buff = malloc(total_u32 * 4);
-
-	if ((start_byte != 0) && (total_u32 > 1)) {
-		/* First bytes not aligned - read the 32 bit word to avoid corrupting
-		 * the other bytes in the word.
-		 */
-		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3), 4, 1, tmp_buff);
-		if (retval != ERROR_OK)
-			goto error_free_buff_w;
-	}
-
-	/* If end of write is not aligned, or the write is less than 4 bytes */
-	if ((end_byte != 0) ||
-		((total_u32 == 1) && (total_bytes != 4))) {
-
-		/* Read the last word to avoid corruption during 32 bit write */
-		int mem_offset = (total_u32-1) * 4;
-		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3) + mem_offset, 4, 1, &tmp_buff[mem_offset]);
-		if (retval != ERROR_OK)
-			goto error_free_buff_w;
-	}
-
-	/* Copy the write buffer over the top of the temporary buffer */
-	memcpy(&tmp_buff[start_byte], buffer, total_bytes);
-
-	/* We now have a 32 bit aligned buffer that can be written */
-
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
-		goto error_free_buff_w;
+		return retval;
 
 	/* Set Normal access mode  */
 	dscr = (dscr & ~DSCR_MA);
@@ -1753,68 +1801,168 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
 		retval = dpm->instr_write_data_dcc_64(dpm,
-				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRX */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
 		dpm->instr_write_data_dcc(dpm,
-				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
-
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
 	}
-	/* Step 1.d   - Change DCC to memory mode */
-	dscr = dscr | DSCR_MA;
-	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_w;
 
+	if (size == 4 && (address % 4) == 0)
+		retval = aarch64_write_cpu_memory_fast(target, count, buffer, &dscr);
+	else
+		retval = aarch64_write_cpu_memory_slow(target, size, count, buffer, &dscr);
 
-	/* Step 2.a   - Do the write */
-	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
-					tmp_buff, 4, total_u32, armv8->debug_base + CPUV8_DBG_DTRRX);
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_w;
-
-	/* Step 3.a   - Switch DTR mode back to Normal mode */
-	dscr = (dscr & ~DSCR_MA);
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_w;
+	if (retval != ERROR_OK) {
+		/* Unset DTR mode */
+		mem_ap_read_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+		dscr &= ~DSCR_MA;
+		mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	}
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
-		goto error_free_buff_w;
+		return retval;
 
 	dpm->dscr = dscr;
 	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		armv8_dpm_handle_exception(dpm);
-		goto error_free_buff_w;
+		return ERROR_FAIL;
 	}
 
 	/* Done */
-	free(tmp_buff);
 	return ERROR_OK;
-
-error_unset_dtr_w:
-	/* Unset DTR mode */
-	mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	dscr = (dscr & ~DSCR_MA);
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-error_free_buff_w:
-	LOG_ERROR("error");
-	free(tmp_buff);
-	return ERROR_FAIL;
 }
 
-static int aarch64_read_apb_ap_memory(struct target *target,
+static int aarch64_read_cpu_memory_slow(struct target *target,
+	uint32_t size, uint32_t count, uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int retval;
+
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	/* change DCC to normal mode (if necessary) */
+	if (*dscr & DSCR_MA) {
+		*dscr &= DSCR_MA;
+		retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	while (count) {
+		uint32_t opcode, data;
+
+		if (size == 1)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_LDRB_IP);
+		else if (size == 2)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_LDRH_IP);
+		else
+			opcode = armv8_opcode(armv8, ARMV8_OPC_LDRW_IP);
+		retval = dpm->instr_execute(dpm, opcode);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (arm->core_state == ARM_STATE_AARCH64)
+			retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 1));
+		else
+			retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 1, 0, 5, 0));
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRTX, &data);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (size == 1)
+			*buffer = (uint8_t)data;
+		else if (size == 2)
+			target_buffer_set_u16(target, buffer, (uint16_t)data);
+		else
+			target_buffer_set_u32(target, buffer, data);
+
+		/* Advance */
+		buffer += size;
+		--count;
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_read_cpu_memory_fast(struct target *target,
+	uint32_t count, uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int retval;
+	uint32_t value;
+
+	/* Mark X1 as dirty */
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
+	} else {
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
+	}
+
+	/* Step 1.e - Change DCC to memory mode */
+	*dscr |= DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	/* Step 1.f - read DBGDTRTX and discard the value */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+
+	count--;
+	/* Read the data - Each read of the DTRTX register causes the instruction to be reissued
+	 * Abort flags are sticky, so can be read at end of transactions
+	 *
+	 * This data is read in aligned to 32 bit boundary.
+	 */
+
+	if (count) {
+		/* Step 2.a - Loop n-1 times, each read of DBGDTRTX reads the data from [X0] and
+		 * increments X0 by 4. */
+		retval = mem_ap_read_buf_noincr(armv8->debug_ap, buffer, 4, count,
+									armv8->debug_base + CPUV8_DBG_DTRTX);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* Step 3.a - set DTR access mode back to Normal mode	*/
+	*dscr &= ~DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Step 3.b - read DBGDTRTX for the final value */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	target_buffer_set_u32(target, buffer + count * 4, value);
+	return retval;
+}
+
+static int aarch64_read_cpu_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
@@ -1823,32 +1971,22 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = &armv8->dpm;
 	struct arm *arm = &armv8->arm;
-	int total_bytes = count * size;
-	int total_u32;
-	int start_byte = address & 0x3;
-	int end_byte   = (address + total_bytes) & 0x3;
-	struct reg *reg;
 	uint32_t dscr;
-	uint8_t *tmp_buff = NULL;
-	uint8_t *u8buf_ptr;
-	uint32_t value;
+
+	LOG_DEBUG("Reading CPU memory address 0x%016" PRIx64 " size %" PRIu32 " count %" PRIu32,
+			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
-	/* Mark register X0, X1 as dirty, as it will be used
+	/* Mark register X0 as dirty, as it will be used
 	 * for transferring the data.
 	 * It will be restored automatically when exiting
 	 * debug mode
 	 */
-	reg = armv8_reg_current(arm, 1);
-	reg->dirty = true;
-
-	reg = armv8_reg_current(arm, 0);
-	reg->dirty = true;
+	armv8_reg_current(arm, 0)->dirty = true;
 
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -1857,7 +1995,7 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
 	/* Set Normal access mode  */
-	dscr = (dscr & ~DSCR_MA);
+	dscr &= ~DSCR_MA;
 	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 
@@ -1866,83 +2004,34 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
 		retval += dpm->instr_write_data_dcc_64(dpm,
-				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
-		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-		retval += dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
-		/* Step 1.e - Change DCC to memory mode */
-		dscr = dscr | DSCR_MA;
-		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-		/* Step 1.f - read DBGDTRTX and discard the value */
-		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
 		retval += dpm->instr_write_data_dcc(dpm,
-				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
-		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-		retval += dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
-		/* Step 1.e - Change DCC to memory mode */
-		dscr = dscr | DSCR_MA;
-		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-		/* Step 1.f - read DBGDTRTX and discard the value */
-		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
-
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
 	}
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_r;
 
-	/* Optimize the read as much as we can, either way we read in a single pass  */
-	if ((start_byte) || (end_byte)) {
-		/* The algorithm only copies 32 bit words, so the buffer
-		 * should be expanded to include the words at either end.
-		 * The first and last words will be read into a temp buffer
-		 * to avoid corruption
-		 */
-		tmp_buff = malloc(total_u32 * 4);
-		if (!tmp_buff)
-			goto error_unset_dtr_r;
+	if (size == 4 && (address % 4) == 0)
+		retval = aarch64_read_cpu_memory_fast(target, count, buffer, &dscr);
+	else
+		retval = aarch64_read_cpu_memory_slow(target, size, count, buffer, &dscr);
 
-		/* use the tmp buffer to read the entire data */
-		u8buf_ptr = tmp_buff;
-	} else
-		/* address and read length are aligned so read directly into the passed buffer */
-		u8buf_ptr = buffer;
-
-	/* Read the data - Each read of the DTRTX register causes the instruction to be reissued
-	 * Abort flags are sticky, so can be read at end of transactions
-	 *
-	 * This data is read in aligned to 32 bit boundary.
-	 */
-
-	/* Step 2.a - Loop n-1 times, each read of DBGDTRTX reads the data from [X0] and
-	 * increments X0 by 4. */
-	retval = mem_ap_read_buf_noincr(armv8->debug_ap, u8buf_ptr, 4, total_u32-1,
-									armv8->debug_base + CPUV8_DBG_DTRTX);
-	if (retval != ERROR_OK)
-			goto error_unset_dtr_r;
-
-	/* Step 3.a - set DTR access mode back to Normal mode	*/
-	dscr = (dscr & ~DSCR_MA);
-	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+	if (dscr & DSCR_MA) {
+		dscr &= ~DSCR_MA;
+		mem_ap_write_atomic_u32(armv8->debug_ap,
 					armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_free_buff_r;
+	}
 
-	/* Step 3.b - read DBGDTRTX for the final value */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
-	memcpy(u8buf_ptr + (total_u32-1) * 4, &value, 4);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
-		goto error_free_buff_r;
+		return retval;
 
 	dpm->dscr = dscr;
 
@@ -1950,29 +2039,11 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		armv8_dpm_handle_exception(dpm);
-		goto error_free_buff_r;
-	}
-
-	/* check if we need to copy aligned data by applying any shift necessary */
-	if (tmp_buff) {
-		memcpy(buffer, tmp_buff + start_byte, total_bytes);
-		free(tmp_buff);
+		return ERROR_FAIL;
 	}
 
 	/* Done */
 	return ERROR_OK;
-
-error_unset_dtr_r:
-	/* Unset DTR mode */
-	mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	dscr = (dscr & ~DSCR_MA);
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-error_free_buff_r:
-	LOG_ERROR("error");
-	free(tmp_buff);
-	return ERROR_FAIL;
 }
 
 static int aarch64_read_phys_memory(struct target *target,
@@ -1986,7 +2057,7 @@ static int aarch64_read_phys_memory(struct target *target,
 		retval = aarch64_mmu_modify(target, 0);
 		if (retval != ERROR_OK)
 			return retval;
-		retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
+		retval = aarch64_read_cpu_memory(target, address, size, count, buffer);
 	}
 	return retval;
 }
@@ -2008,7 +2079,7 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	return aarch64_read_apb_ap_memory(target, address, size, count, buffer);
+	return aarch64_read_cpu_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_write_phys_memory(struct target *target,
@@ -2022,7 +2093,7 @@ static int aarch64_write_phys_memory(struct target *target,
 		retval = aarch64_mmu_modify(target, 0);
 		if (retval != ERROR_OK)
 			return retval;
-		return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+		return aarch64_write_cpu_memory(target, address, size, count, buffer);
 	}
 
 	return retval;
@@ -2045,7 +2116,7 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+	return aarch64_write_cpu_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_handle_target_request(void *priv)
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index d3c0b3f5f..6887b2953 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -42,6 +42,12 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCCIVAC]	= ARMV8_SYS(SYSTEM_DCCIVAC, 0),
 		[ARMV8_OPC_ICIVAU]	= ARMV8_SYS(SYSTEM_ICIVAU, 0),
 		[ARMV8_OPC_HLT]		= ARMV8_HLT(11),
+		[ARMV8_OPC_LDRB_IP]	= ARMV8_LDRB_IP(1, 0),
+		[ARMV8_OPC_LDRH_IP]	= ARMV8_LDRH_IP(1, 0),
+		[ARMV8_OPC_LDRW_IP]	= ARMV8_LDRW_IP(1, 0),
+		[ARMV8_OPC_STRB_IP]	= ARMV8_STRB_IP(1, 0),
+		[ARMV8_OPC_STRH_IP]	= ARMV8_STRH_IP(1, 0),
+		[ARMV8_OPC_STRW_IP]	= ARMV8_STRW_IP(1, 0),
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -63,6 +69,12 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCCIVAC]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 1),
 		[ARMV8_OPC_ICIVAU]	= ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
 		[ARMV8_OPC_HLT]		= ARMV8_HLT_A1(11),
+		[ARMV8_OPC_LDRB_IP]	= ARMV4_5_LDRB_IP(1, 0),
+		[ARMV8_OPC_LDRH_IP]	= ARMV4_5_LDRH_IP(1, 0),
+		[ARMV8_OPC_LDRW_IP]	= ARMV4_5_LDRW_IP(1, 0),
+		[ARMV8_OPC_STRB_IP]	= ARMV4_5_STRB_IP(1, 0),
+		[ARMV8_OPC_STRH_IP]	= ARMV4_5_STRH_IP(1, 0),
+		[ARMV8_OPC_STRW_IP]	= ARMV4_5_STRW_IP(1, 0),
 };
 
 void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 2d8ddd824..987198a87 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -159,6 +159,14 @@
 #define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_32(Rt) (0x11000000 | (Rt << 5) | (0x1F))
 
+#define ARMV8_LDRB_IP(Rd, Rn) (0x38401400 | (Rn << 5) | Rd)
+#define ARMV8_LDRH_IP(Rd, Rn) (0x78402400 | (Rn << 5) | Rd)
+#define ARMV8_LDRW_IP(Rd, Rn) (0xb8404400 | (Rn << 5) | Rd)
+
+#define ARMV8_STRB_IP(Rd, Rn) (0x38001400 | (Rn << 5) | Rd)
+#define ARMV8_STRH_IP(Rd, Rn) (0x78002400 | (Rn << 5) | Rd)
+#define ARMV8_STRW_IP(Rd, Rn) (0xb8004400 | (Rn << 5) | Rd)
+
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
 enum armv8_opcode {
@@ -180,6 +188,12 @@ enum armv8_opcode {
 	ARMV8_OPC_DCCIVAC,
 	ARMV8_OPC_ICIVAU,
 	ARMV8_OPC_HLT,
+	ARMV8_OPC_STRB_IP,
+	ARMV8_OPC_STRH_IP,
+	ARMV8_OPC_STRW_IP,
+	ARMV8_OPC_LDRB_IP,
+	ARMV8_OPC_LDRH_IP,
+	ARMV8_OPC_LDRW_IP,
 	ARMV8_OPC_NUM,
 };
 

From 4553abf9064fe3c0e4ea2ed29a1d2217df74ff5f Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Thu, 15 Jun 2017 08:59:01 +0200
Subject: [PATCH 03/65] arm_adi_v5: fix wrong addressing after change of
 CSW_ADDRINC

Problem: If the same memory location is accessed alternatively
by MEM-AP banked data registers without autoincrement and by standard
autoincremented read/write, TAR register is not updated correctly.

How to replicate: On a Cortex-M issue
    mdw 0xe000edf0
multiple times. When poll is on (poll reads the same memory location)
only the first read is correct.
0xe000edf0: 01000000
0xe000edf0: 00000000
0xe000edf0: 20002640
0xe000edf0: 01000000
0xe000edf0: 00000000
0xe000edf0: 00000000

No problems with poll off.
0xe000edf0: 01000000
0xe000edf0: 01000000
0xe000edf0: 01000000

mem_ap_setup_tar() writes to MEM_AP_REG_TAR if requested TAR value
changed or CSW_ADDRINC_... is currently active.
However if an autoincremented access has been issued and autoinc
switched off in CSW afterwards, TAR does not get updated.

The change introduces mem_ap_update_tar_cache() which is called
after queuing of any access to MEM_AP_REG_DRW. It simulates
TAR increment to keep tar_value in sync with MEM_AP.
Crossing tar autoincrement block boundary invalidates cached value.

mem_ap_write() and mem_ap_read() do not check tar autoincrement
block boundary, mem_ap_setup_tar() is called before each transfer instead.

dap_invalidate_cache() is introduced to ensure invalidation
of all cached values during dap_dp_init() and swd_connect()

Change-Id: I815c2283d2989cffd6ea9a4100ce2f29dc3fb7b4
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/4162
Tested-by: jenkins
Reviewed-by: Christopher Head <chead@zaber.com>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/target/adi_v5_swd.c |   2 +-
 src/target/arm_adi_v5.c | 115 +++++++++++++++++++++++++++++-----------
 src/target/arm_adi_v5.h |   7 +++
 3 files changed, 93 insertions(+), 31 deletions(-)

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index a6aada326..c503f090b 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -124,7 +124,7 @@ static int swd_connect(struct adiv5_dap *dap)
 
 	/* Clear link state, including the SELECT cache. */
 	dap->do_reconnect = false;
-	dap->select = DP_SELECT_INVALID;
+	dap_invalidate_cache(dap);
 
 	swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
 
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 200629023..a0d0f4ef0 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -111,17 +111,68 @@ static int mem_ap_setup_csw(struct adiv5_ap *ap, uint32_t csw)
 
 static int mem_ap_setup_tar(struct adiv5_ap *ap, uint32_t tar)
 {
-	if (tar != ap->tar_value ||
-			(ap->csw_value & CSW_ADDRINC_MASK)) {
+	if (!ap->tar_valid || tar != ap->tar_value) {
 		/* LOG_DEBUG("DAP: Set TAR %x",tar); */
 		int retval = dap_queue_ap_write(ap, MEM_AP_REG_TAR, tar);
 		if (retval != ERROR_OK)
 			return retval;
 		ap->tar_value = tar;
+		ap->tar_valid = true;
 	}
 	return ERROR_OK;
 }
 
+static int mem_ap_read_tar(struct adiv5_ap *ap, uint32_t *tar)
+{
+	int retval = dap_queue_ap_read(ap, MEM_AP_REG_TAR, tar);
+	if (retval != ERROR_OK) {
+		ap->tar_valid = false;
+		return retval;
+	}
+
+	retval = dap_run(ap->dap);
+	if (retval != ERROR_OK) {
+		ap->tar_valid = false;
+		return retval;
+	}
+
+	ap->tar_value = *tar;
+	ap->tar_valid = true;
+	return ERROR_OK;
+}
+
+static uint32_t mem_ap_get_tar_increment(struct adiv5_ap *ap)
+{
+	switch (ap->csw_value & CSW_ADDRINC_MASK) {
+	case CSW_ADDRINC_SINGLE:
+		switch (ap->csw_value & CSW_SIZE_MASK) {
+		case CSW_8BIT:
+			return 1;
+		case CSW_16BIT:
+			return 2;
+		case CSW_32BIT:
+			return 4;
+		}
+	case CSW_ADDRINC_PACKED:
+		return 4;
+	}
+	return 0;
+}
+
+/* mem_ap_update_tar_cache is called after an access to MEM_AP_REG_DRW
+ */
+static void mem_ap_update_tar_cache(struct adiv5_ap *ap)
+{
+	if (!ap->tar_valid)
+		return;
+
+	uint32_t inc = mem_ap_get_tar_increment(ap);
+	if (inc >= max_tar_block_size(ap->tar_autoincr_block, ap->tar_value))
+		ap->tar_valid = false;
+	else
+		ap->tar_value += inc;
+}
+
 /**
  * Queue transactions setting up transfer parameters for the
  * currently selected MEM-AP.
@@ -303,10 +354,6 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 	if (ap->unaligned_access_bad && (address % size != 0))
 		return ERROR_TARGET_UNALIGNED_ACCESS;
 
-	retval = mem_ap_setup_tar(ap, address ^ addr_xor);
-	if (retval != ERROR_OK)
-		return retval;
-
 	while (nbytes > 0) {
 		uint32_t this_size = size;
 
@@ -322,6 +369,10 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 		if (retval != ERROR_OK)
 			break;
 
+		retval = mem_ap_setup_tar(ap, address ^ addr_xor);
+		if (retval != ERROR_OK)
+			return retval;
+
 		/* How many source bytes each transfer will consume, and their location in the DRW,
 		 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
 		uint32_t outvalue = 0;
@@ -361,12 +412,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 		if (retval != ERROR_OK)
 			break;
 
-		/* Rewrite TAR if it wrapped or we're xoring addresses */
-		if (addrinc && (addr_xor || (address % ap->tar_autoincr_block < size && nbytes > 0))) {
-			retval = mem_ap_setup_tar(ap, address ^ addr_xor);
-			if (retval != ERROR_OK)
-				break;
-		}
+		mem_ap_update_tar_cache(ap);
 	}
 
 	/* REVISIT: Might want to have a queued version of this function that does not run. */
@@ -375,8 +421,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 
 	if (retval != ERROR_OK) {
 		uint32_t tar;
-		if (dap_queue_ap_read(ap, MEM_AP_REG_TAR, &tar) == ERROR_OK
-				&& dap_run(dap) == ERROR_OK)
+		if (mem_ap_read_tar(ap, &tar) == ERROR_OK)
 			LOG_ERROR("Failed to write memory at 0x%08"PRIx32, tar);
 		else
 			LOG_ERROR("Failed to write memory and, additionally, failed to find out where");
@@ -436,12 +481,6 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 		return ERROR_FAIL;
 	}
 
-	retval = mem_ap_setup_tar(ap, address);
-	if (retval != ERROR_OK) {
-		free(read_buf);
-		return retval;
-	}
-
 	/* Queue up all reads. Each read will store the entire DRW word in the read buffer. How many
 	 * useful bytes it contains, and their location in the word, depends on the type of transfer
 	 * and alignment. */
@@ -459,6 +498,10 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 		if (retval != ERROR_OK)
 			break;
 
+		retval = mem_ap_setup_tar(ap, address);
+		if (retval != ERROR_OK)
+			break;
+
 		retval = dap_queue_ap_read(ap, MEM_AP_REG_DRW, read_ptr++);
 		if (retval != ERROR_OK)
 			break;
@@ -466,12 +509,7 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 		nbytes -= this_size;
 		address += this_size;
 
-		/* Rewrite TAR if it wrapped */
-		if (addrinc && address % ap->tar_autoincr_block < size && nbytes > 0) {
-			retval = mem_ap_setup_tar(ap, address);
-			if (retval != ERROR_OK)
-				break;
-		}
+		mem_ap_update_tar_cache(ap);
 	}
 
 	if (retval == ERROR_OK)
@@ -486,8 +524,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	 * at least give the caller what we have. */
 	if (retval != ERROR_OK) {
 		uint32_t tar;
-		if (dap_queue_ap_read(ap, MEM_AP_REG_TAR, &tar) == ERROR_OK
-				&& dap_run(dap) == ERROR_OK) {
+		if (mem_ap_read_tar(ap, &tar) == ERROR_OK) {
+			/* TAR is incremented after failed transfer on some devices (eg Cortex-M4) */
 			LOG_ERROR("Failed to read memory at 0x%08"PRIx32, tar);
 			if (nbytes > tar - address)
 				nbytes = tar - address;
@@ -596,6 +634,22 @@ struct adiv5_dap *dap_init(void)
 	return dap;
 }
 
+/**
+ * Invalidate cached DP select and cached TAR and CSW of all APs
+ */
+void dap_invalidate_cache(struct adiv5_dap *dap)
+{
+	dap->select = DP_SELECT_INVALID;
+	dap->last_read = NULL;
+
+	int i;
+	for (i = 0; i <= 255; i++) {
+		/* force csw and tar write on the next mem-ap access */
+		dap->ap[i].tar_valid = false;
+		dap->ap[i].csw_value = 0;
+	}
+}
+
 /**
  * Initialize a DAP.  This sets up the power domains, prepares the DP
  * for further use and activates overrun checking.
@@ -615,8 +669,7 @@ int dap_dp_init(struct adiv5_dap *dap)
 	if (!dap->ops)
 		dap->ops = &jtag_dp_ops;
 
-	dap->select = DP_SELECT_INVALID;
-	dap->last_read = NULL;
+	dap_invalidate_cache(dap);
 
 	for (size_t i = 0; i < 30; i++) {
 		/* DP initialization */
@@ -688,6 +741,8 @@ int mem_ap_init(struct adiv5_ap *ap)
 	int retval;
 	struct adiv5_dap *dap = ap->dap;
 
+	ap->tar_valid = false;
+	ap->csw_value = 0;      /* force csw and tar write */
 	retval = mem_ap_setup_transfer(ap, CSW_8BIT | CSW_ADDRINC_PACKED, 0);
 	if (retval != ERROR_OK)
 		return retval;
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index bf9cb5cce..657427b25 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -102,6 +102,7 @@
 #define AP_REG_IDR			0xFC		/* RO: Identification Register */
 
 /* Fields of the MEM-AP's CSW register */
+#define CSW_SIZE_MASK		7
 #define CSW_8BIT		0
 #define CSW_16BIT		1
 #define CSW_32BIT		2
@@ -180,6 +181,9 @@ struct adiv5_ap {
 
 	/* true if unaligned memory access is not supported by the MEM-AP */
 	bool unaligned_access_bad;
+
+	/* true if tar_value is in sync with TAR register */
+	bool tar_valid;
 };
 
 
@@ -476,6 +480,9 @@ struct adiv5_dap *dap_init(void);
 int dap_dp_init(struct adiv5_dap *dap);
 int mem_ap_init(struct adiv5_ap *ap);
 
+/* Invalidate cached DP select and cached TAR and CSW of all APs */
+void dap_invalidate_cache(struct adiv5_dap *dap);
+
 /* Probe the AP for ROM Table location */
 int dap_get_debugbase(struct adiv5_ap *ap,
 			uint32_t *dbgbase, uint32_t *apid);

From 81a33244df232a27a0f42f39aa34141009da3c3d Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Thu, 15 Jun 2017 11:03:32 +0200
Subject: [PATCH 04/65] arm_adi_v5: reduce some CSW writes

MEM-AP access through banked data registers MEM_AP_REG_BD0..3
does not increment TAR regardless of the current autoincrement mode.
mem_ap_read_u32() and mem_ap_write_u32() can keep the current
autoincrement mode instead of switching autoincrement off.

Change-Id: Ib7ec688d3e04f1da678363cd2819ce90e8910e58
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/4163
Tested-by: jenkins
Reviewed-by: Andreas Bolsch <hyphen0break@gmail.com>
Reviewed-by: Christopher Head <chead@zaber.com>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/target/arm_adi_v5.c | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index a0d0f4ef0..ae6c2c629 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -221,7 +221,8 @@ int mem_ap_read_u32(struct adiv5_ap *ap, uint32_t address,
 	/* Use banked addressing (REG_BDx) to avoid some link traffic
 	 * (updating TAR) when reading several consecutive addresses.
 	 */
-	retval = mem_ap_setup_transfer(ap, CSW_32BIT | CSW_ADDRINC_OFF,
+	retval = mem_ap_setup_transfer(ap,
+			CSW_32BIT | (ap->csw_value & CSW_ADDRINC_MASK),
 			address & 0xFFFFFFF0);
 	if (retval != ERROR_OK)
 		return retval;
@@ -272,7 +273,8 @@ int mem_ap_write_u32(struct adiv5_ap *ap, uint32_t address,
 	/* Use banked addressing (REG_BDx) to avoid some link traffic
 	 * (updating TAR) when writing several consecutive addresses.
 	 */
-	retval = mem_ap_setup_transfer(ap, CSW_32BIT | CSW_ADDRINC_OFF,
+	retval = mem_ap_setup_transfer(ap,
+			CSW_32BIT | (ap->csw_value & CSW_ADDRINC_MASK),
 			address & 0xFFFFFFF0);
 	if (retval != ERROR_OK)
 		return retval;

From 3cba4bd4cb8a8d9e8e27e3f83c647b4ff5123c22 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Wed, 26 Jul 2017 11:23:19 +0200
Subject: [PATCH 05/65] aarch64: fix crash on single-stepping

Make sure to skip un-examined targets (e.g. targets with deferred
examination) on single-stepping

fixes bug #158

Change-Id: Ib40c6fee03079fd078bd52a84615e6f8a081d128
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4184
Tested-by: jenkins
Reviewed-by: Yao Qi <qiyaoltc@gmail.com>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8354d3ac0..b18a12a4c 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -770,6 +770,9 @@ static int aarch64_step_restart_smp(struct target *target)
 			if (curr == target)
 				continue;
 
+			if (!target_was_examined(curr))
+				continue;
+
 			retval = aarch64_check_state_one(curr,
 					PRSR_SDR, PRSR_SDR, &resumed, &prsr);
 			if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {

From 6f5e9941df0f92eac5fa955cc96221da907451b1 Mon Sep 17 00:00:00 2001
From: Forest Crossman <cyrozap@gmail.com>
Date: Fri, 5 Jan 2018 22:11:38 -0600
Subject: [PATCH 06/65] jtag/drivers/kitprog: Enable LOG_DEBUG_IO

Change-Id: Ic01b0acf01e18bcb2bf98c101c7b5888cdc10186
Signed-off-by: Forest Crossman <cyrozap@gmail.com>
Reviewed-on: http://openocd.zylin.com/4323
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/jtag/drivers/kitprog.c | 10 +++-------
 1 file changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index 584da8c94..db5b62e2e 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -681,7 +681,7 @@ static int kitprog_swd_run_queue(void)
 	uint8_t *buffer = kitprog_handle->packet_buffer;
 
 	do {
-		LOG_DEBUG("Executing %d queued transactions", pending_transfer_count);
+		LOG_DEBUG_IO("Executing %d queued transactions", pending_transfer_count);
 
 		if (queued_retval != ERROR_OK) {
 			LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
@@ -714,12 +714,10 @@ static int kitprog_swd_run_queue(void)
 				data &= ~CORUNDETECT;
 			}
 
-#if 0
-			LOG_DEBUG("%s %s reg %x %"PRIx32,
+			LOG_DEBUG_IO("%s %s reg %x %"PRIx32,
 					cmd & SWD_CMD_APnDP ? "AP" : "DP",
 					cmd & SWD_CMD_RnW ? "read" : "write",
 				  (cmd & SWD_CMD_A32) >> 1, data);
-#endif
 
 			buffer[write_count++] = (cmd | SWD_CMD_START | SWD_CMD_PARK) & ~SWD_CMD_STOP;
 			read_count++;
@@ -764,9 +762,7 @@ static int kitprog_swd_run_queue(void)
 			if (pending_transfers[i].cmd & SWD_CMD_RnW) {
 				uint32_t data = le_to_h_u32(&buffer[read_index]);
 
-#if 0
-				LOG_DEBUG("Read result: %"PRIx32, data);
-#endif
+				LOG_DEBUG_IO("Read result: %"PRIx32, data);
 
 				if (pending_transfers[i].buffer)
 					*(uint32_t *)pending_transfers[i].buffer = data;

From 779e95cc326e77f628d5feb7d72a064ffedfd820 Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Wed, 10 Jan 2018 15:17:14 +0100
Subject: [PATCH 07/65] arm_adi_v5: fix regression from
 4553abf9064fe3c0e4ea2ed29a1d2217df74ff5f

Functions mem_ap_read() and mem_ap_write() incremented address even
if addrinc=false. I overlooked this fact and moved mem_ap_setup_tar()
set wrong addresses in no-incr mode.

Fixed by preventing address increment in no-incr mode.

Change-Id: I512e12a6a64e30cf6bc5bf77e3d57d35cc33e058
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Suggested-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-on: http://openocd.zylin.com/4326
Tested-by: jenkins
Reviewed-by: Christopher Head <chead@zaber.com>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/target/arm_adi_v5.c | 28 ++++++++++++++++------------
 1 file changed, 16 insertions(+), 12 deletions(-)

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index ae6c2c629..1ca8e4f00 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -378,33 +378,34 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 		/* How many source bytes each transfer will consume, and their location in the DRW,
 		 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
 		uint32_t outvalue = 0;
+		uint32_t drw_byte_idx = address;
 		if (dap->ti_be_32_quirks) {
 			switch (this_size) {
 			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx & 3) ^ addr_xor);
 				break;
 			case 2:
-				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx & 3) ^ addr_xor);
 				break;
 			case 1:
-				outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (address++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (drw_byte_idx & 3) ^ addr_xor);
 				break;
 			}
 		} else {
 			switch (this_size) {
 			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
 				/* fallthrough */
 			case 2:
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
 				/* fallthrough */
 			case 1:
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
 			}
 		}
 
@@ -415,6 +416,8 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 			break;
 
 		mem_ap_update_tar_cache(ap);
+		if (addrinc)
+			address += this_size;
 	}
 
 	/* REVISIT: Might want to have a queued version of this function that does not run. */
@@ -509,7 +512,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 			break;
 
 		nbytes -= this_size;
-		address += this_size;
+		if (addrinc)
+			address += this_size;
 
 		mem_ap_update_tar_cache(ap);
 	}

From 3de6b5f6e52f46a4b6dcbdeaddb6d75d2bf48519 Mon Sep 17 00:00:00 2001
From: Ake Rehnman <ake.rehnman@gmail.com>
Date: Sat, 21 Jan 2017 10:42:11 +0100
Subject: [PATCH 08/65] jtag/drivers/stlink_usb : implemented and repaired SWIM
 support

Fixed a bug in stlink_usb_read_mem/write_mem preventing large data transfers

The SWIM support in stlink_usb was basically non existent so I have
implemented the missing parts. The bCBWCBLength and dCBWDataTransferLength
for STLINK-V1 protocol was not correct so that was fixed. The reason for
adding SWIM support is to add STM8 support for OpenOCD.

I have tested the driver on:
STM8 discovery board with the built-in STLINK-V1
STM8 discovery board with STLINK-V2 dongle
STM32 vldiscovery board with the built-in STLINK-V1
STM32F1xxx processor with STLINK-V2 dongle

Change-Id: I4aa80a92fb0226174356adaf2f8ff949920a621f
Signed-off-by: Ake Rehnman <ake.rehnman@gmail.com>
Reviewed-on: http://openocd.zylin.com/3952
Tested-by: jenkins
Reviewed-by: Philipp Klaus Krause
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/jtag/drivers/stlink_usb.c | 360 ++++++++++++++++++++++++++++++++--
 1 file changed, 349 insertions(+), 11 deletions(-)

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 64868ea9e..3865b3b7d 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1,4 +1,8 @@
 /***************************************************************************
+ *   SWIM contributions by Ake Rehnman                                     *
+ *   Copyright (C) 2017  Ake Rehnman                                       *
+ *   ake.rehnman(at)gmail.com                                              *
+ *                                                                         *
  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
  *   Mathias Kuester <kesmtp@freenet.de>                                   *
  *                                                                         *
@@ -130,6 +134,8 @@ struct stlink_usb_handle_s {
 	bool reconnect_pending;
 };
 
+#define STLINK_SWIM_ERR_OK             0x00
+#define STLINK_SWIM_BUSY               0x01
 #define STLINK_DEBUG_ERR_OK            0x80
 #define STLINK_DEBUG_ERR_FAULT         0x81
 #define STLINK_SWD_AP_WAIT             0x10
@@ -167,8 +173,36 @@ struct stlink_usb_handle_s {
 
 #define STLINK_DFU_EXIT                0x07
 
-#define STLINK_SWIM_ENTER              0x00
-#define STLINK_SWIM_EXIT               0x01
+/*
+	STLINK_SWIM_ENTER_SEQ
+	1.3ms low then 750Hz then 1.5kHz
+
+	STLINK_SWIM_GEN_RST
+	STM8 DM pulls reset pin low 50us
+
+	STLINK_SWIM_SPEED
+	uint8_t (0=low|1=high)
+
+	STLINK_SWIM_WRITEMEM
+	uint16_t length
+	uint32_t address
+
+	STLINK_SWIM_RESET
+	send syncronization seq (16us low, response 64 clocks low)
+*/
+#define STLINK_SWIM_ENTER                  0x00
+#define STLINK_SWIM_EXIT                   0x01
+#define STLINK_SWIM_READ_CAP               0x02
+#define STLINK_SWIM_SPEED                  0x03
+#define STLINK_SWIM_ENTER_SEQ              0x04
+#define STLINK_SWIM_GEN_RST                0x05
+#define STLINK_SWIM_RESET                  0x06
+#define STLINK_SWIM_ASSERT_RESET           0x07
+#define STLINK_SWIM_DEASSERT_RESET         0x08
+#define STLINK_SWIM_READSTATUS             0x09
+#define STLINK_SWIM_WRITEMEM               0x0a
+#define STLINK_SWIM_READMEM                0x0b
+#define STLINK_SWIM_READBUF                0x0c
 
 #define STLINK_DEBUG_ENTER_JTAG            0x00
 #define STLINK_DEBUG_GETSTATUS             0x01
@@ -252,6 +286,7 @@ static const struct {
 };
 
 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
+static int stlink_swim_status(void *handle);
 
 /** */
 static int stlink_usb_xfer_v1_get_status(void *handle)
@@ -342,7 +377,11 @@ static int stlink_usb_xfer_v1_get_sense(void *handle)
 	return ERROR_OK;
 }
 
-/** */
+/*
+	transfers block in cmdbuf
+	<size> indicates number of bytes in the following
+	data phase.
+*/
 static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 {
 	int err, cmdsize = STLINK_CMD_SIZE_V2;
@@ -350,8 +389,11 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 
 	assert(handle != NULL);
 
-	if (h->version.stlink == 1)
+	if (h->version.stlink == 1) {
 		cmdsize = STLINK_SG_SIZE;
+		/* put length in bCBWCBLength */
+		h->cmdbuf[14] = h->cmdidx-15;
+	}
 
 	err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 
@@ -373,7 +415,6 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
-
 /**
     Converts an STLINK status code held in the first byte of a response
     to an openocd error, logs any error/wait status as debug output.
@@ -388,6 +429,18 @@ static int stlink_usb_error_check(void *handle)
 	if (h->jtag_api == STLINK_JTAG_API_V1)
 		h->databuf[0] = STLINK_DEBUG_ERR_OK;
 
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		switch (h->databuf[0]) {
+			case STLINK_SWIM_ERR_OK:
+				return ERROR_OK;
+			case STLINK_SWIM_BUSY:
+				return ERROR_WAIT;
+			default:
+				LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
+				return ERROR_FAIL;
+		}
+	}
+
 	switch (h->databuf[0]) {
 		case STLINK_DEBUG_ERR_OK:
 			return ERROR_OK;
@@ -448,7 +501,7 @@ static int stlink_usb_error_check(void *handle)
 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
 
     Works for commands where the STLINK_DEBUG status is returned in the first
-    byte of the response packet.
+    byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
 
     Returns an openocd result code.
 */
@@ -456,10 +509,21 @@ static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 {
 	int retries = 0;
 	int res;
+	struct stlink_usb_handle_s *h = handle;
+
 	while (1) {
-		res = stlink_usb_xfer(handle, buf, size);
-		if (res != ERROR_OK)
-			return res;
+		if ((h->transport != HL_TRANSPORT_SWIM) || !retries) {
+			res = stlink_usb_xfer(handle, buf, size);
+			if (res != ERROR_OK)
+				return res;
+		}
+
+		if (h->transport == HL_TRANSPORT_SWIM) {
+			res = stlink_swim_status(handle);
+			if (res != ERROR_OK)
+				return res;
+		}
+
 		res = stlink_usb_error_check(handle);
 		if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 			usleep((1<<retries++) * 1000);
@@ -487,7 +551,17 @@ static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
-/** */
+/*
+	this function writes transfer length in
+	the right place in the cb
+*/
+static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	buf_set_u32(h->cmdbuf+8, 0, 32, size);
+}
+
 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -496,12 +570,16 @@ static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint3
 	strcpy((char *)h->cmdbuf, "USBC");
 	h->cmdidx += 4;
 	/* csw tag not used */
+	buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
 	h->cmdidx += 4;
+	/* cbw data transfer length (in the following data phase in or out) */
 	buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 	h->cmdidx += 4;
+	/* cbw flags */
 	h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
 	h->cmdbuf[h->cmdidx++] = 0; /* lun */
-	h->cmdbuf[h->cmdidx++] = STLINK_CMD_SIZE_V1;
+	/* cdb clength (is filled in at xfer) */
+	h->cmdbuf[h->cmdidx++] = 0;
 }
 
 /** */
@@ -681,6 +759,8 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 		case STLINK_MODE_DEBUG_SWIM:
 			h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 			h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
+			/* no answer for this function... */
+			rx_size = 0;
 			break;
 		case STLINK_MODE_DFU:
 		case STLINK_MODE_MASS:
@@ -825,6 +905,12 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 	}
 
 	if (connect_under_reset) {
+		/* if we are going to use swim we need to switch mode early */
+		if (emode == STLINK_MODE_DEBUG_SWIM) {
+			res = stlink_usb_mode_enter(handle, emode);
+			if (res != ERROR_OK)
+				return res;
+		}
 		res = stlink_usb_assert_srst(handle, 0);
 		if (res != ERROR_OK)
 			return res;
@@ -845,6 +931,199 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 	return ERROR_OK;
 }
 
+/* request status from last swim request */
+static int stlink_swim_status(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 4);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
+	res = stlink_usb_xfer(handle, h->databuf, 4);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+/*
+	the purpose of this function is unknown...
+	capabilites? anyway for swim v6 it returns
+	0001020600000000
+*/
+__attribute__((unused))
+static int stlink_swim_cap(void *handle, uint8_t *cap)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 8);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
+	h->cmdbuf[h->cmdidx++] = 0x01;
+	res = stlink_usb_xfer(handle, h->databuf, 8);
+	if (res != ERROR_OK)
+		return res;
+	memcpy(cap, h->databuf, 8);
+	return ERROR_OK;
+}
+
+/*	debug dongle assert/deassert sreset line */
+static int stlink_swim_assert_reset(void *handle, int reset)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	if (!reset)
+		h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
+	else
+		h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*
+	send swim enter seq
+	1.3ms low then 750Hz then 1.5kHz
+*/
+static int stlink_swim_enter(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*	switch high/low speed swim */
+static int stlink_swim_speed(void *handle, int speed)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
+	if (speed)
+		h->cmdbuf[h->cmdidx++] = 1;
+	else
+		h->cmdbuf[h->cmdidx++] = 0;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*
+	initiate srst from swim.
+	nrst is pulled low for 50us.
+*/
+static int stlink_swim_generate_rst(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*
+	send resyncronize sequence
+	swim is pulled low for 16us
+	reply is 64 clks low
+*/
+static int stlink_swim_resync(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+	unsigned int i;
+	unsigned int datalen = 0;
+	int cmdsize = STLINK_CMD_SIZE_V2;
+
+	if (len > STLINK_DATA_SIZE)
+		return ERROR_FAIL;
+
+	if (h->version.stlink == 1)
+		cmdsize = STLINK_SG_SIZE;
+
+	stlink_usb_init_buffer(handle, h->tx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
+	h_u16_to_be(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+	h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	for (i = 0; i < len; i++) {
+		if (h->cmdidx == cmdsize)
+			h->databuf[datalen++] = *(data++);
+		else
+			h->cmdbuf[h->cmdidx++] = *(data++);
+	}
+	if (h->version.stlink == 1)
+		stlink_usb_set_cbw_transfer_datalength(handle, datalen);
+
+	res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	if (len > STLINK_DATA_SIZE)
+		return ERROR_FAIL;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
+	h_u16_to_be(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+	h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, len);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
+	res = stlink_usb_xfer(handle, data, len);
+	if (res != ERROR_OK)
+		return res;
+
+	return ERROR_OK;
+}
+
 /** */
 static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 {
@@ -853,6 +1132,12 @@ static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 
 	assert(handle != NULL);
 
+	/* there is no swim read core id cmd */
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		*idcode = 0;
+		return ERROR_OK;
+	}
+
 	stlink_usb_init_buffer(handle, h->rx_ep, 4);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -971,6 +1256,18 @@ static enum target_state stlink_usb_state(void *handle)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
+		if (res != ERROR_OK)
+			return TARGET_UNKNOWN;
+
+		res = stlink_swim_resync(handle);
+		if (res != ERROR_OK)
+			return TARGET_UNKNOWN;
+
+		return ERROR_OK;
+	}
+
 	if (h->reconnect_pending) {
 		LOG_INFO("Previous state query failed, trying to reconnect");
 		res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
@@ -1014,6 +1311,9 @@ static int stlink_usb_assert_srst(void *handle, int srst)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM)
+		return stlink_swim_assert_reset(handle, srst);
+
 	if (h->version.stlink == 1)
 		return ERROR_COMMAND_NOTFOUND;
 
@@ -1088,6 +1388,9 @@ static int stlink_usb_reset(void *handle)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM)
+		return stlink_swim_generate_rst(handle);
+
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -1440,6 +1743,11 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 		if (count < bytes_remaining)
 			bytes_remaining = count;
 
+		if (h->transport == HL_TRANSPORT_SWIM) {
+			retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+		} else
 		/* the stlink only supports 8/32bit memory read/writes
 		 * honour 32bit, all others will be handled as 8bit access */
 		if (size == 4) {
@@ -1510,6 +1818,11 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 		if (count < bytes_remaining)
 			bytes_remaining = count;
 
+		if (h->transport == HL_TRANSPORT_SWIM) {
+			retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+		} else
 		/* the stlink only supports 8/32bit memory read/writes
 		 * honour 32bit, all others will be handled as 8bit access */
 		if (size == 4) {
@@ -1574,6 +1887,20 @@ static int stlink_speed(void *handle, int khz, bool query)
 	int speed_diff = INT_MAX;
 	struct stlink_usb_handle_s *h = handle;
 
+	if (h && (h->transport == HL_TRANSPORT_SWIM)) {
+		/*
+			we dont care what the khz rate is
+			we only have low and high speed...
+			before changing speed the SWIM_CSR HS bit
+			must be updated
+		 */
+		if (khz == 0)
+			stlink_swim_speed(handle, 0);
+		else
+			stlink_swim_speed(handle, 1);
+		return khz;
+	}
+
 	/* only supported by stlink/v2 and for firmware >= 22 */
 	if (h && (h->version.stlink == 1 || h->version.jtag < 22))
 		return khz;
@@ -1778,6 +2105,17 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		goto error_open;
 	}
 
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		err = stlink_swim_enter(h);
+		if (err != ERROR_OK) {
+			LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
+			goto error_open;
+		}
+		*fd = h;
+		h->max_mem_packet = STLINK_DATA_SIZE;
+		return ERROR_OK;
+	}
+
 	/* clock speed only supported by stlink/v2 and for firmware >= 22 */
 	if (h->version.stlink >= 2 && h->version.jtag >= 22) {
 		LOG_DEBUG("Supported clock speeds are:");

From a6dea119a3f48ebc2efffdfba960421c9ac43c27 Mon Sep 17 00:00:00 2001
From: Ake Rehnman <ake.rehnman@gmail.com>
Date: Sat, 4 Nov 2017 19:12:17 +0100
Subject: [PATCH 09/65] stlink_usb: Changes to make connect_under_reset work

Leaving mode before closing added
Resolved a collision with JTAG API v1 error/status
checking overwriting SWIM status bytes

Change-Id: I35444ec0e6caace3e072c60085059602f4cdf0a4
Signed-off-by: Ake Rehnman <ake.rehnman@gmail.com>
Reviewed-on: http://openocd.zylin.com/4265
Tested-by: jenkins
Reviewed-by: Philipp Klaus Krause
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/jtag/drivers/stlink_usb.c | 71 +++++++++++++++++++++++++++--------
 1 file changed, 56 insertions(+), 15 deletions(-)

diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 3865b3b7d..6f720b86b 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -425,10 +425,6 @@ static int stlink_usb_error_check(void *handle)
 
 	assert(handle != NULL);
 
-	/* TODO: no error checking yet on api V1 */
-	if (h->jtag_api == STLINK_JTAG_API_V1)
-		h->databuf[0] = STLINK_DEBUG_ERR_OK;
-
 	if (h->transport == HL_TRANSPORT_SWIM) {
 		switch (h->databuf[0]) {
 			case STLINK_SWIM_ERR_OK:
@@ -441,6 +437,10 @@ static int stlink_usb_error_check(void *handle)
 		}
 	}
 
+	/* TODO: no error checking yet on api V1 */
+	if (h->jtag_api == STLINK_JTAG_API_V1)
+		h->databuf[0] = STLINK_DEBUG_ERR_OK;
+
 	switch (h->databuf[0]) {
 		case STLINK_DEBUG_ERR_OK:
 			return ERROR_OK;
@@ -904,23 +904,29 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 		return ERROR_FAIL;
 	}
 
+	/* preliminary SRST assert:
+	 * We want SRST is asserted before activating debug signals (mode_enter).
+	 * As the required mode has not been set, the adapter may not know what pin to use.
+	 * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
+	 * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
+	 * after power on, SWIM_RST stays unchanged */
+	if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
+		stlink_usb_assert_srst(handle, 0);
+		/* do not check the return status here, we will
+		   proceed and enter the desired mode below
+		   and try asserting srst again. */
+
+	res = stlink_usb_mode_enter(handle, emode);
+	if (res != ERROR_OK)
+		return res;
+
+	/* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
 	if (connect_under_reset) {
-		/* if we are going to use swim we need to switch mode early */
-		if (emode == STLINK_MODE_DEBUG_SWIM) {
-			res = stlink_usb_mode_enter(handle, emode);
-			if (res != ERROR_OK)
-				return res;
-		}
 		res = stlink_usb_assert_srst(handle, 0);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-	res = stlink_usb_mode_enter(handle, emode);
-
-	if (res != ERROR_OK)
-		return res;
-
 	res = stlink_usb_current_mode(handle, &mode);
 
 	if (res != ERROR_OK)
@@ -1949,8 +1955,43 @@ static int stlink_speed(void *handle, int khz, bool query)
 /** */
 static int stlink_usb_close(void *handle)
 {
+	int res;
+	uint8_t mode;
+	enum stlink_mode emode;
 	struct stlink_usb_handle_s *h = handle;
 
+	if (h && h->fd)
+		res = stlink_usb_current_mode(handle, &mode);
+	else
+		res = ERROR_FAIL;
+	/* do not exit if return code != ERROR_OK,
+	   it prevents us from closing jtag_libusb */
+
+	if (res == ERROR_OK) {
+		/* try to exit current mode */
+		switch (mode) {
+			case STLINK_DEV_DFU_MODE:
+				emode = STLINK_MODE_DFU;
+				break;
+			case STLINK_DEV_DEBUG_MODE:
+				emode = STLINK_MODE_DEBUG_SWD;
+				break;
+			case STLINK_DEV_SWIM_MODE:
+				emode = STLINK_MODE_DEBUG_SWIM;
+				break;
+			case STLINK_DEV_BOOTLOADER_MODE:
+			case STLINK_DEV_MASS_MODE:
+			default:
+				emode = STLINK_MODE_UNKNOWN;
+				break;
+		}
+
+		if (emode != STLINK_MODE_UNKNOWN)
+			stlink_usb_mode_leave(handle, emode);
+			/* do not check return code, it prevent
+			us from closing jtag_libusb */
+	}
+
 	if (h && h->fd)
 		jtag_libusb_close(h->fd);
 

From cf2b9cbf5c34c8f71cc84d62db7d12a671eceb55 Mon Sep 17 00:00:00 2001
From: Ake Rehnman <ake.rehnman@gmail.com>
Date: Tue, 24 Oct 2017 20:50:39 +0200
Subject: [PATCH 10/65] Added config files for stm8l152 stm8s003 and stm8s105

Change-Id: I26cc401aafac01e5aed8eac605488da5221ffdc2
Signed-off-by: Ake Rehnman <ake.rehnman@gmail.com>
Reviewed-on: http://openocd.zylin.com/4268
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 tcl/target/stm8l152.cfg | 12 ++++++++++++
 tcl/target/stm8s003.cfg | 11 +++++++++++
 tcl/target/stm8s105.cfg |  8 ++++++++
 3 files changed, 31 insertions(+)
 create mode 100644 tcl/target/stm8l152.cfg
 create mode 100644 tcl/target/stm8s003.cfg
 create mode 100644 tcl/target/stm8s105.cfg

diff --git a/tcl/target/stm8l152.cfg b/tcl/target/stm8l152.cfg
new file mode 100644
index 000000000..8545a5ab2
--- /dev/null
+++ b/tcl/target/stm8l152.cfg
@@ -0,0 +1,12 @@
+#config script for STM8L152
+
+set EEPROMSTART 0x1000
+set EEPROMEND 0x13ff
+
+proc stm8_reset_rop {} {
+   mwb 0x4800 0xaa
+   mwb 0x4800 0xaa
+   reset halt
+}
+
+source [find target/stm8l.cfg]
diff --git a/tcl/target/stm8s003.cfg b/tcl/target/stm8s003.cfg
new file mode 100644
index 000000000..34997bec2
--- /dev/null
+++ b/tcl/target/stm8s003.cfg
@@ -0,0 +1,11 @@
+#config script for STM8S003
+
+set FLASHEND 0x9FFF
+set BLOCKSIZE 0x40
+
+proc stm8_reset_rop {} {
+   mwb 0x4800 0x00
+   reset halt
+}
+
+source [find target/stm8s.cfg]
diff --git a/tcl/target/stm8s105.cfg b/tcl/target/stm8s105.cfg
new file mode 100644
index 000000000..820bcf75f
--- /dev/null
+++ b/tcl/target/stm8s105.cfg
@@ -0,0 +1,8 @@
+#config script for STM8S105
+
+proc stm8_reset_rop {} {
+   mwb 0x4800 0x00
+   reset halt
+}
+
+source [find target/stm8s.cfg]

From 133e01d2af2a27d4f04c19285a3d561c5cfc92d6 Mon Sep 17 00:00:00 2001
From: Pushpal Sidhu <psidhu.devel@gmail.com>
Date: Tue, 5 Dec 2017 09:36:17 -0800
Subject: [PATCH 11/65] board: consolidate st_nucleo_l4 boards

We can now use the generic stlink.cfg which allows for both ST-LINK/V2
and V2-1 debuggers.

Change-Id: I229c6fe5f6a6a4f2d3c787a49939846f102f9e24
Signed-off-by: Pushpal Sidhu <psidhu.devel@gmail.com>
Reviewed-on: http://openocd.zylin.com/4313
Tested-by: jenkins
Reviewed-by: Karl Palsson <karlp@tweak.net.au>
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 tcl/board/{st_nucleo_l476rg.cfg => st_nucleo_l4.cfg} | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)
 rename tcl/board/{st_nucleo_l476rg.cfg => st_nucleo_l4.cfg} (51%)

diff --git a/tcl/board/st_nucleo_l476rg.cfg b/tcl/board/st_nucleo_l4.cfg
similarity index 51%
rename from tcl/board/st_nucleo_l476rg.cfg
rename to tcl/board/st_nucleo_l4.cfg
index 4426c3bc9..1ab9da9b4 100644
--- a/tcl/board/st_nucleo_l476rg.cfg
+++ b/tcl/board/st_nucleo_l4.cfg
@@ -1,5 +1,5 @@
-# This is a ST NUCLEO L476RG board with a single STM32L476RGT6 chip.
-# http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1847/PF261636
+# Should work with all STM32L4 Nucleo Dev Boards.
+# http://www.st.com/en/evaluation-tools/stm32-mcu-nucleo.html
 
 source [find interface/stlink.cfg]
 
@@ -9,4 +9,3 @@ source [find target/stm32l4x.cfg]
 
 # use hardware reset
 reset_config srst_only srst_nogate
-

From b34e013965777071aa88e1a1d7f4571dc267929e Mon Sep 17 00:00:00 2001
From: Aleksey Shargalin <myokaski@gmail.com>
Date: Tue, 31 Oct 2017 16:56:01 +0300
Subject: [PATCH 12/65] remote_bitbang_sysfsgpio: fix reset handling

When both SRST and TRST asserted, 'u' is sent to remote bitbang.
Fix for correct handling of such a case

Change-Id: I2a93ff71f5bbae658e6c0c3649a9fbcca2c5a14b
Signed-off-by: Aleksey Shargalin <myokaski@gmail.com>
Reviewed-on: http://openocd.zylin.com/4283
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 contrib/remote_bitbang/remote_bitbang_sysfsgpio.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c b/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
index e59a1bdeb..664130703 100644
--- a/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
+++ b/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
@@ -302,7 +302,7 @@ static void process_remote_protocol(void)
 			break;
 		else if (c == 'b' || c == 'B') /* Blink */
 			continue;
-		else if (c >= 'r' && c <= 'r' + 2) { /* Reset */
+		else if (c >= 'r' && c <= 'r' + 3) { /* Reset */
 			char d = c - 'r';
 			sysfsgpio_reset(!!(d & 2),
 					(d & 1));

From c652f44ca3e7fde093f1fd1ead8228aca4dcd9c4 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Fri, 6 Oct 2017 19:39:21 +0300
Subject: [PATCH 13/65] flash: startup.tcl: do not disable polling when not
 exiting

Change-Id: I31b8a8b4519d65d6587207a71eb08dcee8ddd6fd
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4243
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Tested-by: jenkins
---
 src/flash/startup.tcl | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/flash/startup.tcl b/src/flash/startup.tcl
index fbb8d8ee4..ed905e942 100644
--- a/src/flash/startup.tcl
+++ b/src/flash/startup.tcl
@@ -62,8 +62,10 @@ proc program {filename args} {
 
 		if {[info exists reset]} {
 			# reset target if requested
-			# also disable target polling, we are shutting down anyway
-			poll off
+			if {$exit == 1} {
+				# also disable target polling, we are shutting down anyway
+				poll off
+			}
 			echo "** Resetting Target **"
 			reset run
 		}

From de974eaed3d88d0b1c3a4586ae5f7b49d24b2677 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Fri, 2 Dec 2016 19:04:17 +0300
Subject: [PATCH 14/65] target: arm: disassembler: add v5T Thumb BLX decoding

Reported by thesourcerer on IRC.

Change-Id: I36d4d95de176a19fdd0bf80d8b419be6ca637e98
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3893
Tested-by: jenkins
---
 src/target/arm_disassembler.c | 25 +++++++++++++++++++------
 1 file changed, 19 insertions(+), 6 deletions(-)

diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index 3f1daca4d..f0266d023 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -2896,12 +2896,26 @@ static int t2ev_b_bl(uint32_t opcode, uint32_t address,
 	address += 4;
 	address += offset << 1;
 
-	instruction->type = (opcode & (1 << 14)) ? ARM_BL : ARM_B;
+	char *inst;
+	switch ((opcode >> 12) & 0x5) {
+	case 0x1:
+		inst = "B.W";
+		instruction->type = ARM_B;
+		break;
+	case 0x4:
+		inst = "BLX";
+		instruction->type = ARM_BLX;
+		break;
+	case 0x5:
+		inst = "BL";
+		instruction->type = ARM_BL;
+		break;
+	default:
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
 	instruction->info.b_bl_bx_blx.reg_operand = -1;
 	instruction->info.b_bl_bx_blx.target_address = address;
-	sprintf(cp, "%s\t%#8.8" PRIx32,
-			(opcode & (1 << 14)) ? "BL" : "B.W",
-			address);
+	sprintf(cp, "%s\t%#8.8" PRIx32, inst, address);
 
 	return ERROR_OK;
 }
@@ -3078,10 +3092,9 @@ static int t2ev_b_misc(uint32_t opcode, uint32_t address,
 
 	switch ((opcode >> 12) & 0x5) {
 		case 0x1:
+		case 0x4:
 		case 0x5:
 			return t2ev_b_bl(opcode, address, instruction, cp);
-		case 0x4:
-			goto undef;
 		case 0:
 			if (((opcode >> 23) & 0x07) != 0x07)
 				return t2ev_cond_b(opcode, address, instruction, cp);

From c26bbf7a1b2a11e9cc369536332bff2faa223da8 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Fri, 2 Dec 2016 20:03:07 +0300
Subject: [PATCH 15/65] target: arm: disassembler: decode v6T2 ARM DSB
 instruction

Change-Id: Id91b1a87d34982c72f2a8ab46564c961d1fef9dc
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3894
Tested-by: jenkins
---
 src/target/arm_disassembler.c | 41 +++++++++++++++++++++++++++++++++++
 src/target/arm_disassembler.h |  1 +
 2 files changed, 42 insertions(+)

diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index f0266d023..153667936 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -129,6 +129,47 @@ static int evaluate_pld(uint32_t opcode,
 
 		return ERROR_OK;
 	}
+	/* DSB */
+	if ((opcode & 0x07f000f0) == 0x05700040) {
+		instruction->type = ARM_DSB;
+
+		char *opt;
+		switch (opcode & 0x0000000f) {
+		case 0xf:
+			opt = "SY";
+			break;
+		case 0xe:
+			opt = "ST";
+			break;
+		case 0xb:
+			opt = "ISH";
+			break;
+		case 0xa:
+			opt = "ISHST";
+			break;
+		case 0x7:
+			opt = "NSH";
+			break;
+		case 0x6:
+			opt = "NSHST";
+			break;
+		case 0x3:
+			opt = "OSH";
+			break;
+		case 0x2:
+			opt = "OSHST";
+			break;
+		default:
+			opt = "UNK";
+		}
+
+		snprintf(instruction->text,
+				128,
+				"0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tDSB %s",
+				address, opcode, opt);
+
+		return ERROR_OK;
+	}
 	return evaluate_unknown(opcode, address, instruction);
 }
 
diff --git a/src/target/arm_disassembler.h b/src/target/arm_disassembler.h
index 6f8f65d44..b73f24a89 100644
--- a/src/target/arm_disassembler.h
+++ b/src/target/arm_disassembler.h
@@ -106,6 +106,7 @@ enum arm_instruction_type {
 	ARM_MCRR,
 	ARM_MRRC,
 	ARM_PLD,
+	ARM_DSB,
 	ARM_QADD,
 	ARM_QDADD,
 	ARM_QSUB,

From 2420aa00a458c6847faca2d6b7f9d99718d693f2 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Fri, 2 Dec 2016 20:15:46 +0300
Subject: [PATCH 16/65] target: arm: disassembler: decode v6T2 ARM ISB
 instruction

Change-Id: Iaaa54aee6a74f0b250b83c53e7a3fb7c17718920
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3895
Tested-by: jenkins
---
 src/target/arm_disassembler.c | 12 ++++++++++++
 src/target/arm_disassembler.h |  1 +
 2 files changed, 13 insertions(+)

diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index 153667936..f432f57ca 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -170,6 +170,18 @@ static int evaluate_pld(uint32_t opcode,
 
 		return ERROR_OK;
 	}
+	/* ISB */
+	if ((opcode & 0x07f000f0) == 0x05700060) {
+		instruction->type = ARM_ISB;
+
+		snprintf(instruction->text,
+				128,
+				"0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tISB %s",
+				address, opcode,
+				((opcode & 0x0000000f) == 0xf) ? "SY" : "UNK");
+
+		return ERROR_OK;
+	}
 	return evaluate_unknown(opcode, address, instruction);
 }
 
diff --git a/src/target/arm_disassembler.h b/src/target/arm_disassembler.h
index b73f24a89..e9f4d44cb 100644
--- a/src/target/arm_disassembler.h
+++ b/src/target/arm_disassembler.h
@@ -107,6 +107,7 @@ enum arm_instruction_type {
 	ARM_MRRC,
 	ARM_PLD,
 	ARM_DSB,
+	ARM_ISB,
 	ARM_QADD,
 	ARM_QDADD,
 	ARM_QSUB,

From fe577e0b6333a8a857a1fa78ea788413d4f8bef6 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Fri, 2 Dec 2016 21:54:10 +0300
Subject: [PATCH 17/65] target: arm: disassembler: decode v6T2 ARM MOV{W,T}
 instructions

Change-Id: I32cf2669b1b22d4142f30674cf918e36561a885e
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3899
Tested-by: jenkins
---
 src/target/arm_disassembler.c | 40 ++++++++++++++++++++++++++---------
 1 file changed, 30 insertions(+), 10 deletions(-)

diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index f432f57ca..ef69a203c 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -1615,6 +1615,33 @@ static int evaluate_misc_instr(uint32_t opcode,
 	return ERROR_OK;
 }
 
+static int evaluate_mov_imm(uint32_t opcode,
+			      uint32_t address, struct arm_instruction *instruction)
+{
+	uint16_t immediate;
+	uint8_t Rd;
+	bool T;
+
+	Rd = (opcode & 0xf000) >> 12;
+	T = opcode & 0x00400000;
+	immediate = (opcode & 0xf0000) >> 4 | (opcode & 0xfff);
+
+	instruction->type = ARM_MOV;
+	instruction->info.data_proc.Rd = Rd;
+
+	snprintf(instruction->text,
+		 128,
+		 "0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tMOV%s%s r%i, #0x%" PRIx16,
+		 address,
+		 opcode,
+		 T ? "T" : "W",
+		 COND(opcode),
+		 Rd,
+		 immediate);
+
+	return ERROR_OK;
+}
+
 static int evaluate_data_proc(uint32_t opcode,
 			      uint32_t address, struct arm_instruction *instruction)
 {
@@ -1891,16 +1918,9 @@ int arm_evaluate_opcode(uint32_t opcode, uint32_t address,
 
 	/* catch opcodes with [27:25] = b001 */
 	if ((opcode & 0x0e000000) == 0x02000000) {
-		/* Undefined instruction */
-		if ((opcode & 0x0fb00000) == 0x03000000) {
-			instruction->type = ARM_UNDEFINED_INSTRUCTION;
-			snprintf(instruction->text,
-					128,
-					"0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tUNDEFINED INSTRUCTION",
-					address,
-					opcode);
-			return ERROR_OK;
-		}
+		/* 16-bit immediate load */
+		if ((opcode & 0x0fb00000) == 0x03000000)
+			return evaluate_mov_imm(opcode, address, instruction);
 
 		/* Move immediate to status register */
 		if ((opcode & 0x0fb00000) == 0x03200000)

From 85ac081c221b0b3d0664f318d05d9b200f8f61da Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sat, 31 Dec 2016 21:01:52 +0300
Subject: [PATCH 18/65] jtag: drivers: sysfsgpio, bcm2835gpio: remove
 requirement to have trst or srst

This doesn't seem to make any sense.

Change-Id: I272de0ea3c0e86cc9a798cbd864050aaa597c3f0
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3933
Tested-by: jenkins
---
 src/jtag/drivers/bcm2835gpio.c | 4 ----
 src/jtag/drivers/sysfsgpio.c   | 4 ----
 2 files changed, 8 deletions(-)

diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index a41caf073..a4d0a1c8e 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -432,10 +432,6 @@ static int bcm2835gpio_init(void)
 			LOG_INFO("JTAG and SWD modes enabled");
 		else
 			LOG_INFO("JTAG only mode enabled (specify swclk and swdio gpio to add SWD mode)");
-		if (!is_gpio_valid(trst_gpio) && !is_gpio_valid(srst_gpio)) {
-			LOG_ERROR("Require at least one of trst or srst gpios to be specified");
-			return ERROR_JTAG_INIT_FAILED;
-		}
 	} else if (bcm2835gpio_swd_mode_possible()) {
 		LOG_INFO("SWD only mode enabled (specify tck, tms, tdi and tdo gpios to add JTAG mode)");
 	} else {
diff --git a/src/jtag/drivers/sysfsgpio.c b/src/jtag/drivers/sysfsgpio.c
index 77b727c27..b8acfa2fc 100644
--- a/src/jtag/drivers/sysfsgpio.c
+++ b/src/jtag/drivers/sysfsgpio.c
@@ -592,10 +592,6 @@ static int sysfsgpio_init(void)
 			LOG_INFO("JTAG and SWD modes enabled");
 		else
 			LOG_INFO("JTAG only mode enabled (specify swclk and swdio gpio to add SWD mode)");
-		if (!is_gpio_valid(trst_gpio) && !is_gpio_valid(srst_gpio)) {
-			LOG_ERROR("Require at least one of trst or srst gpios to be specified");
-			return ERROR_JTAG_INIT_FAILED;
-		}
 	} else if (sysfsgpio_swd_mode_possible()) {
 		LOG_INFO("SWD only mode enabled (specify tck, tms, tdi and tdo gpios to add JTAG mode)");
 	} else {

From 3cb7fd4a606c858313f94a29120fe68b06ed967d Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sun, 15 Jan 2017 18:40:54 +0300
Subject: [PATCH 19/65] tools: release.sh: produce only SHA256 hashes

Both MD5 and SHA-1 shouldn't be relied upon since long.

Change-Id: I0dea92fbf0f68b900c0d3a314fb3956d08ba0d48
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3949
Tested-by: jenkins
---
 tools/release.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/tools/release.sh b/tools/release.sh
index abd721a17..ac8af646e 100755
--- a/tools/release.sh
+++ b/tools/release.sh
@@ -102,7 +102,7 @@ do_stage() {
 	for EXT in tar.gz tar.bz2 zip; do
 		local FILE="${PACKAGE_RELEASE}.${EXT}"
 		# create archive signatures
-		for HASH in md5 sha1; do
+		for HASH in sha256; do
 			echo "sign: ${FILE}.${HASH}"
 			${HASH}sum "${FILE}" > "archives/${FILE}.${HASH}"
 		done

From b95e0c13aa64ebbc8407ce8b974b1efd44df84c9 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Tue, 28 Mar 2017 13:23:43 +0300
Subject: [PATCH 20/65] flash/startup: make program accept filenames with
 spaces and other characters

This should allow to process any path names excluding those that have
curly braces.

Change-Id: I87bf9ddede11e2b28d5826878eb1338143f73c03
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4083
Tested-by: jenkins
---
 src/flash/startup.tcl | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/flash/startup.tcl b/src/flash/startup.tcl
index ed905e942..ff053ae1b 100644
--- a/src/flash/startup.tcl
+++ b/src/flash/startup.tcl
@@ -42,6 +42,7 @@ proc program {filename args} {
 
 	# start programming phase
 	echo "** Programming Started **"
+	set filename \{$filename\}
 	if {[info exists address]} {
 		set flash_args "$filename $address"
 	} else {

From 12a838c6d4b9232e495285edee76636f87cf32e5 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Wed, 17 May 2017 11:03:19 +0300
Subject: [PATCH 21/65] server: gdb: always print target information in
 multi-target setups

When multiple targets are declared, it's not always obvious which
target the connection was made for, this can lead to very confusing
errors.

Reported by zjason on IRC.

Change-Id: I52906320394e89cb6cfe82054a3f94b27c999689
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4135
Tested-by: jenkins
---
 src/server/gdb_server.c | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index 626179c3c..fc0868b0a 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -991,7 +991,9 @@ static int gdb_new_connection(struct connection *connection)
 	}
 
 	gdb_actual_connections++;
-	LOG_DEBUG("New GDB Connection: %d, Target %s, state: %s",
+	log_printf_lf(all_targets->next != NULL ? LOG_LVL_INFO : LOG_LVL_DEBUG,
+			__FILE__, __LINE__, __func__,
+			"New GDB Connection: %d, Target %s, state: %s",
 			gdb_actual_connections,
 			target_name(target),
 			target_state_name(target));

From 99b03d3bfbde879f07b02990c9d8bec3b0ee3927 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Thu, 21 Dec 2017 13:40:29 +0300
Subject: [PATCH 22/65] doc: add SVF optional arguments descriptions

Change-Id: I1a3e5b04b6bc4821c4d11c8fcb33b426d6d41bfb
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4314
Tested-by: jenkins
Reviewed-by: Jim Norris <u17263@att.net>
Reviewed-by: Karl Palsson <karlp@tweak.net.au>
---
 doc/openocd.texi | 22 +++++++++++++++++++---
 1 file changed, 19 insertions(+), 3 deletions(-)

diff --git a/doc/openocd.texi b/doc/openocd.texi
index ebd03c4cb..8ff9010a6 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -8779,11 +8779,27 @@ way to represent JTAG test patterns in text files.
 In a debug session using JTAG for its transport protocol,
 OpenOCD supports running such test files.
 
-@deffn Command {svf} filename [@option{quiet}]
+@deffn Command {svf} @file{filename} [@option{-tap @var{tapname}}] [@option{[-]quiet}] @
+                     [@option{[-]nil}] [@option{[-]progress}] [@option{[-]ignore_error}]
 This issues a JTAG reset (Test-Logic-Reset) and then
 runs the SVF script from @file{filename}.
-Unless the @option{quiet} option is specified,
-each command is logged before it is executed.
+
+Arguments can be specified in any order; the optional dash doesn't
+affect their semantics.
+
+Command options:
+@itemize @minus
+@item @option{-tap @var{tapname}} ignore IR and DR headers and footers
+specified by the SVF file with HIR, TIR, HDR and TDR commands;
+instead, calculate them automatically according to the current JTAG
+chain configuration, targetting @var{tapname};
+@item @option{[-]quiet} do not log every command before execution;
+@item @option{[-]nil} ``dry run'', i.e., do not perform any operations
+on the real interface;
+@item @option{[-]progress} enable progress indication;
+@item @option{[-]ignore_error} continue execution despite TDO check
+errors.
+@end itemize
 @end deffn
 
 @section XSVF: Xilinx Serial Vector Format

From dc28a6e593cc53117ba9b8e6e7ab9d20f49c5e4b Mon Sep 17 00:00:00 2001
From: barthess <barthess@yandex.ru>
Date: Sun, 11 Dec 2016 15:11:41 +0300
Subject: [PATCH 23/65] XCF (Xilinx platfrom flash) support.

Change-Id: I4ee6db5f0abdb9fd279cc0edd13f71952a9d295d
Signed-off-by: Uladzimir Pylinski <barthess@yandex.ru>
Reviewed-on: http://openocd.zylin.com/3914
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 doc/openocd.texi          |  46 ++
 src/flash/nor/Makefile.am |   1 +
 src/flash/nor/drivers.c   |   2 +
 src/flash/nor/xcf.c       | 897 ++++++++++++++++++++++++++++++++++++++
 tcl/cpld/xilinx-xcf-p.cfg |  18 +
 tcl/cpld/xilinx-xcf-s.cfg |  18 +
 6 files changed, 982 insertions(+)
 create mode 100644 src/flash/nor/xcf.c
 create mode 100644 tcl/cpld/xilinx-xcf-p.cfg
 create mode 100644 tcl/cpld/xilinx-xcf-s.cfg

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 8ff9010a6..7f5b72e07 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -4926,6 +4926,52 @@ flash bank $_FLASHNAME spi 0x0 0 0 0 \
 @end example
 @end deffn
 
+@deffn {Flash Driver} xcf
+@cindex Xilinx Platform flash driver
+@cindex xcf
+Xilinx FPGAs can be configured from specialized flash ICs named Platform Flash.
+It is (almost) regular NOR flash with erase sectors, program pages, etc. The
+only difference is special registers controlling its FPGA specific behavior.
+They must be properly configured for successful FPGA loading using
+additional @var{xcf} driver command:
+
+@deffn Command {xcf ccb} <bank_id>
+command accepts additional parameters:
+@itemize
+@item @var{external|internal} ... selects clock source.
+@item @var{serial|parallel} ... selects serial or parallel data bus mode.
+@item @var{slave|master} ... selects slave of master mode for flash device.
+@item @var{40|20} ... selects clock frequency in MHz for internal clock
+in master mode.
+@end itemize
+@example
+xcf ccb 0 external parallel slave 40
+@end example
+All of them must be specified even if clock frequency is pointless
+in slave mode. If only bank id specified than command prints current
+CCB register value. Note: there is no need to write this register
+every time you erase/program data sectors because it stores in
+dedicated sector.
+@end deffn
+
+@deffn Command {xcf configure} <bank_id>
+Initiates FPGA loading procedure. Useful if your board has no "configure"
+button.
+@example
+xcf configure 0
+@end example
+@end deffn
+
+Additional driver notes:
+@itemize
+@item Only single revision supported.
+@item Driver automatically detects need of bit reverse, but
+only "bin" (raw binary, do not confuse it with "bit") and "mcs"
+(Intel hex) file types supported.
+@item For additional info check xapp972.pdf and ug380.pdf.
+@end itemize
+@end deffn
+
 @deffn {Flash Driver} lpcspifi
 @cindex NXP SPI Flash Interface
 @cindex SPIFI
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index ebf47756f..6dc61e62f 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -55,6 +55,7 @@ NOR_DRIVERS = \
 	%D%/str9xpec.c \
 	%D%/tms470.c \
 	%D%/virtual.c \
+	%D%/xcf.c \
 	%D%/xmc1xxx.c \
 	%D%/xmc4xxx.c
 
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index eacca0346..3b055d19a 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -67,6 +67,7 @@ extern struct flash_driver str9x_flash;
 extern struct flash_driver str9xpec_flash;
 extern struct flash_driver tms470_flash;
 extern struct flash_driver virtual_flash;
+extern struct flash_driver xcf_flash;
 extern struct flash_driver xmc1xxx_flash;
 extern struct flash_driver xmc4xxx_flash;
 
@@ -122,6 +123,7 @@ static struct flash_driver *flash_drivers[] = {
 	&str9xpec_flash,
 	&tms470_flash,
 	&virtual_flash,
+	&xcf_flash,
 	&xmc1xxx_flash,
 	&xmc4xxx_flash,
 	NULL,
diff --git a/src/flash/nor/xcf.c b/src/flash/nor/xcf.c
new file mode 100644
index 000000000..035791eb3
--- /dev/null
+++ b/src/flash/nor/xcf.c
@@ -0,0 +1,897 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Uladzimir Pylinski aka barthess                 *
+ *   barthess@yandex.ru                                                    *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <string.h>
+
+#include "imp.h"
+#include <jtag/jtag.h>
+#include <helper/time_support.h>
+
+/*
+ ******************************************************************************
+ * DEFINES
+ ******************************************************************************
+ */
+
+#define SECTOR_ERASE_TIMEOUT_MS         (35 * 1000)
+
+#define XCF_PAGE_SIZE                   32
+#define XCF_DATA_SECTOR_SIZE            (1024 * 1024)
+
+#define ID_XCF01S                       0x05044093
+#define ID_XCF02S                       0x05045093
+#define ID_XCF04S                       0x05046093
+#define ID_XCF08P                       0x05057093
+#define ID_XCF16P                       0x05058093
+#define ID_XCF32P                       0x05059093
+#define ID_MEANINGFUL_MASK              0x0FFFFFFF
+
+const char *xcf_name_list[] = {
+	"XCF08P",
+	"XCF16P",
+	"XCF32P",
+	"unknown"
+};
+
+struct xcf_priv {
+	bool probed;
+};
+
+struct xcf_status {
+	bool isc_error;		/* false == OK, true == error */
+	bool prog_error;	/* false == OK, true == error */
+	bool prog_busy;		/* false == idle, true == busy */
+	bool isc_mode;		/* false == normal mode, true == ISC mode */
+};
+
+/*
+ ******************************************************************************
+ * GLOBAL VARIABLES
+ ******************************************************************************
+ */
+static const uint8_t CMD_BYPASS[2]              = {0xFF, 0xFF};
+
+static const uint8_t CMD_ISC_ADDRESS_SHIFT[2]   = {0xEB, 0x00};
+static const uint8_t CMD_ISC_DATA_SHIFT[2]      = {0xED, 0x00};
+static const uint8_t CMD_ISC_DISABLE[2]         = {0xF0, 0x00};
+static const uint8_t CMD_ISC_ENABLE[2]          = {0xE8, 0x00};
+static const uint8_t CMD_ISC_ERASE[2]           = {0xEC, 0x00};
+static const uint8_t CMD_ISC_PROGRAM[2]         = {0xEA, 0x00};
+
+static const uint8_t CMD_XSC_BLANK_CHECK[2]     = {0x0D, 0x00};
+static const uint8_t CMD_XSC_CONFIG[2]          = {0xEE, 0x00};
+static const uint8_t CMD_XSC_DATA_BTC[2]        = {0xF2, 0x00};
+static const uint8_t CMD_XSC_DATA_CCB[2]        = {0x0C, 0x00};
+static const uint8_t CMD_XSC_DATA_DONE[2]       = {0x09, 0x00};
+static const uint8_t CMD_XSC_DATA_SUCR[2]       = {0x0E, 0x00};
+static const uint8_t CMD_XSC_DATA_WRPT[2]       = {0xF7, 0x00};
+static const uint8_t CMD_XSC_OP_STATUS[2]       = {0xE3, 0x00};
+static const uint8_t CMD_XSC_READ[2]            = {0xEF, 0x00};
+static const uint8_t CMD_XSC_UNLOCK[2]          = {0x55, 0xAA};
+
+/*
+ ******************************************************************************
+ * LOCAL FUNCTIONS
+ ******************************************************************************
+ */
+
+static const char *product_name(const struct flash_bank *bank)
+{
+
+	switch (bank->target->tap->idcode & ID_MEANINGFUL_MASK) {
+		case ID_XCF08P:
+			return xcf_name_list[0];
+		case ID_XCF16P:
+			return xcf_name_list[1];
+		case ID_XCF32P:
+			return xcf_name_list[2];
+		default:
+			return xcf_name_list[3];
+	}
+}
+
+static void fill_sector_table(struct flash_bank *bank)
+{
+	/* Note: is_erased and is_protected fields must be set here to an unknown
+	 * state, they will be correctly filled from other API calls. */
+
+	int i = 0;
+
+	for (i = 0; i < bank->num_sectors; i++) {
+		bank->sectors[i].is_erased              = -1;
+		bank->sectors[i].is_protected   = -1;
+	}
+	for (i = 0; i < bank->num_sectors; i++) {
+		bank->sectors[i].size   = XCF_DATA_SECTOR_SIZE;
+		bank->sectors[i].offset = i * XCF_DATA_SECTOR_SIZE;
+	}
+
+	bank->size = bank->num_sectors * XCF_DATA_SECTOR_SIZE;
+}
+
+static struct xcf_status read_status(struct flash_bank *bank)
+{
+	struct xcf_status ret;
+	uint8_t irdata[2];
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_BYPASS;
+	scan.in_value = irdata;
+
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+	jtag_execute_queue();
+
+	ret.isc_error   = ((irdata[0] >> 7) & 3) == 0b01;
+	ret.prog_error  = ((irdata[0] >> 5) & 3) == 0b01;
+	ret.prog_busy   = ((irdata[0] >> 4) & 1) == 0;
+	ret.isc_mode    = ((irdata[0] >> 3) & 1) == 1;
+
+	return ret;
+}
+
+static int isc_enter(struct flash_bank *bank)
+{
+
+	struct xcf_status status = read_status(bank);
+
+	if (true == status.isc_mode)
+		return ERROR_OK;
+	else {
+		struct scan_field scan;
+
+		scan.check_mask = NULL;
+		scan.check_value = NULL;
+		scan.num_bits = 16;
+		scan.out_value = CMD_ISC_ENABLE;
+		scan.in_value = NULL;
+
+		jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+		jtag_execute_queue();
+
+		status = read_status(bank);
+		if (false == status.isc_mode) {
+			LOG_ERROR("*** XCF: FAILED to enter ISC mode");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		return ERROR_OK;
+	}
+}
+
+static int isc_leave(struct flash_bank *bank)
+{
+
+	struct xcf_status status = read_status(bank);
+
+	if (false == status.isc_mode)
+		return ERROR_OK;
+	else {
+		struct scan_field scan;
+
+		scan.check_mask = NULL;
+		scan.check_value = NULL;
+		scan.num_bits = 16;
+		scan.out_value = CMD_ISC_DISABLE;
+		scan.in_value = NULL;
+
+		jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+		jtag_execute_queue();
+		alive_sleep(1);	/* device needs 50 uS to leave ISC mode */
+
+		status = read_status(bank);
+		if (true == status.isc_mode) {
+			LOG_ERROR("*** XCF: FAILED to leave ISC mode");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		return ERROR_OK;
+	}
+}
+
+static int sector_state(uint8_t wrpt, int sector)
+{
+	if (((wrpt >> sector) & 1) == 1)
+		return 0;
+	else
+		return 1;
+}
+
+static uint8_t fill_select_block(int first, int last)
+{
+	uint8_t ret = 0;
+	for (int i = first; i <= last; i++)
+		ret |= 1 << i;
+	return ret;
+}
+
+static int isc_read_register(struct flash_bank *bank, const uint8_t *cmd,
+	uint8_t *data_buf, int num_bits)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.out_value = cmd;
+	scan.in_value = NULL;
+	scan.num_bits = 16;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_DRSHIFT);
+
+	scan.out_value = NULL;
+	scan.in_value = data_buf;
+	scan.num_bits = num_bits;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+
+	return jtag_execute_queue();
+}
+
+static int isc_wait_erase_program(struct flash_bank *bank, int64_t timeout_ms)
+{
+
+	uint8_t isc_default;
+	int64_t t0 = timeval_ms();
+	int64_t dt;
+
+	do {
+		isc_read_register(bank, CMD_XSC_OP_STATUS, &isc_default, 8);
+		if (((isc_default >> 2) & 1) == 1)
+			return ERROR_OK;
+		dt = timeval_ms() - t0;
+	} while (dt <= timeout_ms);
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/*
+ * helper function for procedures without program jtag command at the end
+ */
+static int isc_set_register(struct flash_bank *bank, const uint8_t *cmd,
+	const uint8_t *data_buf, int num_bits, int64_t timeout_ms)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = cmd;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_DRSHIFT);
+
+	scan.num_bits = num_bits;
+	scan.out_value = data_buf;
+	scan.in_value = NULL;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+
+	if (0 == timeout_ms)
+		return jtag_execute_queue();
+	else
+		return isc_wait_erase_program(bank, timeout_ms);
+}
+
+/*
+ * helper function for procedures required program jtag command at the end
+ */
+static int isc_program_register(struct flash_bank *bank, const uint8_t *cmd,
+	const uint8_t *data_buf, int num_bits, int64_t timeout_ms)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = cmd;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_DRSHIFT);
+
+	scan.num_bits = num_bits;
+	scan.out_value = data_buf;
+	scan.in_value = NULL;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IRSHIFT);
+
+	scan.num_bits = 16;
+	scan.out_value = CMD_ISC_PROGRAM;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+
+	if (0 == timeout_ms)
+		return jtag_execute_queue();
+	else
+		return isc_wait_erase_program(bank, timeout_ms);
+}
+
+static int isc_clear_protect(struct flash_bank *bank, int first, int last)
+{
+	uint8_t select_block[3] = {0x0, 0x0, 0x0};
+	select_block[0] = fill_select_block(first, last);
+	return isc_set_register(bank, CMD_XSC_UNLOCK, select_block, 24, 0);
+}
+
+static int isc_set_protect(struct flash_bank *bank, int first, int last)
+{
+	uint8_t wrpt[2] = {0xFF, 0xFF};
+	for (int i = first; i <= last; i++)
+		wrpt[0] &= ~(1 << i);
+
+	return isc_program_register(bank, CMD_XSC_DATA_WRPT, wrpt, 16, 0);
+}
+
+static int isc_erase_sectors(struct flash_bank *bank, int first, int last)
+{
+	uint8_t select_block[3] = {0, 0, 0};
+	select_block[0] = fill_select_block(first, last);
+	int64_t timeout = SECTOR_ERASE_TIMEOUT_MS * (last - first + 1);
+	return isc_set_register(bank, CMD_ISC_ERASE, select_block, 24, timeout);
+}
+
+static int isc_adr_shift(struct flash_bank *bank, int adr)
+{
+	uint8_t adr_buf[3];
+	h_u24_to_le(adr_buf, adr);
+	return isc_set_register(bank, CMD_ISC_ADDRESS_SHIFT, adr_buf, 24, 0);
+}
+
+static int isc_program_data_page(struct flash_bank *bank, const uint8_t *page_buf)
+{
+	return isc_program_register(bank, CMD_ISC_DATA_SHIFT, page_buf, 8 * XCF_PAGE_SIZE, 100);
+}
+
+static void isc_data_read_out(struct flash_bank *bank, uint8_t *buffer, uint32_t count)
+{
+
+	struct scan_field scan;
+
+	/* Do not change this code with isc_read_register() call because it needs
+	 * transition to IDLE state before data retrieving. */
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_XSC_READ;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+
+	scan.num_bits = 8 * count;
+	scan.out_value = NULL;
+	scan.in_value = buffer;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+
+	jtag_execute_queue();
+}
+
+static int isc_set_data_done(struct flash_bank *bank, int sector)
+{
+	uint8_t done = 0xFF;
+	done &= ~(1 << sector);
+	return isc_program_register(bank, CMD_XSC_DATA_DONE, &done, 8, 100);
+}
+
+static void flip_u8(uint8_t *out, const uint8_t *in, int len)
+{
+	for (int i = 0; i < len; i++)
+		out[i] = flip_u32(in[i], 8);
+}
+
+/*
+ * Xilinx bin file contains simple fixed header for automatic bus width detection:
+ * 16 bytes of 0xFF
+ * 4 byte sync word 0xAA995566 or (bit reversed) 0x5599AA66 in MSC file
+ *
+ * Function presumes need of bit reversing if it can not exactly detects
+ * the opposite.
+ */
+bool need_bit_reverse(const uint8_t *buffer)
+{
+	const size_t L = 20;
+	uint8_t reference[L];
+	memset(reference, 0xFF, 16);
+	reference[16] = 0x55;
+	reference[17] = 0x99;
+	reference[18] = 0xAA;
+	reference[19] = 0x66;
+
+	if (0 == memcmp(reference, buffer, L))
+		return false;
+	else
+		return true;
+}
+
+/*
+ * The page address to be programmed is determined by loading the
+ * internal ADDRESS Register using an ISC_ADDRESS_SHIFT instruction sequence.
+ * The page address automatically increments to the next 256-bit
+ * page address after each programming sequence until the last address
+ * in the 8 Mb block is reached. To continue programming the next block,
+ * the next 8 Mb block's starting address must be loaded into the
+ * internal ADDRESS register.
+ */
+static int read_write_data(struct flash_bank *bank, const uint8_t *w_buffer,
+	uint8_t *r_buffer, bool write_flag, uint32_t offset, uint32_t count)
+{
+	int dbg_count = count;
+	int dbg_written = 0;
+	int ret = ERROR_OK;
+	uint8_t *page_buf = malloc(XCF_PAGE_SIZE);
+	bool revbit = true;
+	isc_enter(bank);
+
+	if (offset % XCF_PAGE_SIZE != 0) {
+		ret = ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+		goto EXIT;
+	}
+
+	if ((offset + count) > (uint32_t)(bank->num_sectors * XCF_DATA_SECTOR_SIZE)) {
+		ret = ERROR_FLASH_DST_OUT_OF_BANK;
+		goto EXIT;
+	}
+
+	if ((write_flag) && (0 == offset) && (count >= XCF_PAGE_SIZE))
+		revbit = need_bit_reverse(w_buffer);
+
+	while (count > 0) {
+		uint32_t sector_num = offset / XCF_DATA_SECTOR_SIZE;
+		uint32_t sector_offset = offset - sector_num * XCF_DATA_SECTOR_SIZE;
+		uint32_t sector_bytes = XCF_DATA_SECTOR_SIZE - sector_offset;
+		if (count < sector_bytes)
+			sector_bytes = count;
+		isc_adr_shift(bank, offset);
+		offset += sector_bytes;
+		count -= sector_bytes;
+
+		if (write_flag) {
+			while (sector_bytes > 0) {
+				int len;
+
+				if (sector_bytes < XCF_PAGE_SIZE) {
+					len = sector_bytes;
+					memset(page_buf, 0xFF, XCF_PAGE_SIZE);
+				} else
+					len = XCF_PAGE_SIZE;
+
+				if (revbit)
+					flip_u8(page_buf, w_buffer, len);
+				else
+					memcpy(page_buf, w_buffer, len);
+
+				w_buffer += len;
+				sector_bytes -= len;
+				ret = isc_program_data_page(bank, page_buf);
+				if (ERROR_OK != ret)
+					goto EXIT;
+				else {
+					LOG_DEBUG("written %d bytes from %d", dbg_written, dbg_count);
+					dbg_written += len;
+				}
+			}
+		} else {
+			isc_data_read_out(bank, r_buffer, sector_bytes);
+			flip_u8(r_buffer, r_buffer, sector_bytes);
+			r_buffer += sector_bytes;
+		}
+	}
+
+	/* Set 'done' flags for all data sectors because driver supports
+	 * only single revision. */
+	if (write_flag) {
+		for (int i = 0; i < bank->num_sectors; i++) {
+			ret = isc_set_data_done(bank, i);
+			if (ERROR_OK != ret)
+				goto EXIT;
+		}
+	}
+
+EXIT:
+	free(page_buf);
+	isc_leave(bank);
+	return ret;
+}
+
+static uint16_t isc_read_ccb(struct flash_bank *bank)
+{
+	uint8_t ccb[2];
+	isc_read_register(bank, CMD_XSC_DATA_CCB, ccb, 16);
+	return le_to_h_u16(ccb);
+}
+
+static int gucr_num(const struct flash_bank *bank)
+{
+	return bank->num_sectors;
+}
+
+static int sucr_num(const struct flash_bank *bank)
+{
+	return bank->num_sectors + 1;
+}
+
+static int isc_program_ccb(struct flash_bank *bank, uint16_t ccb)
+{
+	uint8_t buf[2];
+	h_u16_to_le(buf, ccb);
+	return isc_program_register(bank, CMD_XSC_DATA_CCB, buf, 16, 100);
+}
+
+static int isc_program_singe_revision_sucr(struct flash_bank *bank)
+{
+	uint8_t sucr[2] = {0xFC, 0xFF};
+	return isc_program_register(bank, CMD_XSC_DATA_SUCR, sucr, 16, 100);
+}
+
+static int isc_program_single_revision_btc(struct flash_bank *bank)
+{
+	uint8_t buf[4];
+	uint32_t btc = 0xFFFFFFFF;
+	btc &= ~0b1111;
+	btc |= ((bank->num_sectors - 1) << 2);
+	btc &= ~(1 << 4);
+	h_u32_to_le(buf, btc);
+	return isc_program_register(bank, CMD_XSC_DATA_BTC, buf, 32, 100);
+}
+
+static int fpga_configure(struct flash_bank *bank)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_XSC_CONFIG;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+	jtag_execute_queue();
+
+	return ERROR_OK;
+}
+
+/*
+ ******************************************************************************
+ * EXPORTED FUNCTIONS
+ ******************************************************************************
+ */
+
+FLASH_BANK_COMMAND_HANDLER(xcf_flash_bank_command)
+{
+	struct xcf_priv *priv;
+
+	priv = malloc(sizeof(struct xcf_priv));
+	if (priv == NULL) {
+		LOG_ERROR("no memory for flash bank info");
+		return ERROR_FAIL;
+	}
+	bank->driver_priv = priv;
+	priv->probed = false;
+	return ERROR_OK;
+}
+
+static int xcf_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	const struct xcf_priv *priv = bank->driver_priv;
+
+	if (false == priv->probed) {
+		snprintf(buf, buf_size, "\nXCF flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+	snprintf(buf, buf_size, "%s", product_name(bank));
+	return ERROR_OK;
+}
+
+static int xcf_probe(struct flash_bank *bank)
+{
+	struct xcf_priv *priv = bank->driver_priv;
+	uint32_t id;
+
+	if (true == priv->probed)
+		free(bank->sectors);
+	priv->probed = false;
+
+	if (bank->target->tap == NULL) {
+		LOG_ERROR("Target has no JTAG tap");
+		return ERROR_FAIL;
+	}
+
+	/* check idcode and alloc memory for sector table */
+	if (!bank->target->tap->hasidcode)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* guess number of blocks using chip ID */
+	id = bank->target->tap->idcode;
+	switch (id & ID_MEANINGFUL_MASK) {
+		case ID_XCF08P:
+			bank->num_sectors = 1;
+			break;
+		case ID_XCF16P:
+			bank->num_sectors = 2;
+			break;
+		case ID_XCF32P:
+			bank->num_sectors = 4;
+			break;
+		default:
+			LOG_ERROR("Unknown flash device ID 0x%X", id);
+			return ERROR_FAIL;
+			break;
+	}
+
+	bank->sectors = malloc(bank->num_sectors * sizeof(struct flash_sector));
+	if (NULL == bank->sectors) {
+		LOG_ERROR("No memory for sector table");
+		return ERROR_FAIL;
+	}
+	fill_sector_table(bank);
+
+	priv->probed = true;
+	bank->driver_priv = priv;
+
+	LOG_INFO("product name: %s", product_name(bank));
+	LOG_INFO("device id = 0x%X ", bank->target->tap->idcode);
+	LOG_INFO("flash size = %d configuration bits",
+		bank->num_sectors * XCF_DATA_SECTOR_SIZE * 8);
+	LOG_INFO("number of sectors = %d", bank->num_sectors);
+
+	return ERROR_OK;
+}
+
+static int xcf_auto_probe(struct flash_bank *bank)
+{
+	struct xcf_priv *priv = bank->driver_priv;
+
+	if (true == priv->probed)
+		return ERROR_OK;
+	else
+		return xcf_probe(bank);
+}
+
+static int xcf_protect_check(struct flash_bank *bank)
+{
+	uint8_t wrpt[2];
+
+	isc_enter(bank);
+	isc_read_register(bank, CMD_XSC_DATA_WRPT, wrpt, 16);
+	isc_leave(bank);
+
+	for (int i = 0; i < bank->num_sectors; i++)
+		bank->sectors[i].is_protected = sector_state(wrpt[0], i);
+
+	return ERROR_OK;
+}
+
+static int xcf_erase_check(struct flash_bank *bank)
+{
+	uint8_t blankreg;
+	struct scan_field scan;
+
+	isc_enter(bank);
+
+	/* Do not change this code with isc_read_register() call because it needs
+	 * transition to IDLE state and pause before data retrieving. */
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_XSC_BLANK_CHECK;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+	jtag_execute_queue();
+	alive_sleep(500);	/* device needs at least 0.5s to self check */
+
+	scan.num_bits = 8;
+	scan.in_value = &blankreg;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+	jtag_execute_queue();
+
+	isc_leave(bank);
+
+	for (int i = 0; i < bank->num_sectors; i++)
+		bank->sectors[i].is_erased = sector_state(blankreg, i);
+
+	return ERROR_OK;
+}
+
+static int xcf_erase(struct flash_bank *bank, int first, int last)
+{
+	if ((first >= bank->num_sectors)
+		|| (last >= bank->num_sectors)
+		|| (last < first))
+		return ERROR_FLASH_SECTOR_INVALID;
+	else {
+		isc_enter(bank);
+		isc_clear_protect(bank, first, last);
+		int ret = isc_erase_sectors(bank, first, last);
+		isc_leave(bank);
+		return ret;
+	}
+}
+
+static int xcf_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	return read_write_data(bank, NULL, buffer, false, offset, count);
+}
+
+static int xcf_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset,
+	uint32_t count)
+{
+	return read_write_data(bank, buffer, NULL, true, offset, count);
+}
+
+static int xcf_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	int ret;
+
+	isc_enter(bank);
+	if (set)
+		ret = isc_set_protect(bank, first, last);
+	else {
+		/* write protection may be removed only with following erase */
+		isc_clear_protect(bank, first, last);
+		ret = isc_erase_sectors(bank, first, last);
+	}
+	isc_leave(bank);
+
+	return ret;
+}
+
+COMMAND_HANDLER(xcf_handle_ccb_command) {
+
+	if (!((CMD_ARGC == 1) || (CMD_ARGC == 5)))
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	uint16_t ccb = 0xFFFF;
+	isc_enter(bank);
+	uint16_t old_ccb = isc_read_ccb(bank);
+	isc_leave(bank);
+
+	if (CMD_ARGC == 1) {
+		LOG_INFO("current CCB = 0x%X", old_ccb);
+		return ERROR_OK;
+	} else {
+		/* skip over flash bank */
+		CMD_ARGC--;
+		CMD_ARGV++;
+		while (CMD_ARGC) {
+			if (strcmp("external", CMD_ARGV[0]) == 0)
+				ccb |= (1 << 0);
+			else if (strcmp("internal", CMD_ARGV[0]) == 0)
+				ccb &= ~(1 << 0);
+			else if (strcmp("serial", CMD_ARGV[0]) == 0)
+				ccb |= (3 << 1);
+			else if (strcmp("parallel", CMD_ARGV[0]) == 0)
+				ccb &= ~(3 << 1);
+			else if (strcmp("slave", CMD_ARGV[0]) == 0)
+				ccb |= (1 << 3);
+			else if (strcmp("master", CMD_ARGV[0]) == 0)
+				ccb &= ~(1 << 3);
+			else if (strcmp("40", CMD_ARGV[0]) == 0)
+				ccb |= (3 << 4);
+			else if (strcmp("20", CMD_ARGV[0]) == 0)
+				ccb &= ~(1 << 5);
+			else
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			CMD_ARGC--;
+			CMD_ARGV++;
+		}
+
+		isc_enter(bank);
+		int sector;
+
+		/* GUCR sector */
+		sector = gucr_num(bank);
+		isc_clear_protect(bank, sector, sector);
+		int ret = isc_erase_sectors(bank, sector, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_program_ccb(bank, ccb);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_program_single_revision_btc(bank);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_set_data_done(bank, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+
+		/* SUCR sector */
+		sector = sucr_num(bank);
+		isc_clear_protect(bank, sector, sector);
+		ret = isc_erase_sectors(bank, sector, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_program_singe_revision_sucr(bank);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_set_data_done(bank, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+
+EXIT:
+		isc_leave(bank);
+		return ret;
+	}
+}
+
+COMMAND_HANDLER(xcf_handle_configure_command) {
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	return fpga_configure(bank);
+}
+
+static const struct command_registration xcf_exec_command_handlers[] = {
+	{
+		.name = "configure",
+		.handler = xcf_handle_configure_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Initiate FPGA loading procedure."
+	},
+	{
+		.name = "ccb",
+		.handler = xcf_handle_ccb_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id [('external'|'internal') "
+			"('serial'|'parallel') "
+			"('slave'|'master') "
+			"('40'|'20')]",
+		.help = "Write CCB register with supplied options and (silently) BTC "
+			"register with single revision options. Display current "
+			"CCB value when only bank_id supplied. "
+			"Following options available: "
+			"1) external or internal clock source; "
+			"2) serial or parallel bus mode; "
+			"3) slave or master mode; "
+			"4) clock frequency in MHz for internal clock in master mode;"
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration xcf_command_handlers[] = {
+	{
+		.name = "xcf",
+		.mode = COMMAND_ANY,
+		.help = "Xilinx platform flash command group",
+		.usage = "",
+		.chain = xcf_exec_command_handlers
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver xcf_flash = {
+	.name               = "xcf",
+	.usage              = NULL,
+	.commands           = xcf_command_handlers,
+	.flash_bank_command = xcf_flash_bank_command,
+	.erase              = xcf_erase,
+	.protect            = xcf_protect,
+	.write              = xcf_write,
+	.read               = xcf_read,
+	.probe              = xcf_probe,
+	.auto_probe         = xcf_auto_probe,
+	.erase_check        = xcf_erase_check,
+	.protect_check      = xcf_protect_check,
+	.info               = xcf_info
+};
diff --git a/tcl/cpld/xilinx-xcf-p.cfg b/tcl/cpld/xilinx-xcf-p.cfg
new file mode 100644
index 000000000..8e0a26c6f
--- /dev/null
+++ b/tcl/cpld/xilinx-xcf-p.cfg
@@ -0,0 +1,18 @@
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME xcf
+}
+
+# IDs acquired from Xilinx's DS123.pdf
+# XCF08P <v>5057093
+# XCF16P <v>5058093
+# XCF32P <v>5059093
+# The 4 top bits (28:31) are the device revision. Ignore it.
+jtag newtap $_CHIPNAME flash -irlen 16 -ignore-version \
+	-expected-id 0x05057093 \
+	-expected-id 0x05058093 \
+	-expected-id 0x05059093
+
+target create xcf.flash testee -chain-position $_CHIPNAME.flash
+flash bank XCF_P xcf 0 0 0 0 xcf.flash
diff --git a/tcl/cpld/xilinx-xcf-s.cfg b/tcl/cpld/xilinx-xcf-s.cfg
new file mode 100644
index 000000000..a3c79a385
--- /dev/null
+++ b/tcl/cpld/xilinx-xcf-s.cfg
@@ -0,0 +1,18 @@
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME xcf
+}
+
+# IDs acquired from Xilinx's DS123.pdf
+# XCF01S <v>5044093
+# XCF02S <v>5045093
+# XCF04S <v>5046093
+# The 4 top bits (28:31) are the device revision. Ignore it.
+jtag newtap $_CHIPNAME flash -irlen 8 -ignore-version \
+	-expected-id 0x05044093 \
+	-expected-id 0x05045093 \
+	-expected-id 0x05046093
+
+target create xcf.flash testee -chain-position $_CHIPNAME.flash
+flash bank XCF_S xcf 0 0 0 0 xcf.flash

From 43cf6f9b62be7198a92593aa4451d151405655df Mon Sep 17 00:00:00 2001
From: Robert Jordens <jordens@gmail.com>
Date: Thu, 3 Aug 2017 16:25:13 +0200
Subject: [PATCH 24/65] digilent_jtag_smt2_nc: add support

The Digilent SMT2 NC is nominally the connector-less version of the
SMT2. But neither the SMT2 configuration nor the HS3 configuration work
for on the Xilinx KCU105 board where the SMT2 NC is used.

Change-Id: Ieb27cbc6d8b0f9c64ef778e4e0c839acc85ec0ef
Signed-off-by: Robert Jordens <jordens@gmail.com>
Reviewed-on: http://openocd.zylin.com/4187
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)
 create mode 100644 tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg

diff --git a/tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg b/tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg
new file mode 100644
index 000000000..a83a0081e
--- /dev/null
+++ b/tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg
@@ -0,0 +1,18 @@
+#
+# Digilent JTAG-SMT2-NC
+#
+# http://store.digilentinc.com/jtag-smt2-nc-surface-mount-programming-module/
+# https://reference.digilentinc.com/_media/jtag_smt2nc/jtag-smt2-nc_rm.pdf
+#
+# Based on reference sheet (above) and Xilinx KCU105 schematics
+# https://www.xilinx.com/products/boards-and-kits/kcu105.html#documentation
+#
+# Note that the digilent_jtag_smt2 layout does not work and hangs while
+# the ftdi_device_desc from digilent_hs2 is wrong.
+
+interface ftdi
+ftdi_device_desc "Digilent USB Device"
+ftdi_vid_pid 0x0403 0x6014
+ftdi_channel 0
+ftdi_layout_init 0x00e8 0x60eb
+reset_config none

From ea80232c5e67fe07dff00d54d4f4ac7adf9e3e78 Mon Sep 17 00:00:00 2001
From: Felipe Balbi <felipe.balbi@linux.intel.com>
Date: Mon, 20 Feb 2017 17:35:55 +0200
Subject: [PATCH 25/65] target: quark_x10xx: adding missing 'static' keyword

These symbols are only used within this C source file. They don't need
to be exposed to the outside.

Change-Id: Idb04550ecca7f12c3fdc8c6447eeeb871961add3
Signed-off-by: Felipe Balbi <felipe.balbi@linux.intel.com>
Reviewed-on: http://openocd.zylin.com/4012
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/quark_x10xx.c | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/target/quark_x10xx.c b/src/target/quark_x10xx.c
index 189f6cc65..b9a7c1656 100644
--- a/src/target/quark_x10xx.c
+++ b/src/target/quark_x10xx.c
@@ -51,7 +51,7 @@
 #include "lakemont.h"
 #include "x86_32_common.h"
 
-int quark_x10xx_target_create(struct target *t, Jim_Interp *interp)
+static int quark_x10xx_target_create(struct target *t, Jim_Interp *interp)
 {
 	struct x86_32_common *x86_32 = calloc(1, sizeof(struct x86_32_common));
 	if (x86_32 == NULL) {
@@ -64,7 +64,8 @@ int quark_x10xx_target_create(struct target *t, Jim_Interp *interp)
 	return ERROR_OK;
 }
 
-int quark_x10xx_init_target(struct command_context *cmd_ctx, struct target *t)
+static int quark_x10xx_init_target(struct command_context *cmd_ctx,
+		struct target *t)
 {
 	return lakemont_init_target(cmd_ctx, t);
 }

From 1ea313e333f60aed254ef6a7a052a2cebbc884e1 Mon Sep 17 00:00:00 2001
From: Felipe Balbi <felipe.balbi@linux.intel.com>
Date: Fri, 24 Feb 2017 15:21:37 +0200
Subject: [PATCH 26/65] target: lakemon: probemode entry isn't instantaneous

When testing with Intel Galileo GEN2 I have noticed a few iterations
where probemode entry took a little longer. At most I had to read
tapstatus twice. This patch uses a loop of up to 100 iterations to
wait for tapstatus to update with PM entry results.

Change-Id: I1c87d7dde715255b3fd2460d299b9493218533fc
Signed-off-by: Felipe Balbi <felipe.balbi@linux.intel.com>
Reviewed-on: http://openocd.zylin.com/4013
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/lakemont.c | 20 ++++++++++++--------
 1 file changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/target/lakemont.c b/src/target/lakemont.c
index 2bd12fd41..5f92f0d1b 100644
--- a/src/target/lakemont.c
+++ b/src/target/lakemont.c
@@ -444,6 +444,8 @@ static uint32_t get_tapstatus(struct target *t)
 static int enter_probemode(struct target *t)
 {
 	uint32_t tapstatus = 0;
+	int retries = 100;
+
 	tapstatus = get_tapstatus(t);
 	LOG_DEBUG("TS before PM enter = 0x%08" PRIx32, tapstatus);
 	if (tapstatus & TS_PM_BIT) {
@@ -456,15 +458,17 @@ static int enter_probemode(struct target *t)
 	scan.out[0] = 1;
 	if (drscan(t, scan.out, scan.in, 1) != ERROR_OK)
 		return ERROR_FAIL;
-	tapstatus = get_tapstatus(t);
-	LOG_DEBUG("TS after PM enter = 0x%08" PRIx32, tapstatus);
-	if ((tapstatus & TS_PM_BIT) && (!(tapstatus & TS_EN_PM_BIT)))
-		return ERROR_OK;
-	else {
-		LOG_ERROR("%s PM enter error, tapstatus = 0x%08" PRIx32
-				, __func__, tapstatus);
-		return ERROR_FAIL;
+
+	while (retries--) {
+		tapstatus = get_tapstatus(t);
+		LOG_DEBUG("TS after PM enter = 0x%08" PRIx32, tapstatus);
+		if ((tapstatus & TS_PM_BIT) && (!(tapstatus & TS_EN_PM_BIT)))
+			return ERROR_OK;
 	}
+
+	LOG_ERROR("%s PM enter error, tapstatus = 0x%08" PRIx32
+			, __func__, tapstatus);
+	return ERROR_FAIL;
 }
 
 static int exit_probemode(struct target *t)

From ffa6b189a0155411545bb26947419b7a9a2b35e8 Mon Sep 17 00:00:00 2001
From: Felipe Balbi <felipe.balbi@linux.intel.com>
Date: Fri, 24 Feb 2017 15:23:53 +0200
Subject: [PATCH 27/65] target: type: fix indentation

No functional changes, cleanup only

Change-Id: I53c422be16d0a4ff157745d31362f6483093e5eb
Signed-off-by: Felipe Balbi <felipe.balbi@linux.intel.com>
Reviewed-on: http://openocd.zylin.com/4014
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/target_type.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/target/target_type.h b/src/target/target_type.h
index 0960e6d59..0ab22bd5c 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -84,7 +84,7 @@ struct target_type {
 	 * "halt".
 	 *
 	 * reset run; halt
-     */
+	 */
 	int (*deassert_reset)(struct target *target);
 	int (*soft_reset_halt)(struct target *target);
 

From 3accbec9010c3f98dbe3795e438412e45d5b9ce5 Mon Sep 17 00:00:00 2001
From: Felipe Balbi <felipe.balbi@linux.intel.com>
Date: Fri, 24 Feb 2017 15:25:32 +0200
Subject: [PATCH 28/65] target: quark_x10xx: miscellaneous cleanups

Just some misc cleanups without any functional changes. It's just
easier to read.

Change-Id: I66bee31f297bcbdb8cc4446b774d7714fbaa7c83
Signed-off-by: Felipe Balbi <felipe.balbi@linux.intel.com>
Reviewed-on: http://openocd.zylin.com/4015
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/quark_x10xx.c | 62 +++++++++++++++++++---------------------
 1 file changed, 30 insertions(+), 32 deletions(-)

diff --git a/src/target/quark_x10xx.c b/src/target/quark_x10xx.c
index b9a7c1656..525d39a02 100644
--- a/src/target/quark_x10xx.c
+++ b/src/target/quark_x10xx.c
@@ -53,47 +53,45 @@
 
 static int quark_x10xx_target_create(struct target *t, Jim_Interp *interp)
 {
-	struct x86_32_common *x86_32 = calloc(1, sizeof(struct x86_32_common));
-	if (x86_32 == NULL) {
-		LOG_ERROR("%s out of memory", __func__);
+	struct x86_32_common *x86_32 = calloc(1, sizeof(*x86_32));
+
+	if (!x86_32)
 		return ERROR_FAIL;
-	}
+
 	x86_32_common_init_arch_info(t, x86_32);
 	lakemont_init_arch_info(t, x86_32);
 	x86_32->core_type = LMT1;
+
 	return ERROR_OK;
 }
 
-static int quark_x10xx_init_target(struct command_context *cmd_ctx,
-		struct target *t)
-{
-	return lakemont_init_target(cmd_ctx, t);
-}
-
 struct target_type quark_x10xx_target = {
-	.name = "quark_x10xx",
+	.name			= "quark_x10xx",
+
 	/* Quark X1000 SoC */
-	.target_create = quark_x10xx_target_create,
-	.init_target = quark_x10xx_init_target,
+	.target_create		= quark_x10xx_target_create,
+
 	/* lakemont probemode specific code */
-	.poll = lakemont_poll,
-	.arch_state = lakemont_arch_state,
-	.halt = lakemont_halt,
-	.resume = lakemont_resume,
-	.step = lakemont_step,
-	.assert_reset = lakemont_reset_assert,
-	.deassert_reset = lakemont_reset_deassert,
+	.arch_state		= lakemont_arch_state,
+	.assert_reset		= lakemont_reset_assert,
+	.deassert_reset		= lakemont_reset_deassert,
+	.halt			= lakemont_halt,
+	.init_target		= lakemont_init_target,
+	.poll			= lakemont_poll,
+	.resume			= lakemont_resume,
+	.step			= lakemont_step,
+
 	/* common x86 code */
-	.commands = x86_32_command_handlers,
-	.get_gdb_reg_list = x86_32_get_gdb_reg_list,
-	.read_memory = x86_32_common_read_memory,
-	.write_memory = x86_32_common_write_memory,
-	.add_breakpoint = x86_32_common_add_breakpoint,
-	.remove_breakpoint = x86_32_common_remove_breakpoint,
-	.add_watchpoint = x86_32_common_add_watchpoint,
-	.remove_watchpoint = x86_32_common_remove_watchpoint,
-	.virt2phys = x86_32_common_virt2phys,
-	.read_phys_memory = x86_32_common_read_phys_mem,
-	.write_phys_memory = x86_32_common_write_phys_mem,
-	.mmu = x86_32_common_mmu,
+	.add_breakpoint		= x86_32_common_add_breakpoint,
+	.add_watchpoint		= x86_32_common_add_watchpoint,
+	.commands		= x86_32_command_handlers,
+	.get_gdb_reg_list	= x86_32_get_gdb_reg_list,
+	.mmu			= x86_32_common_mmu,
+	.read_memory		= x86_32_common_read_memory,
+	.read_phys_memory	= x86_32_common_read_phys_mem,
+	.remove_breakpoint	= x86_32_common_remove_breakpoint,
+	.remove_watchpoint	= x86_32_common_remove_watchpoint,
+	.virt2phys		= x86_32_common_virt2phys,
+	.write_memory		= x86_32_common_write_memory,
+	.write_phys_memory	= x86_32_common_write_phys_mem,
 };

From 2b44b52478708c3bef58033dfd4f7bb7db74d843 Mon Sep 17 00:00:00 2001
From: Felipe Balbi <felipe.balbi@linux.intel.com>
Date: Fri, 24 Feb 2017 15:26:39 +0200
Subject: [PATCH 29/65] target: lakemon: implement assert_reset and
 deassert_reset

We're using an I/O port reset by default. The only board currently
supported (Galileo) doesn't have SRST routed on the JTAG connector.

When using 'reset halt', we must rely on Reset Break because our
adapters don't have support for PREQ#/PRDY# signals.

Tested with Intel Galileo GEN2.

Change-Id: Ia406e31c156f8001717d5b6a08bd03f71de790d3
Signed-off-by: Felipe Balbi <felipe.balbi@linux.intel.com>
Reviewed-on: http://openocd.zylin.com/4016
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/lakemont.c      | 129 ++++++++++++++++++++++++++++++++++++-
 src/target/x86_32_common.c |  32 +++++++++
 src/target/x86_32_common.h |   2 +
 3 files changed, 160 insertions(+), 3 deletions(-)

diff --git a/src/target/lakemont.c b/src/target/lakemont.c
index 5f92f0d1b..b81213ebd 100644
--- a/src/target/lakemont.c
+++ b/src/target/lakemont.c
@@ -970,6 +970,7 @@ int lakemont_poll(struct target *t)
 			return target_call_event_callbacks(t, TARGET_EVENT_HALTED);
 		}
 	}
+
 	return ERROR_OK;
 }
 
@@ -1115,15 +1116,137 @@ int lakemont_step(struct target *t, int current,
 	return retval;
 }
 
-/* TODO - implement resetbreak fully through CLTAP registers */
+static int lakemont_reset_break(struct target *t)
+{
+	struct x86_32_common *x86_32 = target_to_x86_32(t);
+	struct jtag_tap *saved_tap = x86_32->curr_tap;
+	struct scan_field *fields = &scan.field;
+
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("issuing port 0xcf9 reset");
+
+	/* prepare resetbreak setting the proper bits in CLTAPC_CPU_VPREQ */
+	x86_32->curr_tap = jtag_tap_by_position(1);
+	if (x86_32->curr_tap == NULL) {
+		x86_32->curr_tap = saved_tap;
+		LOG_ERROR("%s could not select quark_x10xx.cltap", __func__);
+		return ERROR_FAIL;
+	}
+
+	fields->in_value  = NULL;
+	fields->num_bits  = 8;
+
+	/* select CLTAPC_CPU_VPREQ instruction*/
+	scan.out[0] = 0x51;
+	fields->out_value = ((uint8_t *)scan.out);
+	jtag_add_ir_scan(x86_32->curr_tap, fields, TAP_IDLE);
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK) {
+		x86_32->curr_tap = saved_tap;
+		LOG_ERROR("%s irscan failed to execute queue", __func__);
+		return retval;
+	}
+
+	/* set enable_preq_on_reset & enable_preq_on_reset2 bits*/
+	scan.out[0] = 0x06;
+	fields->out_value  = ((uint8_t *)scan.out);
+	jtag_add_dr_scan(x86_32->curr_tap, 1, fields, TAP_IDLE);
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s drscan failed to execute queue", __func__);
+		x86_32->curr_tap = saved_tap;
+		return retval;
+	}
+
+	/* restore current tap */
+	x86_32->curr_tap = saved_tap;
+
+	return ERROR_OK;
+}
+
+/*
+ * If we ever get an adapter with support for PREQ# and PRDY#, we should
+ * update this function to add support for using those two signals.
+ *
+ * Meanwhile, we're assuming that we only support reset break.
+ */
 int lakemont_reset_assert(struct target *t)
 {
-	LOG_DEBUG("-");
+	struct x86_32_common *x86_32 = target_to_x86_32(t);
+	/* write 0x6 to I/O port 0xcf9 to cause the reset */
+	uint8_t cf9_reset_val = 0x6;
+	int retval;
+
+	LOG_DEBUG(" ");
+
+	if (t->state != TARGET_HALTED) {
+		LOG_DEBUG("target must be halted first");
+		retval = lakemont_halt(t);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("could not halt target");
+			return retval;
+		}
+		x86_32->forced_halt_for_reset = true;
+	}
+
+	if (t->reset_halt) {
+		retval = lakemont_reset_break(t);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	retval = x86_32_common_write_io(t, 0xcf9, BYTE, &cf9_reset_val);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("could not write to port 0xcf9");
+		return retval;
+	}
+
+	if (!t->reset_halt && x86_32->forced_halt_for_reset) {
+		x86_32->forced_halt_for_reset = false;
+		retval = lakemont_resume(t, true, 0x00, false, true);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* remove breakpoints and watchpoints */
+	x86_32_common_reset_breakpoints_watchpoints(t);
+
 	return ERROR_OK;
 }
 
 int lakemont_reset_deassert(struct target *t)
 {
-	LOG_DEBUG("-");
+	int retval;
+
+	LOG_DEBUG(" ");
+
+	if (target_was_examined(t)) {
+		retval = lakemont_poll(t);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	if (t->reset_halt) {
+		/* entered PM after reset, update the state */
+		retval = lakemont_update_after_probemode_entry(t);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("could not update state after probemode entry");
+			return retval;
+		}
+
+		if (t->state != TARGET_HALTED) {
+			LOG_WARNING("%s: ran after reset and before halt ...",
+				target_name(t));
+			if (target_was_examined(t)) {
+				retval = target_halt(t);
+				if (retval != ERROR_OK)
+					return retval;
+			} else {
+				t->state = TARGET_UNKNOWN;
+			}
+		}
+	}
+
 	return ERROR_OK;
 }
diff --git a/src/target/x86_32_common.c b/src/target/x86_32_common.c
index 34f92eaca..0d87ba5a2 100644
--- a/src/target/x86_32_common.c
+++ b/src/target/x86_32_common.c
@@ -1260,6 +1260,38 @@ static int unset_watchpoint(struct target *t, struct watchpoint *wp)
 	return ERROR_OK;
 }
 
+/* after reset breakpoints and watchpoints in memory are not valid anymore and
+ * debug registers are cleared.
+ * we can't afford to remove sw breakpoints using the default methods as the
+ * memory doesn't have the same layout yet and an access might crash the target,
+ * so we just clear the openocd breakpoints structures.
+ */
+void x86_32_common_reset_breakpoints_watchpoints(struct target *t)
+{
+	struct x86_32_common *x86_32 = target_to_x86_32(t);
+	struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
+	struct breakpoint *next_b;
+	struct watchpoint *next_w;
+
+	while (t->breakpoints) {
+		next_b = t->breakpoints->next;
+		free(t->breakpoints->orig_instr);
+		free(t->breakpoints);
+		t->breakpoints = next_b;
+	}
+
+	while (t->watchpoints) {
+		next_w = t->watchpoints->next;
+		free(t->watchpoints);
+		t->watchpoints = next_w;
+	}
+
+	for (int i = 0; i < x86_32->num_hw_bpoints; i++) {
+		debug_reg_list[i].used = 0;
+		debug_reg_list[i].bp_value = 0;
+	}
+}
+
 static int read_hw_reg_to_cache(struct target *t, int num)
 {
 	uint32_t reg_value;
diff --git a/src/target/x86_32_common.h b/src/target/x86_32_common.h
index 0aaa963d7..14e6e35f7 100644
--- a/src/target/x86_32_common.h
+++ b/src/target/x86_32_common.h
@@ -217,6 +217,7 @@ struct x86_32_common {
 	struct reg_cache *cache;
 	struct jtag_tap *curr_tap;
 	uint32_t stored_pc;
+	int forced_halt_for_reset;
 	int flush;
 
 	/* pm_regs are for probemode save/restore state */
@@ -326,5 +327,6 @@ int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp);
 int x86_32_common_remove_breakpoint(struct target *t, struct breakpoint *bp);
 int x86_32_common_add_watchpoint(struct target *t, struct watchpoint *wp);
 int x86_32_common_remove_watchpoint(struct target *t, struct watchpoint *wp);
+void x86_32_common_reset_breakpoints_watchpoints(struct target *t);
 
 #endif /* OPENOCD_TARGET_X86_32_COMMON_H */

From 64b0d5aac0c5214db01f938550e8b126891d5a08 Mon Sep 17 00:00:00 2001
From: Simon Schubert <2@0x2c.org>
Date: Fri, 1 Sep 2017 20:34:09 +0200
Subject: [PATCH 30/65] cortex_m: add profiling function

Use DWT_PCSR if present (reads nonzero); otherwise do halt-and-sample pc.

Signed-off-by: Simon Schubert <2@0x2c.org>
Change-Id: Id2dc4665e5008cc497a2e6e6493522d038d5af42
Reviewed-on: http://openocd.zylin.com/4211
Tested-by: jenkins
Reviewed-by: Karl Palsson <karlp@tweak.net.au>
Reviewed-by: Christopher Head <chead@zaber.com>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/cortex_m.c | 89 +++++++++++++++++++++++++++++++++++++++++++
 src/target/cortex_m.h |  1 +
 2 files changed, 90 insertions(+)

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 2f8c2a2c8..988323b04 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1707,6 +1707,93 @@ void cortex_m_deinit_target(struct target *target)
 	free(cortex_m);
 }
 
+static int cortex_m_profiling(struct target *target, uint32_t *samples,
+			      uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
+{
+	struct timeval timeout, now;
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	uint32_t reg_value;
+	bool use_pcsr = false;
+	int retval = ERROR_OK;
+	struct reg *reg;
+
+	gettimeofday(&timeout, NULL);
+	timeval_add_time(&timeout, seconds, 0);
+
+	retval = target_read_u32(target, DWT_PCSR, &reg_value);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error while reading PCSR");
+		return retval;
+	}
+
+	if (reg_value != 0) {
+		use_pcsr = true;
+		LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
+	} else {
+		LOG_INFO("Starting profiling. Halting and resuming the"
+			 " target as often as we can...");
+		reg = register_get_by_name(target->reg_cache, "pc", 1);
+	}
+
+	/* Make sure the target is running */
+	target_poll(target);
+	if (target->state == TARGET_HALTED)
+		retval = target_resume(target, 1, 0, 0, 0);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error while resuming target");
+		return retval;
+	}
+
+	uint32_t sample_count = 0;
+
+	for (;;) {
+		if (use_pcsr) {
+			uint32_t read_count = max_num_samples - sample_count;
+			if (read_count > 1024)
+				read_count = 1024;
+			retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
+							(void *)&samples[sample_count],
+							4, read_count, DWT_PCSR);
+			sample_count += read_count;
+		} else {
+			target_poll(target);
+			if (target->state == TARGET_HALTED) {
+				reg_value = buf_get_u32(reg->value, 0, 32);
+				/* current pc, addr = 0, do not handle breakpoints, not debugging */
+				retval = target_resume(target, 1, 0, 0, 0);
+				samples[sample_count++] = reg_value;
+				target_poll(target);
+				alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
+			} else if (target->state == TARGET_RUNNING) {
+				/* We want to quickly sample the PC. */
+				retval = target_halt(target);
+			} else {
+				LOG_INFO("Target not halted or running");
+				retval = ERROR_OK;
+				break;
+			}
+		}
+
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error while reading %s", use_pcsr ? "PCSR" : "target pc");
+			return retval;
+		}
+
+
+		gettimeofday(&now, NULL);
+		if (sample_count >= max_num_samples ||
+			(now.tv_sec >= timeout.tv_sec && now.tv_usec >= timeout.tv_usec)) {
+			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
+			break;
+		}
+	}
+
+	*num_samples = sample_count;
+	return retval;
+}
+
+
 /* REVISIT cache valid/dirty bits are unmaintained.  We could set "valid"
  * on r/w if the core is not running, and clear on resume or reset ... or
  * at least, in a post_restore_context() method.
@@ -2451,4 +2538,6 @@ struct target_type cortexm_target = {
 	.init_target = cortex_m_init_target,
 	.examine = cortex_m_examine,
 	.deinit_target = cortex_m_deinit_target,
+
+	.profiling = cortex_m_profiling,
 };
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 3d9714b90..dbf9ac8a7 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -48,6 +48,7 @@
 
 #define DWT_CTRL	0xE0001000
 #define DWT_CYCCNT	0xE0001004
+#define DWT_PCSR	0xE000101C
 #define DWT_COMP0	0xE0001020
 #define DWT_MASK0	0xE0001024
 #define DWT_FUNCTION0	0xE0001028

From 4e0371bf718c9eb3a862e3c9b113c95cd4c3a479 Mon Sep 17 00:00:00 2001
From: Karl Palsson <karlp@tweak.net.au>
Date: Fri, 8 Sep 2017 10:49:30 +0000
Subject: [PATCH 31/65] hla_target: allow non-intrusive profiling on cortex-m

Leverages the existing work that added profiling via DWT_PCSR.

hla_target doesn't have direct access to the mem_ap for doing a bulk
repeated read, but simply reading the DWT_PCSR register repeatedly is
still ~2 order of magnitude faster than halt/resume.

Change-Id: Ibe451aa95143694398370fdad6939cfb6191d56f
Signed-off-by: Karl Palsson <karlp@tweak.net.au>
Reviewed-on: http://openocd.zylin.com/4220
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/cortex_m.c   | 17 +++++++++++------
 src/target/cortex_m.h   |  2 ++
 src/target/hla_target.c |  1 +
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 988323b04..24063a7e6 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1707,7 +1707,7 @@ void cortex_m_deinit_target(struct target *target)
 	free(cortex_m);
 }
 
-static int cortex_m_profiling(struct target *target, uint32_t *samples,
+int cortex_m_profiling(struct target *target, uint32_t *samples,
 			      uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
 {
 	struct timeval timeout, now;
@@ -1749,13 +1749,18 @@ static int cortex_m_profiling(struct target *target, uint32_t *samples,
 
 	for (;;) {
 		if (use_pcsr) {
-			uint32_t read_count = max_num_samples - sample_count;
-			if (read_count > 1024)
-				read_count = 1024;
-			retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
+			if (armv7m && armv7m->debug_ap) {
+				uint32_t read_count = max_num_samples - sample_count;
+				if (read_count > 1024)
+					read_count = 1024;
+
+				retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
 							(void *)&samples[sample_count],
 							4, read_count, DWT_PCSR);
-			sample_count += read_count;
+				sample_count += read_count;
+			} else {
+				target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
+			}
 		} else {
 			target_poll(target);
 			if (target->state == TARGET_HALTED) {
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index dbf9ac8a7..9500acc1d 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -213,5 +213,7 @@ void cortex_m_enable_breakpoints(struct target *target);
 void cortex_m_enable_watchpoints(struct target *target);
 void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target);
 void cortex_m_deinit_target(struct target *target);
+int cortex_m_profiling(struct target *target, uint32_t *samples,
+	uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
 
 #endif /* OPENOCD_TARGET_CORTEX_M_H */
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index 78dc8c512..a3e683597 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -814,4 +814,5 @@ struct target_type hla_target = {
 	.remove_breakpoint = cortex_m_remove_breakpoint,
 	.add_watchpoint = cortex_m_add_watchpoint,
 	.remove_watchpoint = cortex_m_remove_watchpoint,
+	.profiling = cortex_m_profiling,
 };

From 9cd74c113c3a92dd41485ad4d9ba0a275682712b Mon Sep 17 00:00:00 2001
From: Karl Palsson <karlp@tweak.net.au>
Date: Fri, 8 Sep 2017 15:44:52 +0000
Subject: [PATCH 32/65] profiling: write "correct" sample rate to gmon output

This duration vs sample count is _significantly_ closer to the truth
than simply declaring the value to be 100Hz.

Change-Id: Ie8d8bdf1959e1aa7cead0631cd2c86afe77d1efc
Signed-off-by: Karl Palsson <karlp@tweak.net.au>
Reviewed-on: http://openocd.zylin.com/4221
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/target.c | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/target/target.c b/src/target/target.c
index a9907b0c3..d6781a3c4 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -3838,7 +3838,7 @@ typedef unsigned char UNIT[2];  /* unit of profiling */
 
 /* Dump a gmon.out histogram file. */
 static void write_gmon(uint32_t *samples, uint32_t sampleNum, const char *filename, bool with_range,
-			uint32_t start_address, uint32_t end_address, struct target *target)
+			uint32_t start_address, uint32_t end_address, struct target *target, uint32_t duration_ms)
 {
 	uint32_t i;
 	FILE *f = fopen(filename, "w");
@@ -3906,7 +3906,8 @@ static void write_gmon(uint32_t *samples, uint32_t sampleNum, const char *filena
 	writeLong(f, min, target);			/* low_pc */
 	writeLong(f, max, target);			/* high_pc */
 	writeLong(f, numBuckets, target);	/* # of buckets */
-	writeLong(f, 100, target);			/* KLUDGE! We lie, ca. 100Hz best case. */
+	float sample_rate = sampleNum / (duration_ms / 1000.0);
+	writeLong(f, sample_rate, target);
 	writeString(f, "seconds");
 	for (i = 0; i < (15-strlen("seconds")); i++)
 		writeData(f, &zero, 1);
@@ -3955,6 +3956,7 @@ COMMAND_HANDLER(handle_profile_command)
 		return ERROR_FAIL;
 	}
 
+	uint64_t timestart_ms = timeval_ms();
 	/**
 	 * Some cores let us sample the PC without the
 	 * annoying halt/resume step; for example, ARMv7 PCSR.
@@ -3966,6 +3968,7 @@ COMMAND_HANDLER(handle_profile_command)
 		free(samples);
 		return retval;
 	}
+	uint32_t duration_ms = timeval_ms() - timestart_ms;
 
 	assert(num_of_samples <= MAX_PROFILE_SAMPLE_NUM);
 
@@ -3998,7 +4001,7 @@ COMMAND_HANDLER(handle_profile_command)
 	}
 
 	write_gmon(samples, num_of_samples, CMD_ARGV[1],
-		   with_range, start_address, end_address, target);
+		   with_range, start_address, end_address, target, duration_ms);
 	command_print(CMD_CTX, "Wrote %s", CMD_ARGV[1]);
 
 	free(samples);

From e0010c3e6fe09d9d332b279e661858fc449c0645 Mon Sep 17 00:00:00 2001
From: Marc Schink <openocd-dev@marcschink.de>
Date: Sun, 4 Oct 2015 20:21:00 +0200
Subject: [PATCH 33/65] server/telnet: Handle Ctrl+A and Ctrl+E

Handle the Ctrl+A and Ctrl+E shortcuts which move the cursor to the
beginning and end of the command line, respectively.

Change-Id: I89fa5fd3c5edeb08a3f9320fda766f72ce9d7f64
Signed-off-by: Marc Schink <openocd-dev@marcschink.de>
Reviewed-on: http://openocd.zylin.com/3415
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/server/telnet_server.c | 28 ++++++++++++++++++++++++++++
 1 file changed, 28 insertions(+)

diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 7507afea8..06a9c7a6c 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -303,6 +303,30 @@ static void telnet_history_down(struct connection *connection)
 	telnet_history_go(connection, next_history);
 }
 
+static void telnet_move_cursor(struct connection *connection, size_t pos)
+{
+	struct telnet_connection *tc;
+	size_t tmp;
+
+	tc = connection->priv;
+
+	if (pos < tc->line_cursor) {
+		tmp = tc->line_cursor - pos;
+
+		for (size_t i = 0; i < tmp; i += 16)
+			telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+				MIN(tmp - i, 16));
+	} else {
+		tmp = pos - tc->line_cursor;
+
+		for (size_t i = 0; i < tmp; i += 16)
+			telnet_write(connection, tc->line + tc->line_cursor + i,
+				MIN(tmp - i, 16));
+	}
+
+	tc->line_cursor = pos;
+}
+
 static int telnet_input(struct connection *connection)
 {
 	int bytes_read;
@@ -482,6 +506,10 @@ static int telnet_input(struct connection *connection)
 							telnet_history_up(connection);
 						else if (*buf_p == CTRL('N'))		/* cursor down */
 							telnet_history_down(connection);
+						else if (*buf_p == CTRL('A'))
+							telnet_move_cursor(connection, 0);
+						else if (*buf_p == CTRL('E'))
+							telnet_move_cursor(connection, t_con->line_size);
 						else
 							LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
 					}

From e9493cc20a0c2160696b24757274ec4ee302ac39 Mon Sep 17 00:00:00 2001
From: Jiri Kastner <cz172638@gmail.com>
Date: Mon, 16 Jan 2017 16:57:58 +0100
Subject: [PATCH 34/65] ejtag: added missing instructions.

added missing instructions from latest available ejtag specification
(MD00047 v6.1 at time of writting) for trace control, fast data channel
and pcsample.

Change-Id: I30293951611265ffc2bd896f9d3ca6b310e5cac6
Signed-off-by: Jiri Kastner <cz172638@gmail.com>
Reviewed-on: http://openocd.zylin.com/3950
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/mips_ejtag.h | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 71f5c1b4b..c64e858a3 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -36,6 +36,11 @@
 #define EJTAG_INST_TCBCONTROLA	0x10
 #define EJTAG_INST_TCBCONTROLB	0x11
 #define EJTAG_INST_TCBDATA		0x12
+#define EJTAG_INST_TCBCONTROLC	0x13
+#define EJTAG_INST_PCSAMPLE		0x14
+#define EJTAG_INST_TCBCONTROLD	0x15
+#define EJTAG_INST_TCBCONTROLE	0x16
+#define EJTAG_INST_FDC			0x17
 #define EJTAG_INST_BYPASS		0xFF
 
 /* microchip PIC32MX specific instructions */

From 79cbeafe9f948217d388045882015bdd00df706c Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Tue, 26 Dec 2017 13:44:26 -0800
Subject: [PATCH 35/65] Support listening on port 0.

When listening on port 0, the system will assign a random open port. We
use this to run multiple OpenOCD instances against multiple simulators
as part of regression testing. This mechanism means the various test
instances don't have to coordinate to ensure they don't reuse any ports.

The required changes are minimal:
1. Don't increment the port number when it's 0.
2. Print out which port was assigned by the system.

Change-Id: I404c801fc405e9d8eb8420562c02e78d4db6242f
Signed-off-by: Tim Newsome <tim@sifive.com>
Reviewed-on: http://openocd.zylin.com/4316
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/server/gdb_server.c | 8 +++++++-
 src/server/server.c     | 6 ++++++
 2 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index fc0868b0a..b2d13d72f 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -3090,7 +3090,13 @@ static int gdb_target_add_one(struct target *target)
 		if (!*end) {
 			if (parse_long(gdb_port_next, &portnumber) == ERROR_OK) {
 				free(gdb_port_next);
-				gdb_port_next = alloc_printf("%d", portnumber+1);
+				if (portnumber) {
+					gdb_port_next = alloc_printf("%d", portnumber+1);
+				} else {
+					/* Don't increment if gdb_port is 0, since we're just
+					 * trying to allocate an unused port. */
+					gdb_port_next = strdup("0");
+				}
 			}
 		}
 	}
diff --git a/src/server/server.c b/src/server/server.c
index 517d62a79..1e52e979d 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -298,6 +298,12 @@ int add_service(char *name,
 			free_service(c);
 			return ERROR_FAIL;
 		}
+
+		struct sockaddr_in addr_in;
+		socklen_t addr_in_size = sizeof(addr_in);
+		getsockname(c->fd, (struct sockaddr *)&addr_in, &addr_in_size);
+		LOG_INFO("Listening on port %hu for %s connections",
+				ntohs(addr_in.sin_port), name);
 	} else if (c->type == CONNECTION_STDINOUT) {
 		c->fd = fileno(stdin);
 

From 2f6f60750733c50c815ddc84d09c0694f0693af8 Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Tue, 26 Dec 2017 14:03:42 -0800
Subject: [PATCH 36/65] Accept 64-bit addresses in CRC requests.

Change-Id: I6a3e97ae8ffb4dc2ca1197a8102cb9ba34de0a1e
Signed-off-by: Tim Newsome <tim@sifive.com>
Reviewed-on: http://openocd.zylin.com/4317
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/server/gdb_server.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index b2d13d72f..f90f12bb5 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -2415,13 +2415,13 @@ static int gdb_query_packet(struct connection *connection,
 			char gdb_reply[10];
 			char *separator;
 			uint32_t checksum;
-			uint32_t addr = 0;
+			target_addr_t addr = 0;
 			uint32_t len = 0;
 
 			/* skip command character */
 			packet += 5;
 
-			addr = strtoul(packet, &separator, 16);
+			addr = strtoull(packet, &separator, 16);
 
 			if (*separator != ',') {
 				LOG_ERROR("incomplete read memory packet received, dropping connection");

From ab25205bbfaa73b33d3510086c44cfe06c1ca006 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Tue, 4 Oct 2016 14:33:38 +0300
Subject: [PATCH 37/65] checkpatch: fix more "unescaped left brace" warnings

Unescaped left brace in string literals is deprecated since perl
v5.21.1.

Not a Perl programmer here so please review!

Change-Id: I724d8fa616d603e032e07afb9b8933e0ec95045b
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3801
Tested-by: jenkins
---
 tools/scripts/checkpatch.pl | 28 ++++++++++++++--------------
 1 file changed, 14 insertions(+), 14 deletions(-)

diff --git a/tools/scripts/checkpatch.pl b/tools/scripts/checkpatch.pl
index b977d361f..0a119f1ba 100755
--- a/tools/scripts/checkpatch.pl
+++ b/tools/scripts/checkpatch.pl
@@ -699,7 +699,7 @@ sub ctx_statement_block {
 		# An else is really a conditional as long as its not else if
 		if ($level == 0 && $coff_set == 0 &&
 				(!defined($p) || $p =~ /(?:\s|\}|\+)/) &&
-				$remainder =~ /^(else)(?:\s|{)/ &&
+				$remainder =~ /^(else)(?:\s|\{)/ &&
 				$remainder !~ /^else\s+if\b/) {
 			$coff = $off + length($1) - 1;
 			$coff_set = 1;
@@ -782,7 +782,7 @@ sub statement_block_size {
 	my ($stmt) = @_;
 
 	$stmt =~ s/(^|\n)./$1/g;
-	$stmt =~ s/^\s*{//;
+	$stmt =~ s/^\s*\{//;
 	$stmt =~ s/}\s*$//;
 	$stmt =~ s/^\s*//;
 	$stmt =~ s/\s*$//;
@@ -1136,7 +1136,7 @@ sub annotate_values {
 			print "ASSIGN($1)\n" if ($dbg_values > 1);
 			$type = 'N';
 
-		} elsif ($cur =~/^(;|{|})/) {
+		} elsif ($cur =~/^(;|\{|})/) {
 			print "END($1)\n" if ($dbg_values > 1);
 			$type = 'E';
 			$av_pend_colon = 'O';
@@ -1783,7 +1783,7 @@ sub process {
 			}
 
 			my $s = $stat;
-			$s =~ s/{.*$//s;
+			$s =~ s/\{.*$//s;
 
 			# Ignore goto labels.
 			if ($s =~ /$Ident:\*$/s) {
@@ -1879,7 +1879,7 @@ sub process {
 			#print "realcnt<$realcnt> ctx_cnt<$ctx_cnt>\n";
 			#print "pre<$pre_ctx>\nline<$line>\nctx<$ctx>\nnext<$lines[$ctx_ln - 1]>\n";
 
-			if ($ctx !~ /{\s*/ && defined($lines[$ctx_ln -1]) && $lines[$ctx_ln - 1] =~ /^\+\s*{/) {
+			if ($ctx !~ /\{\s*/ && defined($lines[$ctx_ln -1]) && $lines[$ctx_ln - 1] =~ /^\+\s*\{/) {
 				ERROR("OPEN_BRACE",
 				      "that open brace { should be on the previous line\n" .
 					"$here\n$ctx\n$rawlines[$ctx_ln - 1]\n");
@@ -1920,7 +1920,7 @@ sub process {
 			my $continuation = 0;
 			my $check = 0;
 			$s =~ s/^.*\bdo\b//;
-			$s =~ s/^\s*{//;
+			$s =~ s/^\s*\{//;
 			if ($s =~ s/^\s*\\//) {
 				$continuation = 1;
 			}
@@ -2024,7 +2024,7 @@ sub process {
 		}
 
 # check for initialisation to aggregates open brace on the next line
-		if ($line =~ /^.\s*{/ &&
+		if ($line =~ /^.\s*\{/ &&
 		    $prevline =~ /(?:^|[^=])=\s*$/) {
 			ERROR("OPEN_BRACE",
 			      "that open brace { should be on the previous line\n" . $hereprev);
@@ -2231,7 +2231,7 @@ sub process {
 		}
 
 # open braces for enum, union and struct go on the same line.
-		if ($line =~ /^.\s*{/ &&
+		if ($line =~ /^.\s*\{/ &&
 		    $prevline =~ /^.\s*(?:typedef\s+)?(enum|union|struct)(?:\s+$Ident)?\s*$/) {
 			ERROR("OPEN_BRACE",
 			      "open brace '{' following $1 go on the same line\n" . $hereprev);
@@ -2251,7 +2251,7 @@ sub process {
 			my ($where, $prefix) = ($-[1], $1);
 			if ($prefix !~ /$Type\s+$/ &&
 			    ($where != 0 || $prefix !~ /^.\s+$/) &&
-			    $prefix !~ /{\s+$/) {
+			    $prefix !~ /\{\s+$/) {
 				ERROR("BRACKET_SPACE",
 				      "space prohibited before open square bracket '['\n" . $herecurr);
 			}
@@ -2493,7 +2493,7 @@ sub process {
 ## 		}
 
 #need space before brace following if, while, etc
-		if (($line =~ /\(.*\)\{/ && $line !~ /\($Type\){/) ||
+		if (($line =~ /\(.*\)\{/ && $line !~ /\($Type\)\{/) ||
 		    $line =~ /do\{/) {
 			ERROR("SPACING",
 			      "space required before the open brace '{'\n" . $herecurr);
@@ -2598,7 +2598,7 @@ sub process {
 
 # Check for illegal assignment in if conditional -- and check for trailing
 # statements after the conditional.
-		if ($line =~ /do\s*(?!{)/) {
+		if ($line =~ /do\s*(?!\{)/) {
 			my ($stat_next) = ctx_statement_block($line_nr_next,
 						$remain_next, $off_next);
 			$stat_next =~ s/\n./\n /g;
@@ -2630,7 +2630,7 @@ sub process {
 			substr($s, 0, length($c), '');
 			$s =~ s/\n.*//g;
 			$s =~ s/$;//g; 	# Remove any comments
-			if (length($c) && $s !~ /^\s*{?\s*\\*\s*$/ &&
+			if (length($c) && $s !~ /^\s*\{?\s*\\*\s*$/ &&
 			    $c !~ /}\s*while\s*/)
 			{
 				# Find out how long the conditional actually is.
@@ -2669,7 +2669,7 @@ sub process {
 		if ($line =~ /^.\s*(?:}\s*)?else\b(.*)/) {
 			my $s = $1;
 			$s =~ s/$;//g; 	# Remove any comments
-			if ($s !~ /^\s*(?:\sif|(?:{|)\s*\\?\s*$)/) {
+			if ($s !~ /^\s*(?:\sif|(?:\{|)\s*\\?\s*$)/) {
 				ERROR("TRAILING_STATEMENTS",
 				      "trailing statements should be on next line\n" . $herecurr);
 			}
@@ -2879,7 +2879,7 @@ sub process {
 
 					substr($block, 0, length($cond), '');
 
-					$seen++ if ($block =~ /^\s*{/);
+					$seen++ if ($block =~ /^\s*\{/);
 
 					#print "cond<$cond> block<$block> allowed<$allowed>\n";
 					if (statement_lines($cond) > 1) {

From 1003bc7be7c471d28794280be69632c5f074789f Mon Sep 17 00:00:00 2001
From: Patrick Stewart <patstew@gmail.com>
Date: Wed, 27 Aug 2014 17:17:27 +0100
Subject: [PATCH 38/65] ftdi swd: disable SWD output pin during input

 * Disables the data output pin while SWD is reading, so that a simple FTDI
   SWD interface can be made by connecting TCK to SWD_CLK and TDI+TDO directly
   to SWDIO. Enabled by setting SWDIO_OE to 0.

Change-Id: I7d3b71cf3f4eea163cb320aff69ed95d219190bd
Signed-off-by: Patrick Stewart <patstew@gmail.com>
Signed-off-by: Roger Lendenmann <roger.lendenmann@intel.com>
Reviewed-on: http://openocd.zylin.com/2274
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/jtag/drivers/ftdi.c                  | 15 ++++++-
 tcl/board/nordic_nrf52_ftx232.cfg        | 11 +++++
 tcl/interface/ftdi/ft232h-module-swd.cfg | 52 +++++++++++++++++++++++
 tcl/interface/ftdi/minimodule-swd.cfg    | 54 ++++++++++++++++++++++++
 4 files changed, 130 insertions(+), 2 deletions(-)
 create mode 100644 tcl/board/nordic_nrf52_ftx232.cfg
 create mode 100644 tcl/interface/ftdi/ft232h-module-swd.cfg
 create mode 100644 tcl/interface/ftdi/minimodule-swd.cfg

diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index 32876bac5..e69707e5a 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -1064,8 +1064,19 @@ static int ftdi_swd_init(void)
 static void ftdi_swd_swdio_en(bool enable)
 {
 	struct signal *oe = find_signal_by_name("SWDIO_OE");
-	if (oe)
-		ftdi_set_signal(oe, enable ? '1' : '0');
+	if (oe) {
+		if (oe->data_mask)
+			ftdi_set_signal(oe, enable ? '1' : '0');
+		else {
+			/* Sets TDI/DO pin (pin 2) to input during rx when both pins are connected
+			   to SWDIO */
+			if (enable)
+				direction |= jtag_direction_init & 0x0002U;
+			else
+				direction &= ~0x0002U;
+			mpsse_set_data_bits_low_byte(mpsse_ctx, output & 0xff, direction & 0xff);
+		}
+	}
 }
 
 /**
diff --git a/tcl/board/nordic_nrf52_ftx232.cfg b/tcl/board/nordic_nrf52_ftx232.cfg
new file mode 100644
index 000000000..938efedae
--- /dev/null
+++ b/tcl/board/nordic_nrf52_ftx232.cfg
@@ -0,0 +1,11 @@
+#
+# nordic module NRF52 (nRF52832/52840) attached to an adafruit ft232h module
+# or any FT232H/FT2232H/FT4232H based board/module
+#
+
+source [find interface/ftdi/ft232h-module-swd.cfg]
+#source [find interface/ftdi/minimodule-swd.cfg]
+
+transport select swd
+
+source [find target/nrf52.cfg]
diff --git a/tcl/interface/ftdi/ft232h-module-swd.cfg b/tcl/interface/ftdi/ft232h-module-swd.cfg
new file mode 100644
index 000000000..d2bd1da61
--- /dev/null
+++ b/tcl/interface/ftdi/ft232h-module-swd.cfg
@@ -0,0 +1,52 @@
+#
+# ADAFRUIT FTDI FT232H as a SWD direct connect interface
+# Any FT232H based board may work
+#
+# http://www.ftdichip.com/Products/ICs/FT232H.htm
+#
+#
+
+interface ftdi
+
+ftdi_vid_pid 0x0403 0x6014
+
+# data MSB..LSB       direction (1:out) MSB..LSB
+# 0000'0000'0011'0000 0000'0000'0011'1011
+ftdi_layout_init 0x0030 0x003b
+# 0xfff8 0xfffb
+# Those signal are only required on some platforms or may required to be
+# enabled explicitely (e.g. nrf5x chips).
+ftdi_layout_signal nSRST -data 0x0010 -oe 0x0010
+ftdi_layout_signal nTRST -data 0x0020 -oe 0x0020
+
+# swd enable
+ftdi_layout_signal SWD_EN -data 0
+# tri-state (configure as input) TDO/TIO when reading
+ftdi_layout_signal SWDIO_OE -data 0
+
+transport select swd
+
+# re-configure TDO as tri-state
+#ftdi_layout_signal TDO -data 0x0002 -oe 0x0002
+#ftdi_layout_signal TDI -data 0x0004
+
+# Adafruit      FT232H    JTAG       SWD
+# Name  Pin     Name      Func       Func
+#  D0   J1-3    ADBUS0    TCK        SWDCLK
+#  D1   J1-4    ADBUS1    TDO/DI     SWDIO
+#  D2   J1-5    ADBUS2    TDI/DO     SWDIO
+#  D3   J1-6    ADBUS3    TMS        N/A
+#  D4   J1-7    ADBUS4    (GPIOL0)   /nSRST  optional module reset
+#  D5   J1-8    ADBUS5    (GPIOL1)   /nTRST  optional target reset
+#  D6   J1-9    ADBUS6    (GPIOL2)
+#  D7   J1-10   ADBUS7    (GPIOL3)
+#  C0   J2-1    ACBUS0    (GPIOH0)
+#  C1   J2-2    ACBUS1    (GPIOH1)
+#  C2   J2-3    ACBUS2    (GPIOH2)
+#  C3   J2-4    ACBUS3    (GPIOH3)
+#  C4   J2-5    ACBUS4    (GPIOH4)
+#  C5   J2-6    ACBUS5    (GPIOH5)
+#  C6   J2-7    ACBUS6    (GPIOH6)
+#  C7   J2-8    ACBUS7    (GPIOH7)
+#  C8   J2-9    ACBUS8
+#  C9   J2-10   ACBUS9
diff --git a/tcl/interface/ftdi/minimodule-swd.cfg b/tcl/interface/ftdi/minimodule-swd.cfg
new file mode 100644
index 000000000..5f0b212f5
--- /dev/null
+++ b/tcl/interface/ftdi/minimodule-swd.cfg
@@ -0,0 +1,54 @@
+#
+Supports SWD using the FT2232H or FT4232H minimodule.
+# Each can support 2 SWD interfaces.
+#
+# FT2232H or FT4232H minimodule channel 0 (Channel A)
+# Connector  FTDI              Target
+# Pin        Name
+# ---------  ------            ------
+# CN2-11     VIO               VDD_IO (Or connect to CN2-5 on the minimodule instead for a 3V3 interface)
+# CN2-2      GND               GND
+# CN2-7      ADBUS0 (TCK)      SWCLK
+# CN2-9      ADBUS2 (TDI/TDO)  SWDIO
+# CN2-10     ADBUS1 (TDO/TDI)  SWDIO
+# CN2-14     ADBUS4 (GPIOL0)   nRESET
+#
+# FT2232H minimodule channel 1 (Channel B)
+# FTDI       Target
+# ----       ------
+# CN2-11  -  VDD_IO
+# CN2-2   -  GND
+# CN3-26  -  SWCLK
+# CN3-25  -  SWDIO
+# CN3-24  -  SWDIO
+# CN3-21  -  nRESET
+#
+# FT4232H minimodule channel 1 (Channel B)
+# FTDI       Target
+# ----       ------
+# CN2-11  -  VDD_IO
+# CN2-2   -  GND
+# CN2-18  -  SWCLK
+# CN2-17  -  SWDIO
+# CN2-20  -  SWDIO
+# CN2-22  -  nRESET
+#
+
+interface ftdi
+
+#Select your module type and channel
+
+#ftdi_device_desc "FT2232H MiniModule"
+ftdi_vid_pid 0x0403 0x6010
+#ftdi_channel 1
+
+#ftdi_device_desc "FT4232H MiniModule"
+#ftdi_vid_pid 0x0403 0x6011
+#ftdi_channel 1
+
+ftdi_layout_init 0x0000 0x000b
+ftdi_layout_signal nSRST -data 0x0010 -oe 0x0010
+ftdi_layout_signal SWD_EN -data 0
+ftdi_layout_signal SWDIO_OE -data 0
+
+transport select swd

From 22ad1c0f6057aed642d5b1229b24e52dd26b1ec4 Mon Sep 17 00:00:00 2001
From: Marc Schink <openocd-dev@marcschink.de>
Date: Wed, 13 Apr 2016 11:00:19 +0200
Subject: [PATCH 39/65] server/telnet: Use proper data types

Change-Id: Ie7588e311fa5155d5ee73148dee3d0d931bfc7f5
Signed-off-by: Marc Schink <openocd-dev@marcschink.de>
Reviewed-on: http://openocd.zylin.com/3413
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/server/telnet_server.c | 54 ++++++++++++++++++++++----------------
 src/server/telnet_server.h | 11 ++++----
 2 files changed, 38 insertions(+), 27 deletions(-)

diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 06a9c7a6c..9077b6c46 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -54,7 +54,7 @@ static int telnet_write(struct connection *connection, const void *data,
 
 	if (connection_write(connection, data, len) == len)
 		return ERROR_OK;
-	t_con->closed = 1;
+	t_con->closed = true;
 	return ERROR_SERVER_REMOTE_CLOSED;
 }
 
@@ -101,29 +101,36 @@ static void telnet_log_callback(void *priv, const char *file, unsigned line,
 {
 	struct connection *connection = priv;
 	struct telnet_connection *t_con = connection->priv;
-	int i;
+	size_t i;
+	size_t tmp;
 
-	/* if there is no prompt, simply output the message */
-	if (t_con->line_cursor < 0) {
+	/* If the prompt is not visible, simply output the message. */
+	if (!t_con->prompt_visible) {
 		telnet_outputline(connection, string);
 		return;
 	}
 
-	/* clear the command line */
-	for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
-		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b", i > 16 ? 16 : i);
-	for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
-		telnet_write(connection, "                ", i > 16 ? 16 : i);
-	for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
-		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b", i > 16 ? 16 : i);
+	/* Clear the command line. */
+	tmp = strlen(t_con->prompt) + t_con->line_size;
+
+	for (i = 0; i < tmp; i += 16)
+		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+			MIN(tmp - i, 16));
+
+	for (i = 0; i < tmp; i += 16)
+		telnet_write(connection, "                ", MIN(tmp - i, 16));
+
+	for (i = 0; i < tmp; i += 16)
+		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+			MIN(tmp - i, 16));
 
-	/* output the message */
 	telnet_outputline(connection, string);
 
-	/* put the command line to its previous state */
+	/* Put the command line to its previous state. */
 	telnet_prompt(connection);
 	telnet_write(connection, t_con->line, t_con->line_size);
-	for (i = t_con->line_size; i > t_con->line_cursor; i--)
+
+	for (i = t_con->line_cursor; i < t_con->line_size; i++)
 		telnet_write(connection, "\b", 1);
 }
 
@@ -219,10 +226,11 @@ static int telnet_new_connection(struct connection *connection)
 	connection->priv = telnet_connection;
 
 	/* initialize telnet connection information */
-	telnet_connection->closed = 0;
+	telnet_connection->closed = false;
 	telnet_connection->line_size = 0;
 	telnet_connection->line_cursor = 0;
 	telnet_connection->prompt = strdup("> ");
+	telnet_connection->prompt_visible = true;
 	telnet_connection->state = TELNET_STATE_DATA;
 
 	/* output goes through telnet connection */
@@ -289,7 +297,7 @@ static void telnet_history_up(struct connection *connection)
 {
 	struct telnet_connection *t_con = connection->priv;
 
-	int last_history = (t_con->current_history > 0) ?
+	size_t last_history = (t_con->current_history > 0) ?
 				t_con->current_history - 1 :
 				TELNET_LINE_HISTORY_SIZE-1;
 	telnet_history_go(connection, last_history);
@@ -298,8 +306,9 @@ static void telnet_history_up(struct connection *connection)
 static void telnet_history_down(struct connection *connection)
 {
 	struct telnet_connection *t_con = connection->priv;
+	size_t next_history;
 
-	int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
+	next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
 	telnet_history_go(connection, next_history);
 }
 
@@ -363,7 +372,7 @@ static int telnet_input(struct connection *connection)
 							t_con->line[t_con->line_size++] = *buf_p;
 							t_con->line_cursor++;
 						} else {
-							int i;
+							size_t i;
 							memmove(t_con->line + t_con->line_cursor + 1,
 									t_con->line + t_con->line_cursor,
 									t_con->line_size - t_con->line_cursor);
@@ -398,7 +407,7 @@ static int telnet_input(struct connection *connection)
 							telnet_write(connection, "\r\n\x00", 3);
 
 							if (strcmp(t_con->line, "history") == 0) {
-								int i;
+								size_t i;
 								for (i = 1; i < TELNET_LINE_HISTORY_SIZE; i++) {
 									/* the t_con->next_history line contains empty string
 									 * (unless NULL), thus it is not printed */
@@ -444,7 +453,7 @@ static int telnet_input(struct connection *connection)
 							t_con->line_size = 0;
 
 							/* to suppress prompt in log callback during command execution */
-							t_con->line_cursor = -1;
+							t_con->prompt_visible = false;
 
 							if (strcmp(t_con->line, "shutdown") == 0)
 								telnet_save_history(t_con);
@@ -452,6 +461,7 @@ static int telnet_input(struct connection *connection)
 							retval = command_run_line(command_context, t_con->line);
 
 							t_con->line_cursor = 0;
+							t_con->prompt_visible = true;
 
 							if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
 								return ERROR_SERVER_REMOTE_CLOSED;
@@ -466,7 +476,7 @@ static int telnet_input(struct connection *connection)
 						} else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) {	/* delete character */
 							if (t_con->line_cursor > 0) {
 								if (t_con->line_cursor != t_con->line_size) {
-									int i;
+									size_t i;
 									telnet_write(connection, "\b", 1);
 									t_con->line_cursor--;
 									t_con->line_size--;
@@ -566,7 +576,7 @@ static int telnet_input(struct connection *connection)
 					/* Remove character */
 					if (*buf_p == '~') {
 						if (t_con->line_cursor < t_con->line_size) {
-							int i;
+							size_t i;
 							t_con->line_size--;
 							/* remove char from line buffer */
 							memmove(t_con->line + t_con->line_cursor,
diff --git a/src/server/telnet_server.h b/src/server/telnet_server.h
index f8fb82689..5e238f441 100644
--- a/src/server/telnet_server.h
+++ b/src/server/telnet_server.h
@@ -46,15 +46,16 @@ enum telnet_states {
 
 struct telnet_connection {
 	char *prompt;
+	bool prompt_visible;
 	enum telnet_states state;
 	char line[TELNET_LINE_MAX_SIZE];
-	int line_size;
-	int line_cursor;
+	size_t line_size;
+	size_t line_cursor;
 	char last_escape;
 	char *history[TELNET_LINE_HISTORY_SIZE];
-	int next_history;
-	int current_history;
-	int closed;
+	size_t next_history;
+	size_t current_history;
+	bool closed;
 };
 
 struct telnet_service {

From 867bdb2e9248a974f7db0a99fbe5d2dd8b46d25d Mon Sep 17 00:00:00 2001
From: Robert Jordens <jordens@gmail.com>
Date: Tue, 8 Aug 2017 18:16:35 +0200
Subject: [PATCH 40/65] jtagspi: new protocol that includes transfer length

This commit contains a rewrite of the jtagspi protocol and covers both
changes in the jtagspi.c openocd driver and the bscan_spi
(xilinx_bscan_spi) proxy bitstreams. The changes are as follows:

1. Always perform IR scan to ensure proper clearing of BYPASSed DRs.
2. Insert alignment cycles for all BYPASSed TAPs:

  The previous logic was erroneous. The delay in clock cyles from a bit
  written to the jtag interface to a bit read by the jtag interface is:

  * The number of BYPASSed TAPs before this (jtagspi) tap
  * The length of the jtagspi data register (1)
  * The number of BYPASSed TAPs before this one.

  I.e. it is just the number of enabled TAPs. This also gets rid of the
  configuration parameter DR_LENGTH.

3. Use marker bit to start spi transfer

  If there are TAPs ahead of this one on the JTAG chain, and we are in
  DR-SHIFT, there will be old bits toggled through first before the first
  valid bit destined for the flash.
  This delays the begin of the JTAGSPI transaction until the first high bit.

4. New jtagspi protocol

  A JTAGSPI transfer now consists of:

  * an arbitrary number of 0 bits (from BYPASS registers in front of the
    JTAG2SPI DR)
  * a marker bit (1) indicating the start of the JTAG2SPI transaction
  * 32 bits (big endian) describing the length of the SPI transaction
  * a number of SPI clock cycles (corresponding to 3.) with CS_N asserted
  * an arbitrary number of cycles (to shift MISO/TDO data through
    subsequent BYPASS registers)

5. xilinx_bscan_spi: clean up, add ultrascale

This is tested on the following configurations:

* KC705: XC7K325T
* Sayma AMC: XCKU040
* Sayma AMC + RTM): XCKU040 + XC7A15T, a board with integrated FTDI JTAG
  adapter, SCANSTA JTAG router, a Xilinx Ultrascale XCKU040 and a Xilinx
  Artix 7 15T. https://github.com/m-labs/sinara/wiki/Sayma
* Custom board with Lattice FPGA + XC7A35T
* CUstom board with 3x XCKU115-2FLVA1517E

Change-Id: I7361e9fb284ebb916302941735eebef3612aa103
Signed-off-by: Robert Jordens <jordens@gmail.com>
Reviewed-on: http://openocd.zylin.com/4236
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 .../loaders/flash/fpga/xilinx_bscan_spi.py    | 358 +++++++++++++++---
 doc/openocd.texi                              |   5 +-
 src/flash/nor/jtagspi.c                       |  64 +++-
 tcl/cpld/jtagspi.cfg                          |   8 +-
 4 files changed, 342 insertions(+), 93 deletions(-)

diff --git a/contrib/loaders/flash/fpga/xilinx_bscan_spi.py b/contrib/loaders/flash/fpga/xilinx_bscan_spi.py
index fa4ec2ace..4246aa2f3 100755
--- a/contrib/loaders/flash/fpga/xilinx_bscan_spi.py
+++ b/contrib/loaders/flash/fpga/xilinx_bscan_spi.py
@@ -13,8 +13,11 @@
 #  GNU General Public License for more details.
 #
 
-from migen import *
-from migen.build.generic_platform import *
+import unittest
+
+import migen as mg
+import migen.build.generic_platform as mb
+from migen.genlib import io
 from migen.build import xilinx
 
 
@@ -25,59 +28,256 @@ behind FPGAs.
 Bitstream binaries built with this script are available at:
 https://github.com/jordens/bscan_spi_bitstreams
 
-JTAG signalling is connected directly to SPI signalling. CS_N is
-asserted when the JTAG IR contains the USER1 instruction and the state is
-SHIFT-DR. Xilinx bscan cells sample TDO on falling TCK and forward it.
-MISO requires sampling on rising CLK and leads to one cycle of latency.
+A JTAG2SPI transfer consists of:
+
+1. an arbitrary number of 0 bits (from BYPASS registers in front of the
+   JTAG2SPI DR)
+2. a marker bit (1) indicating the start of the JTAG2SPI transaction
+3. 32 bits (big endian) describing the length of the SPI transaction
+4. a number of SPI clock cycles (corresponding to 3.) with CS_N asserted
+5. an arbitrary number of cycles (to shift MISO/TDO data through subsequent
+   BYPASS registers)
+
+Notes:
+
+* The JTAG2SPI DR is 1 bit long (due to different sampling edges of
+  {MISO,MOSI}/{TDO,TDI}).
+* MOSI is TDI with half a cycle delay.
+* TDO is MISO with half a cycle delay.
+* CAPTURE-DR needs to be performed before SHIFT-DR on the BYPASSed TAPs in
+  JTAG chain to clear the BYPASS registers to 0.
 
 https://github.com/m-labs/migen
 """
 
 
-class Spartan3(Module):
+class JTAG2SPI(mg.Module):
+    def __init__(self, spi=None, bits=32):
+        self.jtag = mg.Record([
+            ("sel", 1),
+            ("shift", 1),
+            ("capture", 1),
+            ("tck", 1),
+            ("tdi", 1),
+            ("tdo", 1),
+        ])
+        self.cs_n = mg.TSTriple()
+        self.clk = mg.TSTriple()
+        self.mosi = mg.TSTriple()
+        self.miso = mg.TSTriple()
+
+        # # #
+
+        self.cs_n.o.reset = mg.Constant(1)
+        self.mosi.o.reset_less = True
+        bits = mg.Signal(bits, reset_less=True)
+        head = mg.Signal(max=len(bits), reset=len(bits) - 1)
+        self.clock_domains.cd_sys = mg.ClockDomain()
+        self.submodules.fsm = mg.FSM("IDLE")
+        if spi is not None:
+            self.specials += [
+                    self.cs_n.get_tristate(spi.cs_n),
+                    self.mosi.get_tristate(spi.mosi),
+                    self.miso.get_tristate(spi.miso),
+            ]
+            if hasattr(spi, "clk"):  # 7 Series drive it fixed
+                self.specials += self.clk.get_tristate(spi.clk)
+                # self.specials += io.DDROutput(1, 0, spi.clk, self.clk.o)
+        self.comb += [
+                self.cd_sys.rst.eq(self.jtag.sel & self.jtag.capture),
+                self.cd_sys.clk.eq(self.jtag.tck),
+                self.cs_n.oe.eq(self.jtag.sel),
+                self.clk.oe.eq(self.jtag.sel),
+                self.mosi.oe.eq(self.jtag.sel),
+                self.miso.oe.eq(0),
+                # Do not suppress CLK toggles outside CS_N asserted.
+                # Xilinx USRCCLK0 requires three dummy cycles to do anything
+                # https://www.xilinx.com/support/answers/52626.html
+                # This is fine since CS_N changes only on falling CLK.
+                self.clk.o.eq(~self.jtag.tck),
+                self.jtag.tdo.eq(self.miso.i),
+        ]
+        # Latency calculation (in half cycles):
+        # 0 (falling TCK, rising CLK):
+        #   JTAG adapter: set TDI
+        # 1 (rising TCK, falling CLK):
+        #   JTAG2SPI: sample TDI -> set MOSI
+        #   SPI: set MISO
+        # 2 (falling TCK, rising CLK):
+        #   SPI: sample MOSI
+        #   JTAG2SPI (BSCAN primitive): sample MISO -> set TDO
+        # 3 (rising TCK, falling CLK):
+        #   JTAG adapter: sample TDO
+        self.fsm.act("IDLE",
+                mg.If(self.jtag.tdi & self.jtag.sel & self.jtag.shift,
+                    mg.NextState("HEAD")
+                )
+        )
+        self.fsm.act("HEAD",
+                mg.If(head == 0,
+                    mg.NextState("XFER")
+                )
+        )
+        self.fsm.act("XFER",
+                mg.If(bits == 0,
+                    mg.NextState("IDLE")
+                ),
+        )
+        self.sync += [
+                self.mosi.o.eq(self.jtag.tdi),
+                self.cs_n.o.eq(~self.fsm.ongoing("XFER")),
+                mg.If(self.fsm.ongoing("HEAD"),
+                    bits.eq(mg.Cat(self.jtag.tdi, bits)),
+                    head.eq(head - 1)
+                ),
+                mg.If(self.fsm.ongoing("XFER"),
+                    bits.eq(bits - 1)
+                )
+        ]
+
+
+class JTAG2SPITest(unittest.TestCase):
+    def setUp(self):
+        self.bits = 8
+        self.dut = JTAG2SPI(bits=self.bits)
+
+    def test_instantiate(self):
+        pass
+
+    def test_initial_conditions(self):
+        def check():
+            yield
+            self.assertEqual((yield self.dut.cs_n.oe), 0)
+            self.assertEqual((yield self.dut.mosi.oe), 0)
+            self.assertEqual((yield self.dut.miso.oe), 0)
+            self.assertEqual((yield self.dut.clk.oe), 0)
+        mg.run_simulation(self.dut, check())
+
+    def test_enable(self):
+        def check():
+            yield self.dut.jtag.sel.eq(1)
+            yield self.dut.jtag.shift.eq(1)
+            yield
+            self.assertEqual((yield self.dut.cs_n.oe), 1)
+            self.assertEqual((yield self.dut.mosi.oe), 1)
+            self.assertEqual((yield self.dut.miso.oe), 0)
+            self.assertEqual((yield self.dut.clk.oe), 1)
+        mg.run_simulation(self.dut, check())
+
+    def run_seq(self, tdi, tdo, spi=None):
+        yield self.dut.jtag.sel.eq(1)
+        yield
+        yield self.dut.jtag.shift.eq(1)
+        for di in tdi:
+            yield self.dut.jtag.tdi.eq(di)
+            yield
+            tdo.append((yield self.dut.jtag.tdo))
+            if spi is not None:
+                v = []
+                for k in "cs_n clk mosi miso".split():
+                    t = getattr(self.dut, k)
+                    v.append("{}>".format((yield t.o)) if (yield t.oe)
+                            else "<{}".format((yield t.i)))
+                spi.append(" ".join(v))
+        yield self.dut.jtag.sel.eq(0)
+        yield
+        yield self.dut.jtag.shift.eq(0)
+        yield
+
+    def test_shift(self):
+        bits = 8
+        data = 0x81
+        tdi = [0, 0, 1]  # dummy from BYPASS TAPs and marker
+        tdi += [((bits - 1) >> j) & 1 for j in range(self.bits - 1, -1, -1)]
+        tdi += [(data >> j) & 1 for j in range(bits)]
+        tdi += [0, 0, 0, 0]  # dummy from BYPASS TAPs
+        tdo = []
+        spi = []
+        mg.run_simulation(self.dut, self.run_seq(tdi, tdo, spi))
+        # print(tdo)
+        for l in spi:
+            print(l)
+
+
+class Spartan3(mg.Module):
     macro = "BSCAN_SPARTAN3"
     toolchain = "ise"
 
     def __init__(self, platform):
         platform.toolchain.bitgen_opt += " -g compress -g UnusedPin:Pullup"
-        self.clock_domains.cd_jtag = ClockDomain(reset_less=True)
-        spi = platform.request("spiflash")
-        shift = Signal()
-        tdo = Signal()
-        sel1 = Signal()
-        self.comb += [
-            self.cd_jtag.clk.eq(spi.clk),
-            spi.cs_n.eq(~shift | ~sel1),
+        self.submodules.j2s = j2s = JTAG2SPI(platform.request("spiflash"))
+        self.specials += [
+                mg.Instance(
+                    self.macro,
+                    o_SHIFT=j2s.jtag.shift, o_SEL1=j2s.jtag.sel,
+                    o_CAPTURE=j2s.jtag.capture,
+                    o_DRCK1=j2s.jtag.tck,
+                    o_TDI=j2s.jtag.tdi, i_TDO1=j2s.jtag.tdo,
+                    i_TDO2=0),
         ]
-        self.sync.jtag += tdo.eq(spi.miso)
-        self.specials += Instance(self.macro,
-                                  o_DRCK1=spi.clk, o_SHIFT=shift,
-                                  o_TDI=spi.mosi, i_TDO1=tdo, i_TDO2=0,
-                                  o_SEL1=sel1)
+        platform.add_period_constraint(j2s.jtag.tck, 6)
 
 
 class Spartan3A(Spartan3):
     macro = "BSCAN_SPARTAN3A"
 
 
-class Spartan6(Module):
+class Spartan6(mg.Module):
     toolchain = "ise"
 
     def __init__(self, platform):
         platform.toolchain.bitgen_opt += " -g compress -g UnusedPin:Pullup"
-        self.clock_domains.cd_jtag = ClockDomain(reset_less=True)
-        spi = platform.request("spiflash")
-        shift = Signal()
-        tdo = Signal()
-        sel = Signal()
-        self.comb += self.cd_jtag.clk.eq(spi.clk), spi.cs_n.eq(~shift | ~sel)
-        self.sync.jtag += tdo.eq(spi.miso)
-        self.specials += Instance("BSCAN_SPARTAN6", p_JTAG_CHAIN=1,
-                                  o_TCK=spi.clk, o_SHIFT=shift, o_SEL=sel,
-                                  o_TDI=spi.mosi, i_TDO=tdo)
+        self.submodules.j2s = j2s = JTAG2SPI(platform.request("spiflash"))
+        # clk = mg.Signal()
+        self.specials += [
+                mg.Instance(
+                    "BSCAN_SPARTAN6", p_JTAG_CHAIN=1,
+                    o_SHIFT=j2s.jtag.shift, o_SEL=j2s.jtag.sel,
+                    o_CAPTURE=j2s.jtag.capture,
+                    o_DRCK=j2s.jtag.tck,
+                    o_TDI=j2s.jtag.tdi, i_TDO=j2s.jtag.tdo),
+                # mg.Instance("BUFG", i_I=clk, o_O=j2s.jtag.tck)
+        ]
+        platform.add_period_constraint(j2s.jtag.tck, 6)
 
 
-class Series7(Module):
+class Series7(mg.Module):
+    toolchain = "vivado"
+
+    def __init__(self, platform):
+        platform.toolchain.bitstream_commands.extend([
+            "set_property BITSTREAM.GENERAL.COMPRESS True [current_design]",
+            "set_property BITSTREAM.CONFIG.UNUSEDPIN Pullnone [current_design]"
+        ])
+        self.submodules.j2s = j2s = JTAG2SPI(platform.request("spiflash"))
+        # clk = mg.Signal()
+        self.specials += [
+                mg.Instance(
+                    "BSCANE2", p_JTAG_CHAIN=1,
+                    o_SHIFT=j2s.jtag.shift, o_SEL=j2s.jtag.sel,
+                    o_CAPTURE=j2s.jtag.capture,
+                    o_DRCK=j2s.jtag.tck,
+                    o_TDI=j2s.jtag.tdi, i_TDO=j2s.jtag.tdo),
+                mg.Instance(
+                    "STARTUPE2", i_CLK=0, i_GSR=0, i_GTS=0,
+                    i_KEYCLEARB=0, i_PACK=1,
+                    i_USRCCLKO=j2s.clk.o, i_USRCCLKTS=~j2s.clk.oe,
+                    i_USRDONEO=1, i_USRDONETS=1),
+                # mg.Instance("BUFG", i_I=clk, o_O=j2s.jtag.tck)
+        ]
+        platform.add_period_constraint(j2s.jtag.tck, 6)
+        try:
+            self.comb += [
+                    platform.request("user_sma_gpio_p").eq(j2s.cs_n.i),
+                    platform.request("user_sma_gpio_n").eq(j2s.clk.o),
+                    platform.request("user_sma_clock_p").eq(j2s.mosi.o),
+                    platform.request("user_sma_clock_n").eq(j2s.miso.i),
+            ]
+        except mb.ConstraintError:
+            pass
+
+
+class Ultrascale(mg.Module):
     toolchain = "vivado"
 
     def __init__(self, platform):
@@ -85,20 +285,33 @@ class Series7(Module):
             "set_property BITSTREAM.GENERAL.COMPRESS True [current_design]",
             "set_property BITSTREAM.CONFIG.UNUSEDPIN Pullnone [current_design]",
         ])
-        self.clock_domains.cd_jtag = ClockDomain(reset_less=True)
-        spi = platform.request("spiflash")
-        clk = Signal()
-        shift = Signal()
-        tdo = Signal()
-        sel = Signal()
-        self.comb += self.cd_jtag.clk.eq(clk), spi.cs_n.eq(~shift | ~sel)
-        self.sync.jtag += tdo.eq(spi.miso)
-        self.specials += Instance("BSCANE2", p_JTAG_CHAIN=1,
-                                  o_SHIFT=shift, o_TCK=clk, o_SEL=sel,
-                                  o_TDI=spi.mosi, i_TDO=tdo)
-        self.specials += Instance("STARTUPE2", i_CLK=0, i_GSR=0, i_GTS=0,
-                                  i_KEYCLEARB=0, i_PACK=1, i_USRCCLKO=clk,
-                                  i_USRCCLKTS=0, i_USRDONEO=1, i_USRDONETS=1)
+        self.submodules.j2s0 = j2s0 = JTAG2SPI()
+        self.submodules.j2s1 = j2s1 = JTAG2SPI(platform.request("spiflash"))
+        di = mg.Signal(4)
+        self.comb += mg.Cat(j2s0.mosi.i, j2s0.miso.i).eq(di)
+        self.specials += [
+                mg.Instance("BSCANE2", p_JTAG_CHAIN=1,
+                    o_SHIFT=j2s0.jtag.shift, o_SEL=j2s0.jtag.sel,
+                    o_CAPTURE=j2s0.jtag.capture,
+                    o_DRCK=j2s0.jtag.tck,
+                    o_TDI=j2s0.jtag.tdi, i_TDO=j2s0.jtag.tdo),
+                mg.Instance("BSCANE2", p_JTAG_CHAIN=2,
+                    o_SHIFT=j2s1.jtag.shift, o_SEL=j2s1.jtag.sel,
+                    o_CAPTURE=j2s1.jtag.capture,
+                    o_DRCK=j2s1.jtag.tck,
+                    o_TDI=j2s1.jtag.tdi, i_TDO=j2s1.jtag.tdo),
+                mg.Instance("STARTUPE3", i_GSR=0, i_GTS=0,
+                    i_KEYCLEARB=0, i_PACK=1,
+                    i_USRDONEO=1, i_USRDONETS=1,
+                    i_USRCCLKO=mg.Mux(j2s0.clk.oe, j2s0.clk.o, j2s1.clk.o),
+                    i_USRCCLKTS=~(j2s0.clk.oe | j2s1.clk.oe),
+                    i_FCSBO=j2s0.cs_n.o, i_FCSBTS=~j2s0.cs_n.oe,
+                    o_DI=di,
+                    i_DO=mg.Cat(j2s0.mosi.o, j2s0.miso.o, 0, 0),
+                    i_DTS=mg.Cat(~j2s0.mosi.oe, ~j2s0.miso.oe, 1, 1))
+        ]
+        platform.add_period_constraint(j2s0.jtag.tck, 6)
+        platform.add_period_constraint(j2s1.jtag.tck, 6)
 
 
 class XilinxBscanSpi(xilinx.XilinxPlatform):
@@ -124,6 +337,7 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         ("fbg484-1", 2): ["L16", None, "H18", "H19", "G18", "F19"],
         ("fbg676-1", 1): ["C23", None, "B24", "A25", "B22", "A22"],
         ("ffg901-1", 1): ["V26", None, "R30", "T30", "R28", "T28"],
+        ("ffg900-1", 1): ["U19", None, "P24", "R25", "R20", "R21"],
         ("ffg1156-1", 1): ["V30", None, "AA33", "AA34", "Y33", "Y34"],
         ("ffg1157-1", 1): ["AL33", None, "AN33", "AN34", "AK34", "AL34"],
         ("ffg1158-1", 1): ["C24", None, "A23", "A24", "B26", "A26"],
@@ -132,6 +346,9 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         ("flg1155-1", 1): ["AL28", None, "AE28", "AF28", "AJ29", "AJ30"],
         ("flg1932-1", 1): ["V32", None, "T33", "R33", "U31", "T31"],
         ("flg1926-1", 1): ["AK33", None, "AN34", "AN35", "AJ34", "AK34"],
+
+        ("ffva1156-2-e", 1): ["G26", None, "M20", "L20", "R21", "R22"],
+        ("ffva1156-2-e", "sayma"): ["K21", None, "M20", "L20", "R21", "R22"],
     }
 
     pinouts = {
@@ -181,6 +398,7 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         "xc7a75t": ("csg324-1", 1, "LVCMOS25", Series7),
         "xc7k160t": ("fbg484-1", 2, "LVCMOS25", Series7),
         "xc7k325t": ("fbg676-1", 1, "LVCMOS25", Series7),
+        "xc7k325t-debug": ("ffg900-1", 1, "LVCMOS25", Series7),
         "xc7k355t": ("ffg901-1", 1, "LVCMOS25", Series7),
         "xc7k410t": ("fbg676-1", 1, "LVCMOS25", Series7),
         "xc7k420t": ("ffg1156-1", 1, "LVCMOS25", Series7),
@@ -197,35 +415,53 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         "xc7vx550t": ("ffg1158-1", 1, "LVCMOS18", Series7),
         "xc7vx690t": ("ffg1157-1", 1, "LVCMOS18", Series7),
         "xc7vx980t": ("ffg1926-1", 1, "LVCMOS18", Series7),
+
+        "xcku040": ("ffva1156-2-e", 1, "LVCMOS18", Ultrascale),
+        "xcku040-sayma": ("ffva1156-2-e", "sayma", "LVCMOS18", Ultrascale),
     }
 
     def __init__(self, device, pins, std, toolchain="ise"):
+        ios = [self.make_spi(0, pins, std, toolchain)]
+        if device == "xc7k325t-ffg900-1":  # debug
+            ios += [
+                ("user_sma_clock_p", 0, mb.Pins("L25"), mb.IOStandard("LVCMOS25")),
+                ("user_sma_clock_n", 0, mb.Pins("K25"), mb.IOStandard("LVCMOS25")),
+                ("user_sma_gpio_p", 0, mb.Pins("Y23"), mb.IOStandard("LVCMOS25")),
+                ("user_sma_gpio_n", 0, mb.Pins("Y24"), mb.IOStandard("LVCMOS25")),
+            ]
+        xilinx.XilinxPlatform.__init__(self, device, ios, toolchain=toolchain)
+
+    @staticmethod
+    def make_spi(i, pins, std, toolchain):
+        pu = "PULLUP" if toolchain == "ise" else "PULLUP TRUE"
+        pd = "PULLDOWN" if toolchain == "ise" else "PULLDOWN TRUE"
         cs_n, clk, mosi, miso = pins[:4]
-        io = ["spiflash", 0,
-              Subsignal("cs_n", Pins(cs_n)),
-              Subsignal("mosi", Pins(mosi)),
-              Subsignal("miso", Pins(miso), Misc("PULLUP")),
-              IOStandard(std),
-              ]
+        io = ["spiflash", i,
+            mb.Subsignal("cs_n", mb.Pins(cs_n), mb.Misc(pu)),
+            mb.Subsignal("mosi", mb.Pins(mosi), mb.Misc(pu)),
+            mb.Subsignal("miso", mb.Pins(miso), mb.Misc(pu)),
+            mb.IOStandard(std),
+            ]
         if clk:
-            io.append(Subsignal("clk", Pins(clk)))
+            io.append(mb.Subsignal("clk", mb.Pins(clk), mb.Misc(pd)))
         for i, p in enumerate(pins[4:]):
-            io.append(Subsignal("pullup{}".format(i), Pins(p), Misc("PULLUP")))
-        xilinx.XilinxPlatform.__init__(self, device, [io], toolchain=toolchain)
+            io.append(mb.Subsignal("pullup{}".format(i), mb.Pins(p),
+                                mb.Misc(pu)))
+        return io
 
     @classmethod
-    def make(cls, device, errors=False):
-        pkg, id, std, Top = cls.pinouts[device]
+    def make(cls, target, errors=False):
+        pkg, id, std, Top = cls.pinouts[target]
         pins = cls.packages[(pkg, id)]
+        device = target.split("-", 1)[0]
         platform = cls("{}-{}".format(device, pkg), pins, std, Top.toolchain)
         top = Top(platform)
-        name = "bscan_spi_{}".format(device)
-        dir = "build_{}".format(device)
+        name = "bscan_spi_{}".format(target)
         try:
-            platform.build(top, build_name=name, build_dir=dir)
+            platform.build(top, build_name=name)
         except Exception as e:
             print(("ERROR: xilinx_bscan_spi build failed "
-                  "for {}: {}").format(device, e))
+                  "for {}: {}").format(target, e))
             if errors:
                 raise
 
diff --git a/doc/openocd.texi b/doc/openocd.texi
index 7f5b72e07..431f11cb2 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -4913,16 +4913,13 @@ functionality is available through the @command{flash write_bank},
 @item @var{ir} ... is loaded into the JTAG IR to map the flash as the JTAG DR.
 For the bitstreams generated from @file{xilinx_bscan_spi.py} this is the
 @var{USER1} instruction.
-@item @var{dr_length} ... is the length of the DR register. This will be 1 for
-@file{xilinx_bscan_spi.py} bitstreams and most other cases.
 @end itemize
 
 @example
 target create $_TARGETNAME testee -chain-position $_CHIPNAME.fpga
 set _XILINX_USER1 0x02
-set _DR_LENGTH 1
 flash bank $_FLASHNAME spi 0x0 0 0 0 \
-           $_TARGETNAME $_XILINX_USER1 $_DR_LENGTH
+           $_TARGETNAME $_XILINX_USER1
 @end example
 @end deffn
 
diff --git a/src/flash/nor/jtagspi.c b/src/flash/nor/jtagspi.c
index a995fc756..a05ff3786 100644
--- a/src/flash/nor/jtagspi.c
+++ b/src/flash/nor/jtagspi.c
@@ -32,14 +32,13 @@ struct jtagspi_flash_bank {
 	const struct flash_device *dev;
 	int probed;
 	uint32_t ir;
-	uint32_t dr_len;
 };
 
 FLASH_BANK_COMMAND_HANDLER(jtagspi_flash_bank_command)
 {
 	struct jtagspi_flash_bank *info;
 
-	if (CMD_ARGC < 8)
+	if (CMD_ARGC < 7)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	info = malloc(sizeof(struct jtagspi_flash_bank));
@@ -52,7 +51,6 @@ FLASH_BANK_COMMAND_HANDLER(jtagspi_flash_bank_command)
 	info->tap = NULL;
 	info->probed = 0;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], info->ir);
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[7], info->dr_len);
 
 	return ERROR_OK;
 }
@@ -63,9 +61,6 @@ static void jtagspi_set_ir(struct flash_bank *bank)
 	struct scan_field field;
 	uint8_t buf[4];
 
-	if (buf_get_u32(info->tap->cur_instr, 0, info->tap->ir_length) == info->ir)
-		return;
-
 	LOG_DEBUG("loading jtagspi ir");
 	buf_set_u32(buf, 0, info->tap->ir_length, info->ir);
 	field.num_bits = info->tap->ir_length;
@@ -84,28 +79,53 @@ static int jtagspi_cmd(struct flash_bank *bank, uint8_t cmd,
 		uint32_t *addr, uint8_t *data, int len)
 {
 	struct jtagspi_flash_bank *info = bank->driver_priv;
-	struct scan_field fields[3];
-	uint8_t cmd_buf[4];
+	struct scan_field fields[6];
+	uint8_t marker = 1;
+	uint8_t xfer_bits_buf[4];
+	uint8_t addr_buf[3];
 	uint8_t *data_buf;
+	uint32_t xfer_bits;
 	int is_read, lenb, n;
 
 	/* LOG_DEBUG("cmd=0x%02x len=%i", cmd, len); */
 
-	n = 0;
-	fields[n].num_bits = 8;
-	cmd_buf[0] = cmd;
-	if (addr) {
-		h_u24_to_be(cmd_buf + 1, *addr);
-		fields[n].num_bits += 24;
-	}
-	flip_u8(cmd_buf, cmd_buf, 4);
-	fields[n].out_value = cmd_buf;
-	fields[n].in_value = NULL;
-	n++;
-
 	is_read = (len < 0);
 	if (is_read)
 		len = -len;
+
+	n = 0;
+
+	fields[n].num_bits = 1;
+	fields[n].out_value = &marker;
+	fields[n].in_value = NULL;
+	n++;
+
+	xfer_bits = 8 + len - 1;
+	/* cmd + read/write - 1 due to the counter implementation */
+	if (addr)
+		xfer_bits += 24;
+	h_u32_to_be(xfer_bits_buf, xfer_bits);
+	flip_u8(xfer_bits_buf, xfer_bits_buf, 4);
+	fields[n].num_bits = 32;
+	fields[n].out_value = xfer_bits_buf;
+	fields[n].in_value = NULL;
+	n++;
+
+	cmd = flip_u32(cmd, 8);
+	fields[n].num_bits = 8;
+	fields[n].out_value = &cmd;
+	fields[n].in_value = NULL;
+	n++;
+
+	if (addr) {
+		h_u24_to_be(addr_buf, *addr);
+		flip_u8(addr_buf, addr_buf, 3);
+		fields[n].num_bits = 24;
+		fields[n].out_value = addr_buf;
+		fields[n].in_value = NULL;
+		n++;
+	}
+
 	lenb = DIV_ROUND_UP(len, 8);
 	data_buf = malloc(lenb);
 	if (lenb > 0) {
@@ -114,10 +134,11 @@ static int jtagspi_cmd(struct flash_bank *bank, uint8_t cmd,
 			return ERROR_FAIL;
 		}
 		if (is_read) {
-			fields[n].num_bits = info->dr_len;
+			fields[n].num_bits = jtag_tap_count_enabled();
 			fields[n].out_value = NULL;
 			fields[n].in_value = NULL;
 			n++;
+
 			fields[n].out_value = NULL;
 			fields[n].in_value = data_buf;
 		} else {
@@ -130,6 +151,7 @@ static int jtagspi_cmd(struct flash_bank *bank, uint8_t cmd,
 	}
 
 	jtagspi_set_ir(bank);
+	/* passing from an IR scan to SHIFT-DR clears BYPASS registers */
 	jtag_add_dr_scan(info->tap, n, fields, TAP_IDLE);
 	jtag_execute_queue();
 
diff --git a/tcl/cpld/jtagspi.cfg b/tcl/cpld/jtagspi.cfg
index 60c3cb109..e720c3959 100644
--- a/tcl/cpld/jtagspi.cfg
+++ b/tcl/cpld/jtagspi.cfg
@@ -6,12 +6,6 @@ if { [info exists JTAGSPI_IR] } {
 	set _JTAGSPI_IR $_USER1
 }
 
-if { [info exists DR_LENGTH] } {
-	set _DR_LENGTH $DR_LENGTH
-} else {
-	set _DR_LENGTH 1
-}
-
 if { [info exists TARGETNAME] } {
 	set _TARGETNAME $TARGETNAME
 } else {
@@ -25,7 +19,7 @@ if { [info exists FLASHNAME] } {
 }
 
 target create $_TARGETNAME testee -chain-position $_CHIPNAME.tap
-flash bank $_FLASHNAME jtagspi 0 0 0 0 $_TARGETNAME $_JTAGSPI_IR $_DR_LENGTH
+flash bank $_FLASHNAME jtagspi 0 0 0 0 $_TARGETNAME $_JTAGSPI_IR
 
 proc jtagspi_init {chain_id proxy_bit} {
 	# load proxy bitstream $proxy_bit and probe spi flash

From f7836bbc75586666d639edd1cf39a871c434d980 Mon Sep 17 00:00:00 2001
From: Jiri Kastner <cz172638@gmail.com>
Date: Mon, 12 Jun 2017 15:26:02 +0200
Subject: [PATCH 41/65] arm_adi_v5: added some partnumbers found in tegra 186
 and tegra 210

Change-Id: Icd4137f3e266364d9728672bd2359fbd9a6c8ce9
Signed-off-by: Jiri Kastner <cz172638@gmail.com>
Signed-off-by: Forest Crossman <cyrozap@gmail.com>
Reviewed-on: http://openocd.zylin.com/4160
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/arm_adi_v5.c | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 1ca8e4f00..a4ca9f12c 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -1040,12 +1040,14 @@ static const struct {
 	{ ARM_ID, 0x4a2, "Cortex-A57 ROM",             "(ROM Table)", },
 	{ ARM_ID, 0x4a3, "Cortex-A53 ROM",             "(v7 Memory Map ROM Table)", },
 	{ ARM_ID, 0x4a4, "Cortex-A72 ROM",             "(ROM Table)", },
+	{ ARM_ID, 0x4a9, "Cortex-A9 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x4af, "Cortex-A15 ROM",             "(ROM Table)", },
 	{ ARM_ID, 0x4c0, "Cortex-M0+ ROM",             "(ROM Table)", },
 	{ ARM_ID, 0x4c3, "Cortex-M3 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x4c4, "Cortex-M4 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x4c7, "Cortex-M7 PPB ROM",          "(Private Peripheral Bus ROM Table)", },
 	{ ARM_ID, 0x4c8, "Cortex-M7 ROM",              "(ROM Table)", },
+	{ ARM_ID, 0x4b5, "Cortex-R5 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x470, "Cortex-M1 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x471, "Cortex-M0 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x906, "CoreSight CTI",              "(Cross Trigger)", },
@@ -1086,7 +1088,7 @@ static const struct {
 	{ ARM_ID, 0x9a9, "Cortex-M7 TPIU",             "(Trace Port Interface Unit)", },
 	{ ARM_ID, 0x9ae, "Cortex-A17 PMU",             "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9af, "Cortex-A15 PMU",             "(Performance Monitor Unit)", },
-	{ ARM_ID, 0x9b7, "Cortex-R7 PMU",              "(Performance Monitoring Unit)", },
+	{ ARM_ID, 0x9b7, "Cortex-R7 PMU",              "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9d3, "Cortex-A53 PMU",             "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9d7, "Cortex-A57 PMU",             "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9d8, "Cortex-A72 PMU",             "(Performance Monitor Unit)", },
@@ -1109,6 +1111,11 @@ static const struct {
 	{ 0x0c1,  0x1ed, "XMC1000 ROM",                "(ROM Table)" },
 	{ 0x0E5,  0x000, "SHARC+/Blackfin+",           "", },
 	{ 0x0F0,  0x440, "Qualcomm QDSS Component v1", "(Qualcomm Designed CoreSight Component v1)", },
+	{ 0x3eb,  0x181, "Tegra 186 ROM",              "(ROM Table)", },
+	{ 0x3eb,  0x211, "Tegra 210 ROM",              "(ROM Table)", },
+	{ 0x3eb,  0x202, "Denver ETM",                 "(Denver Embedded Trace)", },
+	{ 0x3eb,  0x302, "Denver Debug",               "(Debug Unit)", },
+	{ 0x3eb,  0x402, "Denver PMU",                 "(Performance Monitor Unit)", },
 	/* legacy comment: 0x113: what? */
 	{ ANY_ID, 0x120, "TI SDTI",                    "(System Debug Trace Interface)", }, /* from OMAP3 memmap */
 	{ ANY_ID, 0x343, "TI DAPCTL",                  "", }, /* from OMAP3 memmap */

From b3d29cb5441ee5d38e8f7b561a58f03eb269dbe4 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 27 Feb 2017 17:10:19 +0100
Subject: [PATCH 42/65] aarch64: add 'maskisr' command

Allow to configure ISR masking during single-step and add
handling for stepping over WFI with ISR masked.

Change-Id: I7918be7bcda6a1d9badac44fc36c59b52f662fef
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4023
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
---
 doc/openocd.texi     |  5 ++++
 src/target/aarch64.c | 60 ++++++++++++++++++++++++++++++++++++++------
 src/target/aarch64.h |  7 ++++++
 3 files changed, 64 insertions(+), 8 deletions(-)

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 431f11cb2..483b27b7f 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -8418,6 +8418,11 @@ halting or resuming of all cores in the group. The command @code{target smp} def
 group. With SMP handling disabled, all targets need to be treated individually.
 @end deffn
 
+@deffn Command {aarch64 maskisr} [@option{on}|@option{off}]
+Selects whether interrupts will be processed when single stepping. The default configuration is
+@option{on}.
+@end deffn
+
 @section Intel Architecture
 
 Intel Quark X10xx is the first product in the Quark family of SoCs. It is an IA-32
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index b18a12a4c..f1ce91459 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1049,6 +1049,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	int handle_breakpoints)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	int saved_retval = ERROR_OK;
 	int retval;
 	uint32_t edecr;
@@ -1069,7 +1070,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 				armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
 	}
 	/* disable interrupts while stepping */
-	if (retval == ERROR_OK)
+	if (retval == ERROR_OK && aarch64->isrmasking_mode == AARCH64_ISRMASK_ON)
 		retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
 	/* bail out if stepping setup has failed */
 	if (retval != ERROR_OK)
@@ -1113,7 +1114,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		if (retval != ERROR_OK || stepped)
 			break;
 
-		if (timeval_ms() > then + 1000) {
+		if (timeval_ms() > then + 100) {
 			LOG_ERROR("timeout waiting for target %s halt after step",
 					target_name(target));
 			retval = ERROR_TARGET_TIMEOUT;
@@ -1121,8 +1122,14 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		}
 	}
 
+	/*
+	 * At least on one SoC (Renesas R8A7795) stepping over a WFI instruction
+	 * causes a timeout. The core takes the step but doesn't complete it and so
+	 * debug state is never entered. However, you can manually halt the core
+	 * as an external debug even is also a WFI wakeup event.
+	 */
 	if (retval == ERROR_TARGET_TIMEOUT)
-		saved_retval = retval;
+		saved_retval = aarch64_halt_one(target, HALT_SYNC);
 
 	/* restore EDECR */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -1131,9 +1138,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		return retval;
 
 	/* restore interrupts */
-	retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
-	if (retval != ERROR_OK)
-		return ERROR_OK;
+	if (aarch64->isrmasking_mode == AARCH64_ISRMASK_ON) {
+		retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
+		if (retval != ERROR_OK)
+			return ERROR_OK;
+	}
 
 	if (saved_retval != ERROR_OK)
 		return saved_retval;
@@ -2303,9 +2312,9 @@ static int aarch64_examine_first(struct target *target)
 
 	LOG_DEBUG("Configured %i hw breakpoints", aarch64->brp_num);
 
-	target->state = TARGET_RUNNING;
+	target->state = TARGET_UNKNOWN;
 	target->debug_reason = DBG_REASON_NOTHALTED;
-
+	aarch64->isrmasking_mode = AARCH64_ISRMASK_ON;
 	target_set_examined(target);
 	return ERROR_OK;
 }
@@ -2443,6 +2452,34 @@ COMMAND_HANDLER(aarch64_handle_smp_on_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(aarch64_mask_interrupts_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	static const Jim_Nvp nvp_maskisr_modes[] = {
+		{ .name = "off", .value = AARCH64_ISRMASK_OFF },
+		{ .name = "on", .value = AARCH64_ISRMASK_ON },
+		{ .name = NULL, .value = -1 },
+	};
+	const Jim_Nvp *n;
+
+	if (CMD_ARGC > 0) {
+		n = Jim_Nvp_name2value_simple(nvp_maskisr_modes, CMD_ARGV[0]);
+		if (n->name == NULL) {
+			LOG_ERROR("Unknown parameter: %s - should be off or on", CMD_ARGV[0]);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		aarch64->isrmasking_mode = n->value;
+	}
+
+	n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, aarch64->isrmasking_mode);
+	command_print(CMD_CTX, "aarch64 interrupt mask %s", n->name);
+
+	return ERROR_OK;
+}
+
 static const struct command_registration aarch64_exec_command_handlers[] = {
 	{
 		.name = "cache_info",
@@ -2471,6 +2508,13 @@ static const struct command_registration aarch64_exec_command_handlers[] = {
 		.help = "Restart smp handling",
 		.usage = "",
 	},
+	{
+		.name = "maskisr",
+		.handler = aarch64_mask_interrupts_command,
+		.mode = COMMAND_ANY,
+		.help = "mask aarch64 interrupts during single-step",
+		.usage = "['on'|'off']",
+	},
 
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/aarch64.h b/src/target/aarch64.h
index c9ec02dbb..d7886a3d7 100644
--- a/src/target/aarch64.h
+++ b/src/target/aarch64.h
@@ -36,6 +36,11 @@
 
 #define AARCH64_PADDRDBG_CPU_SHIFT 13
 
+enum aarch64_isrmasking_mode {
+	AARCH64_ISRMASK_OFF,
+	AARCH64_ISRMASK_ON,
+};
+
 struct aarch64_brp {
 	int used;
 	int type;
@@ -58,6 +63,8 @@ struct aarch64_common {
 	struct aarch64_brp *brp_list;
 
 	struct armv8_common armv8_common;
+
+	enum aarch64_isrmasking_mode isrmasking_mode;
 };
 
 static inline struct aarch64_common *

From 1482c26a4e656eaa35712895024a56303f4ba7ea Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Apr 2017 11:06:20 +0200
Subject: [PATCH 43/65] aarch64: simplify mode and state handling

Aarch32 and Aarch64 modes don't conflict in CPSR, no need to deconflict
ARMv7-M profile modes either.


Change-Id: I4c437dfa657f9e8a1da3687bc9f21435384b7881
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4144
Tested-by: jenkins
Reviewed-by: Yao Qi <qiyaoltc@gmail.com>
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
---
 src/target/arm.h       | 15 +++++------
 src/target/armv8.c     | 58 +++++-------------------------------------
 src/target/armv8.h     |  2 +-
 src/target/armv8_dpm.c |  7 +----
 4 files changed, 16 insertions(+), 66 deletions(-)

diff --git a/src/target/arm.h b/src/target/arm.h
index f89aa6884..eb4a51f98 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -66,14 +66,13 @@ enum arm_mode {
 	ARM_MODE_USER_THREAD = 1,
 	ARM_MODE_HANDLER = 2,
 
-	/* shift left 4 bits for armv8 64 */
-	ARMV8_64_EL0T = 0x0F,
-	ARMV8_64_EL1T = 0x4F,
-	ARMV8_64_EL1H = 0x5F,
-	ARMV8_64_EL2T = 0x8F,
-	ARMV8_64_EL2H = 0x9F,
-	ARMV8_64_EL3T = 0xCF,
-	ARMV8_64_EL3H = 0xDF,
+	ARMV8_64_EL0T = 0x0,
+	ARMV8_64_EL1T = 0x4,
+	ARMV8_64_EL1H = 0x5,
+	ARMV8_64_EL2T = 0x8,
+	ARMV8_64_EL2H = 0x9,
+	ARMV8_64_EL3T = 0xC,
+	ARMV8_64_EL3H = 0xD,
 
 	ARM_MODE_ANY = -1
 };
diff --git a/src/target/armv8.c b/src/target/armv8.c
index df5e25102..1b8e45016 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -45,8 +45,6 @@ static const struct {
 	const char *name;
 	unsigned psr;
 } armv8_mode_data[] = {
-	/* These special modes are currently only supported
-	 * by ARMv6M and ARMv7M profiles */
 	{
 		.name = "USR",
 		.psr = ARM_MODE_USR,
@@ -112,48 +110,6 @@ const char *armv8_mode_name(unsigned psr_mode)
 	return "UNRECOGNIZED";
 }
 
-int armv8_mode_to_number(enum arm_mode mode)
-{
-	switch (mode) {
-		case ARM_MODE_ANY:
-		/* map MODE_ANY to user mode */
-		case ARM_MODE_USR:
-			return 0;
-		case ARM_MODE_FIQ:
-			return 1;
-		case ARM_MODE_IRQ:
-			return 2;
-		case ARM_MODE_SVC:
-			return 3;
-		case ARM_MODE_ABT:
-			return 4;
-		case ARM_MODE_UND:
-			return 5;
-		case ARM_MODE_SYS:
-			return 6;
-		case ARM_MODE_MON:
-			return 7;
-		case ARMV8_64_EL0T:
-			return 8;
-		case ARMV8_64_EL1T:
-			return 9;
-		case ARMV8_64_EL1H:
-			return 10;
-		case ARMV8_64_EL2T:
-			return 11;
-		case ARMV8_64_EL2H:
-			return 12;
-		case ARMV8_64_EL3T:
-			return 13;
-		case ARMV8_64_EL3H:
-			return 14;
-
-		default:
-			LOG_ERROR("invalid mode value encountered %d", mode);
-			return -1;
-	}
-}
-
 static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regval)
 {
 	struct arm_dpm *dpm = &armv8->dpm;
@@ -533,9 +489,8 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 	/* Older ARMs won't have the J bit */
 	enum arm_state state = 0xFF;
 
-	if (((cpsr & 0x10) >> 4) == 0) {
-		state = ARM_STATE_AARCH64;
-	} else {
+	if ((cpsr & 0x10) != 0) {
+		/* Aarch32 state */
 		if (cpsr & (1 << 5)) {	/* T */
 			if (cpsr & (1 << 24)) { /* J */
 				LOG_WARNING("ThumbEE -- incomplete support");
@@ -549,12 +504,13 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 			} else
 				state = ARM_STATE_ARM;
 		}
+	} else {
+		/* Aarch64 state */
+		state = ARM_STATE_AARCH64;
 	}
+
 	arm->core_state = state;
-	if (arm->core_state == ARM_STATE_AARCH64)
-		arm->core_mode = (mode << 4) | 0xf;
-	else
-		arm->core_mode = mode;
+	arm->core_mode = mode;
 
 	LOG_DEBUG("set CPSR %#8.8x: %s mode, %s state", (unsigned) cpsr,
 		armv8_mode_name(arm->core_mode),
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 02663cab2..0f3e66f65 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -270,7 +270,7 @@ static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
 		return 3;
 	/* all Aarch64 modes */
 	default:
-		return (core_mode >> 6) & 3;
+		return (core_mode >> 2) & 3;
 	}
 }
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index f4e7a0799..c79b1a0ff 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -561,12 +561,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 
 	} else {
 		LOG_DEBUG("setting mode 0x%"PRIx32, mode);
-
-		/* else force to the specified mode */
-		if (is_arm_mode(mode))
-			cpsr = mode;
-		else
-			cpsr = mode >> 4;
+		cpsr = mode;
 	}
 
 	switch (cpsr & 0x1f) {

From 685cdf86ea841caf01da70167b61f7b0b9db4435 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Apr 2017 09:49:37 +0200
Subject: [PATCH 44/65] aarch64: speed up first examination

Don't use atomic dap operations when not necessary. Also remove
loop trying to set core power request, didn't find a platform
where it actually worked and it's slowing examination down.

Change-Id: I44e5c2f289f951b8f4579f08a841172404a52053
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4143
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
---
 src/target/aarch64.c | 60 +++++++++++++++-----------------------------
 1 file changed, 20 insertions(+), 40 deletions(-)

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index f1ce91459..784274a0e 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -180,7 +180,7 @@ static int aarch64_init_debug_access(struct target *target)
 	int retval;
 	uint32_t dummy;
 
-	LOG_DEBUG(" ");
+	LOG_DEBUG("%s", target_name(target));
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
@@ -2173,7 +2173,7 @@ static int aarch64_examine_first(struct target *target)
 	int retval = ERROR_OK;
 	uint64_t debug, ttypr;
 	uint32_t cpuid;
-	uint32_t tmp0, tmp1;
+	uint32_t tmp0, tmp1, tmp2, tmp3;
 	debug = ttypr = cpuid = 0;
 
 	retval = dap_dp_init(swjdp);
@@ -2213,32 +2213,6 @@ static int aarch64_examine_first(struct target *target)
 	} else
 		armv8->debug_base = target->dbgbase;
 
-	uint32_t prsr;
-	int64_t then = timeval_ms();
-	do {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_PRSR, &prsr);
-		if (retval == ERROR_OK) {
-			retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUV8_DBG_PRCR, PRCR_COREPURQ|PRCR_CORENPDRQ);
-			if (retval != ERROR_OK) {
-				LOG_DEBUG("write to PRCR failed");
-				break;
-			}
-		}
-
-		if (timeval_ms() > then + 1000) {
-			retval = ERROR_TARGET_TIMEOUT;
-			break;
-		}
-
-	} while ((prsr & PRSR_PU) == 0);
-
-	if (retval != ERROR_OK) {
-		LOG_ERROR("target %s: failed to set power state of the core.", target_name(target));
-		return retval;
-	}
-
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
 	if (retval != ERROR_OK) {
@@ -2246,34 +2220,40 @@ static int aarch64_examine_first(struct target *target)
 		return retval;
 	}
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+	retval = mem_ap_read_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_MAINID0, &cpuid);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "CPUID");
 		return retval;
 	}
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+	retval = mem_ap_read_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_MEMFEATURE0, &tmp0);
-	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+	retval += mem_ap_read_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_MEMFEATURE0 + 4, &tmp1);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "Memory Model Type");
 		return retval;
 	}
-	ttypr |= tmp1;
-	ttypr = (ttypr << 32) | tmp0;
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp0);
-	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp1);
+	retval = mem_ap_read_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp2);
+	retval += mem_ap_read_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp3);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
 		return retval;
 	}
-	debug |= tmp1;
-	debug = (debug << 32) | tmp0;
+
+	retval = dap_run(armv8->debug_ap->dap);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: examination failed\n", target_name(target));
+		return retval;
+	}
+
+	ttypr |= tmp1;
+	ttypr = (ttypr << 32) | tmp0;
+	debug |= tmp3;
+	debug = (debug << 32) | tmp2;
 
 	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
 	LOG_DEBUG("ttypr = 0x%08" PRIx64, ttypr);

From 2bd78e12392f99ffed0b9fd815000f5bcfd3db14 Mon Sep 17 00:00:00 2001
From: Oleksij Rempel <linux@rempel-privat.de>
Date: Wed, 14 Jun 2017 10:06:38 +0200
Subject: [PATCH 45/65] target: add initial imx7.cfg

Change-Id: I899a215049ff0bc8840463c71018867ef71b5b90
Signed-off-by: Oleksij Rempel <linux@rempel-privat.de>
Reviewed-on: http://openocd.zylin.com/4190
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Tested-by: jenkins
---
 tcl/target/imx7.cfg | 37 +++++++++++++++++++++++++++++++++++++
 1 file changed, 37 insertions(+)
 create mode 100644 tcl/target/imx7.cfg

diff --git a/tcl/target/imx7.cfg b/tcl/target/imx7.cfg
new file mode 100644
index 000000000..d16e95a27
--- /dev/null
+++ b/tcl/target/imx7.cfg
@@ -0,0 +1,37 @@
+if { [info exists CHIPNAME] } {
+   set  _CHIPNAME $CHIPNAME
+} else {
+   set  _CHIPNAME imx7
+}
+
+# CoreSight Debug Access Port
+if { [info exists DAP_TAPID] } {
+        set _DAP_TAPID $DAP_TAPID
+} else {
+        set _DAP_TAPID 0x5ba00477
+}
+
+jtag newtap $_CHIPNAME dap -irlen 4 -ircapture 0x01 -irmask 0x0f \
+        -expected-id $_DAP_TAPID
+
+#
+# Cortex-A7 target
+#
+# GDB target: Cortex-A7, using DAP, configuring only one core
+# Base addresses of cores:
+# core 0  -  0x80070000
+# core 1  -  0x80072000
+set _TARGETNAME $_CHIPNAME.cpu_a7
+target create $_TARGETNAME.0 cortex_a -chain-position $_CHIPNAME.dap \
+        -coreid 0 -dbgbase 0x80070000
+
+target create $_TARGETNAME.1 cortex_a -chain-position $_CHIPNAME.dap \
+        -coreid 1 -dbgbase 0x80072000 -defer-examine
+#
+# Cortex-M4 target
+#
+set _TARGETNAME_2 $_CHIPNAME.cpu_m4
+target create $_TARGETNAME_2 cortex_m -chain-position $_CHIPNAME.dap -ap-num 4 \
+        -defer-examine
+
+targets $_TARGETNAME.0

From edb67962865d5d3cc4a8ec1790b4c8c5327e98fd Mon Sep 17 00:00:00 2001
From: Marc Schink <openocd-dev@marcschink.de>
Date: Wed, 10 Jan 2018 12:45:52 +0100
Subject: [PATCH 46/65] Fix Jim interpreter memory leak

Change-Id: I71d7d97e7dc315c42fc43b65cb5fcecd7bdfb581
Signed-off-by: Marc Schink <openocd-dev@marcschink.de>
Reviewed-on: http://openocd.zylin.com/2959
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/helper/command.c | 9 +++++++++
 src/helper/command.h | 8 ++++++++
 src/openocd.c        | 4 ++--
 3 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/helper/command.c b/src/helper/command.c
index 40e8b0582..cbd52fbf1 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -1339,6 +1339,15 @@ struct command_context *command_init(const char *startup_tcl, Jim_Interp *interp
 	return context;
 }
 
+void command_exit(struct command_context *context)
+{
+	if (!context)
+		return;
+
+	Jim_FreeInterp(context->interp);
+	command_done(context);
+}
+
 int command_context_mode(struct command_context *cmd_ctx, enum command_mode mode)
 {
 	if (!cmd_ctx)
diff --git a/src/helper/command.h b/src/helper/command.h
index bd24156e3..f696ab823 100644
--- a/src/helper/command.h
+++ b/src/helper/command.h
@@ -307,6 +307,14 @@ struct command_context *current_command_context(Jim_Interp *interp);
  * creates a command interpreter.
  */
 struct command_context *command_init(const char *startup_tcl, Jim_Interp *interp);
+/**
+ * Shutdown a command context.
+ *
+ * Free the command context and the associated Jim interpreter.
+ *
+ * @param context The command_context that will be destroyed.
+ */
+void command_exit(struct command_context *context);
 /**
  * Creates a copy of an existing command context.  This does not create
  * a deep copy of the command list, so modifications in one context will
diff --git a/src/openocd.c b/src/openocd.c
index 83329b519..831bd17f2 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -344,8 +344,8 @@ int openocd_main(int argc, char *argv[])
 
 	unregister_all_commands(cmd_ctx, NULL);
 
-	/* free commandline interface */
-	command_done(cmd_ctx);
+	/* Shutdown commandline interface */
+	command_exit(cmd_ctx);
 
 	adapter_quit();
 

From ff623b83fc74d40adcdfb36454393cf307d70f08 Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Sun, 14 Jan 2018 23:33:44 +0100
Subject: [PATCH 47/65] target, arm_adi_v5: catch two allocation errors

Command
	mdw 0 0x40000000
triggers Segmentation fault on an arm.
Size parameter is a nonsence that may happen e.g. if you
mistype mdw instead of mww.

Add checking for calloc() NULL return in mdb/h/w.

Use calloc() instead of malloc() as multiplication
count * sizeof(uint32_t) overflows for size >= 0x40000000.

Change-Id: I968c944d863d1173ef932a7077d526fccb9381ae
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/4349
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
---
 src/target/arm_adi_v5.c | 3 ++-
 src/target/target.c     | 4 ++++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index a4ca9f12c..aa7f4cf64 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -479,7 +479,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	/* Allocate buffer to hold the sequence of DRW reads that will be made. This is a significant
 	 * over-allocation if packed transfers are going to be used, but determining the real need at
 	 * this point would be messy. */
-	uint32_t *read_buf = malloc(count * sizeof(uint32_t));
+	uint32_t *read_buf = calloc(count, sizeof(uint32_t));
+	/* Multiplication count * sizeof(uint32_t) may overflow, calloc() is safe */
 	uint32_t *read_ptr = read_buf;
 	if (read_buf == NULL) {
 		LOG_ERROR("Failed to allocate read buffer");
diff --git a/src/target/target.c b/src/target/target.c
index d6781a3c4..52307dbf4 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -3116,6 +3116,10 @@ COMMAND_HANDLER(handle_md_command)
 		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], count);
 
 	uint8_t *buffer = calloc(count, size);
+	if (buffer == NULL) {
+		LOG_ERROR("Failed to allocate md read buffer");
+		return ERROR_FAIL;
+	}
 
 	struct target *target = get_current_target(CMD_CTX);
 	int retval = fn(target, address, size, count, buffer);

From f02327d85953152743dbb02d9ceeaa5b59da9ebe Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Wed, 17 Jan 2018 17:31:58 +0300
Subject: [PATCH 48/65] server: add safeguards against printing bogus port
 number

Clang static checker emits "Assigned value is garbage or undefined"
warning there as it can't prove that when the socket descriptor is
AF_INET/SOCK_STREAM and getsockname doesn't return an error, sin_port
is guaranteed to be filled in.

Pacify it by obvious means.

Change-Id: I43b5e5ceb41c07d523a81b34a25490c4c5f49a70
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4350
Tested-by: jenkins
Reviewed-by: Tim Newsome <tim@sifive.com>
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/server/server.c | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/server/server.c b/src/server/server.c
index 1e52e979d..46c860f4f 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -300,10 +300,11 @@ int add_service(char *name,
 		}
 
 		struct sockaddr_in addr_in;
+		addr_in.sin_port = 0;
 		socklen_t addr_in_size = sizeof(addr_in);
-		getsockname(c->fd, (struct sockaddr *)&addr_in, &addr_in_size);
-		LOG_INFO("Listening on port %hu for %s connections",
-				ntohs(addr_in.sin_port), name);
+		if (getsockname(c->fd, (struct sockaddr *)&addr_in, &addr_in_size) == 0)
+			LOG_INFO("Listening on port %hu for %s connections",
+				 ntohs(addr_in.sin_port), name);
 	} else if (c->type == CONNECTION_STDINOUT) {
 		c->fd = fileno(stdin);
 

From e3a5f1613bcceb4508917a7f79348167503effab Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Thu, 18 Jan 2018 00:07:29 +0300
Subject: [PATCH 49/65] flash: nor: stm32l4x: fix warning in probe

Reading options word can fail, so this needs to be handled.

Reported by Clang static analyzer.

Change-Id: I9754cab9c4446fa2b20d4b44b0e20724d1bc1beb
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4352
Tested-by: jenkins
Reviewed-by: Tim "mithro" Ansell <mithro@mithis.com>
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/flash/nor/stm32l4x.c | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index fa0c48b4f..6a1fa074e 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -652,6 +652,9 @@ static int stm32l4_probe(struct flash_bank *bank)
 	/* get options to for DUAL BANK. */
 	retval = target_read_u32(target, STM32_FLASH_OPTR, &options);
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* only devices with < 1024 kiB may be set to single bank dual banks */
 	if ((flash_size_in_kb == 1024) || !(options & OPT_DUALBANK))
 		stm32l4_info->option_bytes.bank_b_start = 256;

From e3f3b77729a07e8c0fa036c8ec1e021773142892 Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Wed, 6 Sep 2017 22:34:14 +0200
Subject: [PATCH 50/65] flash Kinetis: make FCF protection more user friendly

The Flash Configuration Field on Kinetis devices requires protection
because it is located in program flash space (at 0x400) and writing
an improper data to it may permanently lock the device. Even an erased
flash sector containing FCF engages security lock (not permanent one)
on the next reset or power cycle.

'kinetis fcf_source protection' mode was introduced in the change #3562.
Flash driver in this mode sets FCF immediately after sector erase to
prevent unintentional security lock. To do so the driver needs to know
FCF values before flash image data is actually processed. Flash
protection bits are available in bank structure, FOPT can be set by
'kinetis fopt' command and securing device by FSEC is not supported.

Nevertheless an inexperienced user flashed the device using an image
with FCF values different from those set in OpenOCD config and
concluded programming did not work as some verify errors showed.

This change tries to write maximum possible from image data
retaining FCF protection.

Check FCF in programmed data and report if some field differs from
values set by OpenOCD flash block protection and 'kinetis fopt' command.
Warn user about verify errors caused by FCF protection.

On devices with ECC flash (K26, K66 and KV5x) it is impossible to change
already programmed FCF - it would result in an ECC error. As FCF was
written just after erase in 'kinetis fcf_source protection' mode
the warning issued during flash write is the only possible action.

On non-ECC flash devices use cumulative flash programming to
set FCF values requested in programmed image data.
Use FSEC from programmed data only if it does not request a secure
mode. Device can be secured only in 'kinetis fcf_source write' mode.
Use FOPT from programmed data if its value was not configured
in OpenOCD config by 'kinetis fopt' command.

Change-Id: If65fbbd7700069f57e4ae32234dce371bff93674
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/4228
Tested-by: jenkins
Reviewed-by: Robert Foss <robert.foss@memcpy.io>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/flash/nor/kinetis.c | 55 +++++++++++++++++++++++++++++++++++------
 1 file changed, 47 insertions(+), 8 deletions(-)

diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 5c0ffbd6f..2c2d062ac 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -287,6 +287,7 @@ struct kinetis_chip {
 
 		FS_NO_CMD_BLOCKSTAT = 0x40,
 		FS_WIDTH_256BIT = 0x80,
+		FS_ECC = 0x100,
 	} flash_support;
 
 	enum {
@@ -388,6 +389,7 @@ static const struct kinetis_type kinetis_types_old[] = {
 
 static bool allow_fcf_writes;
 static uint8_t fcf_fopt = 0xff;
+static bool fcf_fopt_configured;
 static bool create_banks;
 
 
@@ -1881,9 +1883,13 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 {
 	int result;
 	bool set_fcf = false;
+	bool fcf_in_data_valid = false;
 	int sect = 0;
 	struct kinetis_flash_bank *k_bank = bank->driver_priv;
 	struct kinetis_chip *k_chip = k_bank->k_chip;
+	uint8_t fcf_buffer[FCF_SIZE];
+	uint8_t fcf_current[FCF_SIZE];
+	uint8_t fcf_in_data[FCF_SIZE];
 
 	result = kinetis_check_run_mode(k_chip);
 	if (result != ERROR_OK)
@@ -1904,11 +1910,41 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 	}
 
 	if (set_fcf) {
-		uint8_t fcf_buffer[FCF_SIZE];
-		uint8_t fcf_current[FCF_SIZE];
-
 		kinetis_fill_fcf(bank, fcf_buffer);
 
+		fcf_in_data_valid = offset <= FCF_ADDRESS
+					 && offset + count >= FCF_ADDRESS + FCF_SIZE;
+		if (fcf_in_data_valid) {
+			memcpy(fcf_in_data, buffer + FCF_ADDRESS - offset, FCF_SIZE);
+			if (memcmp(fcf_in_data + FCF_FPROT, fcf_buffer, 4)) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Flash protection requested in programmed file differs from current setting.");
+			}
+			if (fcf_in_data[FCF_FDPROT] != fcf_buffer[FCF_FDPROT]) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Data flash protection requested in programmed file differs from current setting.");
+			}
+			if ((fcf_in_data[FCF_FSEC] & 3) != 2) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Device security requested in programmed file!");
+			} else if (k_chip->flash_support & FS_ECC
+			    && fcf_in_data[FCF_FSEC] != fcf_buffer[FCF_FSEC]) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Strange unsecure mode 0x%02" PRIx8
+					 "requested in programmed file!",
+					 fcf_in_data[FCF_FSEC]);
+			}
+			if ((k_chip->flash_support & FS_ECC || fcf_fopt_configured)
+			    && fcf_in_data[FCF_FOPT] != fcf_fopt) {
+				fcf_in_data_valid = false;
+				LOG_INFO("FOPT requested in programmed file differs from current setting.");
+			}
+			if (!fcf_in_data_valid)
+				LOG_INFO("Expect verify errors at FCF (0x408-0x40f).");
+		}
+	}
+
+	if (set_fcf && !fcf_in_data_valid) {
 		if (offset < FCF_ADDRESS) {
 			/* write part preceding FCF */
 			result = kinetis_write_inner(bank, buffer, offset, FCF_ADDRESS - offset);
@@ -1937,9 +1973,10 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 		}
 		return result;
 
-	} else
+	} else {
 		/* no FCF fiddling, normal write */
 		return kinetis_write_inner(bank, buffer, offset, count);
+	}
 }
 
 
@@ -2146,7 +2183,7 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 				k_chip->nvm_sector_size = 4<<10;
 				k_chip->max_flash_prog_size = 1<<10;
 				num_blocks = 4;
-				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR;
+				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_ECC;
 				cpu_mhz = 180;
 				break;
 
@@ -2300,7 +2337,7 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 				k_chip->max_flash_prog_size = 1<<10;
 				num_blocks = 1;
 				maxaddr_shift = 14;
-				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_WIDTH_256BIT;
+				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_WIDTH_256BIT | FS_ECC;
 				k_chip->pflash_base = 0x10000000;
 				k_chip->progr_accel_ram = 0x18000000;
 				cpu_mhz = 240;
@@ -2959,10 +2996,12 @@ COMMAND_HANDLER(kinetis_fopt_handler)
 	if (CMD_ARGC > 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	if (CMD_ARGC == 1)
+	if (CMD_ARGC == 1) {
 		fcf_fopt = (uint8_t)strtoul(CMD_ARGV[0], NULL, 0);
-	else
+		fcf_fopt_configured = true;
+	} else {
 		command_print(CMD_CTX, "FCF_FOPT 0x%02" PRIx8, fcf_fopt);
+	}
 
 	return ERROR_OK;
 }

From b7be0a873ca86cc9972e89ec7d22c25e56d71f0a Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Sun, 15 Oct 2017 10:34:27 +0200
Subject: [PATCH 51/65] flash Kinetis: add K27 and K28 devices
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Tested on FRDM-K28F. Thanks to Thomas Varghese for donating the kit.

Change-Id: Idcdd8bcf992acebd19e5335f7f833356500c45dd
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/4255
Tested-by: jenkins
Reviewed-by: Joakim Nohlgård <joakim.nohlgard@eistec.se>
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/flash/nor/kinetis.c | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 2c2d062ac..48a5de46a 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -2187,6 +2187,17 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 				cpu_mhz = 180;
 				break;
 
+			case KINETIS_SDID_FAMILYID_K2X | KINETIS_SDID_SUBFAMID_KX7:
+				/* K27FN2M0 */
+			case KINETIS_SDID_FAMILYID_K2X | KINETIS_SDID_SUBFAMID_KX8:
+				/* K28FN2M0 */
+				k_chip->pflash_sector_size = 4<<10;
+				k_chip->max_flash_prog_size = 1<<10;
+				num_blocks = 4;
+				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_ECC;
+				cpu_mhz = 150;
+				break;
+
 			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX0:
 			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX1:
 			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX2:

From e22c6484eaedd56d71011dd56e76eacab515a0d6 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sat, 20 Jan 2018 21:10:39 +0300
Subject: [PATCH 52/65] tcl: target: klx: use 1KiB for working area

Some parts have only that much. Reported by robertfoos_ on IRC.

Change-Id: I684fdccfa62cf726466ddc467543a990fd88c4dc
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4369
Reviewed-by: Robert Foss <robert.foss@memcpy.io>
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 tcl/target/klx.cfg | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/tcl/target/klx.cfg b/tcl/target/klx.cfg
index 7dd0404f9..1a2ee6798 100644
--- a/tcl/target/klx.cfg
+++ b/tcl/target/klx.cfg
@@ -12,11 +12,11 @@ if { [info exists CHIPNAME] } {
 }
 
 # Work-area is a space in RAM used for flash programming
-# By default use 4kB
+# By default use 1KiB
 if { [info exists WORKAREASIZE] } {
    set _WORKAREASIZE $WORKAREASIZE
 } else {
-   set _WORKAREASIZE 0x1000
+   set _WORKAREASIZE 0x400
 }
 
 if { [info exists CPUTAPID] } {

From e0fc7a54f287aad414b373410e09faa048f3a9dd Mon Sep 17 00:00:00 2001
From: Christopher Head <chead@zaber.com>
Date: Wed, 24 Jan 2018 14:22:37 -0800
Subject: [PATCH 53/65] Add timeval_compare helper function

Change-Id: Id75727a150912ff778a4fa32ad56467da33a6324
Signed-off-by: Christopher Head <chead@zaber.com>
Reviewed-on: http://openocd.zylin.com/4379
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/helper/time_support.c | 15 +++++++++++++++
 src/helper/time_support.h |  1 +
 2 files changed, 16 insertions(+)

diff --git a/src/helper/time_support.c b/src/helper/time_support.c
index 8337e73ba..05eaf0a9d 100644
--- a/src/helper/time_support.c
+++ b/src/helper/time_support.c
@@ -62,6 +62,21 @@ int timeval_add_time(struct timeval *result, long sec, long usec)
 	return 0;
 }
 
+/* compare two timevals and return -1/0/+1 accordingly */
+int timeval_compare(const struct timeval *x, const struct timeval *y)
+{
+	if (x->tv_sec < y->tv_sec)
+		return -1;
+	else if (x->tv_sec > y->tv_sec)
+		return 1;
+	else if (x->tv_usec < y->tv_usec)
+		return -1;
+	else if (x->tv_usec > y->tv_usec)
+		return 1;
+	else
+		return 0;
+}
+
 int duration_start(struct duration *duration)
 {
 	return gettimeofday(&duration->start, NULL);
diff --git a/src/helper/time_support.h b/src/helper/time_support.h
index 58c8c48bb..7abbdb24d 100644
--- a/src/helper/time_support.h
+++ b/src/helper/time_support.h
@@ -38,6 +38,7 @@
 
 int timeval_subtract(struct timeval *result, struct timeval *x, struct timeval *y);
 int timeval_add_time(struct timeval *result, long sec, long usec);
+int timeval_compare(const struct timeval *x, const struct timeval *y);
 
 /** @returns gettimeofday() timeval as 64-bit in ms */
 int64_t timeval_ms(void);

From 2428722a23d8a08b15331ab95884762e5f329a02 Mon Sep 17 00:00:00 2001
From: Christopher Head <chead@zaber.com>
Date: Wed, 24 Jan 2018 14:35:40 -0800
Subject: [PATCH 54/65] Use timeval helpers

Some of these changes actually fix broken comparisons which could
occasionally fail. Others just clean up the code and make it more clear.

Change-Id: I6c398bdc45fa0d2716f48a74822457d1351f81a5
Signed-off-by: Christopher Head <chead@zaber.com>
Reviewed-on: http://openocd.zylin.com/4380
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/jtag/drivers/presto.c  |  3 +--
 src/target/cortex_m.c      |  3 +--
 src/target/embeddedice.c   |  9 +++++----
 src/target/openrisc/or1k.c |  3 +--
 src/target/target.c        | 28 ++++++----------------------
 src/target/xscale.c        |  3 +--
 6 files changed, 15 insertions(+), 34 deletions(-)

diff --git a/src/jtag/drivers/presto.c b/src/jtag/drivers/presto.c
index 49caa679f..29bc81111 100644
--- a/src/jtag/drivers/presto.c
+++ b/src/jtag/drivers/presto.c
@@ -117,8 +117,7 @@ static int presto_read(uint8_t *buf, uint32_t size)
 		ftbytes += presto->retval;
 
 		gettimeofday(&now, NULL);
-		if ((now.tv_sec > timeout.tv_sec) ||
-				((now.tv_sec == timeout.tv_sec) && (now.tv_usec > timeout.tv_usec)))
+		if (timeval_compare(&now, &timeout) > 0)
 			break;
 	}
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 24063a7e6..79af632ac 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1787,8 +1787,7 @@ int cortex_m_profiling(struct target *target, uint32_t *samples,
 
 
 		gettimeofday(&now, NULL);
-		if (sample_count >= max_num_samples ||
-			(now.tv_sec >= timeout.tv_sec && now.tv_usec >= timeout.tv_usec)) {
+		if (sample_count >= max_num_samples || timeval_compare(&now, &timeout) > 0) {
 			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
 			break;
 		}
diff --git a/src/target/embeddedice.c b/src/target/embeddedice.c
index 09d6fc8a1..7232ef1e4 100644
--- a/src/target/embeddedice.c
+++ b/src/target/embeddedice.c
@@ -28,6 +28,7 @@
 
 #include "embeddedice.h"
 #include "register.h"
+#include <helper/time_support.h>
 
 /**
  * @file
@@ -576,8 +577,8 @@ int embeddedice_handshake(struct arm_jtag *jtag_info, int hsbit, uint32_t timeou
 	uint8_t field2_out[1];
 	int retval;
 	uint32_t hsact;
-	struct timeval lap;
 	struct timeval now;
+	struct timeval timeout_end;
 
 	if (hsbit == EICE_COMM_CTRL_WBIT)
 		hsact = 1;
@@ -610,7 +611,8 @@ int embeddedice_handshake(struct arm_jtag *jtag_info, int hsbit, uint32_t timeou
 	fields[2].in_value = NULL;
 
 	jtag_add_dr_scan(jtag_info->tap, 3, fields, TAP_IDLE);
-	gettimeofday(&lap, NULL);
+	gettimeofday(&timeout_end, NULL);
+	timeval_add_time(&timeout_end, 0, timeout * 1000);
 	do {
 		jtag_add_dr_scan(jtag_info->tap, 3, fields, TAP_IDLE);
 		retval = jtag_execute_queue();
@@ -621,8 +623,7 @@ int embeddedice_handshake(struct arm_jtag *jtag_info, int hsbit, uint32_t timeou
 			return ERROR_OK;
 
 		gettimeofday(&now, NULL);
-	} while ((uint32_t)((now.tv_sec - lap.tv_sec) * 1000
-			+ (now.tv_usec - lap.tv_usec) / 1000) <= timeout);
+	} while (timeval_compare(&now, &timeout_end) <= 0);
 
 	LOG_ERROR("embeddedice handshake timeout");
 	return ERROR_TARGET_TIMEOUT;
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index 3895ddfaf..bcb648c27 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -1248,8 +1248,7 @@ static int or1k_profiling(struct target *target, uint32_t *samples,
 		samples[sample_count++] = reg_value;
 
 		gettimeofday(&now, NULL);
-		if ((sample_count >= max_num_samples) ||
-			((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec))) {
+		if ((sample_count >= max_num_samples) || timeval_compare(&now, &timeout) > 0) {
 			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
 			break;
 		}
diff --git a/src/target/target.c b/src/target/target.c
index 52307dbf4..58d6d8268 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1397,7 +1397,6 @@ int target_register_trace_callback(int (*callback)(struct target *target,
 int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv)
 {
 	struct target_timer_callback **callbacks_p = &target_timer_callbacks;
-	struct timeval now;
 
 	if (callback == NULL)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1414,14 +1413,8 @@ int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int
 	(*callbacks_p)->time_ms = time_ms;
 	(*callbacks_p)->removed = false;
 
-	gettimeofday(&now, NULL);
-	(*callbacks_p)->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
-	time_ms -= (time_ms % 1000);
-	(*callbacks_p)->when.tv_sec = now.tv_sec + (time_ms / 1000);
-	if ((*callbacks_p)->when.tv_usec > 1000000) {
-		(*callbacks_p)->when.tv_usec = (*callbacks_p)->when.tv_usec - 1000000;
-		(*callbacks_p)->when.tv_sec += 1;
-	}
+	gettimeofday(&(*callbacks_p)->when, NULL);
+	timeval_add_time(&(*callbacks_p)->when, 0, time_ms * 1000);
 
 	(*callbacks_p)->priv = priv;
 	(*callbacks_p)->next = NULL;
@@ -1556,14 +1549,8 @@ int target_call_trace_callbacks(struct target *target, size_t len, uint8_t *data
 static int target_timer_callback_periodic_restart(
 		struct target_timer_callback *cb, struct timeval *now)
 {
-	int time_ms = cb->time_ms;
-	cb->when.tv_usec = now->tv_usec + (time_ms % 1000) * 1000;
-	time_ms -= (time_ms % 1000);
-	cb->when.tv_sec = now->tv_sec + time_ms / 1000;
-	if (cb->when.tv_usec > 1000000) {
-		cb->when.tv_usec = cb->when.tv_usec - 1000000;
-		cb->when.tv_sec += 1;
-	}
+	cb->when = *now;
+	timeval_add_time(&cb->when, 0, cb->time_ms * 1000L);
 	return ERROR_OK;
 }
 
@@ -1607,9 +1594,7 @@ static int target_call_timer_callbacks_check_time(int checktime)
 
 		bool call_it = (*callback)->callback &&
 			((!checktime && (*callback)->periodic) ||
-			 now.tv_sec > (*callback)->when.tv_sec ||
-			 (now.tv_sec == (*callback)->when.tv_sec &&
-			  now.tv_usec >= (*callback)->when.tv_usec));
+			 timeval_compare(&now, &(*callback)->when) >= 0);
 
 		if (call_it)
 			target_call_timer_callback(*callback, &now);
@@ -2028,8 +2013,7 @@ static int target_profiling_default(struct target *target, uint32_t *samples,
 			break;
 
 		gettimeofday(&now, NULL);
-		if ((sample_count >= max_num_samples) ||
-			((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec))) {
+		if ((sample_count >= max_num_samples) || timeval_compare(&now, &timeout) >= 0) {
 			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
 			break;
 		}
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 8fe8a2cb9..87a3d0f78 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -404,8 +404,7 @@ static int xscale_read_tx(struct target *target, int consume)
 		}
 
 		gettimeofday(&now, NULL);
-		if ((now.tv_sec > timeout.tv_sec) ||
-			((now.tv_sec == timeout.tv_sec) && (now.tv_usec > timeout.tv_usec))) {
+		if (timeval_compare(&now, &timeout) > 0) {
 			LOG_ERROR("time out reading TX register");
 			return ERROR_TARGET_TIMEOUT;
 		}

From 64f1f7b1c179dcce4e008bef6bf9515c47ae4100 Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Wed, 13 Dec 2017 13:13:22 -0800
Subject: [PATCH 55/65] Add read buffer to bitbang, improving performance.

Previously for every bit scanned OpenOCD would write the bit, wait for
that bit to be scanned, and then read the result. This involves at least
2 context switches. Most of the time the next bit scanned does not
depend on the last bit we read, so with a buffer we now write a bunch of
bits to be scanned all at once, and then we wait for them all to be
scanned and have a result.

This reduces the time for one testcase where OpenOCD connects to a
simulator from 12.30s to 5.35s!

Running all our tests went from 13m13s to 3m55s.

Change-Id: Ie9fcea043ac1d7877a521125334ed47d4b3e1615
Signed-off-by: Tim Newsome <tim@sifive.com>
Reviewed-on: http://openocd.zylin.com/4312
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/helper/replacements.h         |  11 ++
 src/jtag/drivers/at91rm9200.c     |  18 +--
 src/jtag/drivers/bcm2835gpio.c    |  22 ++--
 src/jtag/drivers/bitbang.c        | 178 ++++++++++++++++++++----------
 src/jtag/drivers/bitbang.h        |  34 +++++-
 src/jtag/drivers/dummy.c          |  13 ++-
 src/jtag/drivers/ep93xx.c         |  18 +--
 src/jtag/drivers/imx_gpio.c       |  24 ++--
 src/jtag/drivers/parport.c        |  30 +++--
 src/jtag/drivers/remote_bitbang.c | 175 ++++++++++++++++++++---------
 src/jtag/drivers/sysfsgpio.c      |  14 ++-
 11 files changed, 371 insertions(+), 166 deletions(-)

diff --git a/src/helper/replacements.h b/src/helper/replacements.h
index 1e2fbf20f..f43b7e0f3 100644
--- a/src/helper/replacements.h
+++ b/src/helper/replacements.h
@@ -199,6 +199,17 @@ static inline int close_socket(int sock)
 #endif
 }
 
+static inline void socket_block(int fd)
+{
+#ifdef _WIN32
+	unsigned long nonblock = 0;
+	ioctlsocket(fd, FIONBIO, &nonblock);
+#else
+	int oldopts = fcntl(fd, F_GETFL, 0);
+	fcntl(fd, F_SETFL, oldopts & ~O_NONBLOCK);
+#endif
+}
+
 static inline void socket_nonblock(int fd)
 {
 #ifdef _WIN32
diff --git a/src/jtag/drivers/at91rm9200.c b/src/jtag/drivers/at91rm9200.c
index 8f65413a2..0015da06b 100644
--- a/src/jtag/drivers/at91rm9200.c
+++ b/src/jtag/drivers/at91rm9200.c
@@ -109,9 +109,9 @@ static uint32_t *pio_base;
 
 /* low level command set
  */
-static int at91rm9200_read(void);
-static void at91rm9200_write(int tck, int tms, int tdi);
-static void at91rm9200_reset(int trst, int srst);
+static bb_value_t at91rm9200_read(void);
+static int at91rm9200_write(int tck, int tms, int tdi);
+static int at91rm9200_reset(int trst, int srst);
 
 static int at91rm9200_init(void);
 static int at91rm9200_quit(void);
@@ -123,12 +123,12 @@ static struct bitbang_interface at91rm9200_bitbang = {
 	.blink = 0
 };
 
-static int at91rm9200_read(void)
+static bb_value_t at91rm9200_read(void)
 {
-	return (pio_base[device->TDO_PIO + PIO_PDSR] & device->TDO_MASK) != 0;
+	return (pio_base[device->TDO_PIO + PIO_PDSR] & device->TDO_MASK) ? BB_HIGH : BB_LOW;
 }
 
-static void at91rm9200_write(int tck, int tms, int tdi)
+static int at91rm9200_write(int tck, int tms, int tdi)
 {
 	if (tck)
 		pio_base[device->TCK_PIO + PIO_SODR] = device->TCK_MASK;
@@ -144,10 +144,12 @@ static void at91rm9200_write(int tck, int tms, int tdi)
 		pio_base[device->TDI_PIO + PIO_SODR] = device->TDI_MASK;
 	else
 		pio_base[device->TDI_PIO + PIO_CODR] = device->TDI_MASK;
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void at91rm9200_reset(int trst, int srst)
+static int at91rm9200_reset(int trst, int srst)
 {
 	if (trst == 0)
 		pio_base[device->TRST_PIO + PIO_SODR] = device->TRST_MASK;
@@ -158,6 +160,8 @@ static void at91rm9200_reset(int trst, int srst)
 		pio_base[device->SRST_PIO + PIO_SODR] = device->SRST_MASK;
 	else if (srst == 1)
 		pio_base[device->SRST_PIO + PIO_CODR] = device->SRST_MASK;
+
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(at91rm9200_handle_device_command)
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index a4d0a1c8e..38ef163fa 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -49,9 +49,9 @@ uint32_t bcm2835_peri_base = 0x20000000;
 static int dev_mem_fd;
 static volatile uint32_t *pio_base;
 
-static int bcm2835gpio_read(void);
-static void bcm2835gpio_write(int tck, int tms, int tdi);
-static void bcm2835gpio_reset(int trst, int srst);
+static bb_value_t bcm2835gpio_read(void);
+static int bcm2835gpio_write(int tck, int tms, int tdi);
+static int bcm2835gpio_reset(int trst, int srst);
 
 static int bcm2835_swdio_read(void);
 static void bcm2835_swdio_drive(bool is_output);
@@ -91,12 +91,12 @@ static int speed_coeff = 113714;
 static int speed_offset = 28;
 static unsigned int jtag_delay;
 
-static int bcm2835gpio_read(void)
+static bb_value_t bcm2835gpio_read(void)
 {
-	return !!(GPIO_LEV & 1<<tdo_gpio);
+	return (GPIO_LEV & 1<<tdo_gpio) ? BB_HIGH : BB_LOW;
 }
 
-static void bcm2835gpio_write(int tck, int tms, int tdi)
+static int bcm2835gpio_write(int tck, int tms, int tdi)
 {
 	uint32_t set = tck<<tck_gpio | tms<<tms_gpio | tdi<<tdi_gpio;
 	uint32_t clear = !tck<<tck_gpio | !tms<<tms_gpio | !tdi<<tdi_gpio;
@@ -106,9 +106,11 @@ static void bcm2835gpio_write(int tck, int tms, int tdi)
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
-static void bcm2835gpio_swd_write(int tck, int tms, int tdi)
+static int bcm2835gpio_swd_write(int tck, int tms, int tdi)
 {
 	uint32_t set = tck<<swclk_gpio | tdi<<swdio_gpio;
 	uint32_t clear = !tck<<swclk_gpio | !tdi<<swdio_gpio;
@@ -118,10 +120,12 @@ static void bcm2835gpio_swd_write(int tck, int tms, int tdi)
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void bcm2835gpio_reset(int trst, int srst)
+static int bcm2835gpio_reset(int trst, int srst)
 {
 	uint32_t set = 0;
 	uint32_t clear = 0;
@@ -138,6 +142,8 @@ static void bcm2835gpio_reset(int trst, int srst)
 
 	GPIO_SET = set;
 	GPIO_CLR = clear;
+
+	return ERROR_OK;
 }
 
 static void bcm2835_swdio_drive(bool is_output)
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index c9ec9c9d6..722a5f2ac 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -41,7 +41,7 @@ extern struct jtag_interface *jtag_interface;
  * this function checks the current stable state to decide on the value of TMS
  * to use.
  */
-static void bitbang_stableclocks(int num_cycles);
+static int bitbang_stableclocks(int num_cycles);
 
 static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk);
 
@@ -70,15 +70,11 @@ struct bitbang_interface *bitbang_interface;
 /* The bitbang driver leaves the TCK 0 when in idle */
 static void bitbang_end_state(tap_state_t state)
 {
-	if (tap_is_state_stable(state))
-		tap_set_end_state(state);
-	else {
-		LOG_ERROR("BUG: %i is not a valid end state", state);
-		exit(-1);
-	}
+	assert(tap_is_state_stable(state));
+	tap_set_end_state(state);
 }
 
-static void bitbang_state_move(int skip)
+static int bitbang_state_move(int skip)
 {
 	int i = 0, tms = 0;
 	uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
@@ -86,12 +82,16 @@ static void bitbang_state_move(int skip)
 
 	for (i = skip; i < tms_count; i++) {
 		tms = (tms_scan >> i) & 1;
-		bitbang_interface->write(0, tms, 0);
-		bitbang_interface->write(1, tms, 0);
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
-	bitbang_interface->write(CLOCK_IDLE(), tms, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), tms, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	tap_set_state(tap_get_end_state());
+	return ERROR_OK;
 }
 
 /**
@@ -108,15 +108,18 @@ static int bitbang_execute_tms(struct jtag_command *cmd)
 	int tms = 0;
 	for (unsigned i = 0; i < num_bits; i++) {
 		tms = ((bits[i/8] >> (i % 8)) & 1);
-		bitbang_interface->write(0, tms, 0);
-		bitbang_interface->write(1, tms, 0);
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
-	bitbang_interface->write(CLOCK_IDLE(), tms, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), tms, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	return ERROR_OK;
 }
 
-static void bitbang_path_move(struct pathmove_command *cmd)
+static int bitbang_path_move(struct pathmove_command *cmd)
 {
 	int num_states = cmd->num_states;
 	int state_count;
@@ -135,20 +138,24 @@ static void bitbang_path_move(struct pathmove_command *cmd)
 			exit(-1);
 		}
 
-		bitbang_interface->write(0, tms, 0);
-		bitbang_interface->write(1, tms, 0);
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 
 		tap_set_state(cmd->path[state_count]);
 		state_count++;
 		num_states--;
 	}
 
-	bitbang_interface->write(CLOCK_IDLE(), tms, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), tms, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	tap_set_end_state(tap_get_state());
+	return ERROR_OK;
 }
 
-static void bitbang_runtest(int num_cycles)
+static int bitbang_runtest(int num_cycles)
 {
 	int i;
 
@@ -157,38 +164,50 @@ static void bitbang_runtest(int num_cycles)
 	/* only do a state_move when we're not already in IDLE */
 	if (tap_get_state() != TAP_IDLE) {
 		bitbang_end_state(TAP_IDLE);
-		bitbang_state_move(0);
+		if (bitbang_state_move(0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
 
 	/* execute num_cycles */
 	for (i = 0; i < num_cycles; i++) {
-		bitbang_interface->write(0, 0, 0);
-		bitbang_interface->write(1, 0, 0);
+		if (bitbang_interface->write(0, 0, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, 0, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
-	bitbang_interface->write(CLOCK_IDLE(), 0, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), 0, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	/* finish in end_state */
 	bitbang_end_state(saved_end_state);
 	if (tap_get_state() != tap_get_end_state())
-		bitbang_state_move(0);
+		if (bitbang_state_move(0) != ERROR_OK)
+			return ERROR_FAIL;
+
+	return ERROR_OK;
 }
 
-static void bitbang_stableclocks(int num_cycles)
+static int bitbang_stableclocks(int num_cycles)
 {
 	int tms = (tap_get_state() == TAP_RESET ? 1 : 0);
 	int i;
 
 	/* send num_cycles clocks onto the cable */
 	for (i = 0; i < num_cycles; i++) {
-		bitbang_interface->write(1, tms, 0);
-		bitbang_interface->write(0, tms, 0);
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
+
+	return ERROR_OK;
 }
 
-static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int scan_size)
+static int bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
+		unsigned scan_size)
 {
 	tap_state_t saved_end_state = tap_get_end_state();
-	int bit_cnt;
+	unsigned bit_cnt;
 
 	if (!((!ir_scan &&
 			(tap_get_state() == TAP_DRSHIFT)) ||
@@ -198,12 +217,13 @@ static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int
 		else
 			bitbang_end_state(TAP_DRSHIFT);
 
-		bitbang_state_move(0);
+		if (bitbang_state_move(0) != ERROR_OK)
+			return ERROR_FAIL;
 		bitbang_end_state(saved_end_state);
 	}
 
+	size_t buffered = 0;
 	for (bit_cnt = 0; bit_cnt < scan_size; bit_cnt++) {
-		int val = 0;
 		int tms = (bit_cnt == scan_size-1) ? 1 : 0;
 		int tdi;
 		int bytec = bit_cnt/8;
@@ -217,18 +237,47 @@ static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int
 		if ((type != SCAN_IN) && (buffer[bytec] & bcval))
 			tdi = 1;
 
-		bitbang_interface->write(0, tms, tdi);
-
-		if (type != SCAN_OUT)
-			val = bitbang_interface->read();
-
-		bitbang_interface->write(1, tms, tdi);
+		if (bitbang_interface->write(0, tms, tdi) != ERROR_OK)
+			return ERROR_FAIL;
 
 		if (type != SCAN_OUT) {
-			if (val)
-				buffer[bytec] |= bcval;
-			else
-				buffer[bytec] &= ~bcval;
+			if (bitbang_interface->buf_size) {
+				if (bitbang_interface->sample() != ERROR_OK)
+					return ERROR_FAIL;
+				buffered++;
+			} else {
+				switch (bitbang_interface->read()) {
+					case BB_LOW:
+						buffer[bytec] &= ~bcval;
+						break;
+					case BB_HIGH:
+						buffer[bytec] |= bcval;
+						break;
+					default:
+						return ERROR_FAIL;
+				}
+			}
+		}
+
+		if (bitbang_interface->write(1, tms, tdi) != ERROR_OK)
+			return ERROR_FAIL;
+
+		if (type != SCAN_OUT && bitbang_interface->buf_size &&
+				(buffered == bitbang_interface->buf_size ||
+				 bit_cnt == scan_size - 1)) {
+			for (unsigned i = bit_cnt + 1 - buffered; i <= bit_cnt; i++) {
+				switch (bitbang_interface->read_sample()) {
+					case BB_LOW:
+						buffer[i/8] &= ~(1 << (i % 8));
+						break;
+					case BB_HIGH:
+						buffer[i/8] |= 1 << (i % 8);
+						break;
+					default:
+						return ERROR_FAIL;
+				}
+			}
+			buffered = 0;
 		}
 	}
 
@@ -237,8 +286,10 @@ static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer, int
 		 * the shift state, so we skip the first state
 		 * and move directly to the end state.
 		 */
-		bitbang_state_move(1);
+		if (bitbang_state_move(1) != ERROR_OK)
+			return ERROR_FAIL;
 	}
+	return ERROR_OK;
 }
 
 int bitbang_execute_queue(void)
@@ -259,8 +310,10 @@ int bitbang_execute_queue(void)
 	 */
 	retval = ERROR_OK;
 
-	if (bitbang_interface->blink)
-		bitbang_interface->blink(1);
+	if (bitbang_interface->blink) {
+		if (bitbang_interface->blink(1) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 
 	while (cmd) {
 		switch (cmd->type) {
@@ -273,7 +326,9 @@ int bitbang_execute_queue(void)
 				if ((cmd->cmd.reset->trst == 1) ||
 						(cmd->cmd.reset->srst && (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
 					tap_set_state(TAP_RESET);
-				bitbang_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				if (bitbang_interface->reset(cmd->cmd.reset->trst,
+							cmd->cmd.reset->srst) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 			case JTAG_RUNTEST:
 #ifdef _DEBUG_JTAG_IO_
@@ -282,14 +337,16 @@ int bitbang_execute_queue(void)
 						tap_state_name(cmd->cmd.runtest->end_state));
 #endif
 				bitbang_end_state(cmd->cmd.runtest->end_state);
-				bitbang_runtest(cmd->cmd.runtest->num_cycles);
+				if (bitbang_runtest(cmd->cmd.runtest->num_cycles) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 
 			case JTAG_STABLECLOCKS:
 				/* this is only allowed while in a stable state.  A check for a stable
 				 * state was done in jtag_add_clocks()
 				 */
-				bitbang_stableclocks(cmd->cmd.stableclocks->num_cycles);
+				if (bitbang_stableclocks(cmd->cmd.stableclocks->num_cycles) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 
 			case JTAG_TLR_RESET:
@@ -298,7 +355,8 @@ int bitbang_execute_queue(void)
 						tap_state_name(cmd->cmd.statemove->end_state));
 #endif
 				bitbang_end_state(cmd->cmd.statemove->end_state);
-				bitbang_state_move(0);
+				if (bitbang_state_move(0) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 			case JTAG_PATHMOVE:
 #ifdef _DEBUG_JTAG_IO_
@@ -306,18 +364,22 @@ int bitbang_execute_queue(void)
 						cmd->cmd.pathmove->num_states,
 						tap_state_name(cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]));
 #endif
-				bitbang_path_move(cmd->cmd.pathmove);
+				if (bitbang_path_move(cmd->cmd.pathmove) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 			case JTAG_SCAN:
-#ifdef _DEBUG_JTAG_IO_
-				LOG_DEBUG("%s scan end in %s",
-						(cmd->cmd.scan->ir_scan) ? "IR" : "DR",
-					tap_state_name(cmd->cmd.scan->end_state));
-#endif
 				bitbang_end_state(cmd->cmd.scan->end_state);
 				scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
+#ifdef _DEBUG_JTAG_IO_
+				LOG_DEBUG("%s scan %d bits; end in %s",
+						(cmd->cmd.scan->ir_scan) ? "IR" : "DR",
+						scan_size,
+					tap_state_name(cmd->cmd.scan->end_state));
+#endif
 				type = jtag_scan_type(cmd->cmd.scan);
-				bitbang_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				if (bitbang_scan(cmd->cmd.scan->ir_scan, type, buffer,
+							scan_size) != ERROR_OK)
+					return ERROR_FAIL;
 				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
 					retval = ERROR_JTAG_QUEUE_FAILED;
 				if (buffer)
@@ -338,8 +400,10 @@ int bitbang_execute_queue(void)
 		}
 		cmd = cmd->next;
 	}
-	if (bitbang_interface->blink)
-		bitbang_interface->blink(0);
+	if (bitbang_interface->blink) {
+		if (bitbang_interface->blink(0) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 
 	return retval;
 }
diff --git a/src/jtag/drivers/bitbang.h b/src/jtag/drivers/bitbang.h
index c5b44bfd5..577717ebd 100644
--- a/src/jtag/drivers/bitbang.h
+++ b/src/jtag/drivers/bitbang.h
@@ -24,13 +24,35 @@
 
 #include <jtag/swd.h>
 
+typedef enum {
+	BB_LOW,
+	BB_HIGH,
+	BB_ERROR
+} bb_value_t;
+
+/** Low level callbacks (for bitbang).
+ *
+ * Either read(), or sample() and read_sample() must be implemented.
+ *
+ * The sample functions allow an interface to batch a number of writes and
+ * sample requests together. Not waiting for a value to come back can greatly
+ * increase throughput. */
 struct bitbang_interface {
-	/* low level callbacks (for bitbang)
-	 */
-	int (*read)(void);
-	void (*write)(int tck, int tms, int tdi);
-	void (*reset)(int trst, int srst);
-	void (*blink)(int on);
+	/** Sample TDO. */
+	bb_value_t (*read)(void);
+
+	/** The number of TDO samples that can be buffered up before the caller has
+	 * to call read_sample. */
+	size_t buf_size;
+	/** Sample TDO and put the result in a buffer. */
+	int (*sample)(void);
+	/** Return the next unread value from the buffer. */
+	bb_value_t (*read_sample)(void);
+
+	/** Set TCK, TMS, and TDI to the given values. */
+	int (*write)(int tck, int tms, int tdi);
+	int (*reset)(int trst, int srst);
+	int (*blink)(int on);
 	int (*swdio_read)(void);
 	void (*swdio_drive)(bool on);
 };
diff --git a/src/jtag/drivers/dummy.c b/src/jtag/drivers/dummy.c
index 0f7c12dd4..db1ba13a4 100644
--- a/src/jtag/drivers/dummy.c
+++ b/src/jtag/drivers/dummy.c
@@ -33,14 +33,14 @@ static int clock_count;		/* count clocks in any stable state, only stable states
 
 static uint32_t dummy_data;
 
-static int dummy_read(void)
+static bb_value_t dummy_read(void)
 {
 	int data = 1 & dummy_data;
 	dummy_data = (dummy_data >> 1) | (1 << 31);
-	return data;
+	return data ? BB_HIGH : BB_LOW;
 }
 
-static void dummy_write(int tck, int tms, int tdi)
+static int dummy_write(int tck, int tms, int tdi)
 {
 	/* TAP standard: "state transitions occur on rising edge of clock" */
 	if (tck != dummy_clock) {
@@ -69,9 +69,10 @@ static void dummy_write(int tck, int tms, int tdi)
 		}
 		dummy_clock = tck;
 	}
+	return ERROR_OK;
 }
 
-static void dummy_reset(int trst, int srst)
+static int dummy_reset(int trst, int srst)
 {
 	dummy_clock = 0;
 
@@ -79,10 +80,12 @@ static void dummy_reset(int trst, int srst)
 		dummy_state = TAP_RESET;
 
 	LOG_DEBUG("reset to: %s", tap_state_name(dummy_state));
+	return ERROR_OK;
 }
 
-static void dummy_led(int on)
+static int dummy_led(int on)
 {
+	return ERROR_OK;
 }
 
 static struct bitbang_interface dummy_bitbang = {
diff --git a/src/jtag/drivers/ep93xx.c b/src/jtag/drivers/ep93xx.c
index ccd979502..36fc77747 100644
--- a/src/jtag/drivers/ep93xx.c
+++ b/src/jtag/drivers/ep93xx.c
@@ -41,9 +41,9 @@ static volatile uint8_t *gpio_data_direction_register;
 
 /* low level command set
  */
-static int ep93xx_read(void);
-static void ep93xx_write(int tck, int tms, int tdi);
-static void ep93xx_reset(int trst, int srst);
+static bb_value_t ep93xx_read(void);
+static int ep93xx_write(int tck, int tms, int tdi);
+static int ep93xx_reset(int trst, int srst);
 
 static int ep93xx_init(void);
 static int ep93xx_quit(void);
@@ -67,12 +67,12 @@ static struct bitbang_interface ep93xx_bitbang = {
 	.blink = 0,
 };
 
-static int ep93xx_read(void)
+static bb_value_t ep93xx_read(void)
 {
-	return !!(*gpio_data_register & TDO_BIT);
+	return (*gpio_data_register & TDO_BIT) ? BB_HIGH : BB_LOW;
 }
 
-static void ep93xx_write(int tck, int tms, int tdi)
+static int ep93xx_write(int tck, int tms, int tdi)
 {
 	if (tck)
 		output_value |= TCK_BIT;
@@ -91,10 +91,12 @@ static void ep93xx_write(int tck, int tms, int tdi)
 
 	*gpio_data_register = output_value;
 	nanosleep(&ep93xx_zzzz, NULL);
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void ep93xx_reset(int trst, int srst)
+static int ep93xx_reset(int trst, int srst)
 {
 	if (trst == 0)
 		output_value |= TRST_BIT;
@@ -108,6 +110,8 @@ static void ep93xx_reset(int trst, int srst)
 
 	*gpio_data_register = output_value;
 	nanosleep(&ep93xx_zzzz, NULL);
+
+	return ERROR_OK;
 }
 
 static int set_gonk_mode(void)
diff --git a/src/jtag/drivers/imx_gpio.c b/src/jtag/drivers/imx_gpio.c
index f33d10976..2a822afe9 100644
--- a/src/jtag/drivers/imx_gpio.c
+++ b/src/jtag/drivers/imx_gpio.c
@@ -82,9 +82,9 @@ static inline bool gpio_level(int g)
 	return pio_base[g / 32].dr >> (g & 0x1F) & 1;
 }
 
-static int imx_gpio_read(void);
-static void imx_gpio_write(int tck, int tms, int tdi);
-static void imx_gpio_reset(int trst, int srst);
+static bb_value_t imx_gpio_read(void);
+static int imx_gpio_write(int tck, int tms, int tdi);
+static int imx_gpio_reset(int trst, int srst);
 
 static int imx_gpio_swdio_read(void);
 static void imx_gpio_swdio_drive(bool is_output);
@@ -128,12 +128,12 @@ static int speed_coeff = 50000;
 static int speed_offset = 100;
 static unsigned int jtag_delay;
 
-static int imx_gpio_read(void)
+static bb_value_t imx_gpio_read(void)
 {
-	return gpio_level(tdo_gpio);
+	return gpio_level(tdo_gpio) ? BB_HIGH : BB_LOW;
 }
 
-static void imx_gpio_write(int tck, int tms, int tdi)
+static int imx_gpio_write(int tck, int tms, int tdi)
 {
 	tms ? gpio_set(tms_gpio) : gpio_clear(tms_gpio);
 	tdi ? gpio_set(tdi_gpio) : gpio_clear(tdi_gpio);
@@ -141,25 +141,31 @@ static void imx_gpio_write(int tck, int tms, int tdi)
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
-static void imx_gpio_swd_write(int tck, int tms, int tdi)
+static int imx_gpio_swd_write(int tck, int tms, int tdi)
 {
 	tdi ? gpio_set(swdio_gpio) : gpio_clear(swdio_gpio);
 	tck ? gpio_set(swclk_gpio) : gpio_clear(swclk_gpio);
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void imx_gpio_reset(int trst, int srst)
+static int imx_gpio_reset(int trst, int srst)
 {
 	if (trst_gpio != -1)
 		trst ? gpio_set(trst_gpio) : gpio_clear(trst_gpio);
 
 	if (srst_gpio != -1)
 		srst ? gpio_set(srst_gpio) : gpio_clear(srst_gpio);
+
+	return ERROR_OK;
 }
 
 static void imx_gpio_swdio_drive(bool is_output)
@@ -469,7 +475,7 @@ static int imx_gpio_init(void)
 
 
 	LOG_INFO("imx_gpio mmap: pagesize: %u, regionsize: %u",
-			sysconf(_SC_PAGE_SIZE), IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE);
+			(unsigned int) sysconf(_SC_PAGE_SIZE), IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE);
 	pio_base = mmap(NULL, IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE,
 				PROT_READ | PROT_WRITE,
 				MAP_SHARED, dev_mem_fd, imx_gpio_peri_base);
diff --git a/src/jtag/drivers/parport.c b/src/jtag/drivers/parport.c
index c9e331644..14fa9df9f 100644
--- a/src/jtag/drivers/parport.c
+++ b/src/jtag/drivers/parport.c
@@ -116,7 +116,7 @@ static unsigned long dataport;
 static unsigned long statusport;
 #endif
 
-static int parport_read(void)
+static bb_value_t parport_read(void)
 {
 	int data = 0;
 
@@ -127,9 +127,9 @@ static int parport_read(void)
 #endif
 
 	if ((data ^ cable->INPUT_INVERT) & cable->TDO_MASK)
-		return 1;
+		return BB_HIGH;
 	else
-		return 0;
+		return BB_LOW;
 }
 
 static inline void parport_write_data(void)
@@ -148,7 +148,7 @@ static inline void parport_write_data(void)
 #endif
 }
 
-static void parport_write(int tck, int tms, int tdi)
+static int parport_write(int tck, int tms, int tdi)
 {
 	int i = wait_states + 1;
 
@@ -169,10 +169,12 @@ static void parport_write(int tck, int tms, int tdi)
 
 	while (i-- > 0)
 		parport_write_data();
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void parport_reset(int trst, int srst)
+static int parport_reset(int trst, int srst)
 {
 	LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 
@@ -187,10 +189,12 @@ static void parport_reset(int trst, int srst)
 		dataport_value &= ~cable->SRST_MASK;
 
 	parport_write_data();
+
+	return ERROR_OK;
 }
 
 /* turn LED on parport adapter on (1) or off (0) */
-static void parport_led(int on)
+static int parport_led(int on)
 {
 	if (on)
 		dataport_value |= cable->LED_MASK;
@@ -198,6 +202,8 @@ static void parport_led(int on)
 		dataport_value &= ~cable->LED_MASK;
 
 	parport_write_data();
+
+	return ERROR_OK;
 }
 
 static int parport_speed(int speed)
@@ -365,9 +371,12 @@ static int parport_init(void)
 
 #endif /* PARPORT_USE_PPDEV */
 
-	parport_reset(0, 0);
-	parport_write(0, 0, 0);
-	parport_led(1);
+	if (parport_reset(0, 0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (parport_write(0, 0, 0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (parport_led(1) != ERROR_OK)
+		return ERROR_FAIL;
 
 	bitbang_interface = &parport_bitbang;
 
@@ -376,7 +385,8 @@ static int parport_init(void)
 
 static int parport_quit(void)
 {
-	parport_led(0);
+	if (parport_led(0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	if (parport_exit) {
 		dataport_value = cable->PORT_EXIT;
diff --git a/src/jtag/drivers/remote_bitbang.c b/src/jtag/drivers/remote_bitbang.c
index c8d013666..1f8fc1a11 100644
--- a/src/jtag/drivers/remote_bitbang.c
+++ b/src/jtag/drivers/remote_bitbang.c
@@ -30,40 +30,86 @@
 /* arbitrary limit on host name length: */
 #define REMOTE_BITBANG_HOST_MAX 255
 
-#define REMOTE_BITBANG_RAISE_ERROR(expr ...) \
-	do { \
-		LOG_ERROR(expr); \
-		LOG_ERROR("Terminating openocd."); \
-		exit(-1); \
-	} while (0)
-
 static char *remote_bitbang_host;
 static char *remote_bitbang_port;
 
-FILE *remote_bitbang_in;
-FILE *remote_bitbang_out;
+static FILE *remote_bitbang_file;
+static int remote_bitbang_fd;
 
-static void remote_bitbang_putc(int c)
+/* Circular buffer. When start == end, the buffer is empty. */
+static char remote_bitbang_buf[64];
+static unsigned remote_bitbang_start;
+static unsigned remote_bitbang_end;
+
+static int remote_bitbang_buf_full(void)
 {
-	if (EOF == fputc(c, remote_bitbang_out))
-		REMOTE_BITBANG_RAISE_ERROR("remote_bitbang_putc: %s", strerror(errno));
+	return remote_bitbang_end ==
+		((remote_bitbang_start + sizeof(remote_bitbang_buf) - 1) %
+		 sizeof(remote_bitbang_buf));
+}
+
+/* Read any incoming data, placing it into the buffer. */
+static int remote_bitbang_fill_buf(void)
+{
+	socket_nonblock(remote_bitbang_fd);
+	while (!remote_bitbang_buf_full()) {
+		unsigned contiguous_available_space;
+		if (remote_bitbang_end >= remote_bitbang_start) {
+			contiguous_available_space = sizeof(remote_bitbang_buf) -
+				remote_bitbang_end;
+			if (remote_bitbang_start == 0)
+				contiguous_available_space -= 1;
+		} else {
+			contiguous_available_space = remote_bitbang_start -
+				remote_bitbang_end - 1;
+		}
+		ssize_t count = read(remote_bitbang_fd,
+				remote_bitbang_buf + remote_bitbang_end,
+				contiguous_available_space);
+		if (count > 0) {
+			remote_bitbang_end += count;
+			if (remote_bitbang_end == sizeof(remote_bitbang_buf))
+				remote_bitbang_end = 0;
+		} else if (count == 0) {
+			return ERROR_OK;
+		} else if (count < 0) {
+			if (errno == EAGAIN) {
+				return ERROR_OK;
+			} else {
+				LOG_ERROR("remote_bitbang_fill_buf: %s (%d)",
+						strerror(errno), errno);
+				return ERROR_FAIL;
+			}
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int remote_bitbang_putc(int c)
+{
+	if (EOF == fputc(c, remote_bitbang_file)) {
+		LOG_ERROR("remote_bitbang_putc: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
 }
 
 static int remote_bitbang_quit(void)
 {
-	if (EOF == fputc('Q', remote_bitbang_out)) {
+	if (EOF == fputc('Q', remote_bitbang_file)) {
 		LOG_ERROR("fputs: %s", strerror(errno));
 		return ERROR_FAIL;
 	}
 
-	if (EOF == fflush(remote_bitbang_out)) {
+	if (EOF == fflush(remote_bitbang_file)) {
 		LOG_ERROR("fflush: %s", strerror(errno));
 		return ERROR_FAIL;
 	}
 
 	/* We only need to close one of the FILE*s, because they both use the same */
 	/* underlying file descriptor. */
-	if (EOF == fclose(remote_bitbang_out)) {
+	if (EOF == fclose(remote_bitbang_file)) {
 		LOG_ERROR("fclose: %s", strerror(errno));
 		return ERROR_FAIL;
 	}
@@ -75,53 +121,83 @@ static int remote_bitbang_quit(void)
 	return ERROR_OK;
 }
 
-/* Get the next read response. */
-static int remote_bitbang_rread(void)
+static bb_value_t char_to_int(int c)
 {
-	if (EOF == fflush(remote_bitbang_out)) {
-		remote_bitbang_quit();
-		REMOTE_BITBANG_RAISE_ERROR("fflush: %s", strerror(errno));
-	}
-
-	int c = fgetc(remote_bitbang_in);
 	switch (c) {
 		case '0':
-			return 0;
+			return BB_LOW;
 		case '1':
-			return 1;
+			return BB_HIGH;
 		default:
 			remote_bitbang_quit();
-			REMOTE_BITBANG_RAISE_ERROR(
-					"remote_bitbang: invalid read response: %c(%i)", c, c);
+			LOG_ERROR("remote_bitbang: invalid read response: %c(%i)", c, c);
+			return BB_ERROR;
 	}
 }
 
-static int remote_bitbang_read(void)
+/* Get the next read response. */
+static bb_value_t remote_bitbang_rread(void)
 {
-	remote_bitbang_putc('R');
+	if (EOF == fflush(remote_bitbang_file)) {
+		remote_bitbang_quit();
+		LOG_ERROR("fflush: %s", strerror(errno));
+		return BB_ERROR;
+	}
+
+	/* Enable blocking access. */
+	socket_block(remote_bitbang_fd);
+	char c;
+	ssize_t count = read(remote_bitbang_fd, &c, 1);
+	if (count == 1) {
+		return char_to_int(c);
+	} else {
+		remote_bitbang_quit();
+		LOG_ERROR("read: count=%d, error=%s", (int) count, strerror(errno));
+		return BB_ERROR;
+	}
+}
+
+static int remote_bitbang_sample(void)
+{
+	if (remote_bitbang_fill_buf() != ERROR_OK)
+		return ERROR_FAIL;
+	assert(!remote_bitbang_buf_full());
+	return remote_bitbang_putc('R');
+}
+
+static bb_value_t remote_bitbang_read_sample(void)
+{
+	if (remote_bitbang_start != remote_bitbang_end) {
+		int c = remote_bitbang_buf[remote_bitbang_start];
+		remote_bitbang_start =
+			(remote_bitbang_start + 1) % sizeof(remote_bitbang_buf);
+		return char_to_int(c);
+	}
 	return remote_bitbang_rread();
 }
 
-static void remote_bitbang_write(int tck, int tms, int tdi)
+static int remote_bitbang_write(int tck, int tms, int tdi)
 {
 	char c = '0' + ((tck ? 0x4 : 0x0) | (tms ? 0x2 : 0x0) | (tdi ? 0x1 : 0x0));
-	remote_bitbang_putc(c);
+	return remote_bitbang_putc(c);
 }
 
-static void remote_bitbang_reset(int trst, int srst)
+static int remote_bitbang_reset(int trst, int srst)
 {
 	char c = 'r' + ((trst ? 0x2 : 0x0) | (srst ? 0x1 : 0x0));
-	remote_bitbang_putc(c);
+	return remote_bitbang_putc(c);
 }
 
-static void remote_bitbang_blink(int on)
+static int remote_bitbang_blink(int on)
 {
 	char c = on ? 'B' : 'b';
-	remote_bitbang_putc(c);
+	return remote_bitbang_putc(c);
 }
 
 static struct bitbang_interface remote_bitbang_bitbang = {
-	.read = &remote_bitbang_read,
+	.buf_size = sizeof(remote_bitbang_buf) - 1,
+	.sample = &remote_bitbang_sample,
+	.read_sample = &remote_bitbang_read_sample,
 	.write = &remote_bitbang_write,
 	.reset = &remote_bitbang_reset,
 	.blink = &remote_bitbang_blink,
@@ -131,7 +207,7 @@ static int remote_bitbang_init_tcp(void)
 {
 	struct addrinfo hints = { .ai_family = AF_UNSPEC, .ai_socktype = SOCK_STREAM };
 	struct addrinfo *result, *rp;
-	int fd;
+	int fd = 0;
 
 	LOG_INFO("Connecting to %s:%s",
 			remote_bitbang_host ? remote_bitbang_host : "localhost",
@@ -199,29 +275,24 @@ static int remote_bitbang_init_unix(void)
 
 static int remote_bitbang_init(void)
 {
-	int fd;
 	bitbang_interface = &remote_bitbang_bitbang;
 
+	remote_bitbang_start = 0;
+	remote_bitbang_end = 0;
+
 	LOG_INFO("Initializing remote_bitbang driver");
 	if (remote_bitbang_port == NULL)
-		fd = remote_bitbang_init_unix();
+		remote_bitbang_fd = remote_bitbang_init_unix();
 	else
-		fd = remote_bitbang_init_tcp();
+		remote_bitbang_fd = remote_bitbang_init_tcp();
 
-	if (fd < 0)
-		return fd;
+	if (remote_bitbang_fd < 0)
+		return remote_bitbang_fd;
 
-	remote_bitbang_in = fdopen(fd, "r");
-	if (remote_bitbang_in == NULL) {
-		LOG_ERROR("fdopen: failed to open read stream");
-		close(fd);
-		return ERROR_FAIL;
-	}
-
-	remote_bitbang_out = fdopen(fd, "w");
-	if (remote_bitbang_out == NULL) {
+	remote_bitbang_file = fdopen(remote_bitbang_fd, "w+");
+	if (remote_bitbang_file == NULL) {
 		LOG_ERROR("fdopen: failed to open write stream");
-		fclose(remote_bitbang_in);
+		close(remote_bitbang_fd);
 		return ERROR_FAIL;
 	}
 
diff --git a/src/jtag/drivers/sysfsgpio.c b/src/jtag/drivers/sysfsgpio.c
index b8acfa2fc..5a4651df1 100644
--- a/src/jtag/drivers/sysfsgpio.c
+++ b/src/jtag/drivers/sysfsgpio.c
@@ -244,7 +244,7 @@ static void sysfsgpio_swdio_write(int swclk, int swdio)
  * The sysfs value will read back either '0' or '1'. The trick here is to call
  * lseek to bypass buffering in the sysfs kernel driver.
  */
-static int sysfsgpio_read(void)
+static bb_value_t sysfsgpio_read(void)
 {
 	char buf[1];
 
@@ -257,7 +257,7 @@ static int sysfsgpio_read(void)
 		return 0;
 	}
 
-	return buf[0] != '0';
+	return buf[0] == '0' ? BB_LOW : BB_HIGH;
 }
 
 /*
@@ -266,11 +266,11 @@ static int sysfsgpio_read(void)
  * Seeing as this is the only function where the outputs are changed,
  * we can cache the old value to avoid needlessly writing it.
  */
-static void sysfsgpio_write(int tck, int tms, int tdi)
+static int sysfsgpio_write(int tck, int tms, int tdi)
 {
 	if (swd_mode) {
 		sysfsgpio_swdio_write(tck, tdi);
-		return;
+		return ERROR_OK;
 	}
 
 	const char one[] = "1";
@@ -312,6 +312,8 @@ static void sysfsgpio_write(int tck, int tms, int tdi)
 	last_tdi = tdi;
 	last_tms = tms;
 	last_tck = tck;
+
+	return ERROR_OK;
 }
 
 /*
@@ -319,7 +321,7 @@ static void sysfsgpio_write(int tck, int tms, int tdi)
  *
  * (1) assert or (0) deassert reset lines
  */
-static void sysfsgpio_reset(int trst, int srst)
+static int sysfsgpio_reset(int trst, int srst)
 {
 	LOG_DEBUG("sysfsgpio_reset");
 	const char one[] = "1";
@@ -339,6 +341,8 @@ static void sysfsgpio_reset(int trst, int srst)
 		if (bytes_written != 1)
 			LOG_WARNING("writing trst failed");
 	}
+
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionums)

From a640f139baa9fe4f44d428b1e4e1a9da245532ca Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 22 Jan 2018 12:28:37 +0100
Subject: [PATCH 56/65] aarch64: implement mmu on/off for aarch32

add decoding of aarch32 core modes (register layout is compatible)

Change-Id: I34c3146a7b1f836d3006be2b76b036da055b3d3e
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4374
Tested-by: jenkins
Reviewed-by: Forest Crossman <cyrozap@gmail.com>
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
---
 src/target/aarch64.c | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 784274a0e..bcfce6592 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -161,8 +161,16 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	case ARMV8_64_EL3T:
 		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
 		break;
+
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT:
+	case ARM_MODE_FIQ:
+	case ARM_MODE_IRQ:
+		instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
+		break;
+
 	default:
-		LOG_DEBUG("unknown cpu state 0x%x" PRIx32, armv8->arm.core_state);
+		LOG_DEBUG("unknown cpu state 0x%" PRIx32, armv8->arm.core_mode);
 		break;
 	}
 

From a0c4c9e717d17b1c706f27ece5ce43b6acfa0ba7 Mon Sep 17 00:00:00 2001
From: Daniel Kucera <daniel.kucera@gmail.com>
Date: Sat, 13 Jan 2018 16:37:19 +0100
Subject: [PATCH 57/65] nor/nrf5: added nrf51822 QFAAH2

Change-Id: I59725e098371c63ec3e6aa1d91bfed36b824a182
Signed-off-by: Daniel Kucera <daniel.kucera@gmail.com>
Reviewed-on: http://openocd.zylin.com/4334
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/flash/nor/nrf5.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/flash/nor/nrf5.c b/src/flash/nor/nrf5.c
index 11e57291c..4fa62e30a 100644
--- a/src/flash/nor/nrf5.c
+++ b/src/flash/nor/nrf5.c
@@ -168,6 +168,7 @@ static const struct nrf5_device_spec nrf5_known_devices_table[] = {
 
 	/* nRF51822 Devices (IC rev 3). */
 	NRF5_DEVICE_DEF(0x0072, "51822", "QFAA", "H0",    256),
+	NRF5_DEVICE_DEF(0x00D1, "51822", "QFAA", "H2",    256),
 	NRF5_DEVICE_DEF(0x007B, "51822", "QFAB", "C0",    128),
 	NRF5_DEVICE_DEF(0x0083, "51822", "QFAC", "A0",    256),
 	NRF5_DEVICE_DEF(0x0084, "51822", "QFAC", "A1",    256),

From e60573c528081cbf10269ea447e8c3521fddeed0 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sat, 31 Dec 2016 00:04:48 +0300
Subject: [PATCH 58/65] jtag: drivers: usb_blaster: clarify lowlevel driver
 selection code

This patch should make the code logic more transparent and user
notifications more useful.

It also fixes a warning "array subscript is below array bounds" that
leads to FTBFS on Raspbian GNU/Linux 8 (jessie).

Change-Id: I626b6a5bc013dfee7d36cf196f0abab981d30675
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3931
Tested-by: jenkins
---
 src/jtag/drivers/usb_blaster/usb_blaster.c | 38 ++++++++++++----------
 1 file changed, 21 insertions(+), 17 deletions(-)

diff --git a/src/jtag/drivers/usb_blaster/usb_blaster.c b/src/jtag/drivers/usb_blaster/usb_blaster.c
index a975bd1e2..df9f2a174 100644
--- a/src/jtag/drivers/usb_blaster/usb_blaster.c
+++ b/src/jtag/drivers/usb_blaster/usb_blaster.c
@@ -839,26 +839,30 @@ static int ublast_init(void)
 {
 	int ret, i;
 
-	if (info.lowlevel_name) {
-		for (i = 0; lowlevel_drivers_map[i].name; i++)
-			if (!strcmp(lowlevel_drivers_map[i].name, info.lowlevel_name))
+	for (i = 0; lowlevel_drivers_map[i].name; i++) {
+		if (info.lowlevel_name) {
+			if (!strcmp(lowlevel_drivers_map[i].name, info.lowlevel_name)) {
+				info.drv = lowlevel_drivers_map[i].drv_register();
+				if (!info.drv) {
+					LOG_ERROR("Error registering lowlevel driver \"%s\"",
+						  info.lowlevel_name);
+					return ERROR_JTAG_DEVICE_ERROR;
+				}
 				break;
-		if (lowlevel_drivers_map[i].name)
+			}
+		} else {
 			info.drv = lowlevel_drivers_map[i].drv_register();
-		if (!info.drv) {
-			LOG_ERROR("no lowlevel driver found for %s or lowlevel driver opening error",
-				  info.lowlevel_name);
-			return ERROR_JTAG_DEVICE_ERROR;
+			if (info.drv) {
+				info.lowlevel_name = strdup(lowlevel_drivers_map[i].name);
+				LOG_INFO("No lowlevel driver configured, using %s", info.lowlevel_name);
+				break;
+			}
 		}
-	} else {
-		LOG_INFO("No lowlevel driver configured, will try them all");
-		for (i = 0; !info.drv && lowlevel_drivers_map[i].name; i++)
-			info.drv = lowlevel_drivers_map[i].drv_register();
-		if (!info.drv) {
-			LOG_ERROR("no lowlevel driver found");
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-		info.lowlevel_name = strdup(lowlevel_drivers_map[i-1].name);
+	}
+
+	if (!info.drv) {
+		LOG_ERROR("No lowlevel driver available");
+		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
 	/*

From 1ac0f5d4937ef573a4adb3eafe883ad327c440ce Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Tue, 16 Jan 2018 13:05:04 +0100
Subject: [PATCH 59/65] aarch64: clean up scan-build errors

scan-build reported a couple of problems with code in aarch64.c,
this patch cleans them up. No functional changes.

Change-Id: Ie210237ddc840a8bbcd535f86a3a5faf473132f2
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4346
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c | 36 +++++++++++++++++++++++++++++-------
 1 file changed, 29 insertions(+), 7 deletions(-)

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index bcfce6592..14a2da6c1 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -642,9 +642,11 @@ static int aarch64_prepare_restart_one(struct target *target)
 				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 	}
 
-	/* clear sticky bits in PRSR, SDR is now 0 */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_PRSR, &tmp);
+	if (retval == ERROR_OK) {
+		/* clear sticky bits in PRSR, SDR is now 0 */
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_PRSR, &tmp);
+	}
 
 	return retval;
 }
@@ -1815,6 +1817,8 @@ static int aarch64_write_cpu_memory(struct target *target,
 	dscr = (dscr & ~DSCR_MA);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		/* Write X0 with value 'address' using write procedure */
@@ -1826,10 +1830,13 @@ static int aarch64_write_cpu_memory(struct target *target,
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRX */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-		dpm->instr_write_data_dcc(dpm,
+		retval = dpm->instr_write_data_dcc(dpm,
 				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
 	}
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	if (size == 4 && (address % 4) == 0)
 		retval = aarch64_write_cpu_memory_fast(target, count, buffer, &dscr);
 	else
@@ -1941,13 +1948,21 @@ static int aarch64_read_cpu_memory_fast(struct target *target,
 		retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
 	}
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* Step 1.e - Change DCC to memory mode */
 	*dscr |= DSCR_MA;
 	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* Step 1.f - read DBGDTRTX and discard the value */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+	if (retval != ERROR_OK)
+		return retval;
 
 	count--;
 	/* Read the data - Each read of the DTRTX register causes the instruction to be reissued
@@ -2011,28 +2026,35 @@ static int aarch64_read_cpu_memory(struct target *target,
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
 	/* Set Normal access mode  */
 	dscr &= ~DSCR_MA;
-	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		/* Write X0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
-		retval += dpm->instr_write_data_dcc_64(dpm,
+		retval = dpm->instr_write_data_dcc_64(dpm,
 				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-		retval += dpm->instr_write_data_dcc(dpm,
+		retval = dpm->instr_write_data_dcc(dpm,
 				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
 	}
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	if (size == 4 && (address % 4) == 0)
 		retval = aarch64_read_cpu_memory_fast(target, count, buffer, &dscr);
 	else

From dd890d4cad7c62a65d0efd04c5e0b700eeb5928d Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Thu, 18 Jan 2018 09:58:55 +0100
Subject: [PATCH 60/65] arm_adi_v5: fix return value of mem_ap_read/write for
 size 0

Unhandled marginal case produced a warning in Clang static analyzer.

Change-Id: I3e2fc4182fa4f863acfb972b1e7a512fce5bf33a
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Suggested-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-on: http://openocd.zylin.com/4357
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
---
 src/target/arm_adi_v5.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index aa7f4cf64..dfbc5ade2 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -325,7 +325,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 	const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
 	uint32_t csw_size;
 	uint32_t addr_xor;
-	int retval;
+	int retval = ERROR_OK;
 
 	/* TI BE-32 Quirks mode:
 	 * Writes on big-endian TMS570 behave very strangely. Observed behavior:
@@ -455,7 +455,7 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
 	uint32_t csw_size;
 	uint32_t address = adr;
-	int retval;
+	int retval = ERROR_OK;
 
 	/* TI BE-32 Quirks mode:
 	 * Reads on big-endian TMS570 behave strangely differently than writes.

From fa86553e76ebf6285d5b580a08bfbecdd9ec38c1 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Wed, 17 Jan 2018 22:00:42 +0300
Subject: [PATCH 61/65] x86_32_common: fix some warnings

Mostly "Dead assignment" reported by Clang static analyzer.

Change-Id: Ibf81d2ba2462570ee3a40e57a60c55a1d1fa0c00
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4351
Tested-by: jenkins
---
 src/target/x86_32_common.c | 43 +++++++++++++++++++++++++-------------
 1 file changed, 28 insertions(+), 15 deletions(-)

diff --git a/src/target/x86_32_common.c b/src/target/x86_32_common.c
index 0d87ba5a2..ef4f80680 100644
--- a/src/target/x86_32_common.c
+++ b/src/target/x86_32_common.c
@@ -209,15 +209,16 @@ static int read_phys_mem(struct target *t, uint32_t phys_address,
 			LOG_ERROR("%s invalid read size", __func__);
 			break;
 		}
+		if (retval != ERROR_OK)
+			break;
 	}
 	/* restore CR0.PG bit if needed (regardless of retval) */
 	if (pg_disabled) {
-		retval = x86_32->enable_paging(t);
-		if (retval != ERROR_OK) {
+		int retval2 = x86_32->enable_paging(t);
+		if (retval2 != ERROR_OK) {
 			LOG_ERROR("%s could not enable paging", __func__);
-			return retval;
+			return retval2;
 		}
-		pg_disabled = true;
 	}
 	/* TODO: After reading memory from target, we must replace
 	 * software breakpoints with the original instructions again.
@@ -364,6 +365,9 @@ static int read_mem(struct target *t, uint32_t size,
 			break;
 	}
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* read_hw_reg() will write to 4 bytes (uint32_t)
 	 * Watch out, the buffer passed into read_mem() might be 1 or 2 bytes.
 	 */
@@ -436,6 +440,10 @@ static int write_mem(struct target *t, uint32_t size,
 			LOG_ERROR("%s invalid write mem size", __func__);
 			return ERROR_FAIL;
 	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
 	retval = x86_32->transaction_status(t);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("%s error on mem write", __func__);
@@ -606,7 +614,6 @@ int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 			&& x86_32_common_read_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
 			LOG_ERROR("%s failed to read memory from physical address " TARGET_ADDR_FMT,
 					  __func__, physaddr);
-			retval = ERROR_FAIL;
 		}
 		/* restore PG bit if it was cleared prior (regardless of retval) */
 		retval = x86_32->enable_paging(t);
@@ -662,7 +669,6 @@ int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 			&& x86_32_common_write_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
 			LOG_ERROR("%s failed to write memory to physical address " TARGET_ADDR_FMT,
 					__func__, physaddr);
-			retval = ERROR_FAIL;
 		}
 		/* restore PG bit if it was cleared prior (regardless of retval) */
 		retval = x86_32->enable_paging(t);
@@ -733,15 +739,19 @@ int x86_32_common_read_io(struct target *t, uint32_t addr,
 			LOG_ERROR("%s invalid read io size", __func__);
 			return ERROR_FAIL;
 	}
+
 	/* restore CR0.PG bit if needed */
 	if (pg_disabled) {
-		retval = x86_32->enable_paging(t);
-		if (retval != ERROR_OK) {
+		int retval2 = x86_32->enable_paging(t);
+		if (retval2 != ERROR_OK) {
 			LOG_ERROR("%s could not enable paging", __func__);
-			return retval;
+			return retval2;
 		}
-		pg_disabled = false;
 	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
 	uint32_t regval = 0;
 	retval = x86_32->read_hw_reg(t, EAX, &regval, 0);
 	if (retval != ERROR_OK) {
@@ -818,15 +828,19 @@ int x86_32_common_write_io(struct target *t, uint32_t addr,
 			LOG_ERROR("%s invalid write io size", __func__);
 			return ERROR_FAIL;
 	}
+
 	/* restore CR0.PG bit if needed */
 	if (pg_disabled) {
-		retval = x86_32->enable_paging(t);
-		if (retval != ERROR_OK) {
+		int retval2 = x86_32->enable_paging(t);
+		if (retval2 != ERROR_OK) {
 			LOG_ERROR("%s could not enable paging", __func__);
-			return retval;
+			return retval2;
 		}
-		pg_disabled = false;
 	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
 	retval = x86_32->transaction_status(t);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("%s error on io write", __func__);
@@ -1141,7 +1155,6 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 			}
 		} else {
 			LOG_ERROR("%s core doesn't support SW breakpoints", __func__);
-			error = ERROR_FAIL;
 			return ERROR_FAIL;
 		}
 	}

From ab597027ea98d4fa39980f9674dced42b5193b79 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Tue, 16 Jan 2018 15:08:34 +0300
Subject: [PATCH 62/65] doxygen: fix issues with recent Doxygen versions

This fixes parsing of several documentation files which do not have
dot in the filename.

style.txt change fixes doxygen warnings and ensures proper display of
comment begin/end instructions in HTML.

Tested with Doxygen 1.8.13.

Change-Id: I9fd9ac3b51b47076b8fdbd3e93b90c3eba9b9631
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4342
Tested-by: jenkins
---
 Doxyfile.in          | 2 +-
 doc/manual/style.txt | 6 +++---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/Doxyfile.in b/Doxyfile.in
index 13b0c24f6..6e68b4e37 100644
--- a/Doxyfile.in
+++ b/Doxyfile.in
@@ -216,7 +216,7 @@ OPTIMIZE_OUTPUT_VHDL   = NO
 # .inc files as Fortran files (default is PHP), and .f files as C (default is Fortran), 
 # use: inc=Fortran f=C
 
-EXTENSION_MAPPING      = 
+EXTENSION_MAPPING      = no_extension=C
 
 # If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
 # to include (a tag file for) the STL sources as input, then you should
diff --git a/doc/manual/style.txt b/doc/manual/style.txt
index 2ff2a29ee..0bfae35f7 100644
--- a/doc/manual/style.txt
+++ b/doc/manual/style.txt
@@ -176,10 +176,10 @@ comments.
  * @returns The value(s) returned, or possible error conditions.
  */
 @endverbatim
-  -# The block should start on the line following the opening @c /**.
-  -# The end of the block, \f$*/\f$, should also be on its own line.
+  -# The block should start on the line following the opening @c /\**.
+  -# The end of the block, @c *&zwj;/, should also be on its own line.
   -# Every line in the block should have a @c '*' in-line with its start:
-    - A leading space is required to align the @c '*' with the @c /** line.
+    - A leading space is required to align the @c '*' with the @c /\** line.
     - A single "empty" line should separate the function documentation
       from the block of parameter and return value descriptions.
     - Except to separate paragraphs of documentation, other extra

From 3c9bd7c6f30a34e3930e44dd2e8ce5f5a877b4eb Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sun, 14 Jan 2018 19:28:20 +0300
Subject: [PATCH 63/65] flash: nor: jtagspi: fix jtagspi_read_status() warning

Clang static analyzer says that in certain cases "Assigned value is
garbage or undefined" there.

Change-Id: Ib35a4cf7a553ba9461270a0dc4c4b9b205091e73
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/4338
Tested-by: jenkins
---
 src/flash/nor/jtagspi.c | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/flash/nor/jtagspi.c b/src/flash/nor/jtagspi.c
index a05ff3786..a73812d88 100644
--- a/src/flash/nor/jtagspi.c
+++ b/src/flash/nor/jtagspi.c
@@ -224,9 +224,10 @@ static int jtagspi_probe(struct flash_bank *bank)
 static void jtagspi_read_status(struct flash_bank *bank, uint32_t *status)
 {
 	uint8_t buf;
-	jtagspi_cmd(bank, SPIFLASH_READ_STATUS, NULL, &buf, -8);
-	*status = buf;
-	/* LOG_DEBUG("status=0x%08" PRIx32, *status); */
+	if (jtagspi_cmd(bank, SPIFLASH_READ_STATUS, NULL, &buf, -8) == ERROR_OK) {
+		*status = buf;
+		/* LOG_DEBUG("status=0x%08" PRIx32, *status); */
+	}
 }
 
 static int jtagspi_wait(struct flash_bank *bank, int timeout_ms)

From 5a98ff786e11cec4c4ed4f53c69ab2d6de30eac9 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sat, 31 Dec 2016 16:05:55 +0300
Subject: [PATCH 64/65] tcl: interface: harmonise RPi configs

Make all configs specify same JTAG and SWD GPIO numbers.

Change-Id: I65b09c1671c97f253f0aab88e511de7409d91e0a
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3932
Tested-by: jenkins
---
 tcl/interface/raspberrypi-native.cfg    |  5 ++++-
 tcl/interface/raspberrypi2-native.cfg   | 14 +++++---------
 tcl/interface/sysfsgpio-raspberrypi.cfg | 18 ++++++++++++++----
 3 files changed, 23 insertions(+), 14 deletions(-)

diff --git a/tcl/interface/raspberrypi-native.cfg b/tcl/interface/raspberrypi-native.cfg
index 6b73f3541..c63dfdbc7 100644
--- a/tcl/interface/raspberrypi-native.cfg
+++ b/tcl/interface/raspberrypi-native.cfg
@@ -21,6 +21,10 @@ bcm2835gpio_speed_coeffs 113714 28
 # Header pin numbers: 23 22 19 21
 bcm2835gpio_jtag_nums 11 25 10 9
 
+# Each of the SWD lines need a gpio number set: swclk swdio
+# Header pin numbers: 23 22
+bcm2835gpio_swd_nums 11 25
+
 # If you define trst or srst, use appropriate reset_config
 # Header pin numbers: TRST - 26, SRST - 18
 
@@ -32,4 +36,3 @@ bcm2835gpio_jtag_nums 11 25 10 9
 
 # or if you have both connected,
 # reset_config trst_and_srst srst_push_pull
-
diff --git a/tcl/interface/raspberrypi2-native.cfg b/tcl/interface/raspberrypi2-native.cfg
index f846fa25d..26a31c55e 100644
--- a/tcl/interface/raspberrypi2-native.cfg
+++ b/tcl/interface/raspberrypi2-native.cfg
@@ -19,14 +19,11 @@ bcm2835gpio_speed_coeffs 146203 36
 
 # Each of the JTAG lines need a gpio number set: tck tms tdi tdo
 # Header pin numbers: 23 22 19 21
-# bcm2835gpio_jtag_nums 11 25 10 9
-
-# or if you have both connected,
-# reset_config trst_and_srst srst_push_pull
+bcm2835gpio_jtag_nums 11 25 10 9
 
 # Each of the SWD lines need a gpio number set: swclk swdio
-# Header pin numbers: 22 18
-bcm2835gpio_swd_nums 25 24
+# Header pin numbers: 23 22
+bcm2835gpio_swd_nums 11 25
 
 # If you define trst or srst, use appropriate reset_config
 # Header pin numbers: TRST - 26, SRST - 18
@@ -34,9 +31,8 @@ bcm2835gpio_swd_nums 25 24
 # bcm2835gpio_trst_num 7
 # reset_config trst_only
 
-bcm2835gpio_srst_num 18
-reset_config srst_only srst_push_pull
+# bcm2835gpio_srst_num 24
+# reset_config srst_only srst_push_pull
 
 # or if you have both connected,
 # reset_config trst_and_srst srst_push_pull
-
diff --git a/tcl/interface/sysfsgpio-raspberrypi.cfg b/tcl/interface/sysfsgpio-raspberrypi.cfg
index 363642284..9f5b87c38 100644
--- a/tcl/interface/sysfsgpio-raspberrypi.cfg
+++ b/tcl/interface/sysfsgpio-raspberrypi.cfg
@@ -14,8 +14,18 @@ interface sysfsgpio
 # Header pin numbers: 23 22 19 21
 sysfsgpio_jtag_nums 11 25 10 9
 
-# At least one of srst or trst needs to be specified
-# Header pin numbers: TRST - 26, SRST - 18
-sysfsgpio_trst_num 7
-# sysfsgpio_srst_num 24
+# Each of the SWD lines need a gpio number set: swclk swdio
+# Header pin numbers: 23 22
+sysfsgpio_swd_nums 11 25
 
+# If you define trst or srst, use appropriate reset_config
+# Header pin numbers: TRST - 26, SRST - 18
+
+# sysfsgpio_trst_num 7
+# reset_config trst_only
+
+# sysfsgpio_srst_num 24
+# reset_config srst_only srst_push_pull
+
+# or if you have both connected,
+# reset_config trst_and_srst srst_push_pull

From c2b2a7a3b84913465420ae7fa0394304943cf035 Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Sat, 23 Sep 2017 00:24:19 +0200
Subject: [PATCH 65/65] Kinetis_ke: add KEAx family to texi and cfg comment
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Change-Id: Id8f676b027f57fc540473c1a3a01bdd2ec49a200
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/4232
Tested-by: jenkins
Reviewed-by: Joakim Nohlgård <joakim.nohlgard@eistec.se>
---
 doc/openocd.texi    | 4 ++--
 tcl/target/ke0x.cfg | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/doc/openocd.texi b/doc/openocd.texi
index 483b27b7f..898ffb948 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -5401,7 +5401,7 @@ from NXP (former Freescale) include
 internal flash and use ARM Cortex-M0+ or M4 cores. The driver automatically
 recognizes flash size and a number of flash banks (1-4) using the chip
 identification register, and autoconfigures itself.
-Use kinetis_ke driver for KE0x devices.
+Use kinetis_ke driver for KE0x and KEAx devices.
 
 The @var{kinetis} driver defines option:
 @itemize
@@ -5494,7 +5494,7 @@ Command disables watchdog timer.
 
 @deffn {Flash Driver} kinetis_ke
 @cindex kinetis_ke
-KE0x members of the Kinetis microcontroller family from Freescale include
+KE0x and KEAx members of the Kinetis microcontroller family from NXP include
 internal flash and use ARM Cortex-M0+. The driver automatically recognizes
 the KE0x sub-family using the chip identification register, and
 autoconfigures itself.
diff --git a/tcl/target/ke0x.cfg b/tcl/target/ke0x.cfg
index 7927e0ae0..1f1b1321e 100644
--- a/tcl/target/ke0x.cfg
+++ b/tcl/target/ke0x.cfg
@@ -1,5 +1,5 @@
 #
-# Freescale Kinetis KE0x series devices
+# Freescale Kinetis KE0x and KEAx series devices
 #
 
 source [find target/swj-dp.tcl]