From 727f178ab9d7cee04d883555aa57b917c3f2296c Mon Sep 17 00:00:00 2001
From: Kent Brinkley <jkbrinkley.imgtec@gmail.com>
Date: Tue, 21 Oct 2014 07:46:30 -0700
Subject: mips: Added #define for scan_delay legacy mode default value

Believe in using defines to make maintenance easier.

Change-Id: I8edf151352131bbf2b884dfcd67ca5764b11b13c
Signed-off-by: Kent Brinkley <jkbrinkley.imgtec@gmail.com>
Reviewed-on: http://openocd.zylin.com/2350
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/target/mips32.c   | 4 ++--
 src/target/mips32.h   | 2 ++
 src/target/mips_m4k.c | 2 +-
 3 files changed, 5 insertions(+), 3 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32.c b/src/target/mips32.c
index 3292446..8c77986 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -384,7 +384,7 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 	mips32->read_core_reg = mips32_read_core_reg;
 	mips32->write_core_reg = mips32_write_core_reg;
 
-	mips32->ejtag_info.scan_delay = 2000000;	/* Initial default value */
+	mips32->ejtag_info.scan_delay = MIPS32_SCAN_DELAY_LEGACY_MODE;	/* Initial default value */
 	mips32->ejtag_info.mode = 0;			/* Initial default value */
 
 	return ERROR_OK;
@@ -911,7 +911,7 @@ COMMAND_HANDLER(mips32_handle_scan_delay_command)
 			return ERROR_COMMAND_SYNTAX_ERROR;
 
 	command_print(CMD_CTX, "scan delay: %d nsec", ejtag_info->scan_delay);
-	if (ejtag_info->scan_delay >= 2000000) {
+	if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) {
 		ejtag_info->mode = 0;
 		command_print(CMD_CTX, "running in legacy mode");
 	} else {
diff --git a/src/target/mips32.h b/src/target/mips32.h
index bfd2cf5..8362f5e 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -61,6 +61,8 @@
 #define MIPS32_ARCH_REL1 0x0
 #define MIPS32_ARCH_REL2 0x1
 
+#define MIPS32_SCAN_DELAY_LEGACY_MODE 2000000
+
 /* offsets into mips32 core register cache */
 enum {
 	MIPS32_PC = 37,
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index 0daa71c..db69b95 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -1339,7 +1339,7 @@ COMMAND_HANDLER(mips_m4k_handle_scan_delay_command)
 			return ERROR_COMMAND_SYNTAX_ERROR;
 
 	command_print(CMD_CTX, "scan delay: %d nsec", ejtag_info->scan_delay);
-	if (ejtag_info->scan_delay >= 2000000) {
+	if (ejtag_info->scan_delay >= MIPS32_SCAN_DELAY_LEGACY_MODE) {
 		ejtag_info->mode = 0;
 		command_print(CMD_CTX, "running in legacy mode");
 	} else {
-- 
cgit v1.1


From 7919694832ec989b71b93a9a9563a05c9e503d13 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 14 Aug 2016 18:14:09 +0200
Subject: x86_32_common: Fix typo in function name
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

pyhs -> phys

Change-Id: Ie7edc74f1693b42f26e1e8475a93a7a6b9255cdd
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3672
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/target/x86_32_common.c | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

(limited to 'src/target')

diff --git a/src/target/x86_32_common.c b/src/target/x86_32_common.c
index 3f4c7aa..c21167e 100644
--- a/src/target/x86_32_common.c
+++ b/src/target/x86_32_common.c
@@ -48,7 +48,7 @@ static int read_mem(struct target *t, uint32_t size,
 			uint32_t addr, uint8_t *buf);
 static int write_mem(struct target *t, uint32_t size,
 			uint32_t addr, const uint8_t *buf);
-static int calcaddr_pyhsfromlin(struct target *t, uint32_t addr,
+static int calcaddr_physfromlin(struct target *t, uint32_t addr,
 			uint32_t *physaddr);
 static int read_phys_mem(struct target *t, uint32_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
@@ -134,7 +134,7 @@ int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physic
 
 	} else {
 		/* target halted in protected mode */
-		if (calcaddr_pyhsfromlin(t, address, physical) != ERROR_OK) {
+		if (calcaddr_physfromlin(t, address, physical) != ERROR_OK) {
 			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
 					__func__, address);
 			return ERROR_FAIL;
@@ -444,7 +444,7 @@ static int write_mem(struct target *t, uint32_t size,
 	return retval;
 }
 
-int calcaddr_pyhsfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
+int calcaddr_physfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
 {
 	uint8_t entry_buffer[8];
 
@@ -592,7 +592,7 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 			return retval;
 		}
 		uint32_t physaddr = 0;
-		if (calcaddr_pyhsfromlin(t, addr, &physaddr) != ERROR_OK) {
+		if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
 			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32, __func__, addr);
 			retval = ERROR_FAIL;
 		}
@@ -646,7 +646,7 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 			return retval;
 		}
 		uint32_t physaddr = 0;
-		if (calcaddr_pyhsfromlin(t, addr, &physaddr) != ERROR_OK) {
+		if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
 			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
 					__func__, addr);
 			retval = ERROR_FAIL;
@@ -1016,7 +1016,7 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 	uint8_t opcode = SW_BP_OPCODE;
 	uint8_t readback;
 
-	if (calcaddr_pyhsfromlin(t, bp->address, &physaddr) != ERROR_OK)
+	if (calcaddr_physfromlin(t, bp->address, &physaddr) != ERROR_OK)
 		return ERROR_FAIL;
 	if (read_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
 		return ERROR_FAIL;
@@ -1072,7 +1072,7 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 	uint8_t current_instr;
 
 	/* check that user program has not modified breakpoint instruction */
-	if (calcaddr_pyhsfromlin(t, bp->address, &physaddr) != ERROR_OK)
+	if (calcaddr_physfromlin(t, bp->address, &physaddr) != ERROR_OK)
 		return ERROR_FAIL;
 	if (read_phys_mem(t, physaddr, 1, 1, &current_instr))
 		return ERROR_FAIL;
-- 
cgit v1.1


From 5fd5699859db4be2dadef69c85adbb0acda36e84 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 14 Aug 2016 18:48:38 +0200
Subject: nds32: Fix typo in debug log
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

wathcpoint -> watchpoint

Change-Id: If84cfb5097ed17ef97491667c622ba7d870ac9c2
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3673
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
---
 src/target/nds32_v2.c  | 4 ++--
 src/target/nds32_v3.c  | 4 ++--
 src/target/nds32_v3m.c | 4 ++--
 3 files changed, 6 insertions(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/nds32_v2.c b/src/target/nds32_v2.c
index da8bbbc..4497551 100644
--- a/src/target/nds32_v2.c
+++ b/src/target/nds32_v2.c
@@ -184,7 +184,7 @@ static int nds32_v2_activate_hardware_watchpoint(struct target *target)
 		/* set value */
 		aice_write_debug_reg(aice, NDS_EDM_SR_BPV0 + wp_num, 0);
 
-		LOG_DEBUG("Add hardware wathcpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32, wp_num,
+		LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32, wp_num,
 				wp->address, wp->mask);
 
 	}
@@ -204,7 +204,7 @@ static int nds32_v2_deactivate_hardware_watchpoint(struct target *target)
 		/* disable watchpoint */
 		aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-		LOG_DEBUG("Remove hardware wathcpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
+		LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
 				wp_num, wp->address, wp->mask);
 	}
 
diff --git a/src/target/nds32_v3.c b/src/target/nds32_v3.c
index 43d7054..35b72e3 100644
--- a/src/target/nds32_v3.c
+++ b/src/target/nds32_v3.c
@@ -128,7 +128,7 @@ static int nds32_v3_activate_hardware_watchpoint(struct target *target)
 			/* set value */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPV0 + wp_num, 0);
 
-			LOG_DEBUG("Add hardware wathcpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
+			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
 					wp_num, wp->address, wp->mask);
 
 			wp_num++;
@@ -169,7 +169,7 @@ static int nds32_v3_deactivate_hardware_watchpoint(struct target *target)
 			/* disable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-			LOG_DEBUG("Remove hardware wathcpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" PRIx32
 					" mask %08" PRIx32, wp_num,
 					wp->address, wp->mask);
 			wp_num++;
diff --git a/src/target/nds32_v3m.c b/src/target/nds32_v3m.c
index 919c0c8..2d52bc3 100644
--- a/src/target/nds32_v3m.c
+++ b/src/target/nds32_v3m.c
@@ -125,7 +125,7 @@ static int nds32_v3m_activate_hardware_watchpoint(struct target *target)
 			/* enable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, wp_config);
 
-			LOG_DEBUG("Add hardware wathcpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" PRIx32
 					" mask %08" PRIx32, wp_num, wp->address, wp->mask);
 
 			wp_num++;
@@ -166,7 +166,7 @@ static int nds32_v3m_deactivate_hardware_watchpoint(struct target *target)
 			/* disable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-			LOG_DEBUG("Remove hardware wathcpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" PRIx32
 					" mask %08" PRIx32, wp_num, wp->address, wp->mask);
 			wp_num++;
 		} else if (nds32_v3m->nds32.global_stop) {
-- 
cgit v1.1


From 640894e7316b4e45ee4533c611a3d4c4bee6177a Mon Sep 17 00:00:00 2001
From: Tomas Vanek <vanekt@fbl.cz>
Date: Sat, 17 Sep 2016 17:36:20 +0200
Subject: cortex_m: fix autoincrement range of Cortex-M7

Cortex-M7 has autoincrement range only 1024 bytes,
surprisingly smaller than M3, M4.

Change-Id: I35ff1f0e093aac4af79f98eb3b8058d4295942d1
Signed-off-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-on: http://openocd.zylin.com/3737
Tested-by: jenkins
Reviewed-by: Spencer Oliver <spen@spen-soft.co.uk>
---
 src/target/cortex_m.c | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

(limited to 'src/target')

diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index a6a9309..4566e2e 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1976,10 +1976,14 @@ int cortex_m_examine(struct target *target)
 			armv7m->arm.core_cache->num_regs = ARMV7M_NUM_CORE_REGS_NOFP;
 		}
 
-		if ((i == 3 || i == 4 || i == 7) && !armv7m->stlink) {
-			/* Cortex-M3/M4/M7 have at least 4096 bytes autoincrement range,
-			 * s. ARM IHI 0031C: MEM-AP 7.2.2 */
-			armv7m->debug_ap->tar_autoincr_block = (1 << 12);
+		if (!armv7m->stlink) {
+			if (i == 3 || i == 4)
+				/* Cortex-M3/M4 have 4096 bytes autoincrement range,
+				 * s. ARM IHI 0031C: MEM-AP 7.2.2 */
+				armv7m->debug_ap->tar_autoincr_block = (1 << 12);
+			else if (i == 7)
+				/* Cortex-M7 has only 1024 bytes autoincrement range */
+				armv7m->debug_ap->tar_autoincr_block = (1 << 10);
 		}
 
 		/* Configure trace modules */
-- 
cgit v1.1


From da7459415079002a52f516a6d00a31474d61b495 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 21 Aug 2016 20:12:02 +0200
Subject: target: Fix working_area_phys_spec comment
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

working_area_phys_spec clearly refers to the physical, not virtual address.

Change-Id: I639ea00bb5d05e845b8a56815a571375849f1225
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3714
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/target/target.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/target.h b/src/target/target.h
index 0cee117..a1a30ff 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -154,7 +154,7 @@ struct target {
 										 * upon first allocation from virtual/physical address. */
 	bool working_area_virt_spec;		/* virtual address specified? */
 	uint32_t working_area_virt;			/* virtual address */
-	bool working_area_phys_spec;		/* virtual address specified? */
+	bool working_area_phys_spec;		/* physical address specified? */
 	uint32_t working_area_phys;			/* physical address */
 	uint32_t working_area_size;			/* size in bytes */
 	uint32_t backup_working_area;		/* whether the content of the working area has to be preserved */
-- 
cgit v1.1


From 5fba5068b74cb277683be1f29ed10a234c74d7ce Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 21 Aug 2016 22:12:52 +0200
Subject: breakpoints: Add missing space in error message
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

A space after the format specifier was missing.

Change-Id: Ib67eb0fb0d6e05d765206d30d5e4a74cb41bb47b
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3715
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/breakpoints.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index 9591714..c4a959d 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -403,7 +403,7 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 				|| watchpoint->mask != mask
 				|| watchpoint->rw != rw) {
 				LOG_ERROR("address 0x%8.8" PRIx32
-					"already has watchpoint %d",
+					" already has watchpoint %d",
 					address, watchpoint->unique_id);
 				return ERROR_FAIL;
 			}
-- 
cgit v1.1


From c06ac3b5d357351e34438452a72a14e534910c6f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 21 Aug 2016 22:49:17 +0200
Subject: target: Add missing spaces in error messages
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Insert a space before parenthesis in logs that we will need to touch
for 64-bit target addresses.

While at it, do a couple more surrounding whitespace fixes.

Change-Id: I1080c0470aab51cf7bd56e67e934344d0bf4c5c1
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3716
Tested-by: jenkins
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
---
 src/target/target.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index 56d9eee..fefaa0e 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -2034,8 +2034,8 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
 		return ERROR_OK;
 
 	if ((address + size - 1) < address) {
-		/* GDB can request this when e.g. PC is 0xfffffffc*/
-		LOG_ERROR("address + size wrapped(0x%08x, 0x%08x)",
+		/* GDB can request this when e.g. PC is 0xfffffffc */
+		LOG_ERROR("address + size wrapped (0x%08x, 0x%08x)",
 				  (unsigned)address,
 				  (unsigned)size);
 		return ERROR_FAIL;
@@ -2095,8 +2095,8 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
 		return ERROR_OK;
 
 	if ((address + size - 1) < address) {
-		/* GDB can request this when e.g. PC is 0xfffffffc*/
-		LOG_ERROR("address + size wrapped(0x%08" PRIx32 ", 0x%08" PRIx32 ")",
+		/* GDB can request this when e.g. PC is 0xfffffffc */
+		LOG_ERROR("address + size wrapped (0x%08" PRIx32 ", 0x%08" PRIx32 ")",
 				  address,
 				  size);
 		return ERROR_FAIL;
@@ -4002,7 +4002,7 @@ static int target_mem2array(Jim_Interp *interp, struct target *target, int argc,
 		sprintf(buf, "mem2array address: 0x%08" PRIx32 " is not aligned for %" PRId32 " byte reads",
 				addr,
 				width);
-		Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
+		Jim_AppendStrings(interp, Jim_GetResult(interp), buf, NULL);
 		return JIM_ERR;
 	}
 
@@ -4208,7 +4208,7 @@ static int target_array2mem(Jim_Interp *interp, struct target *target,
 		sprintf(buf, "array2mem address: 0x%08x is not aligned for %d byte reads",
 				(unsigned int)addr,
 				(int)width);
-		Jim_AppendStrings(interp, Jim_GetResult(interp), buf , NULL);
+		Jim_AppendStrings(interp, Jim_GetResult(interp), buf, NULL);
 		return JIM_ERR;
 	}
 
-- 
cgit v1.1


From 10aeff925936a43e873acfd0068b1eea03781051 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 21 Aug 2016 20:56:44 +0200
Subject: target: Clean up format strings
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Clean up some type casts and misuses of format specifiers in preparation
for target address type changes.

Change-Id: Idf08286f41bca636e35a09e8ddc1d71af3d6e151
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3717
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/target.c | 46 +++++++++++++++++++++++-----------------------
 1 file changed, 23 insertions(+), 23 deletions(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index fefaa0e..e3c8168 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -2022,8 +2022,8 @@ static int target_profiling_default(struct target *target, uint32_t *samples,
  */
 int target_write_buffer(struct target *target, uint32_t address, uint32_t size, const uint8_t *buffer)
 {
-	LOG_DEBUG("writing buffer of %i byte at 0x%8.8x",
-			(int)size, (unsigned)address);
+	LOG_DEBUG("writing buffer of %" PRIi32 " byte at 0x%8.8" PRIx32,
+			  size, address);
 
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -2035,9 +2035,9 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
 
 	if ((address + size - 1) < address) {
 		/* GDB can request this when e.g. PC is 0xfffffffc */
-		LOG_ERROR("address + size wrapped (0x%08x, 0x%08x)",
-				  (unsigned)address,
-				  (unsigned)size);
+		LOG_ERROR("address + size wrapped (0x%08" PRIx32 ", 0x%08" PRIx32 ")",
+				  address,
+				  size);
 		return ERROR_FAIL;
 	}
 
@@ -2083,8 +2083,8 @@ static int target_write_buffer_default(struct target *target, uint32_t address,
  */
 int target_read_buffer(struct target *target, uint32_t address, uint32_t size, uint8_t *buffer)
 {
-	LOG_DEBUG("reading buffer of %i byte at 0x%8.8x",
-			  (int)size, (unsigned)address);
+	LOG_DEBUG("reading buffer of %" PRIi32 " byte at 0x%8.8" PRIx32,
+			  size, address);
 
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -2153,7 +2153,7 @@ int target_checksum_memory(struct target *target, uint32_t address, uint32_t siz
 	if (retval != ERROR_OK) {
 		buffer = malloc(size);
 		if (buffer == NULL) {
-			LOG_ERROR("error allocating buffer for section (%d bytes)", (int)size);
+			LOG_ERROR("error allocating buffer for section (%" PRId32 " bytes)", size);
 			return ERROR_COMMAND_SYNTAX_ERROR;
 		}
 		retval = target_read_buffer(target, address, size, buffer);
@@ -2254,7 +2254,7 @@ int target_read_u16(struct target *target, uint32_t address, uint16_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u16(target, value_buf);
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%4.4x",
+		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%4.4" PRIx16,
 				  address,
 				  *value);
 	} else {
@@ -2276,7 +2276,7 @@ int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
 	int retval = target_read_memory(target, address, 1, 1, value);
 
 	if (retval == ERROR_OK) {
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2x",
+		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2" PRIx8,
 				  address,
 				  *value);
 	} else {
@@ -2339,7 +2339,7 @@ int target_write_u16(struct target *target, uint32_t address, uint16_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8x",
+	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8" PRIx16,
 			  address,
 			  value);
 
@@ -2359,7 +2359,7 @@ int target_write_u8(struct target *target, uint32_t address, uint8_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2x",
+	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2" PRIx8,
 			  address, value);
 
 	retval = target_write_memory(target, address, 1, 1, &value);
@@ -4031,10 +4031,10 @@ static int target_mem2array(Jim_Interp *interp, struct target *target, int argc,
 			retval = target_read_memory(target, addr, width, count, buffer);
 		if (retval != ERROR_OK) {
 			/* BOO !*/
-			LOG_ERROR("mem2array: Read @ 0x%08x, w=%d, cnt=%d, failed",
-					  (unsigned int)addr,
-					  (int)width,
-					  (int)count);
+			LOG_ERROR("mem2array: Read @ 0x%08" PRIx32 ", w=%" PRId32 ", cnt=%" PRId32 ", failed",
+					  addr,
+					  width,
+					  count);
 			Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
 			Jim_AppendStrings(interp, Jim_GetResult(interp), "mem2array: cannot read memory", NULL);
 			e = JIM_ERR;
@@ -4205,9 +4205,9 @@ static int target_array2mem(Jim_Interp *interp, struct target *target,
 	} else {
 		char buf[100];
 		Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
-		sprintf(buf, "array2mem address: 0x%08x is not aligned for %d byte reads",
-				(unsigned int)addr,
-				(int)width);
+		sprintf(buf, "array2mem address: 0x%08" PRIx32 " is not aligned for %" PRId32 " byte reads",
+				addr,
+				width);
 		Jim_AppendStrings(interp, Jim_GetResult(interp), buf, NULL);
 		return JIM_ERR;
 	}
@@ -4254,10 +4254,10 @@ static int target_array2mem(Jim_Interp *interp, struct target *target,
 			retval = target_write_memory(target, addr, width, count, buffer);
 		if (retval != ERROR_OK) {
 			/* BOO !*/
-			LOG_ERROR("array2mem: Write @ 0x%08x, w=%d, cnt=%d, failed",
-					  (unsigned int)addr,
-					  (int)width,
-					  (int)count);
+			LOG_ERROR("array2mem: Write @ 0x%08" PRIx32 ", w=%" PRId32 ", cnt=%" PRId32 ", failed",
+					  addr,
+					  width,
+					  count);
 			Jim_SetResult(interp, Jim_NewEmptyStringObj(interp));
 			Jim_AppendStrings(interp, Jim_GetResult(interp), "array2mem: cannot read memory", NULL);
 			e = JIM_ERR;
-- 
cgit v1.1


From eaacb900dd3ce5257dcfec35a8032a873d141b6a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Andreas=20F=C3=A4rber?= <afaerber@suse.de>
Date: Sun, 8 May 2016 23:49:07 +0200
Subject: flash/nor: Add erased_value to drivers and pass it to targets
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

struct flash_driver has a default_padded_value field that is similar,
but it can be changed by the user for the specific purpose of padding.

Add a new erased_value field and initialize it for all targets,
particularly stm32lx, xmc4xxx and virtual.

Use this value in core.c:default_flash_mem_blank_check(), the slow path.

Extend the target API to pass erased_value down to target code.
Adding an argument ensures that we catch all callers.

This allows us to merge xmc4xxx.c:xmc4xxx_blank_check_memory() into
armv7m:armv7m_blank_check_memory().

It further allows us to use default_flash_blank_check() in place of
xmc4xxx.c:xmc4xxx_flash_blank_check(), adding a potential slow path
fallback, as well as stm32lx:stm32lx_erase_check(), adding the potential
armv7m fast path with fallback to default_flash_mem_blank_check().

Fix a mips32 code comment while at it (zeroed -> erased).

The armv4_5 and mips32 target implementations will now error out if an
erase value other than 0xff is used, causing default_flash_blank_check()
to fall back to the default_flank_mem_blank_check() slow path.

Change-Id: I39323fbbc4b71c256cd567e439896d0245d4745f
Signed-off-by: Andreas Färber <afaerber@suse.de>
Reviewed-on: http://openocd.zylin.com/3497
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/arm.h         |  2 +-
 src/target/armv4_5.c     | 10 ++++++++--
 src/target/armv7m.c      | 28 ++++++++++++++++++++++------
 src/target/armv7m.h      |  2 +-
 src/target/mips32.c      | 12 +++++++++---
 src/target/mips32.h      |  2 +-
 src/target/target.c      |  5 +++--
 src/target/target.h      |  2 +-
 src/target/target_type.h |  2 +-
 9 files changed, 47 insertions(+), 18 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm.h b/src/target/arm.h
index 163db23..28022e3 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -232,7 +232,7 @@ int armv4_5_run_algorithm_inner(struct target *target,
 int arm_checksum_memory(struct target *target,
 		uint32_t address, uint32_t count, uint32_t *checksum);
 int arm_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank);
+		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 void arm_set_cpsr(struct arm *arm, uint32_t cpsr);
 struct reg *arm_reg_current(struct arm *arm, unsigned regnum);
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index e6ecfc8..1a31d40 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -1486,7 +1486,7 @@ cleanup:
  *
  */
 int arm_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank)
+	uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *check_algorithm;
 	struct reg_param reg_params[3];
@@ -1502,6 +1502,12 @@ int arm_blank_check_memory(struct target *target,
 
 	assert(sizeof(check_code_le) % 4 == 0);
 
+	if (erased_value != 0xff) {
+		LOG_ERROR("Erase value 0x%02" PRIx8 " not yet supported for ARMv4/v5 targets",
+			erased_value);
+		return ERROR_FAIL;
+	}
+
 	/* make sure we have a working area */
 	retval = target_alloc_working_area(target,
 			sizeof(check_code_le), &check_algorithm);
@@ -1529,7 +1535,7 @@ int arm_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
 	/* armv4 must exit using a hardware breakpoint */
 	if (arm->is_armv4)
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index e2f710f..60b244a 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -726,26 +726,42 @@ cleanup:
 	return retval;
 }
 
-/** Checks whether a memory region is zeroed. */
+/** Checks whether a memory region is erased. */
 int armv7m_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank)
+	uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
 	struct reg_param reg_params[3];
 	struct armv7m_algorithm armv7m_info;
+	const uint8_t *code;
+	uint32_t code_size;
 	int retval;
 
 	static const uint8_t erase_check_code[] = {
 #include "../../contrib/loaders/erase_check/armv7m_erase_check.inc"
 	};
+	static const uint8_t zero_erase_check_code[] = {
+#include "../../contrib/loaders/erase_check/armv7m_0_erase_check.inc"
+	};
+
+	switch (erased_value) {
+	case 0x00:
+		code = zero_erase_check_code;
+		code_size = sizeof(zero_erase_check_code);
+		break;
+	case 0xff:
+	default:
+		code = erase_check_code;
+		code_size = sizeof(erase_check_code);
+	}
 
 	/* make sure we have a working area */
-	if (target_alloc_working_area(target, sizeof(erase_check_code),
+	if (target_alloc_working_area(target, code_size,
 		&erase_check_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
 	retval = target_write_buffer(target, erase_check_algorithm->address,
-			sizeof(erase_check_code), (uint8_t *)erase_check_code);
+			code_size, code);
 	if (retval != ERROR_OK)
 		goto cleanup;
 
@@ -759,7 +775,7 @@ int armv7m_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	init_reg_param(&reg_params[2], "r2", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
 	retval = target_run_algorithm(target,
 			0,
@@ -767,7 +783,7 @@ int armv7m_blank_check_memory(struct target *target,
 			3,
 			reg_params,
 			erase_check_algorithm->address,
-			erase_check_algorithm->address + (sizeof(erase_check_code) - 2),
+			erase_check_algorithm->address + (code_size - 2),
 			10000,
 			&armv7m_info);
 
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 90cad00..304c72d 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -225,7 +225,7 @@ int armv7m_restore_context(struct target *target);
 int armv7m_checksum_memory(struct target *target,
 		uint32_t address, uint32_t count, uint32_t *checksum);
 int armv7m_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank);
+		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found);
 
diff --git a/src/target/mips32.c b/src/target/mips32.c
index 8c77986..27caebe 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -771,9 +771,9 @@ int mips32_checksum_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-/** Checks whether a memory region is zeroed. */
+/** Checks whether a memory region is erased. */
 int mips32_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank)
+		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
 	struct reg_param reg_params[3];
@@ -789,6 +789,12 @@ int mips32_blank_check_memory(struct target *target,
 		0x7000003F		/* sdbbp */
 	};
 
+	if (erased_value != 0xff) {
+		LOG_ERROR("Erase value 0x%02" PRIx8 " not yet supported for MIPS32",
+			erased_value);
+		return ERROR_FAIL;
+	}
+
 	/* make sure we have a working area */
 	if (target_alloc_working_area(target, sizeof(erase_check_code), &erase_check_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
@@ -810,7 +816,7 @@ int mips32_blank_check_memory(struct target *target,
 	buf_set_u32(reg_params[1].value, 0, 32, count);
 
 	init_reg_param(&reg_params[2], "r6", 32, PARAM_IN_OUT);
-	buf_set_u32(reg_params[2].value, 0, 32, 0xff);
+	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
 	int retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
 			erase_check_algorithm->address,
diff --git a/src/target/mips32.h b/src/target/mips32.h
index 8362f5e..d493b39 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -249,6 +249,6 @@ int mips32_get_gdb_reg_list(struct target *target,
 int mips32_checksum_memory(struct target *target, uint32_t address,
 		uint32_t count, uint32_t *checksum);
 int mips32_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank);
+		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 #endif /* OPENOCD_TARGET_MIPS32_H */
diff --git a/src/target/target.c b/src/target/target.c
index e3c8168..9fc9d20 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -2178,7 +2178,8 @@ int target_checksum_memory(struct target *target, uint32_t address, uint32_t siz
 	return retval;
 }
 
-int target_blank_check_memory(struct target *target, uint32_t address, uint32_t size, uint32_t* blank)
+int target_blank_check_memory(struct target *target, uint32_t address, uint32_t size, uint32_t* blank,
+	uint8_t erased_value)
 {
 	int retval;
 	if (!target_was_examined(target)) {
@@ -2189,7 +2190,7 @@ int target_blank_check_memory(struct target *target, uint32_t address, uint32_t
 	if (target->type->blank_check_memory == 0)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
-	retval = target->type->blank_check_memory(target, address, size, blank);
+	retval = target->type->blank_check_memory(target, address, size, blank, erased_value);
 
 	return retval;
 }
diff --git a/src/target/target.h b/src/target/target.h
index a1a30ff..8f6a7d9 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -579,7 +579,7 @@ int target_read_buffer(struct target *target,
 int target_checksum_memory(struct target *target,
 		uint32_t address, uint32_t size, uint32_t *crc);
 int target_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t *blank);
+		uint32_t address, uint32_t size, uint32_t *blank, uint8_t erased_value);
 int target_wait_state(struct target *target, enum target_state state, int ms);
 
 /**
diff --git a/src/target/target_type.h b/src/target/target_type.h
index eaa8a71..2473c62 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -131,7 +131,7 @@ struct target_type {
 	int (*checksum_memory)(struct target *target, uint32_t address,
 			uint32_t count, uint32_t *checksum);
 	int (*blank_check_memory)(struct target *target, uint32_t address,
-			uint32_t count, uint32_t *blank);
+			uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 	/*
 	 * target break-/watchpoint control
-- 
cgit v1.1


From ca19c82d94ae8448520d40b1c4fc3abd44afb857 Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Fri, 23 Sep 2016 14:08:21 -0700
Subject: Make OpenOCD build using -Og.

With -Og gcc doesn't perform as many optimizations, and as a result
warns about some code that it wouldn't otherwise warn about.

These fixes all assign values to otherwise uninitialized variables.

Change-Id: I9a6ea5eadd73673891ecfec568b8b00d78b596a5
Signed-off-by: Tim Newsome <tim@sifive.com>
Reviewed-on: http://openocd.zylin.com/3779
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/xscale.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/xscale.c b/src/target/xscale.c
index 3d5abdf..140ea58 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -2667,7 +2667,7 @@ static int xscale_analyze_trace(struct target *target, struct command_context *c
 	struct xscale_common *xscale = target_to_xscale(target);
 	struct xscale_trace_data *trace_data = xscale->trace.data;
 	int i, retval;
-	uint32_t breakpoint_pc;
+	uint32_t breakpoint_pc = 0;
 	struct arm_instruction instruction;
 	uint32_t current_pc = 0;/* initialized when address determined */
 
-- 
cgit v1.1


From b10037a0b784cab94e21b37c976ad2b5174d52ae Mon Sep 17 00:00:00 2001
From: HarishKumar <harishpresent@gmail.com>
Date: Tue, 9 Dec 2014 19:38:08 +0530
Subject: Tcl commands: Fix improper return status in flash commands and
 load_image.

Nand write command :
nand_fileio_cleanup() always returns ERROR_OK. Due to this,
handle_nand_write_command() retuns ERROR_OK in the case
of nand failure. ERROR_FAIL should be returned.

Flash erase_sector command :
handle_flash_erase_command() always returns ERROR_OK even if
the erase functionality of actual driver implementation fails.
retval value should be returned.

Flash write_bank command :
handle_flash_write_bank_command() returns ERROR_OK even if
fileio_open() and fileio_read fails. ERROR_FAIL should be
returned.

Load_image command :
handle_load_image_command() retuns ERROR_OK even if image_open()
fails. ERROR_FAIL should be returned.
When the buffer is null, breaking the loop without setting
retval = ERROR_FAIL would cause load_image to return ERROR_OK.

Change-Id: Ice32f6036971ab5e8e4dd65edf54b394b001c80c
Signed-off-by: HarishKumar <harishpresent@gmail.com>
Reviewed-on: http://openocd.zylin.com/2431
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/target/target.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index 9fc9d20..c23590e 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -3166,7 +3166,7 @@ COMMAND_HANDLER(handle_load_image_command)
 	duration_start(&bench);
 
 	if (image_open(&image, CMD_ARGV[0], (CMD_ARGC >= 3) ? CMD_ARGV[2] : NULL) != ERROR_OK)
-		return ERROR_OK;
+		return ERROR_FAIL;
 
 	image_size = 0x0;
 	retval = ERROR_OK;
@@ -3176,6 +3176,7 @@ COMMAND_HANDLER(handle_load_image_command)
 			command_print(CMD_CTX,
 						  "error allocating buffer for section (%d bytes)",
 						  (int)(image.sections[i].size));
+			retval = ERROR_FAIL;
 			break;
 		}
 
-- 
cgit v1.1


From d1bdcdcc8d429a2ce06fb35f82a67b2487b72ec3 Mon Sep 17 00:00:00 2001
From: Andrey Smirnov <andrew.smirnov@gmail.com>
Date: Sat, 8 Aug 2015 15:18:16 -0700
Subject: semihosting armv7a: Add support for ARMv7-A

Add semihosting support for ARMv7-A based processors.

Tested with custom Vybrid VF610 based board
and Pandaboard ES (Rev. B1) board (Cortex-A9).

Change-Id: I6b896a61c1c6a1c5dcf89de834486f82dd6c80a2
Signed-off-by: Andrey Smirnov <andrew.smirnov@gmail.com>
Signed-off-by: Tsung-Han Lin <tsunghan.tw@gmail.com>
Reviewed-on: http://openocd.zylin.com/2908
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/target/arm_semihosting.c | 31 ++++++++++++++++++++++++++++---
 src/target/armv7a.c          | 29 +++++++++++++++++++++++++++++
 src/target/armv7a.h          | 13 +++++++++++++
 src/target/cortex_a.c        | 20 +++++++++++++++++++-
 4 files changed, 89 insertions(+), 4 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 2fd6580..63335ff 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -39,8 +39,10 @@
 #include "armv4_5.h"
 #include "arm7_9_common.h"
 #include "armv7m.h"
+#include "armv7a.h"
 #include "cortex_m.h"
 #include "register.h"
+#include "arm_opcodes.h"
 #include "arm_semihosting.h"
 #include <helper/binarybuffer.h>
 #include <helper/log.h>
@@ -415,7 +417,8 @@ static int do_semihosting(struct target *target)
 	/* REVISIT this looks wrong ... ARM11 and Cortex-A8
 	 * should work this way at least sometimes.
 	 */
-	if (is_arm7_9(target_to_arm7_9(target))) {
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(target_to_armv7a(target))) {
 		uint32_t spsr;
 
 		/* return value in R0 */
@@ -468,20 +471,42 @@ static int do_semihosting(struct target *target)
 int arm_semihosting(struct target *target, int *retval)
 {
 	struct arm *arm = target_to_arm(target);
+	struct armv7a_common *armv7a = target_to_armv7a(target);
 	uint32_t pc, lr, spsr;
 	struct reg *r;
 
 	if (!arm->is_semihosting)
 		return 0;
 
-	if (is_arm7_9(target_to_arm7_9(target))) {
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(armv7a)) {
+		uint32_t vbar = 0x00000000;
+
 		if (arm->core_mode != ARM_MODE_SVC)
 			return 0;
 
+		if (is_armv7a(armv7a)) {
+			struct arm_dpm *dpm = armv7a->arm.dpm;
+
+			*retval = dpm->prepare(dpm);
+			if (*retval == ERROR_OK) {
+				*retval = dpm->instr_read_data_r0(dpm,
+								 ARMV4_5_MRC(15, 0, 0, 12, 0, 0),
+								 &vbar);
+
+				dpm->finish(dpm);
+
+				if (*retval != ERROR_OK)
+					return 1;
+			} else {
+				return 1;
+			}
+		}
+
 		/* Check for PC == 0x00000008 or 0xffff0008: Supervisor Call vector. */
 		r = arm->pc;
 		pc = buf_get_u32(r->value, 0, 32);
-		if (pc != 0x00000008 && pc != 0xffff0008)
+		if (pc != (vbar + 0x00000008) && pc != 0xffff0008)
 			return 0;
 
 		r = arm_reg_current(arm, 14);
diff --git a/src/target/armv7a.c b/src/target/armv7a.c
index 37eb1b5..6021def 100644
--- a/src/target/armv7a.c
+++ b/src/target/armv7a.c
@@ -679,11 +679,40 @@ done:
 
 }
 
+static int armv7a_setup_semihosting(struct target *target, int enable)
+{
+	struct armv7a_common *armv7a = target_to_armv7a(target);
+	uint32_t vcr;
+	int ret;
+
+	ret = mem_ap_read_atomic_u32(armv7a->debug_ap,
+					 armv7a->debug_base + CPUDBG_VCR,
+					 &vcr);
+	if (ret < 0) {
+		LOG_ERROR("Failed to read VCR register\n");
+		return ret;
+	}
+
+	if (enable)
+		vcr |= DBG_VCR_SVC_MASK;
+	else
+		vcr &= ~DBG_VCR_SVC_MASK;
+
+	ret = mem_ap_write_atomic_u32(armv7a->debug_ap,
+					  armv7a->debug_base + CPUDBG_VCR,
+					  vcr);
+	if (ret < 0)
+		LOG_ERROR("Failed to write VCR register\n");
+
+	return ret;
+}
+
 int armv7a_init_arch_info(struct target *target, struct armv7a_common *armv7a)
 {
 	struct arm *arm = &armv7a->arm;
 	arm->arch_info = armv7a;
 	target->arch_info = &armv7a->arm;
+	arm->setup_semihosting = armv7a_setup_semihosting;
 	/*  target is useful in all function arm v4 5 compatible */
 	armv7a->arm.target = target;
 	armv7a->arm.common_magic = ARM_COMMON_MAGIC;
diff --git a/src/target/armv7a.h b/src/target/armv7a.h
index 6461ba9..6a62f72 100644
--- a/src/target/armv7a.h
+++ b/src/target/armv7a.h
@@ -134,6 +134,12 @@ target_to_armv7a(struct target *target)
 	return container_of(target->arch_info, struct armv7a_common, arm);
 }
 
+static inline bool is_armv7a(struct armv7a_common *armv7a)
+{
+	return armv7a->common_magic == ARMV7_COMMON_MAGIC;
+}
+
+
 /* register offsets from armv7a.debug_base */
 
 /* See ARMv7a arch spec section C10.2 */
@@ -172,6 +178,13 @@ target_to_armv7a(struct target *target)
 /* See ARMv7a arch spec section C10.8 */
 #define CPUDBG_AUTHSTATUS	0xFB8
 
+/* Masks for Vector Catch register */
+#define DBG_VCR_FIQ_MASK	((1 << 31) | (1 << 7))
+#define DBG_VCR_IRQ_MASK	((1 << 30) | (1 << 6))
+#define DBG_VCR_DATA_ABORT_MASK	((1 << 28) | (1 << 4))
+#define DBG_VCR_PREF_ABORT_MASK	((1 << 27) | (1 << 3))
+#define DBG_VCR_SVC_MASK	((1 << 26) | (1 << 2))
+
 int armv7a_arch_state(struct target *target);
 int armv7a_identify_cache(struct target *target);
 int armv7a_init_arch_info(struct target *target, struct armv7a_common *armv7a);
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index d1590f6..0a689ea 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -53,6 +53,7 @@
 #include "target_request.h"
 #include "target_type.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
 #include <helper/time_support.h>
 
 static int cortex_a_poll(struct target *target);
@@ -915,6 +916,10 @@ static int cortex_a_poll(struct target *target)
 					if (retval != ERROR_OK)
 						return retval;
 				}
+
+				if (arm_semihosting(target, &retval) != 0)
+					return retval;
+
 				target_call_event_callbacks(target,
 					TARGET_EVENT_HALTED);
 			}
@@ -1201,7 +1206,7 @@ static int cortex_a_resume(struct target *target, int current,
 static int cortex_a_debug_entry(struct target *target)
 {
 	int i;
-	uint32_t regfile[16], cpsr, dscr;
+	uint32_t regfile[16], cpsr, spsr, dscr;
 	int retval = ERROR_OK;
 	struct working_area *regfile_working_area = NULL;
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
@@ -1250,6 +1255,7 @@ static int cortex_a_debug_entry(struct target *target)
 	if (cortex_a->fast_reg_read)
 		target_alloc_working_area(target, 64, &regfile_working_area);
 
+
 	/* First load register acessible through core debug port*/
 	if (!regfile_working_area)
 		retval = arm_dpm_read_current_registers(&armv7a->dpm);
@@ -1294,6 +1300,17 @@ static int cortex_a_debug_entry(struct target *target)
 		reg->dirty = reg->valid;
 	}
 
+	/* read Saved PSR */
+	retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
+	/*  store current spsr */
+	if (retval != ERROR_OK)
+		return retval;
+
+	reg = arm->spsr;
+	buf_set_u32(reg->value, 0, 32, spsr);
+	reg->valid = 1;
+	reg->dirty = 0;
+
 #if 0
 /* TODO, Move this */
 	uint32_t cp15_control_register, cp15_cacr, cp15_nacr;
@@ -2953,6 +2970,7 @@ static int cortex_a_examine_first(struct target *target)
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
 	struct armv7a_common *armv7a = &cortex_a->armv7a_common;
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
+
 	int i;
 	int retval = ERROR_OK;
 	uint32_t didr, ctypr, ttypr, cpuid, dbg_osreg;
-- 
cgit v1.1


From 009943010420e49c2632769e9e21502b8166ba41 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 5 Aug 2016 21:50:33 +0200
Subject: cortex_a/r/m: fix handling of un-examined cores

On multi-core systems, with some cores in power-down state, examination
will fail for these cores. Make sure assert- and deassert_reset functions
don't crash due to uninitialized variables.

Change-Id: I472f8d19af2cd3c770c05f3e57a31b35a863b687
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3552
Tested-by: jenkins
Reviewed-by: Jiri Kastner <cz172638@gmail.com>
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/cortex_a.c | 26 +++++++++++++++++---------
 src/target/cortex_m.c | 15 ++++++++++++++-
 2 files changed, 31 insertions(+), 10 deletions(-)

(limited to 'src/target')

diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 0a689ea..256edbc 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -853,7 +853,8 @@ static int cortex_a_halt_smp(struct target *target)
 	head = target->head;
 	while (head != (struct target_list *)NULL) {
 		curr = head->target;
-		if ((curr != target) && (curr->state != TARGET_HALTED))
+		if ((curr != target) && (curr->state != TARGET_HALTED)
+			&& target_was_examined(curr))
 			retval += cortex_a_halt(curr);
 		head = head->next;
 	}
@@ -1156,7 +1157,8 @@ static int cortex_a_restore_smp(struct target *target, int handle_breakpoints)
 	head = target->head;
 	while (head != (struct target_list *)NULL) {
 		curr = head->target;
-		if ((curr != target) && (curr->state != TARGET_RUNNING)) {
+		if ((curr != target) && (curr->state != TARGET_RUNNING)
+			&& target_was_examined(curr)) {
 			/*  resume current address , not in step mode */
 			retval += cortex_a_internal_restore(curr, 1, &address,
 					handle_breakpoints, 0);
@@ -1936,7 +1938,8 @@ static int cortex_a_assert_reset(struct target *target)
 	}
 
 	/* registers are now invalid */
-	register_cache_invalidate(armv7a->arm.core_cache);
+	if (target_was_examined(target))
+		register_cache_invalidate(armv7a->arm.core_cache);
 
 	target->state = TARGET_RESET;
 
@@ -1952,17 +1955,22 @@ static int cortex_a_deassert_reset(struct target *target)
 	/* be certain SRST is off */
 	jtag_add_reset(0, 0);
 
-	retval = cortex_a_poll(target);
-	if (retval != ERROR_OK)
-		return retval;
+	if (target_was_examined(target)) {
+		retval = cortex_a_poll(target);
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
 	if (target->reset_halt) {
 		if (target->state != TARGET_HALTED) {
 			LOG_WARNING("%s: ran after reset and before halt ...",
 				target_name(target));
-			retval = target_halt(target);
-			if (retval != ERROR_OK)
-				return retval;
+			if (target_was_examined(target)) {
+				retval = target_halt(target);
+				if (retval != ERROR_OK)
+					return retval;
+			} else
+				target->state = TARGET_UNKNOWN;
 		}
 	}
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 4566e2e..4270f8f 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -981,6 +981,18 @@ static int cortex_m_assert_reset(struct target *target)
 
 	bool srst_asserted = false;
 
+	if (!target_was_examined(target)) {
+		if (jtag_reset_config & RESET_HAS_SRST) {
+			adapter_assert_reset();
+			if (target->reset_halt)
+				LOG_ERROR("Target not examined, will not halt after reset!");
+			return ERROR_OK;
+		} else {
+			LOG_ERROR("Target not examined, reset NOT asserted!");
+			return ERROR_FAIL;
+		}
+	}
+
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
 	    (jtag_reset_config & RESET_SRST_NO_GATING)) {
 		adapter_assert_reset();
@@ -1101,7 +1113,8 @@ static int cortex_m_deassert_reset(struct target *target)
 	enum reset_types jtag_reset_config = jtag_get_reset_config();
 
 	if ((jtag_reset_config & RESET_HAS_SRST) &&
-	    !(jtag_reset_config & RESET_SRST_NO_GATING)) {
+	    !(jtag_reset_config & RESET_SRST_NO_GATING) &&
+		target_was_examined(target)) {
 		int retval = dap_dp_init(armv7m->debug_ap->dap);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("DP initialisation failed");
-- 
cgit v1.1


From 21832327eed48be9d44607890d11d31b88d1bd30 Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Wed, 10 Aug 2016 09:40:34 +0300
Subject: Replace "daemon" with "server" in user-visible strings

Since OpenOCD doesn't fit most common definitions of the word "daemon",
using it in the documentation is confusing.

Reported by IRC user ohsix.

Change-Id: I688d722771b084b17c2a7af8e83fd64bab6141b8
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3634
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/target/image.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/image.c b/src/target/image.c
index 0612ea7..f97d904 100644
--- a/src/target/image.c
+++ b/src/target/image.c
@@ -729,7 +729,7 @@ int image_open(struct image *image, const char *url, const char *type_string)
 		retval = image_ihex_buffer_complete(image);
 		if (retval != ERROR_OK) {
 			LOG_ERROR(
-				"failed buffering IHEX image, check daemon output for additional information");
+				"failed buffering IHEX image, check server output for additional information");
 			fileio_close(image_ihex->fileio);
 			return retval;
 		}
@@ -780,7 +780,7 @@ int image_open(struct image *image, const char *url, const char *type_string)
 		retval = image_mot_buffer_complete(image);
 		if (retval != ERROR_OK) {
 			LOG_ERROR(
-				"failed buffering S19 image, check daemon output for additional information");
+				"failed buffering S19 image, check server output for additional information");
 			fileio_close(image_mot->fileio);
 			return retval;
 		}
-- 
cgit v1.1


From 53a936afc0092f4a65975d35bab0e10944fad3db Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 11 Nov 2016 14:39:09 +0100
Subject: Add -defer-examine option to target create command

The '-defer-examine' option to target create allows declaring targets
that are present on the chain, but not fully functional.  They will
be skipped by the initial arp_examine as well as arp_examine after
reset.

Manual examine using 'arp_examine' is needed to examine them, with the
idea that some kind of actions is neeed to bring them to a state where
examine will succeed (if at all possible).

In order to allow value less options to target command, I had to relax
the goi.argc check in jim_target_configure().

Change-Id: I9bf4e8d27eb6476dd9353d15f48965a8cfd5c122
Signed-off-by: Esben Haabendal <esben@haabendal.dk>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3076
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/startup.tcl | 14 ++++++++-
 src/target/target.c    | 84 +++++++++++++++++++++++++++++++++++++++++++++-----
 src/target/target.h    |  3 ++
 3 files changed, 93 insertions(+), 8 deletions(-)

(limited to 'src/target')

diff --git a/src/target/startup.tcl b/src/target/startup.tcl
index cf2813b..9bbc6e3 100644
--- a/src/target/startup.tcl
+++ b/src/target/startup.tcl
@@ -65,7 +65,7 @@ proc ocd_process_reset_inner { MODE } {
 	foreach t $targets {
 		if {![using_jtag] || [jtag tapisenabled [$t cget -chain-position]]} {
 			$t invoke-event examine-start
-			set err [catch "$t arp_examine"]
+			set err [catch "$t arp_examine allow-defer"]
 			if { $err == 0 } {
 				$t invoke-event examine-end
 			}
@@ -111,6 +111,12 @@ proc ocd_process_reset_inner { MODE } {
 				continue
 			}
 
+			# don't wait for targets where examination is deferred
+			# they can not be halted anyway at this point
+			if { ![$t was_examined] && [$t examine_deferred] } {
+				continue
+			}
+
 			# Wait upto 1 second for target to halt.  Why 1sec? Cause
 			# the JTAG tap reset signal might be hooked to a slow
 			# resistor/capacitor circuit - and it might take a while
@@ -135,6 +141,12 @@ proc ocd_process_reset_inner { MODE } {
 				continue
 			}
 
+			# don't wait for targets where examination is deferred
+			# they can not be halted anyway at this point
+			if { ![$t was_examined] && [$t examine_deferred] } {
+				continue
+			}
+
 			set err [catch "$t arp_waitstate halted 5000"]
 			# Did it halt?
 			if { $err == 0 } {
diff --git a/src/target/target.c b/src/target/target.c
index c23590e..4c5ddd3 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -283,6 +283,10 @@ const char *target_state_name(struct target *t)
 		LOG_ERROR("Invalid target state: %d", (int)(t->state));
 		cp = "(*BUG*unknown*BUG*)";
 	}
+
+	if (!target_was_examined(t) && t->defer_examine)
+		cp = "examine deferred";
+
 	return cp;
 }
 
@@ -731,6 +735,9 @@ int target_examine(void)
 			continue;
 		}
 
+		if (target->defer_examine)
+			continue;
+
 		retval = target_examine_one(target);
 		if (retval != ERROR_OK)
 			return retval;
@@ -4325,6 +4332,7 @@ enum target_cfg_param {
 	TCFG_CHAIN_POSITION,
 	TCFG_DBGBASE,
 	TCFG_RTOS,
+	TCFG_DEFER_EXAMINE,
 };
 
 static Jim_Nvp nvp_config_opts[] = {
@@ -4339,6 +4347,7 @@ static Jim_Nvp nvp_config_opts[] = {
 	{ .name = "-chain-position",   .value = TCFG_CHAIN_POSITION },
 	{ .name = "-dbgbase",          .value = TCFG_DBGBASE },
 	{ .name = "-rtos",             .value = TCFG_RTOS },
+	{ .name = "-defer-examine",    .value = TCFG_DEFER_EXAMINE },
 	{ .name = NULL, .value = -1 }
 };
 
@@ -4613,6 +4622,13 @@ no_params:
 			}
 			/* loop for more */
 			break;
+
+		case TCFG_DEFER_EXAMINE:
+			/* DEFER_EXAMINE */
+			target->defer_examine = true;
+			/* loop for more */
+			break;
+
 		}
 	} /* while (goi->argc) */
 
@@ -4627,12 +4643,9 @@ static int jim_target_configure(Jim_Interp *interp, int argc, Jim_Obj * const *a
 
 	Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1);
 	goi.isconfigure = !strcmp(Jim_GetString(argv[0], NULL), "configure");
-	int need_args = 1 + goi.isconfigure;
-	if (goi.argc < need_args) {
+	if (goi.argc < 1) {
 		Jim_WrongNumArgs(goi.interp, goi.argc, goi.argv,
-			goi.isconfigure
-				? "missing: -option VALUE ..."
-				: "missing: -option ...");
+				 "missing: -option ...");
 		return JIM_ERR;
 	}
 	struct target *target = Jim_CmdPrivData(goi.interp);
@@ -4883,20 +4896,58 @@ static int jim_target_tap_disabled(Jim_Interp *interp)
 
 static int jim_target_examine(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
-	if (argc != 1) {
-		Jim_WrongNumArgs(interp, 1, argv, "[no parameters]");
+	bool allow_defer = false;
+
+	Jim_GetOptInfo goi;
+	Jim_GetOpt_Setup(&goi, interp, argc - 1, argv + 1);
+	if (goi.argc > 1) {
+		const char *cmd_name = Jim_GetString(argv[0], NULL);
+		Jim_SetResultFormatted(goi.interp,
+				"usage: %s ['allow-defer']", cmd_name);
 		return JIM_ERR;
 	}
+	if (goi.argc > 0 &&
+	    strcmp(Jim_GetString(argv[1], NULL), "allow-defer") == 0) {
+		/* consume it */
+		struct Jim_Obj *obj;
+		int e = Jim_GetOpt_Obj(&goi, &obj);
+		if (e != JIM_OK)
+			return e;
+		allow_defer = true;
+	}
+
 	struct target *target = Jim_CmdPrivData(interp);
 	if (!target->tap->enabled)
 		return jim_target_tap_disabled(interp);
 
+	if (allow_defer && target->defer_examine) {
+		LOG_INFO("Deferring arp_examine of %s", target_name(target));
+		LOG_INFO("Use arp_examine command to examine it manually!");
+		return JIM_OK;
+	}
+
 	int e = target->type->examine(target);
 	if (e != ERROR_OK)
 		return JIM_ERR;
 	return JIM_OK;
 }
 
+static int jim_target_was_examined(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+{
+	struct target *target = Jim_CmdPrivData(interp);
+
+	Jim_SetResultBool(interp, target_was_examined(target));
+	return JIM_OK;
+}
+
+static int jim_target_examine_deferred(Jim_Interp *interp, int argc, Jim_Obj * const *argv)
+{
+	struct target *target = Jim_CmdPrivData(interp);
+
+	Jim_SetResultBool(interp, target->defer_examine);
+	return JIM_OK;
+}
+
 static int jim_target_halt_gdb(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 {
 	if (argc != 1) {
@@ -4964,6 +5015,10 @@ static int jim_target_reset(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 				target_name(target));
 		return JIM_ERR;
 	}
+
+	if (target->defer_examine)
+		target_reset_examined(target);
+
 	/* determine if we should halt or not. */
 	target->reset_halt = !!a;
 	/* When this happens - all workareas are invalid. */
@@ -5174,6 +5229,21 @@ static const struct command_registration target_instance_command_handlers[] = {
 		.mode = COMMAND_EXEC,
 		.jim_handler = jim_target_examine,
 		.help = "used internally for reset processing",
+		.usage = "arp_examine ['allow-defer']",
+	},
+	{
+		.name = "was_examined",
+		.mode = COMMAND_EXEC,
+		.jim_handler = jim_target_was_examined,
+		.help = "used internally for reset processing",
+		.usage = "was_examined",
+	},
+	{
+		.name = "examine_deferred",
+		.mode = COMMAND_EXEC,
+		.jim_handler = jim_target_examine_deferred,
+		.help = "used internally for reset processing",
+		.usage = "examine_deferred",
 	},
 	{
 		.name = "arp_halt_gdb",
diff --git a/src/target/target.h b/src/target/target.h
index 8f6a7d9..7f6ac14 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -130,6 +130,9 @@ struct target {
 	struct jtag_tap *tap;				/* where on the jtag chain is this */
 	int32_t coreid;						/* which device on the TAP? */
 
+	/** Should we defer examine to later */
+	bool defer_examine;
+
 	/**
 	 * Indicates whether this target has been examined.
 	 *
-- 
cgit v1.1


From 0a2f1b29e678c97c7981090f769e841cbd583442 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Wed, 23 Nov 2016 10:15:50 +0100
Subject: cortex_a: remove partnum magic from arp_examine and dbginit

Depending on the Debug implementation the "OS Lock" feature might be
implemented or not. It is not actually depending on the part number of the
implemented ARM core but on the DBGOSLSR.OSLM bits. This patch removes
querying the part number and implements proper parsing of OSLM. Result is
a more generic approach that will work out-of-box on more devices.

Change-Id: I79e052869c2f9af1d7fdedef42faddb7292e7332
Signed-off-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-on: http://openocd.zylin.com/3213
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/arm_dpm.h  |  21 +++++++
 src/target/cortex_a.c | 166 ++++++++++++++------------------------------------
 src/target/cortex_a.h |   2 -
 3 files changed, 68 insertions(+), 121 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index fa87baf..bbcae78 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -198,4 +198,25 @@ void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);
 
 void arm_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
 
+/* PRCR (Device Power-down and Reset Control Register) bits */
+#define PRCR_DEBUG_NO_POWER_DOWN         (1 << 0)
+#define PRCR_WARM_RESET                  (1 << 1)
+#define PRCR_HOLD_NON_DEBUG_RESET        (1 << 2)
+
+/* PRSR (Device Power-down and Reset Status Register) bits */
+#define PRSR_POWERUP_STATUS              (1 << 0)
+#define PRSR_STICKY_POWERDOWN_STATUS     (1 << 1)
+#define PRSR_RESET_STATUS                (1 << 2)
+#define PRSR_STICKY_RESET_STATUS         (1 << 3)
+#define PRSR_HALTED                      (1 << 4)  /* v7.1 Debug only */
+#define PRSR_OSLK                        (1 << 5)  /* v7.1 Debug only */
+#define PRSR_DLK                         (1 << 6)  /* v7.1 Debug only */
+
+/* OSLSR (OS Lock Status Register) bits */
+#define OSLSR_OSLM0                      (1 << 0)
+#define OSLSR_OSLK                       (1 << 1)
+#define OSLSR_nTT                        (1 << 2)
+#define OSLSR_OSLM1                      (1 << 3)
+#define OSLSR_OSLM                       (OSLSR_OSLM0|OSLSR_OSLM1)
+
 #endif /* OPENOCD_TARGET_ARM_DPM_H */
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 256edbc..1f51c4e 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -198,77 +198,15 @@ static int cortex_a_mmu_modify(struct target *target, int enable)
 /*
  * Cortex-A Basic debug access, very low level assumes state is saved
  */
-static int cortex_a8_init_debug_access(struct target *target)
-{
-	struct armv7a_common *armv7a = target_to_armv7a(target);
-	int retval;
-
-	LOG_DEBUG(" ");
-
-	/* Unlocking the debug registers for modification
-	 * The debugport might be uninitialised so try twice */
-	retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
-	if (retval != ERROR_OK) {
-		/* try again */
-		retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-				armv7a->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
-		if (retval == ERROR_OK)
-			LOG_USER(
-				"Locking debug access failed on first, but succeeded on second try.");
-	}
-
-	return retval;
-}
-
-/*
- * Cortex-A Basic debug access, very low level assumes state is saved
- */
 static int cortex_a_init_debug_access(struct target *target)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	int retval;
-	uint32_t dbg_osreg;
-	uint32_t cortex_part_num;
-	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
-
-	LOG_DEBUG(" ");
-	cortex_part_num = (cortex_a->cpuid & CORTEX_A_MIDR_PARTNUM_MASK) >>
-		CORTEX_A_MIDR_PARTNUM_SHIFT;
-
-	switch (cortex_part_num) {
-	case CORTEX_A7_PARTNUM:
-	case CORTEX_A15_PARTNUM:
-		retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-						    armv7a->debug_base + CPUDBG_OSLSR,
-						    &dbg_osreg);
-		if (retval != ERROR_OK)
-			return retval;
-
-		LOG_DEBUG("DBGOSLSR  0x%" PRIx32, dbg_osreg);
-
-		if (dbg_osreg & CPUDBG_OSLAR_LK_MASK)
-			/* Unlocking the DEBUG OS registers for modification */
-			retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-							     armv7a->debug_base + CPUDBG_OSLAR,
-							     0);
-		break;
-
-	case CORTEX_A5_PARTNUM:
-	case CORTEX_A8_PARTNUM:
-	case CORTEX_A9_PARTNUM:
-	default:
-		retval = cortex_a8_init_debug_access(target);
-	}
-
-	if (retval != ERROR_OK)
-		return retval;
-	/* Clear Sticky Power Down status Bit in PRSR to enable access to
-	   the registers in the Core Power Domain */
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
-	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
 
+	/* lock memory-mapped access to debug registers to prevent
+	 * software interference */
+	retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+			armv7a->debug_base + CPUDBG_LOCKACCESS, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -2981,7 +2919,7 @@ static int cortex_a_examine_first(struct target *target)
 
 	int i;
 	int retval = ERROR_OK;
-	uint32_t didr, ctypr, ttypr, cpuid, dbg_osreg;
+	uint32_t didr, cpuid, dbg_osreg;
 
 	retval = dap_dp_init(swjdp);
 	if (retval != ERROR_OK) {
@@ -3043,79 +2981,69 @@ static int cortex_a_examine_first(struct target *target)
 		armv7a->debug_base = target->dbgbase;
 
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_CPUID, &cpuid);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_CPUID, &cpuid);
-	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "CPUID");
-		return retval;
-	}
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_CTYPR, &ctypr);
-	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "CTYPR");
-		return retval;
-	}
-
-	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_TTYPR, &ttypr);
+			armv7a->debug_base + CPUDBG_DIDR, &didr);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "TTYPR");
+		LOG_DEBUG("Examine %s failed", "DIDR");
 		return retval;
 	}
 
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-			armv7a->debug_base + CPUDBG_DIDR, &didr);
+			armv7a->debug_base + CPUDBG_CPUID, &cpuid);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "DIDR");
+		LOG_DEBUG("Examine %s failed", "CPUID");
 		return retval;
 	}
 
-	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
-	LOG_DEBUG("ctypr = 0x%08" PRIx32, ctypr);
-	LOG_DEBUG("ttypr = 0x%08" PRIx32, ttypr);
 	LOG_DEBUG("didr = 0x%08" PRIx32, didr);
+	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
 
-	cortex_a->cpuid = cpuid;
-	cortex_a->ctypr = ctypr;
-	cortex_a->ttypr = ttypr;
 	cortex_a->didr = didr;
+	cortex_a->cpuid = cpuid;
 
-	/* Unlocking the debug registers */
-	if ((cpuid & CORTEX_A_MIDR_PARTNUM_MASK) >> CORTEX_A_MIDR_PARTNUM_SHIFT ==
-		CORTEX_A15_PARTNUM) {
-
-		retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-						     armv7a->debug_base + CPUDBG_OSLAR,
-						     0);
-
-		if (retval != ERROR_OK)
-			return retval;
+	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+				    armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
 
+	if ((dbg_osreg & PRSR_POWERUP_STATUS) == 0) {
+		LOG_ERROR("target->coreid %" PRId32 " powered down!", target->coreid);
+		target->state = TARGET_UNKNOWN; /* TARGET_NO_POWER? */
+		return ERROR_TARGET_INIT_FAILED;
 	}
-	/* Unlocking the debug registers */
-	if ((cpuid & CORTEX_A_MIDR_PARTNUM_MASK) >> CORTEX_A_MIDR_PARTNUM_SHIFT ==
-		CORTEX_A7_PARTNUM) {
 
-		retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
-						     armv7a->debug_base + CPUDBG_OSLAR,
-						     0);
+	if (dbg_osreg & PRSR_STICKY_RESET_STATUS)
+		LOG_DEBUG("target->coreid %" PRId32 " was reset!", target->coreid);
 
-		if (retval != ERROR_OK)
-			return retval;
-
-	}
+	/* Read DBGOSLSR and check if OSLK is implemented */
 	retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
-					    armv7a->debug_base + CPUDBG_PRSR, &dbg_osreg);
-
+				armv7a->debug_base + CPUDBG_OSLSR, &dbg_osreg);
 	if (retval != ERROR_OK)
 		return retval;
+	LOG_DEBUG("target->coreid %" PRId32 " DBGOSLSR 0x%" PRIx32, target->coreid, dbg_osreg);
 
-	LOG_DEBUG("target->coreid %" PRId32 " DBGPRSR  0x%" PRIx32, target->coreid, dbg_osreg);
+	/* check if OS Lock is implemented */
+	if ((dbg_osreg & OSLSR_OSLM) == OSLSR_OSLM0 || (dbg_osreg & OSLSR_OSLM) == OSLSR_OSLM1) {
+		/* check if OS Lock is set */
+		if (dbg_osreg & OSLSR_OSLK) {
+			LOG_DEBUG("target->coreid %" PRId32 " OSLock set! Trying to unlock", target->coreid);
+
+			retval = mem_ap_write_atomic_u32(armv7a->debug_ap,
+							armv7a->debug_base + CPUDBG_OSLAR,
+							0);
+			if (retval == ERROR_OK)
+				retval = mem_ap_read_atomic_u32(armv7a->debug_ap,
+							armv7a->debug_base + CPUDBG_OSLSR, &dbg_osreg);
+
+			/* if we fail to access the register or cannot reset the OSLK bit, bail out */
+			if (retval != ERROR_OK || (dbg_osreg & OSLSR_OSLK) != 0) {
+				LOG_ERROR("target->coreid %" PRId32 " OSLock sticky, core not powered?",
+						target->coreid);
+				target->state = TARGET_UNKNOWN; /* TARGET_NO_POWER? */
+				return ERROR_TARGET_INIT_FAILED;
+			}
+		}
+	}
 
 	armv7a->arm.core_type = ARM_MODE_MON;
 
diff --git a/src/target/cortex_a.h b/src/target/cortex_a.h
index ea08c67..ff03432 100644
--- a/src/target/cortex_a.h
+++ b/src/target/cortex_a.h
@@ -97,8 +97,6 @@ struct cortex_a_common {
 	int fast_reg_read;
 
 	uint32_t cpuid;
-	uint32_t ctypr;
-	uint32_t ttypr;
 	uint32_t didr;
 
 	enum cortex_a_isrmasking_mode isrmasking_mode;
-- 
cgit v1.1


From cac446ca6bb9566f06f705beb168eb5c49bee27e Mon Sep 17 00:00:00 2001
From: Jiri Kastner <cz172638@gmail.com>
Date: Fri, 11 Nov 2016 10:50:13 +0100
Subject: dap_dp_init: remove loop

current loop sounds to me like 'we don't know what we do, let's do it ten times, maybe we will have luck'.
should be enough to 'ping' debug port using reading CRTL_STAT.

tested on cortex-a8, snapdragon, jetson k1, cortex-r5, cortex-r4

Change-Id: Ibc62ac1eca06c141f4fccd5de7b11350ca1f35fd
Signed-off-by: Jiri Kastner <cz172638@gmail.com>
Tested-by: Jiri Kastner <cz172638@gmail.com>
Reviewed-on: http://openocd.zylin.com/3193
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-by: Liviu Dudau <liviu@dudau.co.uk>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/arm_adi_v5.c | 86 ++++++++++++++++++++++++-------------------------
 1 file changed, 42 insertions(+), 44 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index f58afdc..39f2b39 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -612,60 +612,58 @@ int dap_dp_init(struct adiv5_dap *dap)
 	dap->select = DP_SELECT_INVALID;
 	dap->last_read = NULL;
 
-	for (size_t i = 0; i < 10; i++) {
+	for (size_t i = 0; i < 30; i++) {
 		/* DP initialization */
 
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
-
-		retval = dap_queue_dp_write(dap, DP_CTRL_STAT, SSTICKYERR);
-		if (retval != ERROR_OK)
-			continue;
+		retval = dap_dp_read_atomic(dap, DP_CTRL_STAT, NULL);
+		if (retval == ERROR_OK)
+			break;
+	}
 
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, SSTICKYERR);
+	if (retval != ERROR_OK)
+		return retval;
 
-		dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
-		retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
-		if (retval != ERROR_OK)
-			continue;
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
+	if (retval != ERROR_OK)
+		return retval;
 
-		/* Check that we have debug power domains activated */
-		LOG_DEBUG("DAP: wait CDBGPWRUPACK");
-		retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
-					      CDBGPWRUPACK, CDBGPWRUPACK,
-					      DAP_POWER_DOMAIN_TIMEOUT);
-		if (retval != ERROR_OK)
-			continue;
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ;
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
+	if (retval != ERROR_OK)
+		return retval;
 
-		LOG_DEBUG("DAP: wait CSYSPWRUPACK");
-		retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
-					      CSYSPWRUPACK, CSYSPWRUPACK,
-					      DAP_POWER_DOMAIN_TIMEOUT);
-		if (retval != ERROR_OK)
-			continue;
+	/* Check that we have debug power domains activated */
+	LOG_DEBUG("DAP: wait CDBGPWRUPACK");
+	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+				      CDBGPWRUPACK, CDBGPWRUPACK,
+				      DAP_POWER_DOMAIN_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
 
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
+	LOG_DEBUG("DAP: wait CSYSPWRUPACK");
+	retval = dap_dp_poll_register(dap, DP_CTRL_STAT,
+				      CSYSPWRUPACK, CSYSPWRUPACK,
+				      DAP_POWER_DOMAIN_TIMEOUT);
+	if (retval != ERROR_OK)
+		return retval;
 
-		/* With debug power on we can activate OVERRUN checking */
-		dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
-		retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
-		if (retval != ERROR_OK)
-			continue;
-		retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
-		if (retval != ERROR_OK)
-			continue;
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
+	if (retval != ERROR_OK)
+		return retval;
 
-		retval = dap_run(dap);
-		if (retval != ERROR_OK)
-			continue;
+	/* With debug power on we can activate OVERRUN checking */
+	dap->dp_ctrl_stat = CDBGPWRUPREQ | CSYSPWRUPREQ | CORUNDETECT;
+	retval = dap_queue_dp_write(dap, DP_CTRL_STAT, dap->dp_ctrl_stat);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = dap_queue_dp_read(dap, DP_CTRL_STAT, NULL);
+	if (retval != ERROR_OK)
+		return retval;
 
-		break;
-	}
+	retval = dap_run(dap);
+	if (retval != ERROR_OK)
+		return retval;
 
 	return retval;
 }
-- 
cgit v1.1


From 29964c79846ea419e113dc90c0d74f1a7e16e70a Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 10 Nov 2016 20:53:26 +0100
Subject: target: Add verify_image_checksum command

This avoids the secondary binary search if the checksum is different

Change-Id: I986ba7687cea76f30e37a6bca58aabde18198263
Signed-off-by: Evan Hunter <ehunter@broadcom.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/2869
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/target.c | 32 +++++++++++++++++++++++++++-----
 1 file changed, 27 insertions(+), 5 deletions(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index 4c5ddd3..26bae4b 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -3299,7 +3299,13 @@ COMMAND_HANDLER(handle_dump_image_command)
 	return retval;
 }
 
-static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
+enum verify_mode {
+	IMAGE_TEST = 0,
+	IMAGE_VERIFY = 1,
+	IMAGE_CHECKSUM_ONLY = 2
+};
+
+static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode verify)
 {
 	uint8_t *buffer;
 	size_t buf_cnt;
@@ -3357,7 +3363,7 @@ static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
 			break;
 		}
 
-		if (verify) {
+		if (verify >= IMAGE_VERIFY) {
 			/* calculate checksum of image */
 			retval = image_calculate_checksum(buffer, buf_cnt, &checksum);
 			if (retval != ERROR_OK) {
@@ -3370,7 +3376,12 @@ static COMMAND_HELPER(handle_verify_image_command_internal, int verify)
 				free(buffer);
 				break;
 			}
-
+			if ((checksum != mem_checksum) && (verify == IMAGE_CHECKSUM_ONLY)) {
+				LOG_ERROR("checksum mismatch");
+				free(buffer);
+				retval = ERROR_FAIL;
+				goto done;
+			}
 			if (checksum != mem_checksum) {
 				/* failed crc checksum, fall back to a binary compare */
 				uint8_t *data;
@@ -3435,14 +3446,19 @@ done:
 	return retval;
 }
 
+COMMAND_HANDLER(handle_verify_image_checksum_command)
+{
+	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, IMAGE_CHECKSUM_ONLY);
+}
+
 COMMAND_HANDLER(handle_verify_image_command)
 {
-	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, 1);
+	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, IMAGE_VERIFY);
 }
 
 COMMAND_HANDLER(handle_test_image_command)
 {
-	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, 0);
+	return CALL_COMMAND_HANDLER(handle_verify_image_command_internal, IMAGE_TEST);
 }
 
 static int handle_bp_command_list(struct command_context *cmd_ctx)
@@ -6219,6 +6235,12 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.usage = "filename address size",
 	},
 	{
+		.name = "verify_image_checksum",
+		.handler = handle_verify_image_checksum_command,
+		.mode = COMMAND_EXEC,
+		.usage = "filename [offset [type]]",
+	},
+	{
 		.name = "verify_image",
 		.handler = handle_verify_image_command,
 		.mode = COMMAND_EXEC,
-- 
cgit v1.1


From 3b021e013f2e120b8c8e3d141849934edf2bdceb Mon Sep 17 00:00:00 2001
From: Evan Hunter <ehunter@broadcom.com>
Date: Tue, 12 Jan 2016 18:32:18 +0000
Subject: Cortex-R: Force usage of physical memory read/write since there is no
 MMU

Change-Id: I69d6e6301ab66744258fe650a76c7241bebe00a8
Signed-off-by: Evan Hunter <ehunter@broadcom.com>
Reviewed-on: http://openocd.zylin.com/3203
Tested-by: jenkins
Reviewed-by: Girts Folkmanis <opensource@girts.me>
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-by: Jiri Kastner <cz172638@gmail.com>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/cortex_a.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 1f51c4e..91fa48e 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -3522,8 +3522,8 @@ struct target_type cortexr4_target = {
 	/* REVISIT allow exporting VFP3 registers ... */
 	.get_gdb_reg_list = arm_get_gdb_reg_list,
 
-	.read_memory = cortex_a_read_memory,
-	.write_memory = cortex_a_write_memory,
+	.read_memory = cortex_a_read_phys_memory,
+	.write_memory = cortex_a_write_phys_memory,
 
 	.checksum_memory = arm_checksum_memory,
 	.blank_check_memory = arm_blank_check_memory,
-- 
cgit v1.1


From c0e7ccbd87cf491b065bf18bbdb6a73b741c2698 Mon Sep 17 00:00:00 2001
From: Steven Stallion <stallion@squareup.com>
Date: Fri, 22 Jul 2016 15:43:11 -0500
Subject: semihosting: support fileio operation

This patch adds support for bridging semihosting to GDB's File-I/O
remote protocol extension. For the most part operations match up 1:1,
however some require a working area to complete successfully, namely
operations that devolve to read, stat, and gettimeofday.

A new command was added to enable support for fileio named `arm
semihosting_fileio`, which ensures that the default behavior remains
intact for those that prefer it.

Finally, redundant logging was removed from the target_arch_state
function; this permits ARM targets to quiesce log output when polling
for a fileio reply. This prevents filling the logs with halt/resume
messages when using semihosting fileio.

Change-Id: Ifbb864fc2373336a501cc0332675b887b552e1ee
Signed-off-by: Steven Stallion <stallion@squareup.com>
Reviewed-on: http://openocd.zylin.com/3566
Tested-by: jenkins
Reviewed-by: Steven Stallion <sstallion@gmail.com>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/arm.h             |  12 +
 src/target/arm7tdmi.c        |   3 +-
 src/target/arm9tdmi.c        |   2 +
 src/target/arm_semihosting.c | 512 ++++++++++++++++++++++++++++++-------------
 src/target/arm_semihosting.h |   1 +
 src/target/armv4_5.c         |  47 +++-
 src/target/armv7m.c          |   9 +-
 src/target/cortex_a.c        |   1 +
 src/target/cortex_m.c        |   1 +
 src/target/hla_target.c      |   2 +-
 src/target/target.c          |   5 +-
 11 files changed, 437 insertions(+), 158 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm.h b/src/target/arm.h
index 28022e3..226dd65 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -130,6 +130,18 @@ struct arm {
 	/** Flag reporting whether semihosting is active. */
 	bool is_semihosting;
 
+	/** Flag reporting whether semihosting fileio is active. */
+	bool is_semihosting_fileio;
+
+	/** Flag reporting whether semihosting fileio operation is active. */
+	bool semihosting_hit_fileio;
+
+	/** Current semihosting operation. */
+	int semihosting_op;
+
+	/** Current semihosting result. */
+	int semihosting_result;
+
 	/** Value to be returned by semihosting SYS_ERRNO request. */
 	int semihosting_errno;
 
diff --git a/src/target/arm7tdmi.c b/src/target/arm7tdmi.c
index 58ab027..9dcb302 100644
--- a/src/target/arm7tdmi.c
+++ b/src/target/arm7tdmi.c
@@ -30,6 +30,7 @@
 #include "target_type.h"
 #include "register.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
 
 /*
  * For information about ARM7TDMI, see ARM DDI 0210C (r4p1)
@@ -615,7 +616,7 @@ static void arm7tdmi_build_reg_cache(struct target *target)
 int arm7tdmi_init_target(struct command_context *cmd_ctx, struct target *target)
 {
 	arm7tdmi_build_reg_cache(target);
-
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/arm9tdmi.c b/src/target/arm9tdmi.c
index eed965a..82b430f 100644
--- a/src/target/arm9tdmi.c
+++ b/src/target/arm9tdmi.c
@@ -30,6 +30,7 @@
 #include "target_type.h"
 #include "register.h"
 #include "arm_opcodes.h"
+#include "arm_semihosting.h"
 
 /*
  * NOTE:  this holds code that's used with multiple ARM9 processors:
@@ -714,6 +715,7 @@ int arm9tdmi_init_target(struct command_context *cmd_ctx,
 		struct target *target)
 {
 	arm9tdmi_build_reg_cache(target);
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/arm_semihosting.c b/src/target/arm_semihosting.c
index 63335ff..2525119 100644
--- a/src/target/arm_semihosting.c
+++ b/src/target/arm_semihosting.c
@@ -5,6 +5,9 @@
  *   Copyright (C) 2010 by Spencer Oliver                                  *
  *   spen@spen-soft.co.uk                                                  *
  *                                                                         *
+ *   Copyright (C) 2016 by Square, Inc.                                    *
+ *   Steven Stallion <stallion@squareup.com>                               *
+ *                                                                         *
  *   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     *
@@ -43,6 +46,7 @@
 #include "cortex_m.h"
 #include "register.h"
 #include "arm_opcodes.h"
+#include "target_type.h"
 #include "arm_semihosting.h"
 #include <helper/binarybuffer.h>
 #include <helper/log.h>
@@ -63,13 +67,59 @@ static const int open_modeflags[12] = {
 	O_RDWR | O_CREAT | O_APPEND | O_BINARY
 };
 
+static int post_result(struct target *target)
+{
+	struct arm *arm = target_to_arm(target);
+
+	/* REVISIT this looks wrong ... ARM11 and Cortex-A8
+	 * should work this way at least sometimes.
+	 */
+	if (is_arm7_9(target_to_arm7_9(target)) ||
+	    is_armv7a(target_to_armv7a(target))) {
+		uint32_t spsr;
+
+		/* return value in R0 */
+		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, arm->semihosting_result);
+		arm->core_cache->reg_list[0].dirty = 1;
+
+		/* LR --> PC */
+		buf_set_u32(arm->core_cache->reg_list[15].value, 0, 32,
+			buf_get_u32(arm_reg_current(arm, 14)->value, 0, 32));
+		arm->core_cache->reg_list[15].dirty = 1;
+
+		/* saved PSR --> current PSR */
+		spsr = buf_get_u32(arm->spsr->value, 0, 32);
+
+		/* REVISIT should this be arm_set_cpsr(arm, spsr)
+		 * instead of a partially unrolled version?
+		 */
+
+		buf_set_u32(arm->cpsr->value, 0, 32, spsr);
+		arm->cpsr->dirty = 1;
+		arm->core_mode = spsr & 0x1f;
+		if (spsr & 0x20)
+			arm->core_state = ARM_STATE_THUMB;
+
+	} else {
+		/* resume execution, this will be pc+2 to skip over the
+		 * bkpt instruction */
+
+		/* return result in R0 */
+		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, arm->semihosting_result);
+		arm->core_cache->reg_list[0].dirty = 1;
+	}
+
+	return ERROR_OK;
+}
+
 static int do_semihosting(struct target *target)
 {
 	struct arm *arm = target_to_arm(target);
+	struct gdb_fileio_info *fileio_info = target->fileio_info;
 	uint32_t r0 = buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32);
 	uint32_t r1 = buf_get_u32(arm->core_cache->reg_list[1].value, 0, 32);
 	uint8_t params[16];
-	int retval, result;
+	int retval;
 
 	/*
 	 * TODO: lots of security issues are not considered yet, such as:
@@ -77,10 +127,10 @@ static int do_semihosting(struct target *target)
 	 * - no safety checks on opened/deleted/renamed file paths
 	 * Beware the target app you use this support with.
 	 *
-	 * TODO: explore mapping requests to GDB's "File-I/O Remote
-	 * Protocol Extension" ... when GDB is active.
+	 * TODO: unsupported semihosting fileio operations could be
+	 * implemented if we had a small working area at our disposal.
 	 */
-	switch (r0) {
+	switch ((arm->semihosting_op = r0)) {
 	case 0x01:	/* SYS_OPEN */
 		retval = target_read_memory(target, r1, 4, 3, params);
 		if (retval != ERROR_OK)
@@ -89,27 +139,40 @@ static int do_semihosting(struct target *target)
 			uint32_t a = target_buffer_get_u32(target, params+0);
 			uint32_t m = target_buffer_get_u32(target, params+4);
 			uint32_t l = target_buffer_get_u32(target, params+8);
-			if (l <= 255 && m <= 11) {
-				uint8_t fn[256];
-				retval = target_read_memory(target, a, 1, l, fn);
-				if (retval != ERROR_OK)
-					return retval;
-				fn[l] = 0;
-				if (strcmp((char *)fn, ":tt") == 0) {
-					if (m < 4)
-						result = dup(STDIN_FILENO);
-					else
-						result = dup(STDOUT_FILENO);
-				} else {
-					/* cygwin requires the permission setting
-					 * otherwise it will fail to reopen a previously
-					 * written file */
-					result = open((char *)fn, open_modeflags[m], 0644);
+			uint8_t fn[256];
+			retval = target_read_memory(target, a, 1, l, fn);
+			if (retval != ERROR_OK)
+				return retval;
+			fn[l] = 0;
+			if (arm->is_semihosting_fileio) {
+				if (strcmp((char *)fn, ":tt") == 0)
+					arm->semihosting_result = 0;
+				else {
+					arm->semihosting_hit_fileio = true;
+					fileio_info->identifier = "open";
+					fileio_info->param_1 = a;
+					fileio_info->param_2 = l;
+					fileio_info->param_3 = open_modeflags[m];
+					fileio_info->param_4 = 0644;
 				}
-				arm->semihosting_errno =  errno;
 			} else {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+				if (l <= 255 && m <= 11) {
+					if (strcmp((char *)fn, ":tt") == 0) {
+						if (m < 4)
+							arm->semihosting_result = dup(STDIN_FILENO);
+						else
+							arm->semihosting_result = dup(STDOUT_FILENO);
+					} else {
+						/* cygwin requires the permission setting
+						 * otherwise it will fail to reopen a previously
+						 * written file */
+						arm->semihosting_result = open((char *)fn, open_modeflags[m], 0644);
+					}
+					arm->semihosting_errno =  errno;
+				} else {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				}
 			}
 		}
 		break;
@@ -120,33 +183,63 @@ static int do_semihosting(struct target *target)
 			return retval;
 		else {
 			int fd = target_buffer_get_u32(target, params+0);
-			result = close(fd);
-			arm->semihosting_errno = errno;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "close";
+				fileio_info->param_1 = fd;
+			} else {
+				arm->semihosting_result = close(fd);
+				arm->semihosting_errno = errno;
+			}
 		}
 		break;
 
 	case 0x03:	/* SYS_WRITEC */
-		{
+		if (arm->is_semihosting_fileio) {
+			arm->semihosting_hit_fileio = true;
+			fileio_info->identifier = "write";
+			fileio_info->param_1 = 1;
+			fileio_info->param_2 = r1;
+			fileio_info->param_3 = 1;
+		} else {
 			unsigned char c;
 			retval = target_read_memory(target, r1, 1, 1, &c);
 			if (retval != ERROR_OK)
 				return retval;
 			putchar(c);
-			result = 0;
+			arm->semihosting_result = 0;
 		}
 		break;
 
 	case 0x04:	/* SYS_WRITE0 */
-		do {
-			unsigned char c;
-			retval = target_read_memory(target, r1++, 1, 1, &c);
-			if (retval != ERROR_OK)
-				return retval;
-			if (!c)
-				break;
-			putchar(c);
-		} while (1);
-		result = 0;
+		if (arm->is_semihosting_fileio) {
+			size_t count = 0;
+			for (uint32_t a = r1;; a++) {
+				unsigned char c;
+				retval = target_read_memory(target, a, 1, 1, &c);
+				if (retval != ERROR_OK)
+					return retval;
+				if (c == '\0')
+					break;
+				count++;
+			}
+			arm->semihosting_hit_fileio = true;
+			fileio_info->identifier = "write";
+			fileio_info->param_1 = 1;
+			fileio_info->param_2 = r1;
+			fileio_info->param_3 = count;
+		} else {
+			do {
+				unsigned char c;
+				retval = target_read_memory(target, r1++, 1, 1, &c);
+				if (retval != ERROR_OK)
+					return retval;
+				if (!c)
+					break;
+				putchar(c);
+			} while (1);
+			arm->semihosting_result = 0;
+		}
 		break;
 
 	case 0x05:	/* SYS_WRITE */
@@ -157,21 +250,29 @@ static int do_semihosting(struct target *target)
 			int fd = target_buffer_get_u32(target, params+0);
 			uint32_t a = target_buffer_get_u32(target, params+4);
 			size_t l = target_buffer_get_u32(target, params+8);
-			uint8_t *buf = malloc(l);
-			if (!buf) {
-				result = -1;
-				arm->semihosting_errno = ENOMEM;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "write";
+				fileio_info->param_1 = fd;
+				fileio_info->param_2 = a;
+				fileio_info->param_3 = l;
 			} else {
-				retval = target_read_buffer(target, a, l, buf);
-				if (retval != ERROR_OK) {
+				uint8_t *buf = malloc(l);
+				if (!buf) {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = ENOMEM;
+				} else {
+					retval = target_read_buffer(target, a, l, buf);
+					if (retval != ERROR_OK) {
+						free(buf);
+						return retval;
+					}
+					arm->semihosting_result = write(fd, buf, l);
+					arm->semihosting_errno = errno;
+					if (arm->semihosting_result >= 0)
+						arm->semihosting_result = l - arm->semihosting_result;
 					free(buf);
-					return retval;
 				}
-				result = write(fd, buf, l);
-				arm->semihosting_errno = errno;
-				if (result >= 0)
-					result = l - result;
-				free(buf);
 			}
 		}
 		break;
@@ -184,42 +285,60 @@ static int do_semihosting(struct target *target)
 			int fd = target_buffer_get_u32(target, params+0);
 			uint32_t a = target_buffer_get_u32(target, params+4);
 			ssize_t l = target_buffer_get_u32(target, params+8);
-			uint8_t *buf = malloc(l);
-			if (!buf) {
-				result = -1;
-				arm->semihosting_errno = ENOMEM;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "read";
+				fileio_info->param_1 = fd;
+				fileio_info->param_2 = a;
+				fileio_info->param_3 = l;
 			} else {
-				result = read(fd, buf, l);
-				arm->semihosting_errno = errno;
-				if (result >= 0) {
-					retval = target_write_buffer(target, a, result, buf);
-					if (retval != ERROR_OK) {
-						free(buf);
-						return retval;
+				uint8_t *buf = malloc(l);
+				if (!buf) {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = ENOMEM;
+				} else {
+					arm->semihosting_result = read(fd, buf, l);
+					arm->semihosting_errno = errno;
+					if (arm->semihosting_result >= 0) {
+						retval = target_write_buffer(target, a, arm->semihosting_result, buf);
+						if (retval != ERROR_OK) {
+							free(buf);
+							return retval;
+						}
+						arm->semihosting_result = l - arm->semihosting_result;
 					}
-					result = l - result;
+					free(buf);
 				}
-				free(buf);
 			}
 		}
 		break;
 
 	case 0x07:	/* SYS_READC */
-		result = getchar();
+		if (arm->is_semihosting_fileio) {
+			LOG_ERROR("SYS_READC not supported by semihosting fileio");
+			return ERROR_FAIL;
+		}
+		arm->semihosting_result = getchar();
 		break;
 
 	case 0x08:	/* SYS_ISERROR */
 		retval = target_read_memory(target, r1, 4, 1, params);
 		if (retval != ERROR_OK)
 			return retval;
-		result = (target_buffer_get_u32(target, params+0) != 0);
+		arm->semihosting_result = (target_buffer_get_u32(target, params+0) != 0);
 		break;
 
 	case 0x09:	/* SYS_ISTTY */
-		retval = target_read_memory(target, r1, 4, 1, params);
-		if (retval != ERROR_OK)
-			return retval;
-		result = isatty(target_buffer_get_u32(target, params+0));
+		if (arm->is_semihosting_fileio) {
+			arm->semihosting_hit_fileio = true;
+			fileio_info->identifier = "isatty";
+			fileio_info->param_1 = r1;
+		} else {
+			retval = target_read_memory(target, r1, 4, 1, params);
+			if (retval != ERROR_OK)
+				return retval;
+			arm->semihosting_result = isatty(target_buffer_get_u32(target, params+0));
+		}
 		break;
 
 	case 0x0a:	/* SYS_SEEK */
@@ -229,27 +348,39 @@ static int do_semihosting(struct target *target)
 		else {
 			int fd = target_buffer_get_u32(target, params+0);
 			off_t pos = target_buffer_get_u32(target, params+4);
-			result = lseek(fd, pos, SEEK_SET);
-			arm->semihosting_errno = errno;
-			if (result == pos)
-				result = 0;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "lseek";
+				fileio_info->param_1 = fd;
+				fileio_info->param_2 = pos;
+				fileio_info->param_3 = SEEK_SET;
+			} else {
+				arm->semihosting_result = lseek(fd, pos, SEEK_SET);
+				arm->semihosting_errno = errno;
+				if (arm->semihosting_result == pos)
+					arm->semihosting_result = 0;
+			}
 		}
 		break;
 
 	case 0x0c:	/* SYS_FLEN */
+		if (arm->is_semihosting_fileio) {
+			LOG_ERROR("SYS_FLEN not supported by semihosting fileio");
+			return ERROR_FAIL;
+		}
 		retval = target_read_memory(target, r1, 4, 1, params);
 		if (retval != ERROR_OK)
 			return retval;
 		else {
 			int fd = target_buffer_get_u32(target, params+0);
 			struct stat buf;
-			result = fstat(fd, &buf);
-			if (result == -1) {
+			arm->semihosting_result = fstat(fd, &buf);
+			if (arm->semihosting_result == -1) {
 				arm->semihosting_errno = errno;
-				result = -1;
+				arm->semihosting_result = -1;
 				break;
 			}
-			result = buf.st_size;
+			arm->semihosting_result = buf.st_size;
 		}
 		break;
 
@@ -260,17 +391,24 @@ static int do_semihosting(struct target *target)
 		else {
 			uint32_t a = target_buffer_get_u32(target, params+0);
 			uint32_t l = target_buffer_get_u32(target, params+4);
-			if (l <= 255) {
-				uint8_t fn[256];
-				retval = target_read_memory(target, a, 1, l, fn);
-				if (retval != ERROR_OK)
-					return retval;
-				fn[l] = 0;
-				result = remove((char *)fn);
-				arm->semihosting_errno =  errno;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "unlink";
+				fileio_info->param_1 = a;
+				fileio_info->param_2 = l;
 			} else {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+				if (l <= 255) {
+					uint8_t fn[256];
+					retval = target_read_memory(target, a, 1, l, fn);
+					if (retval != ERROR_OK)
+						return retval;
+					fn[l] = 0;
+					arm->semihosting_result = remove((char *)fn);
+					arm->semihosting_errno =  errno;
+				} else {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				}
 			}
 		}
 		break;
@@ -284,31 +422,40 @@ static int do_semihosting(struct target *target)
 			uint32_t l1 = target_buffer_get_u32(target, params+4);
 			uint32_t a2 = target_buffer_get_u32(target, params+8);
 			uint32_t l2 = target_buffer_get_u32(target, params+12);
-			if (l1 <= 255 && l2 <= 255) {
-				uint8_t fn1[256], fn2[256];
-				retval = target_read_memory(target, a1, 1, l1, fn1);
-				if (retval != ERROR_OK)
-					return retval;
-				retval = target_read_memory(target, a2, 1, l2, fn2);
-				if (retval != ERROR_OK)
-					return retval;
-				fn1[l1] = 0;
-				fn2[l2] = 0;
-				result = rename((char *)fn1, (char *)fn2);
-				arm->semihosting_errno =  errno;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "rename";
+				fileio_info->param_1 = a1;
+				fileio_info->param_2 = l1;
+				fileio_info->param_3 = a2;
+				fileio_info->param_4 = l2;
 			} else {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+				if (l1 <= 255 && l2 <= 255) {
+					uint8_t fn1[256], fn2[256];
+					retval = target_read_memory(target, a1, 1, l1, fn1);
+					if (retval != ERROR_OK)
+						return retval;
+					retval = target_read_memory(target, a2, 1, l2, fn2);
+					if (retval != ERROR_OK)
+						return retval;
+					fn1[l1] = 0;
+					fn2[l2] = 0;
+					arm->semihosting_result = rename((char *)fn1, (char *)fn2);
+					arm->semihosting_errno =  errno;
+				} else {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				}
 			}
 		}
 		break;
 
 	case 0x11:	/* SYS_TIME */
-		result = time(NULL);
+		arm->semihosting_result = time(NULL);
 		break;
 
 	case 0x13:	/* SYS_ERRNO */
-		result = arm->semihosting_errno;
+		arm->semihosting_result = arm->semihosting_errno;
 		break;
 
 	case 0x15:	/* SYS_GET_CMDLINE */
@@ -321,12 +468,12 @@ static int do_semihosting(struct target *target)
 			char *arg = "foobar";
 			uint32_t s = strlen(arg) + 1;
 			if (l < s)
-				result = -1;
+				arm->semihosting_result = -1;
 			else {
 				retval = target_write_buffer(target, a, s, (uint8_t *)arg);
 				if (retval != ERROR_OK)
 					return retval;
-				result = 0;
+				arm->semihosting_result = 0;
 			}
 		}
 		break;
@@ -342,7 +489,7 @@ static int do_semihosting(struct target *target)
 			retval = target_write_memory(target, a, 4, 4, params);
 			if (retval != ERROR_OK)
 				return retval;
-			result = 0;
+			arm->semihosting_result = 0;
 		}
 		break;
 
@@ -386,17 +533,24 @@ static int do_semihosting(struct target *target)
 		else {
 			uint32_t len = target_buffer_get_u32(target, params+4);
 			uint32_t c_ptr = target_buffer_get_u32(target, params);
-			uint8_t cmd[256];
-			if (len > 255) {
-				result = -1;
-				arm->semihosting_errno = EINVAL;
+			if (arm->is_semihosting_fileio) {
+				arm->semihosting_hit_fileio = true;
+				fileio_info->identifier = "system";
+				fileio_info->param_1 = c_ptr;
+				fileio_info->param_2 = len;
 			} else {
-				memset(cmd, 0x0, 256);
-				retval = target_read_memory(target, c_ptr, 1, len, cmd);
-				if (retval != ERROR_OK)
-					return retval;
-				else
-					result = system((const char *)cmd);
+				uint8_t cmd[256];
+				if (len > 255) {
+					arm->semihosting_result = -1;
+					arm->semihosting_errno = EINVAL;
+				} else {
+					memset(cmd, 0x0, 256);
+					retval = target_read_memory(target, c_ptr, 1, len, cmd);
+					if (retval != ERROR_OK)
+						return retval;
+					else
+						arm->semihosting_result = system((const char *)cmd);
+				}
 			}
 		}
 		break;
@@ -408,51 +562,84 @@ static int do_semihosting(struct target *target)
 	default:
 		fprintf(stderr, "semihosting: unsupported call %#x\n",
 				(unsigned) r0);
-		result = -1;
+		arm->semihosting_result = -1;
 		arm->semihosting_errno = ENOTSUP;
 	}
 
-	/* resume execution to the original mode */
+	return ERROR_OK;
+}
 
-	/* REVISIT this looks wrong ... ARM11 and Cortex-A8
-	 * should work this way at least sometimes.
+static int get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info)
+{
+	struct arm *arm = target_to_arm(target);
+
+	/* To avoid uneccessary duplication, semihosting prepares the
+	 * fileio_info structure out-of-band when the target halts. See
+	 * do_semihosting for more detail.
 	 */
-	if (is_arm7_9(target_to_arm7_9(target)) ||
-	    is_armv7a(target_to_armv7a(target))) {
-		uint32_t spsr;
+	if (!arm->is_semihosting_fileio || !arm->semihosting_hit_fileio)
+		return ERROR_FAIL;
 
-		/* return value in R0 */
-		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, result);
-		arm->core_cache->reg_list[0].dirty = 1;
+	return ERROR_OK;
+}
 
-		/* LR --> PC */
-		buf_set_u32(arm->core_cache->reg_list[15].value, 0, 32,
-			buf_get_u32(arm_reg_current(arm, 14)->value, 0, 32));
-		arm->core_cache->reg_list[15].dirty = 1;
+static int gdb_fileio_end(struct target *target, int result, int fileio_errno, bool ctrl_c)
+{
+	struct arm *arm = target_to_arm(target);
+	struct gdb_fileio_info *fileio_info = target->fileio_info;
 
-		/* saved PSR --> current PSR */
-		spsr = buf_get_u32(arm->spsr->value, 0, 32);
+	/* clear pending status */
+	arm->semihosting_hit_fileio = false;
 
-		/* REVISIT should this be arm_set_cpsr(arm, spsr)
-		 * instead of a partially unrolled version?
-		 */
+	arm->semihosting_result = result;
+	arm->semihosting_errno = fileio_errno;
 
-		buf_set_u32(arm->cpsr->value, 0, 32, spsr);
-		arm->cpsr->dirty = 1;
-		arm->core_mode = spsr & 0x1f;
-		if (spsr & 0x20)
-			arm->core_state = ARM_STATE_THUMB;
+	/* Some fileio results do not match up with what the semihosting
+	 * operation expects; for these operations, we munge the results
+	 * below:
+	 */
+	switch (arm->semihosting_op) {
+	case 0x05:	/* SYS_WRITE */
+		if (result < 0)
+			arm->semihosting_result = fileio_info->param_3;
+		else
+			arm->semihosting_result = 0;
+		break;
 
-	} else {
-		/* resume execution, this will be pc+2 to skip over the
-		 * bkpt instruction */
+	case 0x06:	/* SYS_READ */
+		if (result == (int)fileio_info->param_3)
+			arm->semihosting_result = 0;
+		if (result <= 0)
+			arm->semihosting_result = fileio_info->param_3;
+		break;
 
-		/* return result in R0 */
-		buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, result);
-		arm->core_cache->reg_list[0].dirty = 1;
+	case 0x0a:	/* SYS_SEEK */
+		if (result > 0)
+			arm->semihosting_result = 0;
+		break;
 	}
 
-	return target_resume(target, 1, 0, 0, 0);
+	return post_result(target);
+}
+
+/**
+ * Initialize ARM semihosting support.
+ *
+ * @param target Pointer to the ARM target to initialize.
+ * @return An error status if there is a problem during initialization.
+ */
+int arm_semihosting_init(struct target *target)
+{
+	target->fileio_info = malloc(sizeof(*target->fileio_info));
+	if (target->fileio_info == NULL) {
+		LOG_ERROR("out of memory");
+		return ERROR_FAIL;
+	}
+
+	target->type->get_gdb_fileio_info = get_gdb_fileio_info;
+	target->type->gdb_fileio_end = gdb_fileio_end;
+
+	return ERROR_OK;
 }
 
 /**
@@ -576,6 +763,35 @@ int arm_semihosting(struct target *target, int *retval)
 		return 0;
 	}
 
-	*retval = do_semihosting(target);
-	return 1;
+	/* Perform semihosting if we are not waiting on a fileio
+	 * operation to complete.
+	 */
+	if (!arm->semihosting_hit_fileio) {
+		*retval = do_semihosting(target);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed semihosting operation");
+			return 0;
+		}
+	}
+
+	/* Post result to target if we are not waiting on a fileio
+	 * operation to complete:
+	 */
+	if (!arm->semihosting_hit_fileio) {
+		*retval = post_result(target);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed to post semihosting result");
+			return 0;
+		}
+
+		*retval = target_resume(target, 1, 0, 0, 0);
+		if (*retval != ERROR_OK) {
+			LOG_ERROR("Failed to resume target");
+			return 0;
+		}
+
+		return 1;
+	}
+
+	return 0;
 }
diff --git a/src/target/arm_semihosting.h b/src/target/arm_semihosting.h
index 7b5c0b2..011f19f 100644
--- a/src/target/arm_semihosting.h
+++ b/src/target/arm_semihosting.h
@@ -19,6 +19,7 @@
 #ifndef OPENOCD_TARGET_ARM_SEMIHOSTING_H
 #define OPENOCD_TARGET_ARM_SEMIHOSTING_H
 
+int arm_semihosting_init(struct target *target);
 int arm_semihosting(struct target *target, int *retval);
 
 #endif /* OPENOCD_TARGET_ARM_SEMIHOSTING_H */
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 1a31d40..624a254 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -666,14 +666,19 @@ int arm_arch_state(struct target *target)
 		return ERROR_FAIL;
 	}
 
+	/* avoid filling log waiting for fileio reply */
+	if (arm->semihosting_hit_fileio)
+		return ERROR_OK;
+
 	LOG_USER("target halted in %s state due to %s, current mode: %s\n"
-		"cpsr: 0x%8.8" PRIx32 " pc: 0x%8.8" PRIx32 "%s",
+		"cpsr: 0x%8.8" PRIx32 " pc: 0x%8.8" PRIx32 "%s%s",
 		arm_state_strings[arm->core_state],
 		debug_reason_name(target),
 		arm_mode_name(arm->core_mode),
 		buf_get_u32(arm->cpsr->value, 0, 32),
 		buf_get_u32(arm->pc->value, 0, 32),
-		arm->is_semihosting ? ", semihosting" : "");
+		arm->is_semihosting ? ", semihosting" : "",
+		arm->is_semihosting_fileio ? " fileio" : "");
 
 	return ERROR_OK;
 }
@@ -1055,6 +1060,37 @@ COMMAND_HANDLER(handle_arm_semihosting_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(handle_arm_semihosting_fileio_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+
+	if (target == NULL) {
+		LOG_ERROR("No target selected");
+		return ERROR_FAIL;
+	}
+
+	struct arm *arm = target_to_arm(target);
+
+	if (!is_arm(arm)) {
+		command_print(CMD_CTX, "current target isn't an ARM");
+		return ERROR_FAIL;
+	}
+
+	if (!arm->is_semihosting) {
+		command_print(CMD_CTX, "semihosting is not enabled");
+		return ERROR_FAIL;
+	}
+
+	if (CMD_ARGC > 0)
+		COMMAND_PARSE_ENABLE(CMD_ARGV[0], arm->is_semihosting_fileio);
+
+	command_print(CMD_CTX, "semihosting fileio is %s",
+		arm->is_semihosting_fileio
+		? "enabled" : "disabled");
+
+	return ERROR_OK;
+}
+
 static const struct command_registration arm_exec_command_handlers[] = {
 	{
 		.name = "reg",
@@ -1097,6 +1133,13 @@ static const struct command_registration arm_exec_command_handlers[] = {
 		.usage = "['enable'|'disable']",
 		.help = "activate support for semihosting operations",
 	},
+	{
+		"semihosting_fileio",
+		.handler = handle_arm_semihosting_fileio_command,
+		.mode = COMMAND_EXEC,
+		.usage = "['enable'|'disable']",
+		.help = "activate support for semihosting fileio operations",
+	},
 
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 60b244a..64d18d7 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -536,11 +536,15 @@ int armv7m_arch_state(struct target *target)
 	struct arm *arm = &armv7m->arm;
 	uint32_t ctrl, sp;
 
+	/* avoid filling log waiting for fileio reply */
+	if (arm->semihosting_hit_fileio)
+		return ERROR_OK;
+
 	ctrl = buf_get_u32(arm->core_cache->reg_list[ARMV7M_CONTROL].value, 0, 32);
 	sp = buf_get_u32(arm->core_cache->reg_list[ARMV7M_R13].value, 0, 32);
 
 	LOG_USER("target halted due to %s, current mode: %s %s\n"
-		"xPSR: %#8.8" PRIx32 " pc: %#8.8" PRIx32 " %csp: %#8.8" PRIx32 "%s",
+		"xPSR: %#8.8" PRIx32 " pc: %#8.8" PRIx32 " %csp: %#8.8" PRIx32 "%s%s",
 		debug_reason_name(target),
 		arm_mode_name(arm->core_mode),
 		armv7m_exception_string(armv7m->exception_number),
@@ -548,7 +552,8 @@ int armv7m_arch_state(struct target *target)
 		buf_get_u32(arm->pc->value, 0, 32),
 		(ctrl & 0x02) ? 'p' : 'm',
 		sp,
-		arm->is_semihosting ? ", semihosting" : "");
+		arm->is_semihosting ? ", semihosting" : "",
+		arm->is_semihosting_fileio ? " fileio" : "");
 
 	return ERROR_OK;
 }
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 91fa48e..ed2f613 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -3103,6 +3103,7 @@ static int cortex_a_init_target(struct command_context *cmd_ctx,
 	struct target *target)
 {
 	/* examine_first() does a bunch of this */
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 4270f8f..f674cc5 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1696,6 +1696,7 @@ static int cortex_m_init_target(struct command_context *cmd_ctx,
 	struct target *target)
 {
 	armv7m_build_reg_cache(target);
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index e02abc4..feeb11f 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -341,7 +341,7 @@ static int adapter_init_target(struct command_context *cmd_ctx,
 	LOG_DEBUG("%s", __func__);
 
 	armv7m_build_reg_cache(target);
-
+	arm_semihosting_init(target);
 	return ERROR_OK;
 }
 
diff --git a/src/target/target.c b/src/target/target.c
index 26bae4b..17902a5 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1944,13 +1944,10 @@ int target_arch_state(struct target *target)
 {
 	int retval;
 	if (target == NULL) {
-		LOG_USER("No target has been configured");
+		LOG_WARNING("No target has been configured");
 		return ERROR_OK;
 	}
 
-	LOG_USER("%s: target state: %s", target_name(target),
-		 target_state_name(target));
-
 	if (target->state != TARGET_HALTED)
 		return ERROR_OK;
 
-- 
cgit v1.1


From 69ff7354d9c9accf09374772310098f1f00e8ccb Mon Sep 17 00:00:00 2001
From: Marc Schink <openocd-dev@marcschink.de>
Date: Sun, 22 May 2016 20:35:34 +0200
Subject: helper: Code cleanup for hexify()

Simplify hexify() and do not longer use 0 as special case for the
parameter 'count' to determine the string length of the binary input.
Instead, use strlen() outside of the function if needed.
Additionally, fix the return value and return the length of the
converted string. The old function always returned 2 * count.

Also, use more appropriate data types for the function parameters and
add a small documentation.

Change-Id: I133a8ab786b8f7c1296afcaf9c0a0b43881e5112
Signed-off-by: Marc Schink <openocd-dev@marcschink.de>
Reviewed-on: http://openocd.zylin.com/3793
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
---
 src/target/smp.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/smp.c b/src/target/smp.c
index 3dc6f6d..bdf81a0 100644
--- a/src/target/smp.c
+++ b/src/target/smp.c
@@ -64,7 +64,8 @@ int gdb_read_smp_packet(struct connection *connection,
 			char hex_buffer[len * 2 + 1];
 			uint8_t buffer[len];
 			buf_set_u32(buffer, 0, len * 8, target->gdb_service->core[0]);
-			int pkt_len = hexify(hex_buffer, (char *)buffer, sizeof(buffer), sizeof(hex_buffer));
+			size_t pkt_len = hexify(hex_buffer, buffer, sizeof(buffer),
+				sizeof(hex_buffer));
 
 			retval = gdb_put_packet(connection, hex_buffer, pkt_len);
 		}
-- 
cgit v1.1


From 097aa2979e0eefa822d4a16f055dcf9fc31e3dde Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sun, 13 Nov 2016 13:23:08 +0100
Subject: cortex_m: allow setting debug ap during create

This patch adds a Cortex-M private configuration option
that allows setting the acess point during target
creation. This circumvents situations in hybrid systems
when the correct access point can not be automatically
detected.

Change-Id: If313a5250e6e66509bb9080f3498feab7781dced
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3639
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/arm_adi_v5.c | 35 +++++++++++++++++++++++++++++++++++
 src/target/arm_adi_v5.h |  6 ++++++
 src/target/cortex_m.c   | 23 ++++++++++++++++++-----
 src/target/cortex_m.h   |  2 ++
 src/target/target.h     |  1 +
 5 files changed, 62 insertions(+), 5 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 39f2b39..eafc2dd 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -1359,6 +1359,41 @@ static int dap_info_command(struct command_context *cmd_ctx,
 	return ERROR_OK;
 }
 
+int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi)
+{
+	struct adiv5_private_config *pc;
+	const char *arg;
+	jim_wide ap_num;
+	int e;
+
+	/* check if argv[0] is for us */
+	arg = Jim_GetString(goi->argv[0], NULL);
+	if (strcmp(arg, "-ap-num"))
+		return JIM_CONTINUE;
+
+	e = Jim_GetOpt_String(goi, &arg, NULL);
+	if (e != JIM_OK)
+		return e;
+
+	if (goi->argc == 0) {
+		Jim_WrongNumArgs(goi->interp, goi->argc, goi->argv, "-ap-num ?ap-number? ...");
+		return JIM_ERR;
+	}
+
+	e = Jim_GetOpt_Wide(goi, &ap_num);
+	if (e != JIM_OK)
+		return e;
+
+	if (target->private_config == NULL) {
+		pc = calloc(1, sizeof(struct adiv5_private_config));
+		target->private_config = pc;
+		pc->ap_num = ap_num;
+	}
+
+
+	return JIM_OK;
+}
+
 COMMAND_HANDLER(handle_dap_info_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index 3220d8b..bf9cb5c 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -504,4 +504,10 @@ int dap_to_jtag(struct target *target);
 
 extern const struct command_registration dap_command_handlers[];
 
+struct adiv5_private_config {
+	int ap_num;
+};
+
+extern int adiv5_jim_configure(struct target *target, Jim_GetOptInfo *goi);
+
 #endif /* OPENOCD_TARGET_ARM_ADI_V5_H */
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index f674cc5..36a7746 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1709,6 +1709,7 @@ void cortex_m_deinit_target(struct target *target)
 	cortex_m_dwt_free(target);
 	armv7m_free_reg_cache(target);
 
+	free(target->private_config);
 	free(cortex_m);
 }
 
@@ -1912,11 +1913,15 @@ int cortex_m_examine(struct target *target)
 			return retval;
 		}
 
-		/* Search for the MEM-AP */
-		retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Could not find MEM-AP to control the core");
-			return retval;
+		if (cortex_m->apsel < 0) {
+			/* Search for the MEM-AP */
+			retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv7m->debug_ap);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("Could not find MEM-AP to control the core");
+				return retval;
+			}
+		} else {
+			armv7m->debug_ap = dap_ap(swjdp, cortex_m->apsel);
 		}
 
 		/* Leave (only) generic DAP stuff for debugport_init(); */
@@ -2180,6 +2185,13 @@ static int cortex_m_target_create(struct target *target, Jim_Interp *interp)
 	cortex_m->common_magic = CORTEX_M_COMMON_MAGIC;
 	cortex_m_init_arch_info(target, cortex_m, target->tap);
 
+	if (target->private_config != NULL) {
+		struct adiv5_private_config *pc =
+				(struct adiv5_private_config *)target->private_config;
+		cortex_m->apsel = pc->ap_num;
+	} else
+		cortex_m->apsel = -1;
+
 	return ERROR_OK;
 }
 
@@ -2441,6 +2453,7 @@ struct target_type cortexm_target = {
 
 	.commands = cortex_m_command_handlers,
 	.target_create = cortex_m_target_create,
+	.target_jim_configure = adiv5_jim_configure,
 	.init_target = cortex_m_init_target,
 	.examine = cortex_m_examine,
 	.deinit_target = cortex_m_deinit_target,
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index eabaac4..3d9714b 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -188,6 +188,8 @@ struct cortex_m_common {
 	enum cortex_m_isrmasking_mode isrmasking_mode;
 
 	struct armv7m_common armv7m;
+
+	int apsel;
 };
 
 static inline struct cortex_m_common *
diff --git a/src/target/target.h b/src/target/target.h
index 7f6ac14..e86b700 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -173,6 +173,7 @@ struct target {
 	struct debug_msg_receiver *dbgmsg;	/* list of debug message receivers */
 	uint32_t dbg_msg_enabled;			/* debug message status */
 	void *arch_info;					/* architecture specific information */
+	void *private_config;				/* pointer to target specific config data (for jim_configure hook) */
 	struct target *next;				/* next target in list */
 
 	int display;						/* display async info in telnet session. Do not display
-- 
cgit v1.1


From cdba6ba0ad63d256f896c834b03732a5eefc7a13 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 10 Nov 2016 20:33:02 +0100
Subject: cortex_a: fix reset for SWD transport

Change b0698501b0e789091e9f9f1427b32af304888769 fixed
reset for i.MX6 and TI Sitara SoCs but broke reset for
cortex-a targets that use SWD. This patch is a work-
around that forces asserting SRST when SWD is used.

Change-Id: I7e39f2a469b9b4b2b74ad48ba49f2eeb58528921
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3641
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/cortex_a.c | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

(limited to 'src/target')

diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index ed2f613..d0eac75 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -54,6 +54,7 @@
 #include "target_type.h"
 #include "arm_opcodes.h"
 #include "arm_semihosting.h"
+#include "jtag/swd.h"
 #include <helper/time_support.h>
 
 static int cortex_a_poll(struct target *target);
@@ -1867,9 +1868,15 @@ static int cortex_a_assert_reset(struct target *target)
 		/* REVISIT handle "pulls" cases, if there's
 		 * hardware that needs them to work.
 		 */
-		if (target->reset_halt)
-			if (jtag_get_reset_config() & RESET_SRST_NO_GATING)
-				jtag_add_reset(0, 1);
+
+		/*
+		 * FIXME: fix reset when transport is SWD. This is a temporary
+		 * work-around for release v0.10 that is not intended to stay!
+		 */
+		if (transport_is_swd() ||
+				(target->reset_halt && (jtag_get_reset_config() & RESET_SRST_NO_GATING)))
+			jtag_add_reset(0, 1);
+
 	} else {
 		LOG_ERROR("%s: how to reset?", target_name(target));
 		return ERROR_FAIL;
-- 
cgit v1.1


From 1039ed3ff2138c9d0d820b143159738f8a73cb2e Mon Sep 17 00:00:00 2001
From: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Date: Wed, 2 Nov 2016 22:34:42 +0100
Subject: xscale: Move debug handler to contrib/loaders

Avoid special rules to generate array at compile time by shipping
the generated file. Convert to Makefile build like the other
loaders.

Change-Id: I5a05edddcfaff3d395086cd3aa33120f8a7aa9dc
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/3864
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/Makefile.am              |  13 -
 src/target/xscale.c                 |   2 +-
 src/target/xscale/build.sh          |   7 -
 src/target/xscale/debug_handler.S   | 716 ------------------------------------
 src/target/xscale/debug_handler.bin | Bin 1592 -> 0 bytes
 src/target/xscale/debug_handler.cmd |  49 ---
 src/target/xscale/protocol.h        |  68 ----
 7 files changed, 1 insertion(+), 854 deletions(-)
 delete mode 100755 src/target/xscale/build.sh
 delete mode 100644 src/target/xscale/debug_handler.S
 delete mode 100755 src/target/xscale/debug_handler.bin
 delete mode 100644 src/target/xscale/debug_handler.cmd
 delete mode 100644 src/target/xscale/protocol.h

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 5b6916d..dc7973d 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -9,19 +9,6 @@ endif
 SUBDIRS = openrisc
 libtarget_la_LIBADD = $(top_builddir)/src/target/openrisc/libopenrisc.la
 
-BIN2C		= $(top_srcdir)/src/helper/bin2char.sh
-
-DEBUG_HANDLER	= $(srcdir)/xscale/debug_handler.bin
-EXTRA_DIST = \
-	startup.tcl \
-	$(wildcard $(srcdir)/xscale/*)
-
-DEBUG_HEADER	= xscale_debug.inc
-BUILT_SOURCES = $(DEBUG_HEADER)
-CLEANFILES = $(DEBUG_HEADER)
-
-$(DEBUG_HEADER): $(DEBUG_HANDLER) $(BIN2C)
-	$(BIN2C) < $< > $@ || { rm -f $@; false; }
 
 METASOURCES = AUTO
 noinst_LTLIBRARIES = libtarget.la
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 140ea58..e54033f 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -73,7 +73,7 @@ static int xscale_read_trace(struct target *);
  * mini-ICache, which is 2K of code writable only via JTAG.
  */
 static const uint8_t xscale_debug_handler[] = {
-#include "xscale_debug.inc"
+#include "../../contrib/loaders/debug/xscale/debug_handler.inc"
 };
 
 static const char *const xscale_reg_list[] = {
diff --git a/src/target/xscale/build.sh b/src/target/xscale/build.sh
deleted file mode 100755
index fc828b2..0000000
--- a/src/target/xscale/build.sh
+++ /dev/null
@@ -1,7 +0,0 @@
-arm-none-eabi-gcc -c debug_handler.S -o debug_handler.o
-arm-none-eabi-ld -EL -n -Tdebug_handler.cmd debug_handler.o -o debug_handler.out
-arm-none-eabi-objcopy -O binary debug_handler.out debug_handler.bin
-
-#arm-none-eabi-gcc -mbig-endian -c debug_handler.S -o debug_handler_be.o
-#arm-none-eabi-ld -EB -n -Tdebug_handler.cmd debug_handler_be.o -o debug_handler_be.out
-#arm-none-eabi-objcopy -O binary debug_handler_be.out debug_handler_be.bin
diff --git a/src/target/xscale/debug_handler.S b/src/target/xscale/debug_handler.S
deleted file mode 100644
index 66dfa88..0000000
--- a/src/target/xscale/debug_handler.S
+++ /dev/null
@@ -1,716 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2006 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   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/>. *
- ***************************************************************************/
-#include "protocol.h"
-
-    .text
-    .align  4
-
-@ Disable thumb mode
-    .code 32
-
-@ send word to debugger
-.macro m_send_to_debugger reg
-1:
-	mrc p14, 0, r15, c14, c0, 0
-	bvs 1b
-	mcr p14, 0, \reg, c8, c0, 0
-.endm
-
-@ receive word from debugger
-.macro m_receive_from_debugger reg
-1:
-	mrc p14, 0, r15, c14, c0, 0
-	bpl 1b
-	mrc p14, 0, \reg, c9, c0, 0
-.endm
-
-@ save register on debugger, small
-.macro m_small_save_reg reg
-	mov r0, \reg
-	bl send_to_debugger
-.endm
-
-@ save status register on debugger, small
-.macro m_small_save_psr
-	mrs r0, spsr
-	bl send_to_debugger
-.endm
-
-@ wait for all outstanding coprocessor accesses to complete
-.macro m_cpwait
-	mrc p15, 0, r0, c2, c0, 0
-	mov r0, r0
-	sub pc, pc, #4
-.endm
-
-.global reset_handler
-.global undef_handler
-.global swi_handler
-.global prefetch_abort_handler
-.global data_abort_handler
-.global irq_handler
-.global fiq_handler
-
-.section .part1 , "ax"
-
-reset_handler:
-	@ read DCSR
-	mrc p14, 0, r13, c10, c0
-	@ check if global enable bit (GE) is set
-	ands r13, r13, #0x80000000
-
-	bne debug_handler
-
-	@ set global enable bit (GE)
-	mov r13, #0xc0000000
-	mcr p14, 0, r13, c10, c0
-
-debug_handler:
-
-	@ save r0 without modifying other registers
-	m_send_to_debugger r0
-
-	@ save lr (program PC) without branching (use macro)
-	m_send_to_debugger r14
-
-	@ save non-banked registers and spsr (program CPSR)
-	m_small_save_reg r1
-	m_small_save_reg r2
-	m_small_save_reg r3
-	m_small_save_reg r4
-	m_small_save_reg r5
-	m_small_save_reg r6
-	m_small_save_reg r7
-	m_small_save_psr
-
-	mrs r0, spsr
-
-	@ prepare program PSR for debug use (clear Thumb, set I/F to disable interrupts)
-	bic r0, r0, #PSR_T
-	orr r0, r0, #(PSR_I | PSR_F)
-
-	@ examine mode bits
-	and r1, r0, #MODE_MASK
-	cmp r1, #MODE_USR
-
-	bne not_user_mode
-
-	@ replace USR mode with SYS
-	bic r0, r0, #MODE_MASK
-	orr r0, r0, #MODE_SYS
-
-not_user_mode:
-
-	b save_banked_registers
-
-@ command loop
-@ wait for command from debugger, than execute desired function
-get_command:
-	bl receive_from_debugger
-
-	@ 0x0n - register access
-	cmp r0, #0x0
-	beq get_banked_registers
-
-	cmp r0, #0x1
-	beq set_banked_registers
-
-	@ 0x1n - read memory
-	cmp r0, #0x11
-	beq read_byte
-
-	cmp r0, #0x12
-	beq read_half_word
-
-	cmp r0, #0x14
-	beq read_word
-
-	@ 0x2n - write memory
-	cmp r0, #0x21
-	beq write_byte
-
-	cmp r0, #0x22
-	beq write_half_word
-
-	cmp r0, #0x24
-	beq write_word
-
-	@ 0x3n - program execution
-	cmp r0, #0x30
-	beq resume
-
-	cmp r0, #0x31
-	beq resume_w_trace
-
-	@ 0x4n - coprocessor access
-	cmp r0, #0x40
-	beq read_cp_reg
-
-	cmp r0, #0x41
-	beq write_cp_reg
-
-	@ 0x5n - cache and mmu functions
-	cmp r0, #0x50
-	beq clean_d_cache
-
-	cmp r0, #0x51
-	beq invalidate_d_cache
-
-	cmp r0, #0x52
-	beq invalidate_i_cache
-
-	cmp r0, #0x53
-	beq cpwait
-
-	@ 0x6n - misc functions
-	cmp r0, #0x60
-	beq clear_sa
-
-	cmp r0, #0x61
-	beq read_trace_buffer
-
-	cmp r0, #0x62
-	beq clean_trace_buffer
-
-	@ return (back to get_command)
-	b get_command
-
-@ ----
-
-@ resume program execution
-resume:
-	@ restore CPSR (SPSR_dbg)
-	bl receive_from_debugger
-	msr spsr, r0
-
-	@ restore registers (r7 - r0)
-	bl receive_from_debugger @ r7
-	mov r7, r0
-	bl receive_from_debugger @ r6
-	mov r6, r0
-	bl receive_from_debugger @ r5
-	mov r5, r0
-	bl receive_from_debugger @ r4
-	mov r4, r0
-	bl receive_from_debugger @ r3
-	mov r3, r0
-	bl receive_from_debugger @ r2
-	mov r2, r0
-	bl receive_from_debugger @ r1
-	mov r1, r0
-	bl receive_from_debugger @ r0
-
-	@ resume addresss
-	m_receive_from_debugger lr
-
-	@ branch back to application code, restoring CPSR
-	subs pc, lr, #0
-
-@ get banked registers
-@ receive mode bits from host, then run into save_banked_registers to
-
-get_banked_registers:
-	bl receive_from_debugger
-
-@ save banked registers
-@ r0[4:0]: desired mode bits
-save_banked_registers:
-	@ backup CPSR
-	mrs r7, cpsr
-	msr cpsr_c, r0
-	nop
-
-	@ keep current mode bits in r1 for later use
-	and r1, r0, #MODE_MASK
-
-	@ backup banked registers
-	m_send_to_debugger r8
-	m_send_to_debugger r9
-	m_send_to_debugger r10
-	m_send_to_debugger r11
-	m_send_to_debugger r12
-	m_send_to_debugger r13
-	m_send_to_debugger r14
-
-	@ if not in SYS mode (or USR, which we replaced with SYS before)
-	cmp r1, #MODE_SYS
-
-	beq no_spsr_to_save
-
-	@ backup SPSR
-	mrs r0, spsr
-	m_send_to_debugger r0
-
-no_spsr_to_save:
-
-	@ restore CPSR for SDS
-	msr cpsr_c, r7
-	nop
-
-   	@ return
-	b get_command
-
-@ ----
-
-
-@ set banked registers
-@ receive mode bits from host, then run into save_banked_registers to
-
-set_banked_registers:
-	bl receive_from_debugger
-
-@ restore banked registers
-@ r0[4:0]: desired mode bits
-restore_banked_registers:
-	@ backup CPSR
-	mrs r7, cpsr
-	msr cpsr_c, r0
-	nop
-
-	@ keep current mode bits in r1 for later use
-	and r1, r0, #MODE_MASK
-
-	@ set banked registers
-	m_receive_from_debugger r8
-	m_receive_from_debugger r9
-	m_receive_from_debugger r10
-	m_receive_from_debugger r11
-	m_receive_from_debugger r12
-	m_receive_from_debugger r13
-	m_receive_from_debugger r14
-
-	@ if not in SYS mode (or USR, which we replaced with SYS before)
-	cmp r1, #MODE_SYS
-
-	beq no_spsr_to_restore
-
-	@ set SPSR
-	m_receive_from_debugger r0
-	msr spsr, r0
-
-no_spsr_to_restore:
-
-	@ restore CPSR for SDS
-	msr cpsr_c, r7
-	nop
-
-   	@ return
-	b get_command
-
-@ ----
-
-read_byte:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-rb_loop:
-	ldrb r0, [r2], #1
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	bl send_to_debugger
-
-	subs r1, r1, #1
-	bne rb_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-read_half_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-rh_loop:
-	ldrh r0, [r2], #2
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	bl send_to_debugger
-
-	subs r1, r1, #1
-	bne rh_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-read_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-rw_loop:
-	ldr r0, [r2], #4
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	bl send_to_debugger
-
-	subs r1, r1, #1
-	bne rw_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-write_byte:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-wb_loop:
-	bl receive_from_debugger
-	strb r0, [r2], #1
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	subs r1, r1, #1
-	bne wb_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-write_half_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-wh_loop:
-	bl receive_from_debugger
-	strh r0, [r2], #2
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	subs r1, r1, #1
-	bne wh_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-write_word:
-	@ r2: address
-	bl receive_from_debugger
-	mov r2, r0
-
-	@ r1: count
-	bl receive_from_debugger
-	mov r1, r0
-
-ww_loop:
-	bl receive_from_debugger
-	str r0, [r2], #4
-
-	@ drain write- (and fill-) buffer to work around XScale errata
-	mcr p15, 0, r8, c7, c10, 4
-
-	subs r1, r1, #1
-	bne ww_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-clear_sa:
-	@ read DCSR
-	mrc p14, 0, r0, c10, c0
-
-	@ clear SA bit
-	bic r0, r0, #0x20
-
-	@ write DCSR
-	mcr p14, 0, r0, c10, c0
-
-	@ return
-	b get_command
-
-@ ----
-
-clean_d_cache:
-	@ r0: cache clean area
-	bl receive_from_debugger
-
-	mov r1, #1024
-clean_loop:
-	mcr p15, 0, r0, c7, c2, 5
-	add r0, r0, #32
-	subs r1, r1, #1
-	bne clean_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-invalidate_d_cache:
-	mcr p15, 0, r0, c7, c6, 0
-
-	@ return
-	b get_command
-
-@ ----
-
-invalidate_i_cache:
-	mcr p15, 0, r0, c7, c5, 0
-
-	@ return
-	b get_command
-
-@ ----
-
-cpwait:
-	m_cpwait
-
-	@return
-	b get_command
-
-@ ----
-
-.section .part2 , "ax"
-
-read_cp_reg:
-	@ requested cp register
-	bl receive_from_debugger
-
-	adr r1, read_cp_table
-	add pc, r1, r0, lsl #3
-
-read_cp_table:
-	mrc p15, 0, r0, c0, c0, 0  @ XSCALE_MAINID
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c0, c0, 1  @ XSCALE_CACHETYPE
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c1, c0, 0  @ XSCALE_CTRL
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c1, c0, 1  @ XSCALE_AUXCTRL
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c2, c0, 0  @ XSCALE_TTB
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c3, c0, 0  @ XSCALE_DAC
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c5, c0, 0  @ XSCALE_FSR
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c6, c0, 0  @ XSCALE_FAR
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c13, c0, 0  @ XSCALE_PID
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c15, c0, 0  @ XSCALE_CP_ACCESS
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c8, 0  @ XSCALE_IBCR0
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c9, 0  @ XSCALE_IBCR1
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c0, 0  @ XSCALE_DBR0
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c3, 0  @ XSCALE_DBR1
-	b read_cp_reg_reply
-	mrc p15, 0, r0, c14, c4, 0  @ XSCALE_DBCON
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1
-	b read_cp_reg_reply
-	mrc p14, 0, r0, c10, c0, 0 @ XSCALE_DCSR
-	b read_cp_reg_reply
-
-read_cp_reg_reply:
-	bl send_to_debugger
-
-	@ return
-	b get_command
-
-@ ----
-
-write_cp_reg:
-	@ requested cp register
-	bl receive_from_debugger
-	mov r1, r0
-
-	@ value to be written
-	bl receive_from_debugger
-
-	adr r2, write_cp_table
-	add pc, r2, r1, lsl #3
-
-write_cp_table:
-	mcr p15, 0, r0, c0, c0, 0  @ XSCALE_MAINID (0x0)
-	b get_command
-	mcr p15, 0, r0, c0, c0, 1  @ XSCALE_CACHETYPE (0x1)
-	b get_command
-	mcr p15, 0, r0, c1, c0, 0  @ XSCALE_CTRL (0x2)
-	b get_command
-	mcr p15, 0, r0, c1, c0, 1  @ XSCALE_AUXCTRL (0x3)
-	b get_command
-	mcr p15, 0, r0, c2, c0, 0  @ XSCALE_TTB (0x4)
-	b get_command
-	mcr p15, 0, r0, c3, c0, 0  @ XSCALE_DAC (0x5)
-	b get_command
-	mcr p15, 0, r0, c5, c0, 0  @ XSCALE_FSR (0x6)
-	b get_command
-	mcr p15, 0, r0, c6, c0, 0  @ XSCALE_FAR (0x7)
-	b get_command
-	mcr p15, 0, r0, c13, c0, 0  @ XSCALE_PID (0x8)
-	b get_command
-	mcr p15, 0, r0, c15, c0, 0  @ XSCALE_CP_ACCESS (0x9)
-	b get_command
-	mcr p15, 0, r0, c14, c8, 0  @ XSCALE_IBCR0 (0xa)
-	b get_command
-	mcr p15, 0, r0, c14, c9, 0  @ XSCALE_IBCR1 (0xb)
-	b get_command
-	mcr p15, 0, r0, c14, c0, 0  @ XSCALE_DBR0 (0xc)
-	b get_command
-	mcr p15, 0, r0, c14, c3, 0  @ XSCALE_DBR1 (0xd)
-	b get_command
-	mcr p15, 0, r0, c14, c4, 0  @ XSCALE_DBCON (0xe)
-	b get_command
-	mcr p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG (0xf)
-	b get_command
-	mcr p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 (0x10)
-	b get_command
-	mcr p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 (0x11)
-	b get_command
-	mcr p14, 0, r0, c10, c0, 0 @ XSCALE_DCSR (0x12)
-	b get_command
-
-@ ----
-
-read_trace_buffer:
-
-	@ dump 256 entries from trace buffer
-	mov	r1, #256
-read_tb_loop:
-	mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG
-	bl send_to_debugger
-	subs r1, r1, #1
-	bne read_tb_loop
-
-	@ dump checkpoint register 0
-	mrc p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 (0x10)
-	bl send_to_debugger
-
-	@ dump checkpoint register 1
-	mrc p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 (0x11)
-	bl send_to_debugger
-
-	@ return
-	b get_command
-
-@ ----
-
-clean_trace_buffer:
-
-	@ clean 256 entries from trace buffer
-	mov	r1, #256
-clean_tb_loop:
-	mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG
-	subs r1, r1, #1
-	bne clean_tb_loop
-
-	@ return
-	b get_command
-
-@ ----
-
-
-@ resume program execution with trace buffer enabled
-resume_w_trace:
-	@ restore CPSR (SPSR_dbg)
-	bl receive_from_debugger
-	msr spsr, r0
-
-	@ restore registers (r7 - r0)
-	bl receive_from_debugger @ r7
-	mov r7, r0
-	bl receive_from_debugger @ r6
-	mov r6, r0
-	bl receive_from_debugger @ r5
-	mov r5, r0
-	bl receive_from_debugger @ r4
-	mov r4, r0
-	bl receive_from_debugger @ r3
-	mov r3, r0
-	bl receive_from_debugger @ r2
-	mov r2, r0
-	bl receive_from_debugger @ r1
-	mov r1, r0
-	bl receive_from_debugger @ r0
-
-	@ resume addresss
-	m_receive_from_debugger lr
-
-	mrc p14, 0, r13, c10, c0, 0 @ XSCALE_DCSR
-	orr r13, r13, #1
-	mcr p14, 0, r13, c10, c0, 0 @ XSCALE_DCSR
-
-	@ branch back to application code, restoring CPSR
-	subs pc, lr, #0
-
-undef_handler:
-swi_handler:
-prefetch_abort_handler:
-data_abort_handler:
-irq_handler:
-fiq_handler:
-1:
-	b 1b
-
-send_to_debugger:
-	m_send_to_debugger r0
-	mov pc, lr
-
-receive_from_debugger:
-	m_receive_from_debugger r0
-	mov pc, lr
-
diff --git a/src/target/xscale/debug_handler.bin b/src/target/xscale/debug_handler.bin
deleted file mode 100755
index 2dde185..0000000
Binary files a/src/target/xscale/debug_handler.bin and /dev/null differ
diff --git a/src/target/xscale/debug_handler.cmd b/src/target/xscale/debug_handler.cmd
deleted file mode 100644
index d943b13..0000000
--- a/src/target/xscale/debug_handler.cmd
+++ /dev/null
@@ -1,49 +0,0 @@
-/* identify the Entry Point  */
-ENTRY(reset_handler)
-
-/* specify the mini-ICache memory areas  */
-MEMORY
-{
-	mini_icache_0 (x)  				: ORIGIN = 0x0, LENGTH = 1024	/* first part of mini icache (sets 0-31) */
-	mini_icache_1 (x)  				: ORIGIN = 0x400, LENGTH = 1024	/* second part of mini icache (sets 0-31) */
-}
-
-/* now define the output sections  */
-SECTIONS
-{
-	.part1 :
-	{
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		*(.part1)
-	} >mini_icache_0
-
-	.part2 :
-	{
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		LONG(0)
-		*(.part2)
-		FILL(0x0)
-	} >mini_icache_1
-
-	/DISCARD/ :
-	{
-		*(.text)
-		*(.glue_7)
-		*(.glue_7t)
-		*(.data)
-		*(.bss)
-	}
-}
diff --git a/src/target/xscale/protocol.h b/src/target/xscale/protocol.h
deleted file mode 100644
index cb01655..0000000
--- a/src/target/xscale/protocol.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2006 by Dominic Rath                                    *
- *   Dominic.Rath@gmx.de                                                   *
- *                                                                         *
- *   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/>. *
- ***************************************************************************/
-
-#define REG_R0 0
-#define REG_R1 1
-#define REG_R2 2
-#define REG_R3 3
-#define REG_R4 4
-#define REG_R5 5
-#define REG_R6 6
-#define REG_R7 7
-#define REG_R8 8
-#define REG_R9 9
-#define REG_R10 10
-#define REG_R11 11
-#define REG_R12 12
-#define REG_R13 13
-#define REG_R14 14
-#define REG_R15 15
-#define REG_CPSR 16
-#define REG_SPSR 17
-
-#define MODE_USR 0x10
-#define MODE_FIQ 0x11
-#define MODE_IRQ 0x12
-#define MODE_SVC 0x13
-#define MODE_ABT 0x17
-#define MODE_UND 0x1b
-#define MODE_SYS 0x1f
-
-#define MODE_ANY 0x40
-#define MODE_CURRENT 0x80
-
-#define MODE_MASK 0x1f
-#define PSR_I 0x80
-#define PSR_F 0x40
-#define PSR_T 0x20
-
-#define XSCALE_DBG_MAINID 0x0
-#define XSCALE_DBG_CACHETYPE 0x1
-#define XSCALE_DBG_CTRL 0x2
-#define XSCALE_DBG_AUXCTRL 0x3
-#define XSCALE_DBG_TTB 0x4
-#define XSCALE_DBG_DAC 0x5
-#define XSCALE_DBG_FSR 0x6
-#define XSCALE_DBG_FAR 0x7
-#define XSCALE_DBG_PID 0x8
-#define XSCALE_DBG_CPACCESS 0x9
-#define XSCALE_DBG_IBCR0 0xa
-#define XSCALE_DBG_IBCR1 0xb
-#define XSCALE_DBG_DBR0 0xc
-#define XSCALE_DBG_DBR1 0xd
-#define XSCALE_DBG_DBCON 0xe
-- 
cgit v1.1


From 50e2253ab9433e5ff2d01a7338477a44e7cf8a6f Mon Sep 17 00:00:00 2001
From: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Date: Sat, 13 Aug 2016 10:42:30 +0200
Subject: target: Replace malloc+manual zeroing with calloc

Change-Id: I3c782c34b59cb36b8ca1d36e9c804c67bae5cb45
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/3667
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/target.c | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index 17902a5..c8a3ce4 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -5408,14 +5408,7 @@ static int target_create(Jim_GetOptInfo *goi)
 	target->halt_issued			= false;
 
 	/* initialize trace information */
-	target->trace_info = malloc(sizeof(struct trace));
-	target->trace_info->num_trace_points         = 0;
-	target->trace_info->trace_points_size        = 0;
-	target->trace_info->trace_points             = NULL;
-	target->trace_info->trace_history_size       = 0;
-	target->trace_info->trace_history            = NULL;
-	target->trace_info->trace_history_pos        = 0;
-	target->trace_info->trace_history_overflowed = 0;
+	target->trace_info = calloc(1, sizeof(struct trace));
 
 	target->dbgmsg          = NULL;
 	target->dbg_msg_enabled = 0;
-- 
cgit v1.1


From 702b150a6bf92fe56cf0cea72ccfad2d06eb64bc Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Sun, 13 Nov 2016 15:02:55 +0300
Subject: target: LS1_SAP: fix "declaration of 'read' shadows a global" warning

Change-Id: I7070193819cc134f9fe1427c20a11160ec415ccb
Reported-by: Jens Bauer <jens-lists@gpio.dk>
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3877
Tested-by: jenkins
---
 src/target/ls1_sap.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'src/target')

diff --git a/src/target/ls1_sap.c b/src/target/ls1_sap.c
index 944e725..b4d814b 100644
--- a/src/target/ls1_sap.c
+++ b/src/target/ls1_sap.c
@@ -127,7 +127,7 @@ static void ls1_sap_set_addr_high(struct jtag_tap *tap, uint16_t addr_high)
 }
 
 static void ls1_sap_memory_cmd(struct jtag_tap *tap, uint32_t address,
-			       int32_t size, int read)
+			       int32_t size, bool rnw)
 {
 	struct scan_field field;
 	uint8_t cmd[8];
@@ -138,7 +138,7 @@ static void ls1_sap_memory_cmd(struct jtag_tap *tap, uint32_t address,
 	field.out_value = cmd;
 	buf_set_u64(cmd, 0, 9, 0);
 	buf_set_u64(cmd, 9, 3, size);
-	buf_set_u64(cmd, 12, 1, !!read);
+	buf_set_u64(cmd, 12, 1, rnw);
 	buf_set_u64(cmd, 13, 3, 0);
 	buf_set_u64(cmd, 16, 32, address);
 	buf_set_u64(cmd, 48, 16, 0);
@@ -190,7 +190,7 @@ static int ls1_sap_read_memory(struct target *target, uint32_t address,
 	ls1_sap_set_addr_high(target->tap, 0);
 
 	while (count--) {
-		ls1_sap_memory_cmd(target->tap, address, size, 1);
+		ls1_sap_memory_cmd(target->tap, address, size, true);
 		ls1_sap_memory_read(target->tap, size, buffer);
 		address += size;
 		buffer += size;
@@ -213,7 +213,7 @@ static int ls1_sap_write_memory(struct target *target, uint32_t address,
 	ls1_sap_set_addr_high(target->tap, 0);
 
 	while (count--) {
-		ls1_sap_memory_cmd(target->tap, address, size, 0);
+		ls1_sap_memory_cmd(target->tap, address, size, false);
 		ls1_sap_memory_write(target->tap, size, buffer);
 		address += size;
 		buffer += size;
-- 
cgit v1.1


From 5be455a710c57bbbbd49c2d671b42098db7be5dc Mon Sep 17 00:00:00 2001
From: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Date: Sun, 6 Nov 2016 20:19:26 +0100
Subject: Convert to non-recursive make

Change-Id: I11f8bc8553957e2ff083c09e72e16881e4d3bb6f
Signed-off-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Reviewed-on: http://openocd.zylin.com/3865
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/Makefile.am          | 312 ++++++++++++++++++++--------------------
 src/target/breakpoints.h        |   2 +
 src/target/openrisc/Makefile.am |  30 ++--
 3 files changed, 167 insertions(+), 177 deletions(-)

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index dc7973d..eb0d62e 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -1,18 +1,15 @@
-include $(top_srcdir)/common.mk
-
 if OOCD_TRACE
-OOCD_TRACE_FILES = oocd_trace.c
+OOCD_TRACE_FILES = %D%/oocd_trace.c
 else
 OOCD_TRACE_FILES =
 endif
 
-SUBDIRS = openrisc
-libtarget_la_LIBADD = $(top_builddir)/src/target/openrisc/libopenrisc.la
+%C%_libtarget_la_LIBADD = %D%/openrisc/libopenrisc.la
 
+STARTUP_TCL_SRCS += %D%/startup.tcl
 
-METASOURCES = AUTO
-noinst_LTLIBRARIES = libtarget.la
-libtarget_la_SOURCES = \
+noinst_LTLIBRARIES += %D%/libtarget.la
+%C%_libtarget_la_SOURCES = \
 	$(TARGET_CORE_SRC) \
 	$(ARM_DEBUG_SRC) \
 	$(ARMV4_5_SRC) \
@@ -23,178 +20,175 @@ libtarget_la_SOURCES = \
 	$(MIPS32_SRC) \
 	$(NDS32_SRC) \
 	$(INTEL_IA32_SRC) \
-	avrt.c \
-	dsp563xx.c \
-	dsp563xx_once.c \
-	dsp5680xx.c \
-	hla_target.c
+	%D%/avrt.c \
+	%D%/dsp563xx.c \
+	%D%/dsp563xx_once.c \
+	%D%/dsp5680xx.c \
+	%D%/hla_target.c
 
 TARGET_CORE_SRC = \
-	algorithm.c \
-	register.c \
-	image.c \
-	breakpoints.c \
-	target.c \
-	target_request.c \
-	testee.c \
-	smp.c
+	%D%/algorithm.c \
+	%D%/register.c \
+	%D%/image.c \
+	%D%/breakpoints.c \
+	%D%/target.c \
+	%D%/target_request.c \
+	%D%/testee.c \
+	%D%/smp.c
 
 ARMV4_5_SRC = \
-	armv4_5.c \
-	armv4_5_mmu.c \
-	armv4_5_cache.c \
+	%D%/armv4_5.c \
+	%D%/armv4_5_mmu.c \
+	%D%/armv4_5_cache.c \
 	$(ARM7_9_SRC)
 
 ARM7_9_SRC = \
-	arm7_9_common.c \
-	arm7tdmi.c \
-	arm720t.c \
-	arm9tdmi.c \
-	arm920t.c \
-	arm966e.c \
-	arm946e.c \
-	arm926ejs.c \
-	feroceon.c
+	%D%/arm7_9_common.c \
+	%D%/arm7tdmi.c \
+	%D%/arm720t.c \
+	%D%/arm9tdmi.c \
+	%D%/arm920t.c \
+	%D%/arm966e.c \
+	%D%/arm946e.c \
+	%D%/arm926ejs.c \
+	%D%/feroceon.c
 
 ARM_MISC_SRC = \
-	fa526.c \
-	xscale.c
+	%D%/fa526.c \
+	%D%/xscale.c
 
 ARMV6_SRC = \
-	arm11.c \
-	arm11_dbgtap.c
+	%D%/arm11.c \
+	%D%/arm11_dbgtap.c
 
 ARMV7_SRC = \
-	armv7m.c \
-	armv7m_trace.c \
-	cortex_m.c \
-	armv7a.c \
-	cortex_a.c \
-	ls1_sap.c
+	%D%/armv7m.c \
+	%D%/armv7m_trace.c \
+	%D%/cortex_m.c \
+	%D%/armv7a.c \
+	%D%/cortex_a.c \
+	%D%/ls1_sap.c
 
 ARM_DEBUG_SRC = \
-	arm_dpm.c \
-	arm_jtag.c \
-	arm_disassembler.c \
-	arm_simulator.c \
-	arm_semihosting.c \
-	arm_adi_v5.c \
-	armv7a_cache.c \
-	armv7a_cache_l2x.c \
-	adi_v5_jtag.c \
-	adi_v5_swd.c \
-	embeddedice.c \
-	trace.c \
-	etb.c \
-	etm.c \
+	%D%/arm_dpm.c \
+	%D%/arm_jtag.c \
+	%D%/arm_disassembler.c \
+	%D%/arm_simulator.c \
+	%D%/arm_semihosting.c \
+	%D%/arm_adi_v5.c \
+	%D%/armv7a_cache.c \
+	%D%/armv7a_cache_l2x.c \
+	%D%/adi_v5_jtag.c \
+	%D%/adi_v5_swd.c \
+	%D%/embeddedice.c \
+	%D%/trace.c \
+	%D%/etb.c \
+	%D%/etm.c \
 	$(OOCD_TRACE_FILES) \
-	etm_dummy.c
+	%D%/etm_dummy.c
 
 AVR32_SRC = \
-	avr32_ap7k.c \
-	avr32_jtag.c \
-	avr32_mem.c \
-	avr32_regs.c
+	%D%/avr32_ap7k.c \
+	%D%/avr32_jtag.c \
+	%D%/avr32_mem.c \
+	%D%/avr32_regs.c
 
 MIPS32_SRC = \
-	mips32.c \
-	mips_m4k.c \
-	mips32_pracc.c \
-	mips32_dmaacc.c \
-	mips_ejtag.c
+	%D%/mips32.c \
+	%D%/mips_m4k.c \
+	%D%/mips32_pracc.c \
+	%D%/mips32_dmaacc.c \
+	%D%/mips_ejtag.c
 
 NDS32_SRC = \
-	nds32.c \
-	nds32_reg.c \
-	nds32_cmd.c \
-	nds32_disassembler.c \
-	nds32_tlb.c \
-	nds32_v2.c \
-	nds32_v3_common.c \
-	nds32_v3.c \
-	nds32_v3m.c \
-	nds32_aice.c
+	%D%/nds32.c \
+	%D%/nds32_reg.c \
+	%D%/nds32_cmd.c \
+	%D%/nds32_disassembler.c \
+	%D%/nds32_tlb.c \
+	%D%/nds32_v2.c \
+	%D%/nds32_v3_common.c \
+	%D%/nds32_v3.c \
+	%D%/nds32_v3m.c \
+	%D%/nds32_aice.c
 
 INTEL_IA32_SRC = \
-	quark_x10xx.c \
-	quark_d20xx.c \
-	lakemont.c \
-	x86_32_common.c
-
-noinst_HEADERS = \
-	algorithm.h \
-	arm.h \
-	arm_dpm.h \
-	arm_jtag.h \
-	arm_adi_v5.h \
-	armv7a_cache.h \
-	armv7a_cache_l2x.h \
-	arm_disassembler.h \
-	arm_opcodes.h \
-	arm_simulator.h \
-	arm_semihosting.h \
-	arm7_9_common.h \
-	arm7tdmi.h \
-	arm720t.h \
-	arm9tdmi.h \
-	arm920t.h \
-	arm926ejs.h \
-	arm966e.h \
-	arm946e.h \
-	arm11.h \
-	arm11_dbgtap.h \
-	armv4_5.h \
-	armv4_5_mmu.h \
-	armv4_5_cache.h \
-	armv7a.h \
-	armv7m.h \
-	armv7m_trace.h \
-	avrt.h \
-	dsp563xx.h \
-	dsp563xx_once.h \
-	dsp5680xx.h \
-	breakpoints.h \
-	cortex_m.h \
-	cortex_a.h \
-	embeddedice.h \
-	etb.h \
-	etm.h \
-	etm_dummy.h \
-	image.h \
-	mips32.h \
-	mips_m4k.h \
-	mips_ejtag.h \
-	mips32_pracc.h \
-	mips32_dmaacc.h \
-	oocd_trace.h \
-	register.h \
-	target.h \
-	target_type.h \
-	trace.h \
-	target_request.h \
-	trace.h \
-	xscale.h \
-	smp.h \
-	avr32_ap7k.h \
-	avr32_jtag.h \
-	avr32_mem.h \
-	avr32_regs.h \
-	nds32.h \
-	nds32_cmd.h \
-	nds32_disassembler.h \
-	nds32_edm.h \
-	nds32_insn.h \
-	nds32_reg.h \
-	nds32_tlb.h \
-	nds32_v2.h \
-	nds32_v3_common.h \
-	nds32_v3.h \
-	nds32_v3m.h \
-	nds32_aice.h \
-	lakemont.h \
-	x86_32_common.h
-
-ocddatadir = $(pkglibdir)
-nobase_dist_ocddata_DATA =
-
-MAINTAINERCLEANFILES = $(srcdir)/Makefile.in
+	%D%/quark_x10xx.c \
+	%D%/quark_d20xx.c \
+	%D%/lakemont.c \
+	%D%/x86_32_common.c
+
+%C%_libtarget_la_SOURCES += \
+	%D%/algorithm.h \
+	%D%/arm.h \
+	%D%/arm_dpm.h \
+	%D%/arm_jtag.h \
+	%D%/arm_adi_v5.h \
+	%D%/armv7a_cache.h \
+	%D%/armv7a_cache_l2x.h \
+	%D%/arm_disassembler.h \
+	%D%/arm_opcodes.h \
+	%D%/arm_simulator.h \
+	%D%/arm_semihosting.h \
+	%D%/arm7_9_common.h \
+	%D%/arm7tdmi.h \
+	%D%/arm720t.h \
+	%D%/arm9tdmi.h \
+	%D%/arm920t.h \
+	%D%/arm926ejs.h \
+	%D%/arm966e.h \
+	%D%/arm946e.h \
+	%D%/arm11.h \
+	%D%/arm11_dbgtap.h \
+	%D%/armv4_5.h \
+	%D%/armv4_5_mmu.h \
+	%D%/armv4_5_cache.h \
+	%D%/armv7a.h \
+	%D%/armv7m.h \
+	%D%/armv7m_trace.h \
+	%D%/avrt.h \
+	%D%/dsp563xx.h \
+	%D%/dsp563xx_once.h \
+	%D%/dsp5680xx.h \
+	%D%/breakpoints.h \
+	%D%/cortex_m.h \
+	%D%/cortex_a.h \
+	%D%/embeddedice.h \
+	%D%/etb.h \
+	%D%/etm.h \
+	%D%/etm_dummy.h \
+	%D%/image.h \
+	%D%/mips32.h \
+	%D%/mips_m4k.h \
+	%D%/mips_ejtag.h \
+	%D%/mips32_pracc.h \
+	%D%/mips32_dmaacc.h \
+	%D%/oocd_trace.h \
+	%D%/register.h \
+	%D%/target.h \
+	%D%/target_type.h \
+	%D%/trace.h \
+	%D%/target_request.h \
+	%D%/trace.h \
+	%D%/xscale.h \
+	%D%/smp.h \
+	%D%/avr32_ap7k.h \
+	%D%/avr32_jtag.h \
+	%D%/avr32_mem.h \
+	%D%/avr32_regs.h \
+	%D%/nds32.h \
+	%D%/nds32_cmd.h \
+	%D%/nds32_disassembler.h \
+	%D%/nds32_edm.h \
+	%D%/nds32_insn.h \
+	%D%/nds32_reg.h \
+	%D%/nds32_tlb.h \
+	%D%/nds32_v2.h \
+	%D%/nds32_v3_common.h \
+	%D%/nds32_v3.h \
+	%D%/nds32_v3m.h \
+	%D%/nds32_aice.h \
+	%D%/lakemont.h \
+	%D%/x86_32_common.h
+
+include %D%/openrisc/Makefile.am
diff --git a/src/target/breakpoints.h b/src/target/breakpoints.h
index 6e260ab..842fc18 100644
--- a/src/target/breakpoints.h
+++ b/src/target/breakpoints.h
@@ -19,6 +19,8 @@
 #ifndef OPENOCD_TARGET_BREAKPOINTS_H
 #define OPENOCD_TARGET_BREAKPOINTS_H
 
+#include <stdint.h>
+
 struct target;
 
 enum breakpoint_type {
diff --git a/src/target/openrisc/Makefile.am b/src/target/openrisc/Makefile.am
index b00a30d..5a2549a 100644
--- a/src/target/openrisc/Makefile.am
+++ b/src/target/openrisc/Makefile.am
@@ -1,18 +1,12 @@
-include $(top_srcdir)/common.mk
-
-noinst_LTLIBRARIES = libopenrisc.la
-libopenrisc_la_SOURCES = $(OPENRISC_SRC)
-
-OPENRISC_SRC = \
-	or1k.c \
-	or1k_du_adv.c \
-	or1k_tap_mohor.c \
-	or1k_tap_vjtag.c \
-	or1k_tap_xilinx_bscan.c \
-	jsp_server.c
-
-noinst_HEADERS = \
-	or1k.h \
-	or1k_du.h \
-	or1k_tap.h \
-	jsp_server.h
+noinst_LTLIBRARIES += %D%/libopenrisc.la
+%C%_libopenrisc_la_SOURCES = \
+	%D%/or1k.c \
+	%D%/or1k_du_adv.c \
+	%D%/or1k_tap_mohor.c \
+	%D%/or1k_tap_vjtag.c \
+	%D%/or1k_tap_xilinx_bscan.c \
+	%D%/jsp_server.c \
+	%D%/or1k.h \
+	%D%/or1k_du.h \
+	%D%/or1k_tap.h \
+	%D%/jsp_server.h
-- 
cgit v1.1


From d491b88dd94afb7c796dbe89ea52aecdefebf1cc Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Mon, 12 Dec 2016 11:50:17 +0300
Subject: target: cortex_a: fix segfault when SPSR is not properly handled

OpenOCD doesn't (yet) know how to handle HYP mode properly so spsr
register is not getting initialised when OpenOCD connects to a target
stopped in this mode.

Reported on IRC by thinkfat and nearffxx.

Change-Id: I4bda9ba0c582c8e9cacefe708cc4a3d947151f84
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3906
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-by: Chengyu Zheng <chengyu.zheng@polimi.it>
---
 src/target/cortex_a.c | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

(limited to 'src/target')

diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index d0eac75..8c8a2b7 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -1241,16 +1241,18 @@ static int cortex_a_debug_entry(struct target *target)
 		reg->dirty = reg->valid;
 	}
 
-	/* read Saved PSR */
-	retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
-	/*  store current spsr */
-	if (retval != ERROR_OK)
-		return retval;
+	if (arm->spsr) {
+		/* read Saved PSR */
+		retval = cortex_a_dap_read_coreregister_u32(target, &spsr, 17);
+		/*  store current spsr */
+		if (retval != ERROR_OK)
+			return retval;
 
-	reg = arm->spsr;
-	buf_set_u32(reg->value, 0, 32, spsr);
-	reg->valid = 1;
-	reg->dirty = 0;
+		reg = arm->spsr;
+		buf_set_u32(reg->value, 0, 32, spsr);
+		reg->valid = 1;
+		reg->dirty = 0;
+	}
 
 #if 0
 /* TODO, Move this */
-- 
cgit v1.1


From 6f34e64b09b2a61156e837b3077dae619c60f2ea Mon Sep 17 00:00:00 2001
From: Paul Fertser <fercerpav@gmail.com>
Date: Tue, 29 Nov 2016 21:14:15 +0300
Subject: target: armv7a: remove unused level_num field from armv7a_cachesize

This was never used and produces warnings on some systems.

Change-Id: I48d2c5b79890bb2d70c5fae95278b8eb62743398
Reported-by: Tommy Murphy <tm1234@users.sf.net>
Signed-off-by: Paul Fertser <fercerpav@gmail.com>
Reviewed-on: http://openocd.zylin.com/3891
Tested-by: jenkins
---
 src/target/armv7a.h | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/armv7a.h b/src/target/armv7a.h
index 6a62f72..c5c03c8 100644
--- a/src/target/armv7a.h
+++ b/src/target/armv7a.h
@@ -48,7 +48,6 @@ struct armv7a_l2x_cache {
 };
 
 struct armv7a_cachesize {
-	uint32_t level_num;
 	/*  cache dimensionning */
 	uint32_t linelen;
 	uint32_t associativity;
-- 
cgit v1.1


From 2934ac454e32b21b1bd624189745d9f21ee79b46 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 12 Jan 2017 17:06:12 +0100
Subject: adi_v5_jtag: make sure SSTICKYERR is cleared after a POR

Don't terminate the transaction end-check early if debug power-loss
was detected, without clearing SSTICKYERR.

Change-Id: I83b6a4a20523eea42e48a15297f972a730aa21a8
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3947
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/adi_v5_jtag.c | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index 2717c9e..c7dc4f7 100644
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -574,8 +574,6 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 		if ((ctrlstat & (CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) !=
 						(CDBGPWRUPREQ | CDBGPWRUPACK | CSYSPWRUPREQ | CSYSPWRUPACK)) {
 			LOG_ERROR("Debug regions are unpowered, an unexpected reset might have happened");
-			retval = ERROR_JTAG_DEVICE_ERROR;
-			goto done;
 		}
 
 		if (ctrlstat & SSTICKYERR)
@@ -590,10 +588,7 @@ static int jtagdp_transaction_endcheck(struct adiv5_dap *dap)
 		if (retval != ERROR_OK)
 			goto done;
 
-		if (ctrlstat & SSTICKYERR) {
-			retval = ERROR_JTAG_DEVICE_ERROR;
-			goto done;
-		}
+		retval = ERROR_JTAG_DEVICE_ERROR;
 	}
 
  done:
-- 
cgit v1.1


From 59820c18d0d8be93344b00a10a2a4e56601fb094 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 19 Jan 2017 16:57:04 +0100
Subject: arm_dpm: avoid duplicating the register cache

This bug was already attempted to fix in an earlier patch but
merging the "defer-examine" feature caused a regression, which this patch
tries to fix again.

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

(limited to 'src/target')

diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 8ad6575..62c6175 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -972,11 +972,14 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	arm->read_core_reg = arm_dpm_read_core_reg;
 	arm->write_core_reg = arm_dpm_write_core_reg;
 
-	cache = arm_build_reg_cache(target, arm);
-	if (!cache)
-		return ERROR_FAIL;
+	/* avoid duplicating the register cache */
+	if (arm->core_cache == NULL) {
+		cache = arm_build_reg_cache(target, arm);
+		if (!cache)
+			return ERROR_FAIL;
 
-	*register_get_last_cache_p(&target->reg_cache) = cache;
+		*register_get_last_cache_p(&target->reg_cache) = cache;
+	}
 
 	/* coprocessor access setup */
 	arm->mrc = dpm_mrc;
-- 
cgit v1.1


From 47b8cf84202bf792cf66fbfa01169e9592236b8a Mon Sep 17 00:00:00 2001
From: Dongxue Zhang <elta.era@gmail.com>
Date: Mon, 23 Sep 2013 16:27:03 +0800
Subject: target: Add 64-bit target address support
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Define a target_addr_t type to support 32-bit and 64-bit addresses at
the same time. Also define matching TARGET_PRI*ADDR format macros as
well as a convenient TARGET_ADDR_FMT.

In targets that are 32-bit (avr32, nds32, arm7/9/11, fm4, xmc1000)
be least invasive by leaving the formatting unchanged apart from the
type;
for generic code adopt TARGET_ADDR_FMT as unified address format.

Don't silently change gdb formatting here, leave that to later.

Add COMMAND_PARSE_ADDRESS() macro to abstract the address type.
Implement it using its own parse_target_addr() function, in the hopes
of catching pointer type mismatches better.

Add '--disable-target64' configure option to revert to previous 32-bit
target address behavior.

Change-Id: I2e91d205862ceb14f94b3e72a7e99ee0373a85d5
Signed-off-by: Dongxue Zhang <elta.era@gmail.com>
Signed-off-by: David Ung <david.ung.42@gmail.com>
[AF: Default to enabling (Paul Fertser), rename macros, simplify]
Signed-off-by: Andreas Färber <afaerber@suse.de>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/algorithm.h        |   2 +-
 src/target/arm.h              |   6 +-
 src/target/arm11.c            |  22 +--
 src/target/arm720t.c          |   8 +-
 src/target/arm7_9_common.c    |  37 ++---
 src/target/arm7_9_common.h    |  16 +--
 src/target/arm920t.c          |  10 +-
 src/target/arm920t.h          |   4 +-
 src/target/arm926ejs.c        |  10 +-
 src/target/arm926ejs.h        |   2 +-
 src/target/arm946e.c          |   4 +-
 src/target/armv4_5.c          |  16 +--
 src/target/armv4_5_mmu.h      |   5 +-
 src/target/armv7a.h           |   2 +-
 src/target/armv7a_cache_l2x.c |  46 +++----
 src/target/armv7a_cache_l2x.h |   2 +-
 src/target/armv7m.c           |  12 +-
 src/target/armv7m.h           |  10 +-
 src/target/avr32_ap7k.c       |  14 +-
 src/target/avrt.c             |   8 +-
 src/target/breakpoints.c      |  43 +++---
 src/target/breakpoints.h      |  19 +--
 src/target/cortex_a.c         |  67 ++++-----
 src/target/cortex_m.c         |  14 +-
 src/target/dsp563xx.c         |  22 +--
 src/target/dsp5680xx.c        |  14 +-
 src/target/feroceon.c         |   4 +-
 src/target/hla_target.c       |  20 +--
 src/target/image.h            |   2 +-
 src/target/lakemont.c         |   4 +-
 src/target/lakemont.h         |   4 +-
 src/target/ls1_sap.c          |  12 +-
 src/target/mips32.c           |  12 +-
 src/target/mips32.h           |   6 +-
 src/target/mips32_pracc.c     |   2 +-
 src/target/mips_m4k.c         |  30 ++--
 src/target/nds32.c            |  20 +--
 src/target/nds32.h            |  14 +-
 src/target/nds32_aice.c       |   4 +-
 src/target/nds32_aice.h       |   4 +-
 src/target/nds32_tlb.c        |   8 +-
 src/target/nds32_tlb.h        |   8 +-
 src/target/nds32_v2.c         |  26 ++--
 src/target/nds32_v3.c         |   8 +-
 src/target/nds32_v3_common.c  |  22 +--
 src/target/nds32_v3_common.h  |  14 +-
 src/target/nds32_v3m.c        |   8 +-
 src/target/openrisc/or1k.c    |  33 +++--
 src/target/target.c           | 309 ++++++++++++++++++++++++++++--------------
 src/target/target.h           |  47 ++++---
 src/target/target_type.h      |  33 +++--
 src/target/x86_32_common.c    |  85 ++++++------
 src/target/x86_32_common.h    |  10 +-
 src/target/xscale.c           |  26 ++--
 54 files changed, 663 insertions(+), 527 deletions(-)

(limited to 'src/target')

diff --git a/src/target/algorithm.h b/src/target/algorithm.h
index d216a82..8894241 100644
--- a/src/target/algorithm.h
+++ b/src/target/algorithm.h
@@ -26,7 +26,7 @@ enum param_direction {
 };
 
 struct mem_param {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t size;
 	uint8_t *value;
 	enum param_direction direction;
diff --git a/src/target/arm.h b/src/target/arm.h
index 226dd65..77a2f7b 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -231,7 +231,7 @@ int arm_init_arch_info(struct target *target, struct arm *arm);
 int armv4_5_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		int timeout_ms, void *arch_info);
 int armv4_5_run_algorithm_inner(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
@@ -242,9 +242,9 @@ int armv4_5_run_algorithm_inner(struct target *target,
 				int timeout_ms, void *arch_info));
 
 int arm_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum);
+		target_addr_t address, uint32_t count, uint32_t *checksum);
 int arm_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 void arm_set_cpsr(struct arm *arm, uint32_t cpsr);
 struct reg *arm_reg_current(struct arm *arm, unsigned regnum);
diff --git a/src/target/arm11.c b/src/target/arm11.c
index cbe4d45..13fbd20 100644
--- a/src/target/arm11.c
+++ b/src/target/arm11.c
@@ -42,7 +42,7 @@
 
 
 static int arm11_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints);
+		target_addr_t address, int handle_breakpoints);
 
 
 /** Check and if necessary take control of the system
@@ -449,7 +449,7 @@ static uint32_t arm11_nextpc(struct arm11_common *arm11, int current, uint32_t a
 }
 
 static int arm11_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	/*	  LOG_DEBUG("current %d  address %08x  handle_breakpoints %d  debug_execution %d", */
 	/*	current, address, handle_breakpoints, debug_execution); */
@@ -467,7 +467,7 @@ static int arm11_resume(struct target *target, int current,
 
 	address = arm11_nextpc(arm11, current, address);
 
-	LOG_DEBUG("RESUME PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("RESUME PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 	/* clear breakpoints/watchpoints and VCR*/
 	CHECK_RETVAL(arm11_sc7_clear_vbw(arm11));
@@ -481,7 +481,7 @@ static int arm11_resume(struct target *target, int current,
 
 		for (bp = target->breakpoints; bp; bp = bp->next) {
 			if (bp->address == address) {
-				LOG_DEBUG("must step over %08" PRIx32 "", bp->address);
+				LOG_DEBUG("must step over %08" TARGET_PRIxADDR "", bp->address);
 				arm11_step(target, 1, 0, 0);
 				break;
 			}
@@ -507,7 +507,7 @@ static int arm11_resume(struct target *target, int current,
 
 			CHECK_RETVAL(arm11_sc7_run(arm11, brp, ARRAY_SIZE(brp)));
 
-			LOG_DEBUG("Add BP %d at %08" PRIx32, brp_num,
+			LOG_DEBUG("Add BP %d at %08" TARGET_PRIxADDR, brp_num,
 				bp->address);
 
 			brp_num++;
@@ -557,7 +557,7 @@ static int arm11_resume(struct target *target, int current,
 }
 
 static int arm11_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	LOG_DEBUG("target->state: %s",
 		target_state_name(target));
@@ -571,7 +571,7 @@ static int arm11_step(struct target *target, int current,
 
 	address = arm11_nextpc(arm11, current, address);
 
-	LOG_DEBUG("STEP PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("STEP PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 
 	/** \todo TODO: Thumb not supported here */
@@ -583,13 +583,13 @@ static int arm11_step(struct target *target, int current,
 	/* skip over BKPT */
 	if ((next_instruction & 0xFFF00070) == 0xe1200070) {
 		address = arm11_nextpc(arm11, 0, address + 4);
-		LOG_DEBUG("Skipping BKPT %08" PRIx32, address);
+		LOG_DEBUG("Skipping BKPT %08" TARGET_PRIxADDR, address);
 	}
 	/* skip over Wait for interrupt / Standby
 	 * mcr	15, 0, r?, cr7, cr0, {4} */
 	else if ((next_instruction & 0xFFFF0FFF) == 0xee070f90) {
 		address = arm11_nextpc(arm11, 0, address + 4);
-		LOG_DEBUG("Skipping WFI %08" PRIx32, address);
+		LOG_DEBUG("Skipping WFI %08" TARGET_PRIxADDR, address);
 	}
 	/* ignore B to self */
 	else if ((next_instruction & 0xFEFFFFFF) == 0xeafffffe)
@@ -887,7 +887,7 @@ static int arm11_read_memory_inner(struct target *target,
 }
 
 static int arm11_read_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -1043,7 +1043,7 @@ static int arm11_write_memory_inner(struct target *target,
 }
 
 static int arm11_write_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
 	/* pointer increment matters only for multi-unit writes ...
diff --git a/src/target/arm720t.c b/src/target/arm720t.c
index 3991e19..bcbfa9d 100644
--- a/src/target/arm720t.c
+++ b/src/target/arm720t.c
@@ -271,7 +271,7 @@ static int arm720_mmu(struct target *target, int *enabled)
 }
 
 static int arm720_virt2phys(struct target *target,
-		uint32_t virtual, uint32_t *physical)
+		target_addr_t virtual, target_addr_t *physical)
 {
 	uint32_t cb;
 	struct arm720t_common *arm720t = target_to_arm720(target);
@@ -286,7 +286,7 @@ static int arm720_virt2phys(struct target *target,
 }
 
 static int arm720t_read_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
 	struct arm720t_common *arm720t = target_to_arm720(target);
@@ -309,7 +309,7 @@ static int arm720t_read_memory(struct target *target,
 }
 
 static int arm720t_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct arm720t_common *arm720t = target_to_arm720(target);
 
@@ -317,7 +317,7 @@ static int arm720t_read_phys_memory(struct target *target,
 }
 
 static int arm720t_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct arm720t_common *arm720t = target_to_arm720(target);
 
diff --git a/src/target/arm7_9_common.c b/src/target/arm7_9_common.c
index c1d5c79..7fd1ed9 100644
--- a/src/target/arm7_9_common.c
+++ b/src/target/arm7_9_common.c
@@ -101,7 +101,8 @@ static void arm7_9_assign_wp(struct arm7_9_common *arm7_9, struct breakpoint *br
 		arm7_9->wp_available--;
 	} else
 		LOG_ERROR("BUG: no hardware comparator available");
-	LOG_DEBUG("BPID: %" PRId32 " (0x%08" PRIx32 ") using hw wp: %d",
+
+	LOG_DEBUG("BPID: %" PRId32 " (0x%08" TARGET_PRIxADDR ") using hw wp: %d",
 			breakpoint->unique_id,
 			breakpoint->address,
 			breakpoint->set);
@@ -187,7 +188,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	int retval = ERROR_OK;
 
-	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" PRIx32 ", Type: %d",
+	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" TARGET_PRIxADDR ", Type: %d",
 		breakpoint->unique_id,
 		breakpoint->address,
 		breakpoint->type);
@@ -244,7 +245,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 			if (retval != ERROR_OK)
 				return retval;
 			if (verify != arm7_9->arm_bkpt) {
-				LOG_ERROR("Unable to set 32 bit software breakpoint at address %08" PRIx32
+				LOG_ERROR("Unable to set 32 bit software breakpoint at address %08" TARGET_PRIxADDR
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -264,7 +265,7 @@ static int arm7_9_set_breakpoint(struct target *target, struct breakpoint *break
 			if (retval != ERROR_OK)
 				return retval;
 			if (verify != arm7_9->thumb_bkpt) {
-				LOG_ERROR("Unable to set thumb software breakpoint at address %08" PRIx32
+				LOG_ERROR("Unable to set thumb software breakpoint at address %08" TARGET_PRIxADDR
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -299,7 +300,7 @@ static int arm7_9_unset_breakpoint(struct target *target, struct breakpoint *bre
 	int retval = ERROR_OK;
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 
-	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" PRIx32,
+	LOG_DEBUG("BPID: %" PRId32 ", Address: 0x%08" TARGET_PRIxADDR,
 		breakpoint->unique_id,
 		breakpoint->address);
 
@@ -1692,7 +1693,7 @@ static void arm7_9_enable_breakpoints(struct target *target)
 
 int arm7_9_resume(struct target *target,
 	int current,
-	uint32_t address,
+	target_addr_t address,
 	int handle_breakpoints,
 	int debug_execution)
 {
@@ -1724,7 +1725,7 @@ int arm7_9_resume(struct target *target,
 		breakpoint = breakpoint_find(target,
 				buf_get_u32(arm->pc->value, 0, 32));
 		if (breakpoint != NULL) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (id: %" PRId32,
+			LOG_DEBUG("unset breakpoint at 0x%8.8" TARGET_PRIxADDR " (id: %" PRId32,
 				breakpoint->address,
 				breakpoint->unique_id);
 			retval = arm7_9_unset_breakpoint(target, breakpoint);
@@ -1783,7 +1784,7 @@ int arm7_9_resume(struct target *target,
 			LOG_DEBUG("new PC after step: 0x%8.8" PRIx32,
 				buf_get_u32(arm->pc->value, 0, 32));
 
-			LOG_DEBUG("set breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("set breakpoint at 0x%8.8" TARGET_PRIxADDR "", breakpoint->address);
 			retval = arm7_9_set_breakpoint(target, breakpoint);
 			if (retval != ERROR_OK)
 				return retval;
@@ -1894,7 +1895,7 @@ void arm7_9_disable_eice_step(struct target *target)
 	embeddedice_store_reg(&arm7_9->eice_cache->reg_list[EICE_W1_CONTROL_VALUE]);
 }
 
-int arm7_9_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
+int arm7_9_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
 {
 	struct arm7_9_common *arm7_9 = target_to_arm7_9(target);
 	struct arm *arm = &arm7_9->arm;
@@ -2094,7 +2095,7 @@ static int arm7_9_write_core_reg(struct target *target, struct reg *r,
 }
 
 int arm7_9_read_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -2109,7 +2110,7 @@ int arm7_9_read_memory(struct target *target,
 	int retval;
 	int last_reg = 0;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		address, size, count);
 
 	if (target->state != TARGET_HALTED) {
@@ -2247,7 +2248,8 @@ int arm7_9_read_memory(struct target *target,
 
 	if (((cpsr & 0x1f) == ARM_MODE_ABT) && (arm->core_mode != ARM_MODE_ABT)) {
 		LOG_WARNING(
-			"memory read caused data abort (address: 0x%8.8" PRIx32 ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
+			"memory read caused data abort "
+			"(address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
 			address,
 			size,
 			count);
@@ -2263,7 +2265,7 @@ int arm7_9_read_memory(struct target *target,
 }
 
 int arm7_9_write_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -2460,7 +2462,8 @@ int arm7_9_write_memory(struct target *target,
 
 	if (((cpsr & 0x1f) == ARM_MODE_ABT) && (arm->core_mode != ARM_MODE_ABT)) {
 		LOG_WARNING(
-			"memory write caused data abort (address: 0x%8.8" PRIx32 ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
+			"memory write caused data abort "
+			"(address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%" PRIx32 ", count: 0x%" PRIx32 ")",
 			address,
 			size,
 			count);
@@ -2476,7 +2479,7 @@ int arm7_9_write_memory(struct target *target,
 }
 
 int arm7_9_write_memory_opt(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -2576,7 +2579,7 @@ static const uint32_t dcc_code[] = {
 };
 
 int arm7_9_bulk_write_memory(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t count,
 	const uint8_t *buffer)
 {
@@ -2632,7 +2635,7 @@ int arm7_9_bulk_write_memory(struct target *target,
 		uint32_t endaddress = buf_get_u32(reg_params[0].value, 0, 32);
 		if (endaddress != (address + count*4)) {
 			LOG_ERROR(
-				"DCC write failed, expected end address 0x%08" PRIx32 " got 0x%0" PRIx32 "",
+				"DCC write failed, expected end address 0x%08" TARGET_PRIxADDR " got 0x%0" PRIx32 "",
 				(address + count*4),
 				endaddress);
 			retval = ERROR_FAIL;
diff --git a/src/target/arm7_9_common.h b/src/target/arm7_9_common.h
index 044384b..811f9c5 100644
--- a/src/target/arm7_9_common.h
+++ b/src/target/arm7_9_common.h
@@ -122,13 +122,13 @@ struct arm7_9_common {
 	 * Used as a fallback when bulk writes are unavailable, or for writing data needed to
 	 * do the bulk writes.
 	 */
-	int (*write_memory)(struct target *target, uint32_t address,
+	int (*write_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
 	/**
 	 * Write target memory in multiples of 4 bytes, optimized for
 	 * writing large quantities of data.
 	 */
-	int (*bulk_write_memory)(struct target *target, uint32_t address,
+	int (*bulk_write_memory)(struct target *target, target_addr_t address,
 			uint32_t count, const uint8_t *buffer);
 };
 
@@ -155,19 +155,19 @@ int arm7_9_early_halt(struct target *target);
 int arm7_9_soft_reset_halt(struct target *target);
 
 int arm7_9_halt(struct target *target);
-int arm7_9_resume(struct target *target, int current, uint32_t address,
+int arm7_9_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution);
-int arm7_9_step(struct target *target, int current, uint32_t address,
+int arm7_9_step(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints);
-int arm7_9_read_memory(struct target *target, uint32_t address,
+int arm7_9_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
-int arm7_9_write_memory(struct target *target, uint32_t address,
+int arm7_9_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
-int arm7_9_write_memory_opt(struct target *target, uint32_t address,
+int arm7_9_write_memory_opt(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
 int arm7_9_write_memory_no_opt(struct target *target, uint32_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
-int arm7_9_bulk_write_memory(struct target *target, uint32_t address,
+int arm7_9_bulk_write_memory(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer);
 
 int arm7_9_run_algorithm(struct target *target, int num_mem_params,
diff --git a/src/target/arm920t.c b/src/target/arm920t.c
index 2c96d19..7927a2b 100644
--- a/src/target/arm920t.c
+++ b/src/target/arm920t.c
@@ -553,7 +553,7 @@ static int arm920_mmu(struct target *target, int *enabled)
 }
 
 static int arm920_virt2phys(struct target *target,
-	uint32_t virt, uint32_t *phys)
+	target_addr_t virt, target_addr_t *phys)
 {
 	uint32_t cb;
 	struct arm920t_common *arm920t = target_to_arm920(target);
@@ -568,7 +568,7 @@ static int arm920_virt2phys(struct target *target,
 }
 
 /** Reads a buffer, in the specified word size, with current MMU settings. */
-int arm920t_read_memory(struct target *target, uint32_t address,
+int arm920t_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
@@ -580,7 +580,7 @@ int arm920t_read_memory(struct target *target, uint32_t address,
 
 
 static int arm920t_read_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
 	struct arm920t_common *arm920t = target_to_arm920(target);
@@ -590,7 +590,7 @@ static int arm920t_read_phys_memory(struct target *target,
 }
 
 static int arm920t_write_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
 	struct arm920t_common *arm920t = target_to_arm920(target);
@@ -600,7 +600,7 @@ static int arm920t_write_phys_memory(struct target *target,
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm920t_write_memory(struct target *target, uint32_t address,
+int arm920t_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
diff --git a/src/target/arm920t.h b/src/target/arm920t.h
index 3401b09..2e3b08c 100644
--- a/src/target/arm920t.h
+++ b/src/target/arm920t.h
@@ -55,9 +55,9 @@ struct arm920t_tlb_entry {
 int arm920t_arch_state(struct target *target);
 int arm920t_soft_reset_halt(struct target *target);
 int arm920t_read_memory(struct target *target,
-	uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+	target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 int arm920t_write_memory(struct target *target,
-	uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+	target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 int arm920t_post_debug_entry(struct target *target);
 void arm920t_pre_restore_context(struct target *target);
 int arm920t_get_ttb(struct target *target, uint32_t *result);
diff --git a/src/target/arm926ejs.c b/src/target/arm926ejs.c
index d7c043e..58de778 100644
--- a/src/target/arm926ejs.c
+++ b/src/target/arm926ejs.c
@@ -594,7 +594,7 @@ int arm926ejs_soft_reset_halt(struct target *target)
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm926ejs_write_memory(struct target *target, uint32_t address,
+int arm926ejs_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
@@ -623,7 +623,7 @@ int arm926ejs_write_memory(struct target *target, uint32_t address,
 				return retval;
 		}
 
-		uint32_t pa;
+		target_addr_t pa;
 		retval = target->type->virt2phys(target, address, &pa);
 		if (retval != ERROR_OK)
 			return retval;
@@ -655,7 +655,7 @@ int arm926ejs_write_memory(struct target *target, uint32_t address,
 }
 
 static int arm926ejs_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size,
+		target_addr_t address, uint32_t size,
 		uint32_t count, const uint8_t *buffer)
 {
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
@@ -665,7 +665,7 @@ static int arm926ejs_write_phys_memory(struct target *target,
 }
 
 static int arm926ejs_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size,
+		target_addr_t address, uint32_t size,
 		uint32_t count, uint8_t *buffer)
 {
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
@@ -736,7 +736,7 @@ COMMAND_HANDLER(arm926ejs_handle_cache_info_command)
 	return armv4_5_handle_cache_info_command(CMD_CTX, &arm926ejs->armv4_5_mmu.armv4_5_cache);
 }
 
-static int arm926ejs_virt2phys(struct target *target, uint32_t virtual, uint32_t *physical)
+static int arm926ejs_virt2phys(struct target *target, target_addr_t virtual, target_addr_t *physical)
 {
 	uint32_t cb;
 	struct arm926ejs_common *arm926ejs = target_to_arm926(target);
diff --git a/src/target/arm926ejs.h b/src/target/arm926ejs.h
index 02b4ef8..d4fd0cb 100644
--- a/src/target/arm926ejs.h
+++ b/src/target/arm926ejs.h
@@ -47,7 +47,7 @@ int arm926ejs_init_arch_info(struct target *target,
 		struct arm926ejs_common *arm926ejs, struct jtag_tap *tap);
 int arm926ejs_arch_state(struct target *target);
 int arm926ejs_write_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 int arm926ejs_soft_reset_halt(struct target *target);
 
 extern const struct command_registration arm926ejs_command_handlers[];
diff --git a/src/target/arm946e.c b/src/target/arm946e.c
index 5ee31cc..06c9fc3 100644
--- a/src/target/arm946e.c
+++ b/src/target/arm946e.c
@@ -487,7 +487,7 @@ uint32_t arm946e_invalidate_icache(struct target *target, uint32_t address,
 }
 
 /** Writes a buffer, in the specified word size, with current MMU settings. */
-int arm946e_write_memory(struct target *target, uint32_t address,
+int arm946e_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
@@ -535,7 +535,7 @@ int arm946e_write_memory(struct target *target, uint32_t address,
 
 }
 
-int arm946e_read_memory(struct target *target, uint32_t address,
+int arm946e_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
diff --git a/src/target/armv4_5.c b/src/target/armv4_5.c
index 624a254..2029ca9 100644
--- a/src/target/armv4_5.c
+++ b/src/target/armv4_5.c
@@ -816,7 +816,7 @@ COMMAND_HANDLER(handle_arm_disassemble_command)
 	}
 
 	struct arm *arm = target_to_arm(target);
-	uint32_t address;
+	target_addr_t address;
 	int count = 1;
 	int thumb = 0;
 
@@ -840,7 +840,7 @@ COMMAND_HANDLER(handle_arm_disassemble_command)
 			COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], count);
 		/* FALL THROUGH */
 		case 1:
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
 			if (address & 0x01) {
 				if (!thumb) {
 					command_print(CMD_CTX, "Disassemble as Thumb");
@@ -1434,8 +1434,8 @@ int armv4_5_run_algorithm(struct target *target,
 	struct mem_param *mem_params,
 	int num_reg_params,
 	struct reg_param *reg_params,
-	uint32_t entry_point,
-	uint32_t exit_point,
+	target_addr_t entry_point,
+	target_addr_t exit_point,
 	int timeout_ms,
 	void *arch_info)
 {
@@ -1444,8 +1444,8 @@ int armv4_5_run_algorithm(struct target *target,
 			mem_params,
 			num_reg_params,
 			reg_params,
-			entry_point,
-			exit_point,
+			(uint32_t)entry_point,
+			(uint32_t)exit_point,
 			timeout_ms,
 			arch_info,
 			armv4_5_run_algorithm_completion);
@@ -1456,7 +1456,7 @@ int armv4_5_run_algorithm(struct target *target,
  *
  */
 int arm_checksum_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *checksum)
+	target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	struct working_area *crc_algorithm;
 	struct arm_algorithm arm_algo;
@@ -1529,7 +1529,7 @@ cleanup:
  *
  */
 int arm_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+	target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *check_algorithm;
 	struct reg_param reg_params[3];
diff --git a/src/target/armv4_5_mmu.h b/src/target/armv4_5_mmu.h
index 0f52121..7beaf4e 100644
--- a/src/target/armv4_5_mmu.h
+++ b/src/target/armv4_5_mmu.h
@@ -25,8 +25,9 @@ struct target;
 
 struct armv4_5_mmu_common {
 	int (*get_ttb)(struct target *target, uint32_t *result);
-	int (*read_memory)(struct target *target, uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
-	int (*write_memory)(struct target *target, uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+	int (*read_memory)(struct target *target, target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+	int (*write_memory)(struct target *target, target_addr_t address,
+			    uint32_t size, uint32_t count, const uint8_t *buffer);
 	int (*disable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
 	int (*enable_mmu_caches)(struct target *target, int mmu, int d_u_cache, int i_cache);
 	struct armv4_5_cache_common armv4_5_cache;
diff --git a/src/target/armv7a.h b/src/target/armv7a.h
index c5c03c8..14112e4 100644
--- a/src/target/armv7a.h
+++ b/src/target/armv7a.h
@@ -90,7 +90,7 @@ struct armv7a_mmu_common {
 	uint32_t ttbr_mask[2];
 	uint32_t ttbr_range[2];
 
-	int (*read_physical_memory)(struct target *target, uint32_t address, uint32_t size,
+	int (*read_physical_memory)(struct target *target, target_addr_t address, uint32_t size,
 			uint32_t count, uint8_t *buffer);
 	struct armv7a_cache_common armv7a_cache;
 	uint32_t mmu_enabled;
diff --git a/src/target/armv7a_cache_l2x.c b/src/target/armv7a_cache_l2x.c
index 7988438..e181f26 100644
--- a/src/target/armv7a_cache_l2x.c
+++ b/src/target/armv7a_cache_l2x.c
@@ -63,12 +63,12 @@ int arm7a_l2x_flush_all_data(struct target *target)
 
 	l2_way_val = (1 << l2x_cache->way) - 1;
 
-	return target_write_phys_memory(target,
+	return target_write_phys_u32(target,
 			l2x_cache->base + L2X0_CLEAN_INV_WAY,
-			4, 1, (uint8_t *)&l2_way_val);
+			l2_way_val);
 }
 
-int armv7a_l2x_cache_flush_virt(struct target *target, uint32_t virt,
+int armv7a_l2x_cache_flush_virt(struct target *target, target_addr_t virt,
 					uint32_t size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -83,16 +83,15 @@ int armv7a_l2x_cache_flush_virt(struct target *target, uint32_t virt,
 		return retval;
 
 	for (i = 0; i < size; i += linelen) {
-		uint32_t pa, offs = virt + i;
+		target_addr_t pa, offs = virt + i;
 
 		/* FIXME: use less verbose virt2phys? */
 		retval = target->type->virt2phys(target, offs, &pa);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = target_write_phys_memory(target,
-				l2x_cache->base + L2X0_CLEAN_INV_LINE_PA,
-				4, 1, (uint8_t *)&pa);
+		retval = target_write_phys_u32(target,
+				l2x_cache->base + L2X0_CLEAN_INV_LINE_PA, pa);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -104,7 +103,7 @@ done:
 	return retval;
 }
 
-static int armv7a_l2x_cache_inval_virt(struct target *target, uint32_t virt,
+static int armv7a_l2x_cache_inval_virt(struct target *target, target_addr_t virt,
 					uint32_t size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -119,16 +118,15 @@ static int armv7a_l2x_cache_inval_virt(struct target *target, uint32_t virt,
 		return retval;
 
 	for (i = 0; i < size; i += linelen) {
-		uint32_t pa, offs = virt + i;
+		target_addr_t pa, offs = virt + i;
 
 		/* FIXME: use less verbose virt2phys? */
 		retval = target->type->virt2phys(target, offs, &pa);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = target_write_phys_memory(target,
-				l2x_cache->base + L2X0_INV_LINE_PA,
-				4, 1, (uint8_t *)&pa);
+		retval = target_write_phys_u32(target,
+				l2x_cache->base + L2X0_INV_LINE_PA, pa);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -140,7 +138,7 @@ done:
 	return retval;
 }
 
-static int armv7a_l2x_cache_clean_virt(struct target *target, uint32_t virt,
+static int armv7a_l2x_cache_clean_virt(struct target *target, target_addr_t virt,
 					unsigned int size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -155,16 +153,15 @@ static int armv7a_l2x_cache_clean_virt(struct target *target, uint32_t virt,
 		return retval;
 
 	for (i = 0; i < size; i += linelen) {
-		uint32_t pa, offs = virt + i;
+		target_addr_t pa, offs = virt + i;
 
 		/* FIXME: use less verbose virt2phys? */
 		retval = target->type->virt2phys(target, offs, &pa);
 		if (retval != ERROR_OK)
 			goto done;
 
-		retval = target_write_phys_memory(target,
-				l2x_cache->base + L2X0_CLEAN_LINE_PA,
-				4, 1, (uint8_t *)&pa);
+		retval = target_write_phys_u32(target,
+				l2x_cache->base + L2X0_CLEAN_LINE_PA, pa);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -252,7 +249,8 @@ COMMAND_HANDLER(arm7a_l2x_cache_flush_all_command)
 COMMAND_HANDLER(arm7a_l2x_cache_flush_virt_cmd)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	uint32_t virt, size;
+	target_addr_t virt;
+	uint32_t size;
 
 	if (CMD_ARGC == 0 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -262,7 +260,7 @@ COMMAND_HANDLER(arm7a_l2x_cache_flush_virt_cmd)
 	else
 		size = 1;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], virt);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], virt);
 
 	return armv7a_l2x_cache_flush_virt(target, virt, size);
 }
@@ -270,7 +268,8 @@ COMMAND_HANDLER(arm7a_l2x_cache_flush_virt_cmd)
 COMMAND_HANDLER(arm7a_l2x_cache_inval_virt_cmd)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	uint32_t virt, size;
+	target_addr_t virt;
+	uint32_t size;
 
 	if (CMD_ARGC == 0 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -280,7 +279,7 @@ COMMAND_HANDLER(arm7a_l2x_cache_inval_virt_cmd)
 	else
 		size = 1;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], virt);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], virt);
 
 	return armv7a_l2x_cache_inval_virt(target, virt, size);
 }
@@ -288,7 +287,8 @@ COMMAND_HANDLER(arm7a_l2x_cache_inval_virt_cmd)
 COMMAND_HANDLER(arm7a_l2x_cache_clean_virt_cmd)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	uint32_t virt, size;
+	target_addr_t virt;
+	uint32_t size;
 
 	if (CMD_ARGC == 0 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -298,7 +298,7 @@ COMMAND_HANDLER(arm7a_l2x_cache_clean_virt_cmd)
 	else
 		size = 1;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], virt);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], virt);
 
 	return armv7a_l2x_cache_clean_virt(target, virt, size);
 }
diff --git a/src/target/armv7a_cache_l2x.h b/src/target/armv7a_cache_l2x.h
index 3d9ad81..f98b554 100644
--- a/src/target/armv7a_cache_l2x.h
+++ b/src/target/armv7a_cache_l2x.h
@@ -151,7 +151,7 @@ struct l2c_init_data {
 
 extern const struct command_registration arm7a_l2x_cache_command_handler[];
 
-int armv7a_l2x_cache_flush_virt(struct target *target, uint32_t virt,
+int armv7a_l2x_cache_flush_virt(struct target *target, target_addr_t virt,
 					uint32_t size);
 int arm7a_l2x_flush_all_data(struct target *target);
 
diff --git a/src/target/armv7m.c b/src/target/armv7m.c
index 64d18d7..e0911c3 100644
--- a/src/target/armv7m.c
+++ b/src/target/armv7m.c
@@ -318,7 +318,7 @@ int armv7m_get_gdb_reg_list(struct target *target, struct reg **reg_list[],
 int armv7m_run_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t entry_point, uint32_t exit_point,
+	target_addr_t entry_point, target_addr_t exit_point,
 	int timeout_ms, void *arch_info)
 {
 	int retval;
@@ -343,7 +343,7 @@ int armv7m_run_algorithm(struct target *target,
 int armv7m_start_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t entry_point, uint32_t exit_point,
+	target_addr_t entry_point, target_addr_t exit_point,
 	void *arch_info)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -431,7 +431,7 @@ int armv7m_start_algorithm(struct target *target,
 int armv7m_wait_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t exit_point, int timeout_ms,
+	target_addr_t exit_point, int timeout_ms,
 	void *arch_info)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -461,7 +461,7 @@ int armv7m_wait_algorithm(struct target *target,
 
 	armv7m->load_core_reg_u32(target, 15, &pc);
 	if (exit_point && (pc != exit_point)) {
-		LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 ", expected 0x%" PRIx32,
+		LOG_DEBUG("failed algorithm halted at 0x%" PRIx32 ", expected 0x%" TARGET_PRIxADDR,
 			pc,
 			exit_point);
 		return ERROR_TARGET_TIMEOUT;
@@ -682,7 +682,7 @@ int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m)
 
 /** Generates a CRC32 checksum of a memory region. */
 int armv7m_checksum_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *checksum)
+	target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	struct working_area *crc_algorithm;
 	struct armv7m_algorithm armv7m_info;
@@ -733,7 +733,7 @@ cleanup:
 
 /** Checks whether a memory region is erased. */
 int armv7m_blank_check_memory(struct target *target,
-	uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+	target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
 	struct reg_param reg_params[3];
diff --git a/src/target/armv7m.h b/src/target/armv7m.h
index 304c72d..284bb9c 100644
--- a/src/target/armv7m.h
+++ b/src/target/armv7m.h
@@ -203,19 +203,19 @@ int armv7m_init_arch_info(struct target *target, struct armv7m_common *armv7m);
 int armv7m_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		int timeout_ms, void *arch_info);
 
 int armv7m_start_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		void *arch_info);
 
 int armv7m_wait_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t exit_point, int timeout_ms,
+		target_addr_t exit_point, int timeout_ms,
 		void *arch_info);
 
 int armv7m_invalidate_core_regs(struct target *target);
@@ -223,9 +223,9 @@ int armv7m_invalidate_core_regs(struct target *target);
 int armv7m_restore_context(struct target *target);
 
 int armv7m_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum);
+		target_addr_t address, uint32_t count, uint32_t *checksum);
 int armv7m_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 int armv7m_maybe_skip_bkpt_inst(struct target *target, bool *inst_found);
 
diff --git a/src/target/avr32_ap7k.c b/src/target/avr32_ap7k.c
index e5634f2..f8da8d5 100644
--- a/src/target/avr32_ap7k.c
+++ b/src/target/avr32_ap7k.c
@@ -312,7 +312,7 @@ static int avr32_ap7k_deassert_reset(struct target *target)
 }
 
 static int avr32_ap7k_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 	struct breakpoint *breakpoint = NULL;
@@ -348,7 +348,7 @@ static int avr32_ap7k_resume(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("unset breakpoint at 0x%8.8" TARGET_PRIxADDR "", breakpoint->address);
 #if 0
 			avr32_ap7k_unset_breakpoint(target, breakpoint);
 			avr32_ap7k_single_step_core(target);
@@ -394,7 +394,7 @@ static int avr32_ap7k_resume(struct target *target, int current,
 }
 
 static int avr32_ap7k_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	LOG_ERROR("%s: implement me", __func__);
 
@@ -431,12 +431,12 @@ static int avr32_ap7k_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-static int avr32_ap7k_read_memory(struct target *target, uint32_t address,
+static int avr32_ap7k_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		address,
 		size,
 		count);
@@ -472,12 +472,12 @@ static int avr32_ap7k_read_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int avr32_ap7k_write_memory(struct target *target, uint32_t address,
+static int avr32_ap7k_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct avr32_ap7k_common *ap7k = target_to_ap7k(target);
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		address,
 		size,
 		count);
diff --git a/src/target/avrt.c b/src/target/avrt.c
index 40a1297..1e1898c 100644
--- a/src/target/avrt.c
+++ b/src/target/avrt.c
@@ -33,9 +33,9 @@ static int avr_init_target(struct command_context *cmd_ctx, struct target *targe
 static int avr_arch_state(struct target *target);
 static int avr_poll(struct target *target);
 static int avr_halt(struct target *target);
-static int avr_resume(struct target *target, int current, uint32_t address,
+static int avr_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution);
-static int avr_step(struct target *target, int current, uint32_t address,
+static int avr_step(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints);
 
 static int avr_assert_reset(struct target *target);
@@ -116,14 +116,14 @@ static int avr_halt(struct target *target)
 	return ERROR_OK;
 }
 
-static int avr_resume(struct target *target, int current, uint32_t address,
+static int avr_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution)
 {
 	LOG_DEBUG("%s", __func__);
 	return ERROR_OK;
 }
 
-static int avr_step(struct target *target, int current, uint32_t address, int handle_breakpoints)
+static int avr_step(struct target *target, int current, target_addr_t address, int handle_breakpoints)
 {
 	LOG_DEBUG("%s", __func__);
 	return ERROR_OK;
diff --git a/src/target/breakpoints.c b/src/target/breakpoints.c
index c4a959d..7cf4a69 100644
--- a/src/target/breakpoints.c
+++ b/src/target/breakpoints.c
@@ -42,7 +42,7 @@ static const char * const watchpoint_rw_strings[] = {
 static int bpwp_unique_id;
 
 int breakpoint_add_internal(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t length,
 	enum breakpoint_type type)
 {
@@ -60,7 +60,7 @@ int breakpoint_add_internal(struct target *target,
 			 * breakpoint" ... check all the parameters before
 			 * succeeding.
 			 */
-			LOG_DEBUG("Duplicate Breakpoint address: 0x%08" PRIx32 " (BP %" PRIu32 ")",
+			LOG_DEBUG("Duplicate Breakpoint address: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
 				address, breakpoint->unique_id);
 			return ERROR_OK;
 		}
@@ -98,7 +98,7 @@ fail:
 			return retval;
 	}
 
-	LOG_DEBUG("added %s breakpoint at 0x%8.8" PRIx32 " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+	LOG_DEBUG("added %s breakpoint at " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address, (*breakpoint_p)->length,
 		(*breakpoint_p)->unique_id);
@@ -159,7 +159,7 @@ int context_breakpoint_add_internal(struct target *target,
 }
 
 int hybrid_breakpoint_add_internal(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t asid,
 	uint32_t length,
 	enum breakpoint_type type)
@@ -180,7 +180,7 @@ int hybrid_breakpoint_add_internal(struct target *target,
 				asid, breakpoint->unique_id);
 			return -1;
 		} else if ((breakpoint->address == address) && (breakpoint->asid == 0)) {
-			LOG_DEBUG("Duplicate Breakpoint IVA: 0x%08" PRIx32 " (BP %" PRIu32 ")",
+			LOG_DEBUG("Duplicate Breakpoint IVA: " TARGET_ADDR_FMT " (BP %" PRIu32 ")",
 				address, breakpoint->unique_id);
 			return -1;
 
@@ -208,7 +208,7 @@ int hybrid_breakpoint_add_internal(struct target *target,
 		return retval;
 	}
 	LOG_DEBUG(
-		"added %s Hybrid breakpoint at address 0x%8.8" PRIx32 " of length 0x%8.8x, (BPID: %" PRIu32 ")",
+		"added %s Hybrid breakpoint at address " TARGET_ADDR_FMT " of length 0x%8.8x, (BPID: %" PRIu32 ")",
 		breakpoint_type_strings[(*breakpoint_p)->type],
 		(*breakpoint_p)->address,
 		(*breakpoint_p)->length,
@@ -218,7 +218,7 @@ int hybrid_breakpoint_add_internal(struct target *target,
 }
 
 int breakpoint_add(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t length,
 	enum breakpoint_type type)
 {
@@ -263,7 +263,7 @@ int context_breakpoint_add(struct target *target,
 		return context_breakpoint_add_internal(target, asid, length, type);
 }
 int hybrid_breakpoint_add(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t asid,
 	uint32_t length,
 	enum breakpoint_type type)
@@ -310,7 +310,7 @@ static void breakpoint_free(struct target *target, struct breakpoint *breakpoint
 	free(breakpoint);
 }
 
-int breakpoint_remove_internal(struct target *target, uint32_t address)
+int breakpoint_remove_internal(struct target *target, target_addr_t address)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
 
@@ -329,11 +329,11 @@ int breakpoint_remove_internal(struct target *target, uint32_t address)
 		return 1;
 	} else {
 		if (!target->smp)
-			LOG_ERROR("no breakpoint at address 0x%8.8" PRIx32 " found", address);
+			LOG_ERROR("no breakpoint at address " TARGET_ADDR_FMT " found", address);
 		return 0;
 	}
 }
-void breakpoint_remove(struct target *target, uint32_t address)
+void breakpoint_remove(struct target *target, target_addr_t address)
 {
 	int found = 0;
 	if (target->smp) {
@@ -346,7 +346,7 @@ void breakpoint_remove(struct target *target, uint32_t address)
 			head = head->next;
 		}
 		if (found == 0)
-			LOG_ERROR("no breakpoint at address 0x%8.8" PRIx32 " found", address);
+			LOG_ERROR("no breakpoint at address " TARGET_ADDR_FMT " found", address);
 	} else
 		breakpoint_remove_internal(target, address);
 }
@@ -375,7 +375,7 @@ void breakpoint_clear_target(struct target *target)
 
 }
 
-struct breakpoint *breakpoint_find(struct target *target, uint32_t address)
+struct breakpoint *breakpoint_find(struct target *target, target_addr_t address)
 {
 	struct breakpoint *breakpoint = target->breakpoints;
 
@@ -388,7 +388,7 @@ struct breakpoint *breakpoint_find(struct target *target, uint32_t address)
 	return NULL;
 }
 
-int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
+int watchpoint_add(struct target *target, target_addr_t address, uint32_t length,
 	enum watchpoint_rw rw, uint32_t value, uint32_t mask)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
@@ -402,7 +402,7 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 				|| watchpoint->value != value
 				|| watchpoint->mask != mask
 				|| watchpoint->rw != rw) {
-				LOG_ERROR("address 0x%8.8" PRIx32
+				LOG_ERROR("address " TARGET_ADDR_FMT
 					" already has watchpoint %d",
 					address, watchpoint->unique_id);
 				return ERROR_FAIL;
@@ -436,7 +436,7 @@ int watchpoint_add(struct target *target, uint32_t address, uint32_t length,
 		default:
 			reason = "unrecognized error";
 bye:
-			LOG_ERROR("can't add %s watchpoint at 0x%8.8" PRIx32 ", %s",
+			LOG_ERROR("can't add %s watchpoint at " TARGET_ADDR_FMT ", %s",
 				watchpoint_rw_strings[(*watchpoint_p)->rw],
 				address, reason);
 			free(*watchpoint_p);
@@ -444,7 +444,7 @@ bye:
 			return retval;
 	}
 
-	LOG_DEBUG("added %s watchpoint at 0x%8.8" PRIx32
+	LOG_DEBUG("added %s watchpoint at " TARGET_ADDR_FMT
 		" of length 0x%8.8" PRIx32 " (WPID: %d)",
 		watchpoint_rw_strings[(*watchpoint_p)->rw],
 		(*watchpoint_p)->address,
@@ -475,7 +475,7 @@ static void watchpoint_free(struct target *target, struct watchpoint *watchpoint
 	free(watchpoint);
 }
 
-void watchpoint_remove(struct target *target, uint32_t address)
+void watchpoint_remove(struct target *target, target_addr_t address)
 {
 	struct watchpoint *watchpoint = target->watchpoints;
 
@@ -488,7 +488,7 @@ void watchpoint_remove(struct target *target, uint32_t address)
 	if (watchpoint)
 		watchpoint_free(target, watchpoint);
 	else
-		LOG_ERROR("no watchpoint at address 0x%8.8" PRIx32 " found", address);
+		LOG_ERROR("no watchpoint at address " TARGET_ADDR_FMT " found", address);
 }
 
 void watchpoint_clear_target(struct target *target)
@@ -499,7 +499,8 @@ void watchpoint_clear_target(struct target *target)
 		watchpoint_free(target, target->watchpoints);
 }
 
-int watchpoint_hit(struct target *target, enum watchpoint_rw *rw, uint32_t *address)
+int watchpoint_hit(struct target *target, enum watchpoint_rw *rw,
+		   target_addr_t *address)
 {
 	int retval;
 	struct watchpoint *hit_watchpoint;
@@ -511,7 +512,7 @@ int watchpoint_hit(struct target *target, enum watchpoint_rw *rw, uint32_t *addr
 	*rw = hit_watchpoint->rw;
 	*address = hit_watchpoint->address;
 
-	LOG_DEBUG("Found hit watchpoint at 0x%8.8" PRIx32 " (WPID: %d)",
+	LOG_DEBUG("Found hit watchpoint at " TARGET_ADDR_FMT " (WPID: %d)",
 		hit_watchpoint->address,
 		hit_watchpoint->unique_id);
 
diff --git a/src/target/breakpoints.h b/src/target/breakpoints.h
index 842fc18..51bd05a 100644
--- a/src/target/breakpoints.h
+++ b/src/target/breakpoints.h
@@ -33,7 +33,7 @@ enum watchpoint_rw {
 };
 
 struct breakpoint {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t asid;
 	int length;
 	enum breakpoint_type type;
@@ -45,7 +45,7 @@ struct breakpoint {
 };
 
 struct watchpoint {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t length;
 	uint32_t mask;
 	uint32_t value;
@@ -57,22 +57,23 @@ struct watchpoint {
 
 void breakpoint_clear_target(struct target *target);
 int breakpoint_add(struct target *target,
-		uint32_t address, uint32_t length, enum breakpoint_type type);
+		target_addr_t address, uint32_t length, enum breakpoint_type type);
 int context_breakpoint_add(struct target *target,
 		uint32_t asid, uint32_t length, enum breakpoint_type type);
 int hybrid_breakpoint_add(struct target *target,
-		uint32_t address, uint32_t asid, uint32_t length, enum breakpoint_type type);
-void breakpoint_remove(struct target *target, uint32_t address);
+		target_addr_t address, uint32_t asid, uint32_t length, enum breakpoint_type type);
+void breakpoint_remove(struct target *target, target_addr_t address);
 
-struct breakpoint *breakpoint_find(struct target *target, uint32_t address);
+struct breakpoint *breakpoint_find(struct target *target, target_addr_t address);
 
 void watchpoint_clear_target(struct target *target);
 int watchpoint_add(struct target *target,
-		uint32_t address, uint32_t length,
+		target_addr_t address, uint32_t length,
 		enum watchpoint_rw rw, uint32_t value, uint32_t mask);
-void watchpoint_remove(struct target *target, uint32_t address);
+void watchpoint_remove(struct target *target, target_addr_t address);
 
 /* report type and address of just hit watchpoint */
-int watchpoint_hit(struct target *target, enum watchpoint_rw *rw, uint32_t *address);
+int watchpoint_hit(struct target *target, enum watchpoint_rw *rw,
+		target_addr_t *address);
 
 #endif /* OPENOCD_TARGET_BREAKPOINTS_H */
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 8c8a2b7..62ac361 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -75,7 +75,7 @@ static int cortex_a_dap_write_coreregister_u32(struct target *target,
 static int cortex_a_mmu(struct target *target, int *enabled);
 static int cortex_a_mmu_modify(struct target *target, int enable);
 static int cortex_a_virt2phys(struct target *target,
-	uint32_t virt, uint32_t *phys);
+	target_addr_t virt, target_addr_t *phys);
 static int cortex_a_read_cpu_memory(struct target *target,
 	uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 
@@ -937,7 +937,7 @@ static int cortex_a_halt(struct target *target)
 }
 
 static int cortex_a_internal_restore(struct target *target, int current,
-	uint32_t *address, int handle_breakpoints, int debug_execution)
+	target_addr_t *address, int handle_breakpoints, int debug_execution)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct arm *arm = &armv7a->arm;
@@ -1092,7 +1092,7 @@ static int cortex_a_restore_smp(struct target *target, int handle_breakpoints)
 	int retval = 0;
 	struct target_list *head;
 	struct target *curr;
-	uint32_t address;
+	target_addr_t address;
 	head = target->head;
 	while (head != (struct target_list *)NULL) {
 		curr = head->target;
@@ -1110,7 +1110,7 @@ static int cortex_a_restore_smp(struct target *target, int handle_breakpoints)
 }
 
 static int cortex_a_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	int retval = 0;
 	/* dummy resume for smp toggle in order to reduce gdb impact  */
@@ -1134,11 +1134,11 @@ static int cortex_a_resume(struct target *target, int current,
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-		LOG_DEBUG("target resumed at 0x%" PRIx32, address);
+		LOG_DEBUG("target resumed at " TARGET_ADDR_FMT, address);
 	} else {
 		target->state = TARGET_DEBUG_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
-		LOG_DEBUG("target debug resumed at 0x%" PRIx32, address);
+		LOG_DEBUG("target debug resumed at " TARGET_ADDR_FMT, address);
 	}
 
 	return ERROR_OK;
@@ -1344,7 +1344,7 @@ int cortex_a_set_dscr_bits(struct target *target, unsigned long bit_mask, unsign
 	return retval;
 }
 
-static int cortex_a_step(struct target *target, int current, uint32_t address,
+static int cortex_a_step(struct target *target, int current, target_addr_t address,
 	int handle_breakpoints)
 {
 	struct cortex_a_common *cortex_a = target_to_cortex_a(target);
@@ -2639,7 +2639,7 @@ out:
  */
 
 static int cortex_a_read_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -2650,7 +2650,7 @@ static int cortex_a_read_phys_memory(struct target *target,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("Reading memory at real address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32,
+	LOG_DEBUG("Reading memory at real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
 		address, size, count);
 
 	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num))
@@ -2664,14 +2664,14 @@ static int cortex_a_read_phys_memory(struct target *target,
 	return retval;
 }
 
-static int cortex_a_read_memory(struct target *target, uint32_t address,
+static int cortex_a_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int retval;
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Reading memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	cortex_a_prep_memaccess(target, 0);
 	retval = cortex_a_read_cpu_memory(target, address, size, count, buffer);
@@ -2680,11 +2680,11 @@ static int cortex_a_read_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
+static int cortex_a_read_memory_ahb(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int mmu_enabled = 0;
-	uint32_t virt, phys;
+	target_addr_t virt, phys;
 	int retval;
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
@@ -2694,8 +2694,8 @@ static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
 		return target_read_memory(target, address, size, count, buffer);
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Reading memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Reading memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	/* determine if MMU was enabled on target stop */
 	if (!armv7a->is_armv7r) {
@@ -2710,7 +2710,8 @@ static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
 		if (retval != ERROR_OK)
 			return retval;
 
-		LOG_DEBUG("Reading at virtual address. Translating v:0x%" PRIx32 " to r:0x%" PRIx32,
+		LOG_DEBUG("Reading at virtual address. "
+			  "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT,
 			  virt, phys);
 		address = phys;
 	}
@@ -2724,7 +2725,7 @@ static int cortex_a_read_memory_ahb(struct target *target, uint32_t address,
 }
 
 static int cortex_a_write_phys_memory(struct target *target,
-	uint32_t address, uint32_t size,
+	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -2735,8 +2736,8 @@ static int cortex_a_write_phys_memory(struct target *target,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("Writing memory to real address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Writing memory to real address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	if (armv7a->memory_ap_available && (apsel == armv7a->memory_ap->ap_num))
 		return mem_ap_write_buf(armv7a->memory_ap, buffer, size, count, address);
@@ -2749,14 +2750,14 @@ static int cortex_a_write_phys_memory(struct target *target,
 	return retval;
 }
 
-static int cortex_a_write_memory(struct target *target, uint32_t address,
+static int cortex_a_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Writing memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	/* memory writes bypass the caches, must flush before writing */
 	armv7a_cache_auto_flush_on_write(target, address, size * count);
@@ -2767,11 +2768,11 @@ static int cortex_a_write_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
+static int cortex_a_write_memory_ahb(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int mmu_enabled = 0;
-	uint32_t virt, phys;
+	target_addr_t virt, phys;
 	int retval;
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct adiv5_dap *swjdp = armv7a->arm.dap;
@@ -2781,8 +2782,8 @@ static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
 		return target_write_memory(target, address, size, count, buffer);
 
 	/* cortex_a handles unaligned memory access */
-	LOG_DEBUG("Writing memory at address 0x%" PRIx32 "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
+	LOG_DEBUG("Writing memory at address " TARGET_ADDR_FMT "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
 
 	/* determine if MMU was enabled on target stop */
 	if (!armv7a->is_armv7r) {
@@ -2797,7 +2798,8 @@ static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
 		if (retval != ERROR_OK)
 			return retval;
 
-		LOG_DEBUG("Writing to virtual address. Translating v:0x%" PRIx32 " to r:0x%" PRIx32,
+		LOG_DEBUG("Writing to virtual address. "
+			  "Translating v:" TARGET_ADDR_FMT " to r:" TARGET_ADDR_FMT,
 			  virt,
 			  phys);
 		address = phys;
@@ -2811,7 +2813,7 @@ static int cortex_a_write_memory_ahb(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int cortex_a_read_buffer(struct target *target, uint32_t address,
+static int cortex_a_read_buffer(struct target *target, target_addr_t address,
 				uint32_t count, uint8_t *buffer)
 {
 	uint32_t size;
@@ -2845,7 +2847,7 @@ static int cortex_a_read_buffer(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int cortex_a_write_buffer(struct target *target, uint32_t address,
+static int cortex_a_write_buffer(struct target *target, target_addr_t address,
 				 uint32_t count, const uint8_t *buffer)
 {
 	uint32_t size;
@@ -3202,7 +3204,7 @@ static int cortex_a_mmu(struct target *target, int *enabled)
 }
 
 static int cortex_a_virt2phys(struct target *target,
-	uint32_t virt, uint32_t *phys)
+	target_addr_t virt, target_addr_t *phys)
 {
 	int retval = ERROR_FAIL;
 	struct armv7a_common *armv7a = target_to_armv7a(target);
@@ -3220,7 +3222,8 @@ static int cortex_a_virt2phys(struct target *target,
 		retval = cortex_a_mmu_modify(target, 1);
 		if (retval != ERROR_OK)
 			goto done;
-		retval = armv7a_mmu_translate_va_pa(target, virt,  phys, 1);
+		retval = armv7a_mmu_translate_va_pa(target, (uint32_t)virt,
+						    (uint32_t *)phys, 1);
 	}
 done:
 	return retval;
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 36a7746..e80cd23 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -682,7 +682,7 @@ void cortex_m_enable_breakpoints(struct target *target)
 }
 
 static int cortex_m_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
 	struct breakpoint *breakpoint = NULL;
@@ -750,7 +750,7 @@ static int cortex_m_resume(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT " (ID: %" PRIu32 ")",
 				breakpoint->address,
 				breakpoint->unique_id);
 			cortex_m_unset_breakpoint(target, breakpoint);
@@ -782,7 +782,7 @@ static int cortex_m_resume(struct target *target, int current,
 
 /* int irqstepcount = 0; */
 static int cortex_m_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	struct cortex_m_common *cortex_m = target_to_cm(target);
 	struct armv7m_common *armv7m = &cortex_m->armv7m;
@@ -1198,7 +1198,7 @@ int cortex_m_set_breakpoint(struct target *target, struct breakpoint *breakpoint
 		breakpoint->set = true;
 	}
 
-	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: " TARGET_ADDR_FMT " Length: %d (set=%d)",
 		breakpoint->unique_id,
 		(int)(breakpoint->type),
 		breakpoint->address,
@@ -1219,7 +1219,7 @@ int cortex_m_unset_breakpoint(struct target *target, struct breakpoint *breakpoi
 		return ERROR_OK;
 	}
 
-	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: 0x%08" PRIx32 " Length: %d (set=%d)",
+	LOG_DEBUG("BPID: %" PRIu32 ", Type: %d, Address: " TARGET_ADDR_FMT " Length: %d (set=%d)",
 		breakpoint->unique_id,
 		(int)(breakpoint->type),
 		breakpoint->address,
@@ -1664,7 +1664,7 @@ static int cortex_m_store_core_reg_u32(struct target *target,
 	return ERROR_OK;
 }
 
-static int cortex_m_read_memory(struct target *target, uint32_t address,
+static int cortex_m_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
@@ -1678,7 +1678,7 @@ static int cortex_m_read_memory(struct target *target, uint32_t address,
 	return mem_ap_read_buf(armv7m->debug_ap, buffer, size, count, address);
 }
 
-static int cortex_m_write_memory(struct target *target, uint32_t address,
+static int cortex_m_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct armv7m_common *armv7m = target_to_armv7m(target);
diff --git a/src/target/dsp563xx.c b/src/target/dsp563xx.c
index 783a019..1d728df 100644
--- a/src/target/dsp563xx.c
+++ b/src/target/dsp563xx.c
@@ -1117,7 +1117,7 @@ static int dsp563xx_halt(struct target *target)
 
 static int dsp563xx_resume(struct target *target,
 	int current,
-	uint32_t address,
+	target_addr_t address,
 	int handle_breakpoints,
 	int debug_execution)
 {
@@ -1290,7 +1290,7 @@ static int dsp563xx_step_ex(struct target *target,
 
 static int dsp563xx_step(struct target *target,
 	int current,
-	uint32_t address,
+	target_addr_t address,
 	int handle_breakpoints)
 {
 	int err;
@@ -1374,7 +1374,7 @@ static int dsp563xx_deassert_reset(struct target *target)
 static int dsp563xx_run_algorithm(struct target *target,
 	int num_mem_params, struct mem_param *mem_params,
 	int num_reg_params, struct reg_param *reg_params,
-	uint32_t entry_point, uint32_t exit_point,
+	target_addr_t entry_point, target_addr_t exit_point,
 	int timeout_ms, void *arch_info)
 {
 	int i;
@@ -1592,7 +1592,7 @@ static int dsp563xx_read_memory_core(struct target *target,
 
 static int dsp563xx_read_memory(struct target *target,
 	int mem_type,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -1660,7 +1660,7 @@ static int dsp563xx_read_memory(struct target *target,
 }
 
 static int dsp563xx_read_memory_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	uint8_t *buffer)
@@ -1671,7 +1671,7 @@ static int dsp563xx_read_memory_default(struct target *target,
 }
 
 static int dsp563xx_read_buffer_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint8_t *buffer)
 {
@@ -1682,7 +1682,7 @@ static int dsp563xx_read_buffer_default(struct target *target,
 
 static int dsp563xx_write_memory_core(struct target *target,
 	int mem_type,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -1694,7 +1694,7 @@ static int dsp563xx_write_memory_core(struct target *target,
 	const uint8_t *b;
 
 	LOG_DEBUG(
-		"memtype: %d address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+		"memtype: %d address: 0x%8.8" TARGET_PRIxADDR ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 		mem_type,
 		address,
 		size,
@@ -1766,7 +1766,7 @@ static int dsp563xx_write_memory_core(struct target *target,
 
 static int dsp563xx_write_memory(struct target *target,
 	int mem_type,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -1834,7 +1834,7 @@ static int dsp563xx_write_memory(struct target *target,
 }
 
 static int dsp563xx_write_memory_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	uint32_t count,
 	const uint8_t *buffer)
@@ -1844,7 +1844,7 @@ static int dsp563xx_write_memory_default(struct target *target,
 }
 
 static int dsp563xx_write_buffer_default(struct target *target,
-	uint32_t address,
+	target_addr_t address,
 	uint32_t size,
 	const uint8_t *buffer)
 {
diff --git a/src/target/dsp5680xx.c b/src/target/dsp5680xx.c
index 205d8fe..a50f2cd 100644
--- a/src/target/dsp5680xx.c
+++ b/src/target/dsp5680xx.c
@@ -1011,7 +1011,7 @@ static int dsp5680xx_poll(struct target *target)
 }
 
 static int dsp5680xx_resume(struct target *target, int current,
-			    uint32_t address, int hb, int d)
+			    target_addr_t address, int hb, int d)
 {
 	if (target->state == TARGET_RUNNING) {
 		LOG_USER("Target already running.");
@@ -1168,7 +1168,7 @@ static int dsp5680xx_read_32_single(struct target *t, uint32_t a,
 	return retval;
 }
 
-static int dsp5680xx_read(struct target *t, uint32_t a, uint32_t size,
+static int dsp5680xx_read(struct target *t, target_addr_t a, uint32_t size,
 			  uint32_t count, uint8_t *buf)
 {
 	struct target *target = t;
@@ -1423,7 +1423,7 @@ static int dsp5680xx_write_32(struct target *t, uint32_t a, uint32_t c,
  *
  * @return
  */
-static int dsp5680xx_write(struct target *t, uint32_t a, uint32_t s, uint32_t c,
+static int dsp5680xx_write(struct target *t, target_addr_t a, uint32_t s, uint32_t c,
 			   const uint8_t *b)
 {
 	/* TODO Cannot write 32bit to odd address, will write 0x12345678  as 0x5678 0x0012 */
@@ -1468,7 +1468,7 @@ static int dsp5680xx_write(struct target *t, uint32_t a, uint32_t s, uint32_t c,
 	return retval;
 }
 
-static int dsp5680xx_write_buffer(struct target *t, uint32_t a, uint32_t size,
+static int dsp5680xx_write_buffer(struct target *t, target_addr_t a, uint32_t size,
 				  const uint8_t *b)
 {
 	check_halt_and_debug(t);
@@ -1485,7 +1485,7 @@ static int dsp5680xx_write_buffer(struct target *t, uint32_t a, uint32_t size,
  *
  * @return
  */
-static int dsp5680xx_read_buffer(struct target *t, uint32_t a, uint32_t size,
+static int dsp5680xx_read_buffer(struct target *t, target_addr_t a, uint32_t size,
 				 uint8_t *buf)
 {
 	check_halt_and_debug(t);
@@ -1505,7 +1505,7 @@ static int dsp5680xx_read_buffer(struct target *t, uint32_t a, uint32_t size,
  *
  * @return
  */
-static int dsp5680xx_checksum_memory(struct target *t, uint32_t a, uint32_t s,
+static int dsp5680xx_checksum_memory(struct target *t, target_addr_t a, uint32_t s,
 				     uint32_t *checksum)
 {
 	return ERROR_FAIL;
@@ -2262,7 +2262,7 @@ int dsp5680xx_f_lock(struct target *target)
 	return retval;
 }
 
-static int dsp5680xx_step(struct target *target, int current, uint32_t address,
+static int dsp5680xx_step(struct target *target, int current, target_addr_t address,
 			  int handle_breakpoints)
 {
 	err_check(ERROR_FAIL, DSP5680XX_ERROR_NOT_IMPLEMENTED_STEP,
diff --git a/src/target/feroceon.c b/src/target/feroceon.c
index f12e4e4..6b14ab6 100644
--- a/src/target/feroceon.c
+++ b/src/target/feroceon.c
@@ -460,7 +460,7 @@ static int feroceon_examine_debug_reason(struct target *target)
 }
 
 static int feroceon_bulk_write_memory(struct target *target,
-		uint32_t address, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t count, const uint8_t *buffer)
 {
 	int retval;
 	struct arm *arm = target->arch_info;
@@ -565,7 +565,7 @@ static int feroceon_bulk_write_memory(struct target *target,
 			buf_get_u32(arm->core_cache->reg_list[0].value, 0, 32);
 		if (endaddress != address + count*4) {
 			LOG_ERROR("DCC write failed,"
-				" expected end address 0x%08" PRIx32
+				" expected end address 0x%08" TARGET_PRIxADDR
 				" got 0x%0" PRIx32 "",
 				address + count*4, endaddress);
 			retval = ERROR_FAIL;
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index feeb11f..78dc8c5 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -584,7 +584,7 @@ static int adapter_halt(struct target *target)
 }
 
 static int adapter_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints,
+		target_addr_t address, int handle_breakpoints,
 		int debug_execution)
 {
 	int res;
@@ -594,8 +594,8 @@ static int adapter_resume(struct target *target, int current,
 	struct breakpoint *breakpoint = NULL;
 	struct reg *pc;
 
-	LOG_DEBUG("%s %d 0x%08" PRIx32 " %d %d", __func__, current, address,
-			handle_breakpoints, debug_execution);
+	LOG_DEBUG("%s %d " TARGET_ADDR_FMT " %d %d", __func__, current,
+			address, handle_breakpoints, debug_execution);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
@@ -642,7 +642,7 @@ static int adapter_resume(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 " (ID: %" PRIu32 ")",
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT " (ID: %" PRIu32 ")",
 					breakpoint->address,
 					breakpoint->unique_id);
 			cortex_m_unset_breakpoint(target, breakpoint);
@@ -675,7 +675,7 @@ static int adapter_resume(struct target *target, int current,
 }
 
 static int adapter_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints)
+		target_addr_t address, int handle_breakpoints)
 {
 	int res;
 	struct hl_interface_s *adapter = target_to_adapter(target);
@@ -738,7 +738,7 @@ static int adapter_step(struct target *target, int current,
 	return ERROR_OK;
 }
 
-static int adapter_read_memory(struct target *target, uint32_t address,
+static int adapter_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count,
 		uint8_t *buffer)
 {
@@ -747,12 +747,13 @@ static int adapter_read_memory(struct target *target, uint32_t address,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("%s 0x%08" PRIx32 " %" PRIu32 " %" PRIu32, __func__, address, size, count);
+	LOG_DEBUG("%s " TARGET_ADDR_FMT " %" PRIu32 " %" PRIu32,
+			  __func__, address, size, count);
 
 	return adapter->layout->api->read_mem(adapter->handle, address, size, count, buffer);
 }
 
-static int adapter_write_memory(struct target *target, uint32_t address,
+static int adapter_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count,
 		const uint8_t *buffer)
 {
@@ -761,7 +762,8 @@ static int adapter_write_memory(struct target *target, uint32_t address,
 	if (!count || !buffer)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("%s 0x%08" PRIx32 " %" PRIu32 " %" PRIu32, __func__, address, size, count);
+	LOG_DEBUG("%s " TARGET_ADDR_FMT " %" PRIu32 " %" PRIu32,
+			  __func__, address, size, count);
 
 	return adapter->layout->api->write_mem(adapter->handle, address, size, count, buffer);
 }
diff --git a/src/target/image.h b/src/target/image.h
index 9bf7af3..9907a5f 100644
--- a/src/target/image.h
+++ b/src/target/image.h
@@ -46,7 +46,7 @@ enum image_type {
 };
 
 struct imagesection {
-	uint32_t base_address;
+	target_addr_t base_address;
 	uint32_t size;
 	int flags;
 	void *private;		/* private data */
diff --git a/src/target/lakemont.c b/src/target/lakemont.c
index 27efc69..2bd12fd 100644
--- a/src/target/lakemont.c
+++ b/src/target/lakemont.c
@@ -994,7 +994,7 @@ int lakemont_halt(struct target *t)
 	}
 }
 
-int lakemont_resume(struct target *t, int current, uint32_t address,
+int lakemont_resume(struct target *t, int current, target_addr_t address,
 			int handle_breakpoints, int debug_execution)
 {
 	struct breakpoint *bp = NULL;
@@ -1036,7 +1036,7 @@ int lakemont_resume(struct target *t, int current, uint32_t address,
 }
 
 int lakemont_step(struct target *t, int current,
-			uint32_t address, int handle_breakpoints)
+			target_addr_t address, int handle_breakpoints)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	uint32_t eflags = buf_get_u32(x86_32->cache->reg_list[EFLAGS].value, 0, 32);
diff --git a/src/target/lakemont.h b/src/target/lakemont.h
index b07a056..98efd44 100644
--- a/src/target/lakemont.h
+++ b/src/target/lakemont.h
@@ -95,10 +95,10 @@ int lakemont_init_arch_info(struct target *t, struct x86_32_common *x86_32);
 int lakemont_poll(struct target *t);
 int lakemont_arch_state(struct target *t);
 int lakemont_halt(struct target *t);
-int lakemont_resume(struct target *t, int current, uint32_t address,
+int lakemont_resume(struct target *t, int current, target_addr_t address,
 			int handle_breakpoints, int debug_execution);
 int lakemont_step(struct target *t, int current,
-			uint32_t address, int handle_breakpoints);
+			target_addr_t address, int handle_breakpoints);
 int lakemont_reset_assert(struct target *t);
 int lakemont_reset_deassert(struct target *t);
 int lakemont_update_after_probemode_entry(struct target *t);
diff --git a/src/target/ls1_sap.c b/src/target/ls1_sap.c
index b4d814b..bc46ed4 100644
--- a/src/target/ls1_sap.c
+++ b/src/target/ls1_sap.c
@@ -64,14 +64,14 @@ static int ls1_sap_halt(struct target *target)
 	return ERROR_OK;
 }
 
-static int ls1_sap_resume(struct target *target, int current, uint32_t address,
+static int ls1_sap_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution)
 {
 	LOG_DEBUG("%s", __func__);
 	return ERROR_OK;
 }
 
-static int ls1_sap_step(struct target *target, int current, uint32_t address,
+static int ls1_sap_step(struct target *target, int current, target_addr_t address,
 				int handle_breakpoints)
 {
 	LOG_DEBUG("%s", __func__);
@@ -178,10 +178,10 @@ static void ls1_sap_memory_write(struct jtag_tap *tap, uint32_t size,
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 }
 
-static int ls1_sap_read_memory(struct target *target, uint32_t address,
+static int ls1_sap_read_memory(struct target *target, target_addr_t address,
 			       uint32_t size, uint32_t count, uint8_t *buffer)
 {
-	LOG_DEBUG("Reading memory at physical address 0x%" PRIx32
+	LOG_DEBUG("Reading memory at physical address 0x%" TARGET_PRIxADDR
 		  "; size %" PRId32 "; count %" PRId32, address, size, count);
 
 	if (count == 0 || buffer == NULL)
@@ -199,11 +199,11 @@ static int ls1_sap_read_memory(struct target *target, uint32_t address,
 	return jtag_execute_queue();
 }
 
-static int ls1_sap_write_memory(struct target *target, uint32_t address,
+static int ls1_sap_write_memory(struct target *target, target_addr_t address,
 				uint32_t size, uint32_t count,
 				const uint8_t *buffer)
 {
-	LOG_DEBUG("Writing memory at physical address 0x%" PRIx32
+	LOG_DEBUG("Writing memory at physical address 0x%" TARGET_PRIxADDR
 		  "; size %" PRId32 "; count %" PRId32, address, size, count);
 
 
diff --git a/src/target/mips32.c b/src/target/mips32.c
index 27caebe..cec4549 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -391,8 +391,8 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 }
 
 /* run to exit point. return error if exit point was not reached. */
-static int mips32_run_and_wait(struct target *target, uint32_t entry_point,
-		int timeout_ms, uint32_t exit_point, struct mips32_common *mips32)
+static int mips32_run_and_wait(struct target *target, target_addr_t entry_point,
+		int timeout_ms, target_addr_t exit_point, struct mips32_common *mips32)
 {
 	uint32_t pc;
 	int retval;
@@ -425,8 +425,8 @@ static int mips32_run_and_wait(struct target *target, uint32_t entry_point,
 
 int mips32_run_algorithm(struct target *target, int num_mem_params,
 		struct mem_param *mem_params, int num_reg_params,
-		struct reg_param *reg_params, uint32_t entry_point,
-		uint32_t exit_point, int timeout_ms, void *arch_info)
+		struct reg_param *reg_params, target_addr_t entry_point,
+		target_addr_t exit_point, int timeout_ms, void *arch_info)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips32_algorithm *mips32_algorithm_info = arch_info;
@@ -696,7 +696,7 @@ int mips32_enable_interrupts(struct target *target, int enable)
 	return ERROR_OK;
 }
 
-int mips32_checksum_memory(struct target *target, uint32_t address,
+int mips32_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum)
 {
 	struct working_area *crc_algorithm;
@@ -773,7 +773,7 @@ int mips32_checksum_memory(struct target *target, uint32_t address,
 
 /** Checks whether a memory region is erased. */
 int mips32_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value)
 {
 	struct working_area *erase_check_algorithm;
 	struct reg_param reg_params[3];
diff --git a/src/target/mips32.h b/src/target/mips32.h
index d493b39..2e7067f 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -232,7 +232,7 @@ struct reg_cache *mips32_build_reg_cache(struct target *target);
 int mips32_run_algorithm(struct target *target,
 		int num_mem_params, struct mem_param *mem_params,
 		int num_reg_params, struct reg_param *reg_params,
-		uint32_t entry_point, uint32_t exit_point,
+		target_addr_t entry_point, target_addr_t exit_point,
 		int timeout_ms, void *arch_info);
 
 int mips32_configure_break_unit(struct target *target);
@@ -246,9 +246,9 @@ int mips32_register_commands(struct command_context *cmd_ctx);
 int mips32_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
-int mips32_checksum_memory(struct target *target, uint32_t address,
+int mips32_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum);
 int mips32_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
+		target_addr_t address, uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 #endif /* OPENOCD_TARGET_MIPS32_H */
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 7cc0424..5456e8c 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -1035,7 +1035,7 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		ejtag_info->fast_access_save = write_t;
 	}
 
-	LOG_DEBUG("%s using 0x%.8" PRIx32 " for write handler", __func__, source->address);
+	LOG_DEBUG("%s using 0x%.8" TARGET_PRIxADDR " for write handler", __func__, source->address);
 
 	jmp_code[0] |= UPPER16(source->address);
 	jmp_code[1] |= LOWER16(source->address);
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index db69b95..bd3c602 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -44,7 +44,7 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 		uint32_t address, int handle_breakpoints,
 		int debug_execution);
 static int mips_m4k_halt(struct target *target);
-static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
+static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer);
 
 static int mips_m4k_examine_debug_reason(struct target *target)
@@ -469,7 +469,8 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT "",
+					  breakpoint->address);
 			mips_m4k_unset_breakpoint(target, breakpoint);
 			mips_m4k_single_step_core(target);
 			mips_m4k_set_breakpoint(target, breakpoint);
@@ -500,7 +501,7 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 }
 
 static int mips_m4k_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	int retval = ERROR_OK;
 
@@ -527,7 +528,7 @@ static int mips_m4k_resume(struct target *target, int current,
 }
 
 static int mips_m4k_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints)
+		target_addr_t address, int handle_breakpoints)
 {
 	/* get pointers to arch-specific information */
 	struct mips32_common *mips32 = target_to_mips32(target);
@@ -655,7 +656,7 @@ static int mips_m4k_set_breakpoint(struct target *target,
 			if (retval != ERROR_OK)
 				return retval;
 			if (verify != MIPS32_SDBBP) {
-				LOG_ERROR("Unable to set 32bit breakpoint at address %08" PRIx32
+				LOG_ERROR("Unable to set 32-bit breakpoint at address " TARGET_ADDR_FMT
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -674,7 +675,7 @@ static int mips_m4k_set_breakpoint(struct target *target,
 			if (retval != ERROR_OK)
 				return retval;
 			if (verify != MIPS16_SDBBP) {
-				LOG_ERROR("Unable to set 16bit breakpoint at address %08" PRIx32
+				LOG_ERROR("Unable to set 16-bit breakpoint at address " TARGET_ADDR_FMT
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -949,13 +950,13 @@ static void mips_m4k_enable_watchpoints(struct target *target)
 	}
 }
 
-static int mips_m4k_read_memory(struct target *target, uint32_t address,
+static int mips_m4k_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
@@ -1008,13 +1009,13 @@ static int mips_m4k_read_memory(struct target *target, uint32_t address,
 	return retval;
 }
 
-static int mips_m4k_write_memory(struct target *target, uint32_t address,
+static int mips_m4k_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "",
 			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
@@ -1139,7 +1140,7 @@ static int mips_m4k_examine(struct target *target)
 	return ERROR_OK;
 }
 
-static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
+static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
@@ -1148,7 +1149,8 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
 	int retval;
 	int write_t = 1;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32 "", address, count);
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", count: 0x%8.8" PRIx32 "",
+			  address, count);
 
 	/* check alignment */
 	if (address & 0x3u)
@@ -1175,8 +1177,8 @@ static int mips_m4k_bulk_write_memory(struct target *target, uint32_t address,
 
 	if (address <= fast_data_area->address + fast_data_area->size &&
 			fast_data_area->address <= address + count) {
-		LOG_ERROR("fast_data (0x%8.8" PRIx32 ") is within write area "
-			  "(0x%8.8" PRIx32 "-0x%8.8" PRIx32 ").",
+		LOG_ERROR("fast_data (" TARGET_ADDR_FMT ") is within write area "
+			  "(" TARGET_ADDR_FMT "-" TARGET_ADDR_FMT ").",
 			  fast_data_area->address, address, address + count);
 		LOG_ERROR("Change work-area-phys or load_image address!");
 		return ERROR_FAIL;
diff --git a/src/target/nds32.c b/src/target/nds32.c
index 2926b23..e4bb17f 100644
--- a/src/target/nds32.c
+++ b/src/target/nds32.c
@@ -823,7 +823,7 @@ int nds32_read_memory(struct target *target, uint32_t address,
 	return aice_read_mem_unit(aice, address, size, count, buffer);
 }
 
-int nds32_read_phys_memory(struct target *target, uint32_t address,
+int nds32_read_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct aice_port_s *aice = target_to_aice(target);
@@ -932,7 +932,7 @@ int nds32_write_memory(struct target *target, uint32_t address,
 	return aice_write_mem_unit(aice, address, size, count, buffer);
 }
 
-int nds32_write_phys_memory(struct target *target, uint32_t address,
+int nds32_write_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct aice_port_s *aice = target_to_aice(target);
@@ -1674,7 +1674,7 @@ int nds32_init_arch_info(struct target *target, struct nds32 *nds32)
 	return ERROR_OK;
 }
 
-int nds32_virtual_to_physical(struct target *target, uint32_t address, uint32_t *physical)
+int nds32_virtual_to_physical(struct target *target, target_addr_t address, target_addr_t *physical)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
 
@@ -1692,7 +1692,7 @@ int nds32_virtual_to_physical(struct target *target, uint32_t address, uint32_t
 	return ERROR_FAIL;
 }
 
-int nds32_cache_sync(struct target *target, uint32_t address, uint32_t length)
+int nds32_cache_sync(struct target *target, target_addr_t address, uint32_t length)
 {
 	struct aice_port_s *aice = target_to_aice(target);
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -1738,7 +1738,7 @@ int nds32_cache_sync(struct target *target, uint32_t address, uint32_t length)
 			/* Because PSW.IT is turned off under debug exception, address MUST
 			 * be physical address.  L1I_VA_INVALIDATE uses PSW.IT to decide
 			 * address translation or not. */
-			uint32_t physical_addr;
+			target_addr_t physical_addr;
 			if (ERROR_FAIL == target->type->virt2phys(target, cur_address,
 						&physical_addr))
 				return ERROR_FAIL;
@@ -1764,7 +1764,7 @@ uint32_t nds32_nextpc(struct nds32 *nds32, int current, uint32_t address)
 }
 
 int nds32_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints)
+		target_addr_t address, int handle_breakpoints)
 {
 	LOG_DEBUG("target->state: %s",
 			target_state_name(target));
@@ -1778,7 +1778,7 @@ int nds32_step(struct target *target, int current,
 
 	address = nds32_nextpc(nds32, current, address);
 
-	LOG_DEBUG("STEP PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("STEP PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 	/** set DSSIM */
 	uint32_t ir14_value;
@@ -2120,9 +2120,9 @@ int nds32_poll(struct target *target)
 }
 
 int nds32_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		target_addr_t address, int handle_breakpoints, int debug_execution)
 {
-	LOG_DEBUG("current %d address %08" PRIx32
+	LOG_DEBUG("current %d address %08" TARGET_PRIxADDR
 			" handle_breakpoints %d"
 			" debug_execution %d",
 			current, address, handle_breakpoints, debug_execution);
@@ -2136,7 +2136,7 @@ int nds32_resume(struct target *target, int current,
 
 	address = nds32_nextpc(nds32, current, address);
 
-	LOG_DEBUG("RESUME PC %08" PRIx32 "%s", address, !current ? "!" : "");
+	LOG_DEBUG("RESUME PC %08" TARGET_PRIxADDR "%s", address, !current ? "!" : "");
 
 	if (!debug_execution)
 		target_free_all_working_areas(target);
diff --git a/src/target/nds32.h b/src/target/nds32.h
index 88af4f3..141dbf4 100644
--- a/src/target/nds32.h
+++ b/src/target/nds32.h
@@ -400,23 +400,23 @@ extern int nds32_get_mapped_reg(struct nds32 *nds32, unsigned regnum, uint32_t *
 extern int nds32_set_mapped_reg(struct nds32 *nds32, unsigned regnum, uint32_t value);
 
 extern int nds32_edm_config(struct nds32 *nds32);
-extern int nds32_cache_sync(struct target *target, uint32_t address, uint32_t length);
+extern int nds32_cache_sync(struct target *target, target_addr_t address, uint32_t length);
 extern int nds32_mmu(struct target *target, int *enabled);
-extern int nds32_virtual_to_physical(struct target *target, uint32_t address,
-		uint32_t *physical);
-extern int nds32_read_phys_memory(struct target *target, uint32_t address,
+extern int nds32_virtual_to_physical(struct target *target, target_addr_t address,
+		target_addr_t *physical);
+extern int nds32_read_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
-extern int nds32_write_phys_memory(struct target *target, uint32_t address,
+extern int nds32_write_phys_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
 extern uint32_t nds32_nextpc(struct nds32 *nds32, int current, uint32_t address);
 extern int nds32_examine_debug_reason(struct nds32 *nds32);
 extern int nds32_step(struct target *target, int current,
-		uint32_t address, int handle_breakpoints);
+		target_addr_t address, int handle_breakpoints);
 extern int nds32_target_state(struct nds32 *nds32, enum target_state *state);
 extern int nds32_halt(struct target *target);
 extern int nds32_poll(struct target *target);
 extern int nds32_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution);
+		target_addr_t address, int handle_breakpoints, int debug_execution);
 extern int nds32_assert_reset(struct target *target);
 extern int nds32_init(struct nds32 *nds32);
 extern int nds32_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *fileio_info);
diff --git a/src/target/nds32_aice.c b/src/target/nds32_aice.c
index bdfafb5..e494a3e 100644
--- a/src/target/nds32_aice.c
+++ b/src/target/nds32_aice.c
@@ -42,8 +42,8 @@ int aice_write_reg_64(struct aice_port_s *aice, uint32_t num, uint64_t val)
 	return aice->port->api->write_reg_64(aice->coreid, num, val);
 }
 
-int aice_read_tlb(struct aice_port_s *aice, uint32_t virtual_address,
-		uint32_t *physical_address)
+int aice_read_tlb(struct aice_port_s *aice, target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	if (aice->port->api->read_tlb == NULL) {
 		LOG_WARNING("Not implemented: %s", __func__);
diff --git a/src/target/nds32_aice.h b/src/target/nds32_aice.h
index ae801ed..5ea3b16 100644
--- a/src/target/nds32_aice.h
+++ b/src/target/nds32_aice.h
@@ -23,8 +23,8 @@
 
 int aice_read_reg_64(struct aice_port_s *aice, uint32_t num, uint64_t *val);
 int aice_write_reg_64(struct aice_port_s *aice, uint32_t num, uint64_t val);
-int aice_read_tlb(struct aice_port_s *aice, uint32_t virtual_address,
-		uint32_t *physical_address);
+int aice_read_tlb(struct aice_port_s *aice, target_addr_t virtual_address,
+		target_addr_t *physical_address);
 int aice_cache_ctl(struct aice_port_s *aice, uint32_t subtype, uint32_t address);
 int aice_set_retry_times(struct aice_port_s *aice, uint32_t a_retry_times);
 int aice_program_edm(struct aice_port_s *aice, char *command_sequence);
diff --git a/src/target/nds32_tlb.c b/src/target/nds32_tlb.c
index 6a91a0f..c4bce1a 100644
--- a/src/target/nds32_tlb.c
+++ b/src/target/nds32_tlb.c
@@ -22,8 +22,8 @@
 #include "nds32_aice.h"
 #include "nds32_tlb.h"
 
-int nds32_probe_tlb(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address)
+int nds32_probe_tlb(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	struct target *target = nds32->target;
 	struct aice_port_s *aice = target_to_aice(target);
@@ -38,8 +38,8 @@ struct page_table_walker_info_s page_table_info[PAGE_SIZE_NUM] = {
 	{0xFF000000, 22, 0x00FFE000, 11, 0x00001FFF, 0xFFFFF000, 0xFFFFE000, 0xFFFFE000},
 };
 
-int nds32_walk_page_table(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address)
+int nds32_walk_page_table(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address)
 {
 	struct target *target = nds32->target;
 	uint32_t value_mr1;
diff --git a/src/target/nds32_tlb.h b/src/target/nds32_tlb.h
index ada2c19..62512c1 100644
--- a/src/target/nds32_tlb.h
+++ b/src/target/nds32_tlb.h
@@ -39,9 +39,9 @@ struct page_table_walker_info_s {
 	uint32_t ppn_mask;
 };
 
-extern int nds32_probe_tlb(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address);
-extern int nds32_walk_page_table(struct nds32 *nds32, const uint32_t virtual_address,
-		uint32_t *physical_address);
+extern int nds32_probe_tlb(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address);
+extern int nds32_walk_page_table(struct nds32 *nds32, const target_addr_t virtual_address,
+		target_addr_t *physical_address);
 
 #endif /* OPENOCD_TARGET_NDS32_TLB_H */
diff --git a/src/target/nds32_v2.c b/src/target/nds32_v2.c
index 4497551..29489a0 100644
--- a/src/target/nds32_v2.c
+++ b/src/target/nds32_v2.c
@@ -112,7 +112,7 @@ static int nds32_v2_activate_hardware_breakpoint(struct target *target)
 				/* enable breakpoint (physical address) */
 				aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + hbr_index, 0xA);
 
-			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 					bp->address);
 
 			hbr_index++;
@@ -139,7 +139,7 @@ static int nds32_v2_deactivate_hardware_breakpoint(struct target *target)
 		else
 			return ERROR_FAIL;
 
-		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 				bp->address);
 
 		hbr_index++;
@@ -184,7 +184,7 @@ static int nds32_v2_activate_hardware_watchpoint(struct target *target)
 		/* set value */
 		aice_write_debug_reg(aice, NDS_EDM_SR_BPV0 + wp_num, 0);
 
-		LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32, wp_num,
+		LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR " mask %08" PRIx32, wp_num,
 				wp->address, wp->mask);
 
 	}
@@ -204,7 +204,7 @@ static int nds32_v2_deactivate_hardware_watchpoint(struct target *target)
 		/* disable watchpoint */
 		aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-		LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
+		LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR " mask %08" PRIx32,
 				wp_num, wp->address, wp->mask);
 	}
 
@@ -405,7 +405,7 @@ static int nds32_v2_deassert_reset(struct target *target)
 }
 
 static int nds32_v2_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum)
+		target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	LOG_WARNING("Not implemented: %s", __func__);
 
@@ -561,8 +561,8 @@ static int nds32_v2_run_algorithm(struct target *target,
 		struct mem_param *mem_params,
 		int num_reg_params,
 		struct reg_param *reg_params,
-		uint32_t entry_point,
-		uint32_t exit_point,
+		target_addr_t entry_point,
+		target_addr_t exit_point,
 		int timeout_ms,
 		void *arch_info)
 {
@@ -635,11 +635,11 @@ static int nds32_v2_examine(struct target *target)
 	return ERROR_OK;
 }
 
-static int nds32_v2_translate_address(struct target *target, uint32_t *address)
+static int nds32_v2_translate_address(struct target *target, target_addr_t *address)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
 	struct nds32_memory *memory = &(nds32->memory);
-	uint32_t physical_address;
+	target_addr_t physical_address;
 
 	/* Following conditions need to do address translation
 	 * 1. BUS mode
@@ -656,7 +656,7 @@ static int nds32_v2_translate_address(struct target *target, uint32_t *address)
 	return ERROR_OK;
 }
 
-static int nds32_v2_read_buffer(struct target *target, uint32_t address,
+static int nds32_v2_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -676,7 +676,7 @@ static int nds32_v2_read_buffer(struct target *target, uint32_t address,
 	return nds32_read_buffer(target, address, size, buffer);
 }
 
-static int nds32_v2_write_buffer(struct target *target, uint32_t address,
+static int nds32_v2_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -696,7 +696,7 @@ static int nds32_v2_write_buffer(struct target *target, uint32_t address,
 	return nds32_write_buffer(target, address, size, buffer);
 }
 
-static int nds32_v2_read_memory(struct target *target, uint32_t address,
+static int nds32_v2_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -716,7 +716,7 @@ static int nds32_v2_read_memory(struct target *target, uint32_t address,
 	return nds32_read_memory(target, address, size, count, buffer);
 }
 
-static int nds32_v2_write_memory(struct target *target, uint32_t address,
+static int nds32_v2_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
diff --git a/src/target/nds32_v3.c b/src/target/nds32_v3.c
index 35b72e3..e5d146b 100644
--- a/src/target/nds32_v3.c
+++ b/src/target/nds32_v3.c
@@ -53,7 +53,7 @@ static int nds32_v3_activate_hardware_breakpoint(struct target *target)
 				/* enable breakpoint (physical address) */
 				aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + hbr_index, 0xA);
 
-			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+			LOG_DEBUG("Add hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 					bp->address);
 		} else {
 			return ERROR_FAIL;
@@ -81,7 +81,7 @@ static int nds32_v3_deactivate_hardware_breakpoint(struct target *target)
 			return ERROR_FAIL;
 		}
 
-		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" PRIx32, hbr_index,
+		LOG_DEBUG("Remove hardware BP %" PRId32 " at %08" TARGET_PRIxADDR, hbr_index,
 				bp->address);
 	}
 
@@ -128,7 +128,7 @@ static int nds32_v3_activate_hardware_watchpoint(struct target *target)
 			/* set value */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPV0 + wp_num, 0);
 
-			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" PRIx32 " mask %08" PRIx32,
+			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR " mask %08" PRIx32,
 					wp_num, wp->address, wp->mask);
 
 			wp_num++;
@@ -169,7 +169,7 @@ static int nds32_v3_deactivate_hardware_watchpoint(struct target *target)
 			/* disable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR
 					" mask %08" PRIx32, wp_num,
 					wp->address, wp->mask);
 			wp_num++;
diff --git a/src/target/nds32_v3_common.c b/src/target/nds32_v3_common.c
index 191f4b5..271ffdd 100644
--- a/src/target/nds32_v3_common.c
+++ b/src/target/nds32_v3_common.c
@@ -368,7 +368,7 @@ int nds32_v3_target_request_data(struct target *target,
 }
 
 int nds32_v3_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum)
+		target_addr_t address, uint32_t count, uint32_t *checksum)
 {
 	LOG_WARNING("Not implemented: %s", __func__);
 
@@ -434,8 +434,8 @@ int nds32_v3_run_algorithm(struct target *target,
 		struct mem_param *mem_params,
 		int num_reg_params,
 		struct reg_param *reg_params,
-		uint32_t entry_point,
-		uint32_t exit_point,
+		target_addr_t entry_point,
+		target_addr_t exit_point,
 		int timeout_ms,
 		void *arch_info)
 {
@@ -444,7 +444,7 @@ int nds32_v3_run_algorithm(struct target *target,
 	return ERROR_FAIL;
 }
 
-int nds32_v3_read_buffer(struct target *target, uint32_t address,
+int nds32_v3_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -456,7 +456,7 @@ int nds32_v3_read_buffer(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
@@ -502,7 +502,7 @@ int nds32_v3_read_buffer(struct target *target, uint32_t address,
 	return result;
 }
 
-int nds32_v3_write_buffer(struct target *target, uint32_t address,
+int nds32_v3_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -514,7 +514,7 @@ int nds32_v3_write_buffer(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
@@ -564,7 +564,7 @@ int nds32_v3_write_buffer(struct target *target, uint32_t address,
 	return nds32_write_buffer(target, address, size, buffer);
 }
 
-int nds32_v3_read_memory(struct target *target, uint32_t address,
+int nds32_v3_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -576,7 +576,7 @@ int nds32_v3_read_memory(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
@@ -622,7 +622,7 @@ int nds32_v3_read_memory(struct target *target, uint32_t address,
 	return result;
 }
 
-int nds32_v3_write_memory(struct target *target, uint32_t address,
+int nds32_v3_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct nds32 *nds32 = target_to_nds32(target);
@@ -634,7 +634,7 @@ int nds32_v3_write_memory(struct target *target, uint32_t address,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	uint32_t physical_address;
+	target_addr_t physical_address;
 	/* BUG: If access range crosses multiple pages, the translation will not correct
 	 * for second page or so. */
 
diff --git a/src/target/nds32_v3_common.h b/src/target/nds32_v3_common.h
index 1f5df19..23393e5 100644
--- a/src/target/nds32_v3_common.h
+++ b/src/target/nds32_v3_common.h
@@ -34,7 +34,7 @@ void nds32_v3_common_register_callback(struct nds32_v3_common_callback *callback
 int nds32_v3_target_request_data(struct target *target,
 		uint32_t size, uint8_t *buffer);
 int nds32_v3_checksum_memory(struct target *target,
-		uint32_t address, uint32_t count, uint32_t *checksum);
+		target_addr_t address, uint32_t count, uint32_t *checksum);
 int nds32_v3_hit_watchpoint(struct target *target,
 		struct watchpoint **hit_watchpoint);
 int nds32_v3_target_create_common(struct target *target, struct nds32 *nds32);
@@ -43,17 +43,17 @@ int nds32_v3_run_algorithm(struct target *target,
 		struct mem_param *mem_params,
 		int num_reg_params,
 		struct reg_param *reg_params,
-		uint32_t entry_point,
-		uint32_t exit_point,
+		target_addr_t entry_point,
+		target_addr_t exit_point,
 		int timeout_ms,
 		void *arch_info);
-int nds32_v3_read_buffer(struct target *target, uint32_t address,
+int nds32_v3_read_buffer(struct target *target, target_addr_t address,
 		uint32_t size, uint8_t *buffer);
-int nds32_v3_write_buffer(struct target *target, uint32_t address,
+int nds32_v3_write_buffer(struct target *target, target_addr_t address,
 		uint32_t size, const uint8_t *buffer);
-int nds32_v3_read_memory(struct target *target, uint32_t address,
+int nds32_v3_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer);
-int nds32_v3_write_memory(struct target *target, uint32_t address,
+int nds32_v3_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer);
 int nds32_v3_init_target(struct command_context *cmd_ctx,
 		struct target *target);
diff --git a/src/target/nds32_v3m.c b/src/target/nds32_v3m.c
index 2d52bc3..86903a5 100644
--- a/src/target/nds32_v3m.c
+++ b/src/target/nds32_v3m.c
@@ -50,7 +50,7 @@ static int nds32_v3m_activate_hardware_breakpoint(struct target *target)
 				/* enable breakpoint (physical address) */
 				aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + brp_num, 0xA);
 
-			LOG_DEBUG("Add hardware BP %u at %08" PRIx32, brp_num,
+			LOG_DEBUG("Add hardware BP %u at %08" TARGET_PRIxADDR, brp_num,
 					bp->address);
 
 			brp_num--;
@@ -78,7 +78,7 @@ static int nds32_v3m_deactivate_hardware_breakpoint(struct target *target)
 		else
 			return ERROR_FAIL;
 
-		LOG_DEBUG("Remove hardware BP %u at %08" PRIx32, brp_num,
+		LOG_DEBUG("Remove hardware BP %u at %08" TARGET_PRIxADDR, brp_num,
 				bp->address);
 
 		brp_num--;
@@ -125,7 +125,7 @@ static int nds32_v3m_activate_hardware_watchpoint(struct target *target)
 			/* enable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, wp_config);
 
-			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Add hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR
 					" mask %08" PRIx32, wp_num, wp->address, wp->mask);
 
 			wp_num++;
@@ -166,7 +166,7 @@ static int nds32_v3m_deactivate_hardware_watchpoint(struct target *target)
 			/* disable watchpoint */
 			aice_write_debug_reg(aice, NDS_EDM_SR_BPC0 + wp_num, 0x0);
 
-			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" PRIx32
+			LOG_DEBUG("Remove hardware watchpoint %" PRId32 " at %08" TARGET_PRIxADDR
 					" mask %08" PRIx32, wp_num, wp->address, wp->mask);
 			wp_num++;
 		} else if (nds32_v3m->nds32.global_stop) {
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index 2cc869f..3895ddf 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -861,7 +861,7 @@ static int or1k_resume_or_step(struct target *target, int current,
 		/* Single step past breakpoint at current address */
 		breakpoint = breakpoint_find(target, resume_pc);
 		if (breakpoint) {
-			LOG_DEBUG("Unset breakpoint at 0x%08" PRIx32, breakpoint->address);
+			LOG_DEBUG("Unset breakpoint at 0x%08" TARGET_PRIxADDR, breakpoint->address);
 			retval = or1k_remove_breakpoint(target, breakpoint);
 			if (retval != ERROR_OK)
 				return retval;
@@ -897,7 +897,8 @@ static int or1k_resume_or_step(struct target *target, int current,
 }
 
 static int or1k_resume(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		       target_addr_t address, int handle_breakpoints,
+		       int debug_execution)
 {
 	return or1k_resume_or_step(target, current, address,
 				   handle_breakpoints,
@@ -906,7 +907,7 @@ static int or1k_resume(struct target *target, int current,
 }
 
 static int or1k_step(struct target *target, int current,
-		     uint32_t address, int handle_breakpoints)
+		     target_addr_t address, int handle_breakpoints)
 {
 	return or1k_resume_or_step(target, current, address,
 				   handle_breakpoints,
@@ -922,7 +923,7 @@ static int or1k_add_breakpoint(struct target *target,
 	struct or1k_du *du_core = or1k_to_du(or1k);
 	uint8_t data;
 
-	LOG_DEBUG("Adding breakpoint: addr 0x%08" PRIx32 ", len %d, type %d, set: %d, id: %" PRId32,
+	LOG_DEBUG("Adding breakpoint: addr 0x%08" TARGET_PRIxADDR ", len %d, type %d, set: %d, id: %" PRId32,
 		  breakpoint->address, breakpoint->length, breakpoint->type,
 		  breakpoint->set, breakpoint->unique_id);
 
@@ -937,7 +938,7 @@ static int or1k_add_breakpoint(struct target *target,
 					 1,
 					 &data);
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Error while reading the instruction at 0x%08" PRIx32,
+		LOG_ERROR("Error while reading the instruction at 0x%08" TARGET_PRIxADDR,
 			   breakpoint->address);
 		return retval;
 	}
@@ -958,14 +959,15 @@ static int or1k_add_breakpoint(struct target *target,
 					  or1k_trap_insn);
 
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Error while writing OR1K_TRAP_INSTR at 0x%08" PRIx32,
+		LOG_ERROR("Error while writing OR1K_TRAP_INSTR at 0x%08" TARGET_PRIxADDR,
 			   breakpoint->address);
 		return retval;
 	}
 
 	/* invalidate instruction cache */
+	uint32_t addr = breakpoint->address;
 	retval = du_core->or1k_jtag_write_cpu(&or1k->jtag,
-			OR1K_ICBIR_CPU_REG_ADD, 1, &breakpoint->address);
+			OR1K_ICBIR_CPU_REG_ADD, 1, &addr);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while invalidating the ICACHE");
 		return retval;
@@ -980,7 +982,7 @@ static int or1k_remove_breakpoint(struct target *target,
 	struct or1k_common *or1k = target_to_or1k(target);
 	struct or1k_du *du_core = or1k_to_du(or1k);
 
-	LOG_DEBUG("Removing breakpoint: addr 0x%08" PRIx32 ", len %d, type %d, set: %d, id: %" PRId32,
+	LOG_DEBUG("Removing breakpoint: addr 0x%08" TARGET_PRIxADDR ", len %d, type %d, set: %d, id: %" PRId32,
 		  breakpoint->address, breakpoint->length, breakpoint->type,
 		  breakpoint->set, breakpoint->unique_id);
 
@@ -996,14 +998,15 @@ static int or1k_remove_breakpoint(struct target *target,
 					  breakpoint->orig_instr);
 
 	if (retval != ERROR_OK) {
-		LOG_ERROR("Error while writing back the instruction at 0x%08" PRIx32,
+		LOG_ERROR("Error while writing back the instruction at 0x%08" TARGET_PRIxADDR,
 			   breakpoint->address);
 		return retval;
 	}
 
 	/* invalidate instruction cache */
+	uint32_t addr = breakpoint->address;
 	retval = du_core->or1k_jtag_write_cpu(&or1k->jtag,
-			OR1K_ICBIR_CPU_REG_ADD, 1, &breakpoint->address);
+			OR1K_ICBIR_CPU_REG_ADD, 1, &addr);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("Error while invalidating the ICACHE");
 		return retval;
@@ -1026,13 +1029,13 @@ static int or1k_remove_watchpoint(struct target *target,
 	return ERROR_OK;
 }
 
-static int or1k_read_memory(struct target *target, uint32_t address,
+static int or1k_read_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct or1k_common *or1k = target_to_or1k(target);
 	struct or1k_du *du_core = or1k_to_du(or1k);
 
-	LOG_DEBUG("Read memory at 0x%08" PRIx32 ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
+	LOG_DEBUG("Read memory at 0x%08" TARGET_PRIxADDR ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_ERROR("Target not halted");
@@ -1053,13 +1056,13 @@ static int or1k_read_memory(struct target *target, uint32_t address,
 	return du_core->or1k_jtag_read_memory(&or1k->jtag, address, size, count, buffer);
 }
 
-static int or1k_write_memory(struct target *target, uint32_t address,
+static int or1k_write_memory(struct target *target, target_addr_t address,
 		uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct or1k_common *or1k = target_to_or1k(target);
 	struct or1k_du *du_core = or1k_to_du(or1k);
 
-	LOG_DEBUG("Write memory at 0x%08" PRIx32 ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
+	LOG_DEBUG("Write memory at 0x%08" TARGET_PRIxADDR ", size: %" PRIu32 ", count: 0x%08" PRIx32, address, size, count);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("Target not halted");
@@ -1203,7 +1206,7 @@ int or1k_get_gdb_fileio_info(struct target *target, struct gdb_fileio_info *file
 	return ERROR_FAIL;
 }
 
-static int or1k_checksum_memory(struct target *target, uint32_t address,
+static int or1k_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum) {
 
 	return ERROR_FAIL;
diff --git a/src/target/target.c b/src/target/target.c
index c8a3ce4..40baf8e 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -58,9 +58,9 @@
 /* default halt wait timeout (ms) */
 #define DEFAULT_HALT_TIMEOUT 5000
 
-static int target_read_buffer_default(struct target *target, uint32_t address,
+static int target_read_buffer_default(struct target *target, target_addr_t address,
 		uint32_t count, uint8_t *buffer);
-static int target_write_buffer_default(struct target *target, uint32_t address,
+static int target_write_buffer_default(struct target *target, target_addr_t address,
 		uint32_t count, const uint8_t *buffer);
 static int target_array2mem(Jim_Interp *interp, struct target *target,
 		int argc, Jim_Obj * const *argv);
@@ -597,7 +597,8 @@ int target_halt(struct target *target)
  * hand the infrastructure for running such helpers might use this
  * procedure but rely on hardware breakpoint to detect termination.)
  */
-int target_resume(struct target *target, int current, uint32_t address, int handle_breakpoints, int debug_execution)
+int target_resume(struct target *target, int current, target_addr_t address,
+		int handle_breakpoints, int debug_execution)
 {
 	int retval;
 
@@ -668,7 +669,7 @@ static int target_process_reset(struct command_context *cmd_ctx, enum target_res
 }
 
 static int identity_virt2phys(struct target *target,
-		uint32_t virtual, uint32_t *physical)
+		target_addr_t virtual, target_addr_t *physical)
 {
 	*physical = virtual;
 	return ERROR_OK;
@@ -1034,7 +1035,7 @@ int target_run_flash_async_algorithm(struct target *target,
 }
 
 int target_read_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1048,7 +1049,7 @@ int target_read_memory(struct target *target,
 }
 
 int target_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1062,7 +1063,7 @@ int target_read_phys_memory(struct target *target,
 }
 
 int target_write_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1076,7 +1077,7 @@ int target_write_memory(struct target *target,
 }
 
 int target_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -1164,7 +1165,7 @@ int target_get_gdb_reg_list(struct target *target,
 	return target->type->get_gdb_reg_list(target, reg_list, reg_list_size, reg_class);
 }
 int target_step(struct target *target,
-		int current, uint32_t address, int handle_breakpoints)
+		int current, target_addr_t address, int handle_breakpoints)
 {
 	return target->type->step(target, current, address, handle_breakpoints);
 }
@@ -1635,7 +1636,7 @@ static void print_wa_layout(struct target *target)
 	struct working_area *c = target->working_areas;
 
 	while (c) {
-		LOG_DEBUG("%c%c 0x%08"PRIx32"-0x%08"PRIx32" (%"PRIu32" bytes)",
+		LOG_DEBUG("%c%c " TARGET_ADDR_FMT "-" TARGET_ADDR_FMT " (%" PRIu32 " bytes)",
 			c->backup ? 'b' : ' ', c->free ? ' ' : '*',
 			c->address, c->address + c->size - 1, c->size);
 		c = c->next;
@@ -1720,7 +1721,7 @@ int target_alloc_working_area_try(struct target *target, uint32_t size, struct w
 		if (!enabled) {
 			if (target->working_area_phys_spec) {
 				LOG_DEBUG("MMU disabled, using physical "
-					"address for working memory 0x%08"PRIx32,
+					"address for working memory " TARGET_ADDR_FMT,
 					target->working_area_phys);
 				target->working_area = target->working_area_phys;
 			} else {
@@ -1731,7 +1732,7 @@ int target_alloc_working_area_try(struct target *target, uint32_t size, struct w
 		} else {
 			if (target->working_area_virt_spec) {
 				LOG_DEBUG("MMU enabled, using virtual "
-					"address for working memory 0x%08"PRIx32,
+					"address for working memory " TARGET_ADDR_FMT,
 					target->working_area_virt);
 				target->working_area = target->working_area_virt;
 			} else {
@@ -1774,7 +1775,8 @@ int target_alloc_working_area_try(struct target *target, uint32_t size, struct w
 	/* Split the working area into the requested size */
 	target_split_working_area(c, size);
 
-	LOG_DEBUG("allocated new working area of %"PRIu32" bytes at address 0x%08"PRIx32, size, c->address);
+	LOG_DEBUG("allocated new working area of %" PRIu32 " bytes at address " TARGET_ADDR_FMT,
+			  size, c->address);
 
 	if (target->backup_working_area) {
 		if (c->backup == NULL) {
@@ -1818,7 +1820,7 @@ static int target_restore_working_area(struct target *target, struct working_are
 	if (target->backup_working_area && area->backup != NULL) {
 		retval = target_write_memory(target, area->address, 4, area->size / 4, area->backup);
 		if (retval != ERROR_OK)
-			LOG_ERROR("failed to restore %"PRIu32" bytes of working area at address 0x%08"PRIx32,
+			LOG_ERROR("failed to restore %" PRIu32 " bytes of working area at address " TARGET_ADDR_FMT,
 					area->size, area->address);
 	}
 
@@ -1842,7 +1844,7 @@ static int target_free_working_area_restore(struct target *target, struct workin
 
 	area->free = true;
 
-	LOG_DEBUG("freed %"PRIu32" bytes of working area at address 0x%08"PRIx32,
+	LOG_DEBUG("freed %" PRIu32 " bytes of working area at address " TARGET_ADDR_FMT,
 			area->size, area->address);
 
 	/* mark user pointer invalid */
@@ -2024,9 +2026,9 @@ static int target_profiling_default(struct target *target, uint32_t *samples,
  * mode respectively, otherwise data is handled as quickly as
  * possible
  */
-int target_write_buffer(struct target *target, uint32_t address, uint32_t size, const uint8_t *buffer)
+int target_write_buffer(struct target *target, target_addr_t address, uint32_t size, const uint8_t *buffer)
 {
-	LOG_DEBUG("writing buffer of %" PRIi32 " byte at 0x%8.8" PRIx32,
+	LOG_DEBUG("writing buffer of %" PRIi32 " byte at " TARGET_ADDR_FMT,
 			  size, address);
 
 	if (!target_was_examined(target)) {
@@ -2039,7 +2041,7 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
 
 	if ((address + size - 1) < address) {
 		/* GDB can request this when e.g. PC is 0xfffffffc */
-		LOG_ERROR("address + size wrapped (0x%08" PRIx32 ", 0x%08" PRIx32 ")",
+		LOG_ERROR("address + size wrapped (" TARGET_ADDR_FMT ", 0x%08" PRIx32 ")",
 				  address,
 				  size);
 		return ERROR_FAIL;
@@ -2048,7 +2050,8 @@ int target_write_buffer(struct target *target, uint32_t address, uint32_t size,
 	return target->type->write_buffer(target, address, size, buffer);
 }
 
-static int target_write_buffer_default(struct target *target, uint32_t address, uint32_t count, const uint8_t *buffer)
+static int target_write_buffer_default(struct target *target,
+	target_addr_t address, uint32_t count, const uint8_t *buffer)
 {
 	uint32_t size;
 
@@ -2085,9 +2088,9 @@ static int target_write_buffer_default(struct target *target, uint32_t address,
  * mode respectively, otherwise data is handled as quickly as
  * possible
  */
-int target_read_buffer(struct target *target, uint32_t address, uint32_t size, uint8_t *buffer)
+int target_read_buffer(struct target *target, target_addr_t address, uint32_t size, uint8_t *buffer)
 {
-	LOG_DEBUG("reading buffer of %" PRIi32 " byte at 0x%8.8" PRIx32,
+	LOG_DEBUG("reading buffer of %" PRIi32 " byte at " TARGET_ADDR_FMT,
 			  size, address);
 
 	if (!target_was_examined(target)) {
@@ -2100,7 +2103,7 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
 
 	if ((address + size - 1) < address) {
 		/* GDB can request this when e.g. PC is 0xfffffffc */
-		LOG_ERROR("address + size wrapped (0x%08" PRIx32 ", 0x%08" PRIx32 ")",
+		LOG_ERROR("address + size wrapped (" TARGET_ADDR_FMT ", 0x%08" PRIx32 ")",
 				  address,
 				  size);
 		return ERROR_FAIL;
@@ -2109,7 +2112,7 @@ int target_read_buffer(struct target *target, uint32_t address, uint32_t size, u
 	return target->type->read_buffer(target, address, size, buffer);
 }
 
-static int target_read_buffer_default(struct target *target, uint32_t address, uint32_t count, uint8_t *buffer)
+static int target_read_buffer_default(struct target *target, target_addr_t address, uint32_t count, uint8_t *buffer)
 {
 	uint32_t size;
 
@@ -2142,7 +2145,7 @@ static int target_read_buffer_default(struct target *target, uint32_t address, u
 	return ERROR_OK;
 }
 
-int target_checksum_memory(struct target *target, uint32_t address, uint32_t size, uint32_t* crc)
+int target_checksum_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t* crc)
 {
 	uint8_t *buffer;
 	int retval;
@@ -2182,7 +2185,7 @@ int target_checksum_memory(struct target *target, uint32_t address, uint32_t siz
 	return retval;
 }
 
-int target_blank_check_memory(struct target *target, uint32_t address, uint32_t size, uint32_t* blank,
+int target_blank_check_memory(struct target *target, target_addr_t address, uint32_t size, uint32_t* blank,
 	uint8_t erased_value)
 {
 	int retval;
@@ -2199,7 +2202,7 @@ int target_blank_check_memory(struct target *target, uint32_t address, uint32_t
 	return retval;
 }
 
-int target_read_u64(struct target *target, uint64_t address, uint64_t *value)
+int target_read_u64(struct target *target, target_addr_t address, uint64_t *value)
 {
 	uint8_t value_buf[8];
 	if (!target_was_examined(target)) {
@@ -2211,19 +2214,19 @@ int target_read_u64(struct target *target, uint64_t address, uint64_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u64(target, value_buf);
-		LOG_DEBUG("address: 0x%" PRIx64 ", value: 0x%16.16" PRIx64 "",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%16.16" PRIx64 "",
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%" PRIx64 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_read_u32(struct target *target, uint32_t address, uint32_t *value)
+int target_read_u32(struct target *target, target_addr_t address, uint32_t *value)
 {
 	uint8_t value_buf[4];
 	if (!target_was_examined(target)) {
@@ -2235,19 +2238,19 @@ int target_read_u32(struct target *target, uint32_t address, uint32_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u32(target, value_buf);
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8" PRIx32 "",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx32 "",
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%8.8" PRIx32 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_read_u16(struct target *target, uint32_t address, uint16_t *value)
+int target_read_u16(struct target *target, target_addr_t address, uint16_t *value)
 {
 	uint8_t value_buf[2];
 	if (!target_was_examined(target)) {
@@ -2259,19 +2262,19 @@ int target_read_u16(struct target *target, uint32_t address, uint16_t *value)
 
 	if (retval == ERROR_OK) {
 		*value = target_buffer_get_u16(target, value_buf);
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%4.4" PRIx16,
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%4.4" PRIx16,
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%8.8" PRIx32 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
+int target_read_u8(struct target *target, target_addr_t address, uint8_t *value)
 {
 	if (!target_was_examined(target)) {
 		LOG_ERROR("Target not examined yet");
@@ -2281,19 +2284,19 @@ int target_read_u8(struct target *target, uint32_t address, uint8_t *value)
 	int retval = target_read_memory(target, address, 1, 1, value);
 
 	if (retval == ERROR_OK) {
-		LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2" PRIx8,
+		LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%2.2" PRIx8,
 				  address,
 				  *value);
 	} else {
 		*value = 0x0;
-		LOG_DEBUG("address: 0x%8.8" PRIx32 " failed",
+		LOG_DEBUG("address: " TARGET_ADDR_FMT " failed",
 				  address);
 	}
 
 	return retval;
 }
 
-int target_write_u64(struct target *target, uint64_t address, uint64_t value)
+int target_write_u64(struct target *target, target_addr_t address, uint64_t value)
 {
 	int retval;
 	uint8_t value_buf[8];
@@ -2302,7 +2305,7 @@ int target_write_u64(struct target *target, uint64_t address, uint64_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%" PRIx64 ", value: 0x%16.16" PRIx64 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%16.16" PRIx64 "",
 			  address,
 			  value);
 
@@ -2314,7 +2317,7 @@ int target_write_u64(struct target *target, uint64_t address, uint64_t value)
 	return retval;
 }
 
-int target_write_u32(struct target *target, uint32_t address, uint32_t value)
+int target_write_u32(struct target *target, target_addr_t address, uint32_t value)
 {
 	int retval;
 	uint8_t value_buf[4];
@@ -2323,7 +2326,7 @@ int target_write_u32(struct target *target, uint32_t address, uint32_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8" PRIx32 "",
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx32 "",
 			  address,
 			  value);
 
@@ -2335,7 +2338,7 @@ int target_write_u32(struct target *target, uint32_t address, uint32_t value)
 	return retval;
 }
 
-int target_write_u16(struct target *target, uint32_t address, uint16_t value)
+int target_write_u16(struct target *target, target_addr_t address, uint16_t value)
 {
 	int retval;
 	uint8_t value_buf[2];
@@ -2344,7 +2347,7 @@ int target_write_u16(struct target *target, uint32_t address, uint16_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%8.8" PRIx16,
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx16,
 			  address,
 			  value);
 
@@ -2356,7 +2359,7 @@ int target_write_u16(struct target *target, uint32_t address, uint16_t value)
 	return retval;
 }
 
-int target_write_u8(struct target *target, uint32_t address, uint8_t value)
+int target_write_u8(struct target *target, target_addr_t address, uint8_t value)
 {
 	int retval;
 	if (!target_was_examined(target)) {
@@ -2364,7 +2367,7 @@ int target_write_u8(struct target *target, uint32_t address, uint8_t value)
 		return ERROR_FAIL;
 	}
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", value: 0x%2.2" PRIx8,
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%2.2" PRIx8,
 			  address, value);
 
 	retval = target_write_memory(target, address, 1, 1, &value);
@@ -2374,6 +2377,87 @@ int target_write_u8(struct target *target, uint32_t address, uint8_t value)
 	return retval;
 }
 
+int target_write_phys_u64(struct target *target, target_addr_t address, uint64_t value)
+{
+	int retval;
+	uint8_t value_buf[8];
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%16.16" PRIx64 "",
+			  address,
+			  value);
+
+	target_buffer_set_u64(target, value_buf, value);
+	retval = target_write_phys_memory(target, address, 8, 1, value_buf);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
+int target_write_phys_u32(struct target *target, target_addr_t address, uint32_t value)
+{
+	int retval;
+	uint8_t value_buf[4];
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx32 "",
+			  address,
+			  value);
+
+	target_buffer_set_u32(target, value_buf, value);
+	retval = target_write_phys_memory(target, address, 4, 1, value_buf);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
+int target_write_phys_u16(struct target *target, target_addr_t address, uint16_t value)
+{
+	int retval;
+	uint8_t value_buf[2];
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%8.8" PRIx16,
+			  address,
+			  value);
+
+	target_buffer_set_u16(target, value_buf, value);
+	retval = target_write_phys_memory(target, address, 2, 1, value_buf);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
+int target_write_phys_u8(struct target *target, target_addr_t address, uint8_t value)
+{
+	int retval;
+	if (!target_was_examined(target)) {
+		LOG_ERROR("Target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", value: 0x%2.2" PRIx8,
+			  address, value);
+
+	retval = target_write_phys_memory(target, address, 1, 1, &value);
+	if (retval != ERROR_OK)
+		LOG_DEBUG("failed: %i", retval);
+
+	return retval;
+}
+
 static int find_target(struct command_context *cmd_ctx, const char *name)
 {
 	struct target *target = get_target(name);
@@ -2877,9 +2961,9 @@ COMMAND_HANDLER(handle_resume_command)
 	/* with no CMD_ARGV, resume from current pc, addr = 0,
 	 * with one arguments, addr = CMD_ARGV[0],
 	 * handle breakpoints, not debugging */
-	uint32_t addr = 0;
+	target_addr_t addr = 0;
 	if (CMD_ARGC == 1) {
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 		current = 0;
 	}
 
@@ -2896,10 +2980,10 @@ COMMAND_HANDLER(handle_step_command)
 	/* with no CMD_ARGV, step from current pc, addr = 0,
 	 * with one argument addr = CMD_ARGV[0],
 	 * handle breakpoints, debugging */
-	uint32_t addr = 0;
+	target_addr_t addr = 0;
 	int current_pc = 1;
 	if (CMD_ARGC == 1) {
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 		current_pc = 0;
 	}
 
@@ -2909,7 +2993,7 @@ COMMAND_HANDLER(handle_step_command)
 }
 
 static void handle_md_output(struct command_context *cmd_ctx,
-		struct target *target, uint32_t address, unsigned size,
+		struct target *target, target_addr_t address, unsigned size,
 		unsigned count, const uint8_t *buffer)
 {
 	const unsigned line_bytecnt = 32;
@@ -2920,6 +3004,9 @@ static void handle_md_output(struct command_context *cmd_ctx,
 
 	const char *value_fmt;
 	switch (size) {
+	case 8:
+		value_fmt = "%16.16llx ";
+		break;
 	case 4:
 		value_fmt = "%8.8x ";
 		break;
@@ -2939,13 +3026,16 @@ static void handle_md_output(struct command_context *cmd_ctx,
 		if (i % line_modulo == 0) {
 			output_len += snprintf(output + output_len,
 					sizeof(output) - output_len,
-					"0x%8.8x: ",
-					(unsigned)(address + (i*size)));
+					TARGET_ADDR_FMT ": ",
+					(address + (i * size)));
 		}
 
-		uint32_t value = 0;
+		uint64_t value = 0;
 		const uint8_t *value_ptr = buffer + i * size;
 		switch (size) {
+		case 8:
+			value = target_buffer_get_u64(target, value_ptr);
+			break;
 		case 4:
 			value = target_buffer_get_u32(target, value_ptr);
 			break;
@@ -2973,6 +3063,9 @@ COMMAND_HANDLER(handle_md_command)
 
 	unsigned size = 0;
 	switch (CMD_NAME[2]) {
+	case 'd':
+		size = 8;
+		break;
 	case 'w':
 		size = 4;
 		break;
@@ -2988,7 +3081,7 @@ COMMAND_HANDLER(handle_md_command)
 
 	bool physical = strcmp(CMD_ARGV[0], "phys") == 0;
 	int (*fn)(struct target *target,
-			uint32_t address, uint32_t size_value, uint32_t count, uint8_t *buffer);
+			target_addr_t address, uint32_t size_value, uint32_t count, uint8_t *buffer);
 	if (physical) {
 		CMD_ARGC--;
 		CMD_ARGV++;
@@ -2998,8 +3091,8 @@ COMMAND_HANDLER(handle_md_command)
 	if ((CMD_ARGC < 1) || (CMD_ARGC > 2))
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t address;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+	target_addr_t address;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
 
 	unsigned count = 1;
 	if (CMD_ARGC == 2)
@@ -3018,14 +3111,14 @@ COMMAND_HANDLER(handle_md_command)
 }
 
 typedef int (*target_write_fn)(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 
 static int target_fill_mem(struct target *target,
-		uint32_t address,
+		target_addr_t address,
 		target_write_fn fn,
 		unsigned data_size,
 		/* value */
-		uint32_t b,
+		uint64_t b,
 		/* count */
 		unsigned c)
 {
@@ -3040,6 +3133,9 @@ static int target_fill_mem(struct target *target,
 
 	for (unsigned i = 0; i < chunk_size; i++) {
 		switch (data_size) {
+		case 8:
+			target_buffer_set_u64(target, target_buf + i * data_size, b);
+			break;
 		case 4:
 			target_buffer_set_u32(target, target_buf + i * data_size, b);
 			break;
@@ -3088,11 +3184,11 @@ COMMAND_HANDLER(handle_mw_command)
 	if ((CMD_ARGC < 2) || (CMD_ARGC > 3))
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t address;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], address);
+	target_addr_t address;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], address);
 
-	uint32_t value;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], value);
+	target_addr_t value;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[1], value);
 
 	unsigned count = 1;
 	if (CMD_ARGC == 3)
@@ -3101,6 +3197,9 @@ COMMAND_HANDLER(handle_mw_command)
 	struct target *target = get_current_target(CMD_CTX);
 	unsigned wordsize;
 	switch (CMD_NAME[2]) {
+		case 'd':
+			wordsize = 8;
+			break;
 		case 'w':
 			wordsize = 4;
 			break;
@@ -3118,7 +3217,7 @@ COMMAND_HANDLER(handle_mw_command)
 }
 
 static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
-		uint32_t *min_address, uint32_t *max_address)
+		target_addr_t *min_address, target_addr_t *max_address)
 {
 	if (CMD_ARGC < 1 || CMD_ARGC > 5)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -3126,8 +3225,8 @@ static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
 	/* a base address isn't always necessary,
 	 * default to 0x0 (i.e. don't relocate) */
 	if (CMD_ARGC >= 2) {
-		uint32_t addr;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], addr);
+		target_addr_t addr;
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image->base_address = addr;
 		image->base_address_set = 1;
 	} else
@@ -3136,9 +3235,9 @@ static COMMAND_HELPER(parse_load_image_command_CMD_ARGV, struct image *image,
 	image->start_address_set = 0;
 
 	if (CMD_ARGC >= 4)
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], *min_address);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[3], *min_address);
 	if (CMD_ARGC == 5) {
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[4], *max_address);
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[4], *max_address);
 		/* use size (given) to find max (required) */
 		*max_address += *min_address;
 	}
@@ -3154,8 +3253,8 @@ COMMAND_HANDLER(handle_load_image_command)
 	uint8_t *buffer;
 	size_t buf_cnt;
 	uint32_t image_size;
-	uint32_t min_address = 0;
-	uint32_t max_address = 0xffffffff;
+	target_addr_t min_address = 0;
+	target_addr_t max_address = -1;
 	int i;
 	struct image image;
 
@@ -3214,7 +3313,7 @@ COMMAND_HANDLER(handle_load_image_command)
 				break;
 			}
 			image_size += length;
-			command_print(CMD_CTX, "%u bytes written at address 0x%8.8" PRIx32 "",
+			command_print(CMD_CTX, "%u bytes written at address " TARGET_ADDR_FMT "",
 					(unsigned int)length,
 					image.sections[i].base_address + offset);
 		}
@@ -3239,15 +3338,15 @@ COMMAND_HANDLER(handle_dump_image_command)
 	struct fileio *fileio;
 	uint8_t *buffer;
 	int retval, retvaltemp;
-	uint32_t address, size;
+	target_addr_t address, size;
 	struct duration bench;
 	struct target *target = get_current_target(CMD_CTX);
 
 	if (CMD_ARGC != 3)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], address);
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], size);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[1], address);
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[2], size);
 
 	uint32_t buf_size = (size > 4096) ? 4096 : size;
 	buffer = malloc(buf_size);
@@ -3328,8 +3427,8 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 	duration_start(&bench);
 
 	if (CMD_ARGC >= 2) {
-		uint32_t addr;
-		COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], addr);
+		target_addr_t addr;
+		COMMAND_PARSE_ADDRESS(CMD_ARGV[1], addr);
 		image.base_address = addr;
 		image.base_address_set = 1;
 	} else {
@@ -3419,7 +3518,7 @@ static COMMAND_HELPER(handle_verify_image_command_internal, enum verify_mode ver
 				free(data);
 			}
 		} else {
-			command_print(CMD_CTX, "address 0x%08" PRIx32 " length 0x%08zx",
+			command_print(CMD_CTX, "address " TARGET_ADDR_FMT " length 0x%08zx",
 						  image.sections[i].base_address,
 						  buf_cnt);
 		}
@@ -3466,7 +3565,7 @@ static int handle_bp_command_list(struct command_context *cmd_ctx)
 		if (breakpoint->type == BKPT_SOFT) {
 			char *buf = buf_to_str(breakpoint->orig_instr,
 					breakpoint->length, 16);
-			command_print(cmd_ctx, "IVA breakpoint: 0x%8.8" PRIx32 ", 0x%x, %i, 0x%s",
+			command_print(cmd_ctx, "IVA breakpoint: " TARGET_ADDR_FMT ", 0x%x, %i, 0x%s",
 					breakpoint->address,
 					breakpoint->length,
 					breakpoint->set, buf);
@@ -3477,13 +3576,13 @@ static int handle_bp_command_list(struct command_context *cmd_ctx)
 							breakpoint->asid,
 							breakpoint->length, breakpoint->set);
 			else if ((breakpoint->address != 0) && (breakpoint->asid != 0)) {
-				command_print(cmd_ctx, "Hybrid breakpoint(IVA): 0x%8.8" PRIx32 ", 0x%x, %i",
+				command_print(cmd_ctx, "Hybrid breakpoint(IVA): " TARGET_ADDR_FMT ", 0x%x, %i",
 							breakpoint->address,
 							breakpoint->length, breakpoint->set);
 				command_print(cmd_ctx, "\t|--->linked with ContextID: 0x%8.8" PRIx32,
 							breakpoint->asid);
 			} else
-				command_print(cmd_ctx, "Breakpoint(IVA): 0x%8.8" PRIx32 ", 0x%x, %i",
+				command_print(cmd_ctx, "Breakpoint(IVA): " TARGET_ADDR_FMT ", 0x%x, %i",
 							breakpoint->address,
 							breakpoint->length, breakpoint->set);
 		}
@@ -3494,7 +3593,7 @@ static int handle_bp_command_list(struct command_context *cmd_ctx)
 }
 
 static int handle_bp_command_set(struct command_context *cmd_ctx,
-		uint32_t addr, uint32_t asid, uint32_t length, int hw)
+		target_addr_t addr, uint32_t asid, uint32_t length, int hw)
 {
 	struct target *target = get_current_target(cmd_ctx);
 	int retval;
@@ -3502,7 +3601,7 @@ static int handle_bp_command_set(struct command_context *cmd_ctx,
 	if (asid == 0) {
 		retval = breakpoint_add(target, addr, length, hw);
 		if (ERROR_OK == retval)
-			command_print(cmd_ctx, "breakpoint set at 0x%8.8" PRIx32 "", addr);
+			command_print(cmd_ctx, "breakpoint set at " TARGET_ADDR_FMT "", addr);
 		else {
 			LOG_ERROR("Failure setting breakpoint, the same address(IVA) is already used");
 			return retval;
@@ -3537,7 +3636,7 @@ static int handle_bp_command_set(struct command_context *cmd_ctx,
 
 COMMAND_HANDLER(handle_bp_command)
 {
-	uint32_t addr;
+	target_addr_t addr;
 	uint32_t asid;
 	uint32_t length;
 	int hw = BKPT_SOFT;
@@ -3548,17 +3647,15 @@ COMMAND_HANDLER(handle_bp_command)
 
 		case 2:
 			asid = 0;
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], length);
 			return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
 
 		case 3:
 			if (strcmp(CMD_ARGV[2], "hw") == 0) {
 				hw = BKPT_HARD;
-				COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
-
+				COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 				COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], length);
-
 				asid = 0;
 				return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
 			} else if (strcmp(CMD_ARGV[2], "hw_ctx") == 0) {
@@ -3571,7 +3668,7 @@ COMMAND_HANDLER(handle_bp_command)
 
 		case 4:
 			hw = BKPT_HARD;
-			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+			COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], asid);
 			COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], length);
 			return handle_bp_command_set(CMD_CTX, addr, asid, length, hw);
@@ -3586,8 +3683,8 @@ COMMAND_HANDLER(handle_rbp_command)
 	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t addr;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], addr);
+	target_addr_t addr;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], addr);
 
 	struct target *target = get_current_target(CMD_CTX);
 	breakpoint_remove(target, addr);
@@ -3603,7 +3700,7 @@ COMMAND_HANDLER(handle_wp_command)
 		struct watchpoint *watchpoint = target->watchpoints;
 
 		while (watchpoint) {
-			command_print(CMD_CTX, "address: 0x%8.8" PRIx32
+			command_print(CMD_CTX, "address: " TARGET_ADDR_FMT
 					", len: 0x%8.8" PRIx32
 					", r/w/a: %i, value: 0x%8.8" PRIx32
 					", mask: 0x%8.8" PRIx32,
@@ -3688,14 +3785,14 @@ COMMAND_HANDLER(handle_virt2phys_command)
 	if (CMD_ARGC != 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	uint32_t va;
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], va);
-	uint32_t pa;
+	target_addr_t va;
+	COMMAND_PARSE_ADDRESS(CMD_ARGV[0], va);
+	target_addr_t pa;
 
 	struct target *target = get_current_target(CMD_CTX);
 	int retval = target->type->virt2phys(target, va, &pa);
 	if (retval == ERROR_OK)
-		command_print(CMD_CTX, "Physical address 0x%08" PRIx32 "", pa);
+		command_print(CMD_CTX, "Physical address " TARGET_ADDR_FMT "", pa);
 
 	return retval;
 }
@@ -4771,7 +4868,7 @@ static int jim_target_md(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
 	}
 
 	int (*fn)(struct target *target,
-			uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+			target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 	fn = target_read_memory;
 
 	int e;
@@ -5643,7 +5740,7 @@ static const struct command_registration target_subcommand_handlers[] = {
 };
 
 struct FastLoad {
-	uint32_t address;
+	target_addr_t address;
 	uint8_t *data;
 	int length;
 
@@ -5670,8 +5767,8 @@ COMMAND_HANDLER(handle_fast_load_image_command)
 	uint8_t *buffer;
 	size_t buf_cnt;
 	uint32_t image_size;
-	uint32_t min_address = 0;
-	uint32_t max_address = 0xffffffff;
+	target_addr_t min_address = 0;
+	target_addr_t max_address = -1;
 	int i;
 
 	struct image image;
@@ -6142,6 +6239,13 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.usage = "[address]",
 	},
 	{
+		.name = "mdd",
+		.handler = handle_md_command,
+		.mode = COMMAND_EXEC,
+		.help = "display memory words",
+		.usage = "['phys'] address [count]",
+	},
+	{
 		.name = "mdw",
 		.handler = handle_md_command,
 		.mode = COMMAND_EXEC,
@@ -6163,6 +6267,13 @@ static const struct command_registration target_exec_command_handlers[] = {
 		.usage = "['phys'] address [count]",
 	},
 	{
+		.name = "mwd",
+		.handler = handle_mw_command,
+		.mode = COMMAND_EXEC,
+		.help = "write memory word",
+		.usage = "['phys'] address value [count]",
+	},
+	{
 		.name = "mww",
 		.handler = handle_mw_command,
 		.mode = COMMAND_EXEC,
diff --git a/src/target/target.h b/src/target/target.h
index e86b700..41c9333 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -93,7 +93,7 @@ enum target_endianness {
 };
 
 struct working_area {
-	uint32_t address;
+	target_addr_t address;
 	uint32_t size;
 	bool free;
 	uint8_t *backup;
@@ -156,9 +156,9 @@ struct target {
 	uint32_t working_area;				/* working area (initialised RAM). Evaluated
 										 * upon first allocation from virtual/physical address. */
 	bool working_area_virt_spec;		/* virtual address specified? */
-	uint32_t working_area_virt;			/* virtual address */
+	target_addr_t working_area_virt;			/* virtual address */
 	bool working_area_phys_spec;		/* physical address specified? */
-	uint32_t working_area_phys;			/* physical address */
+	target_addr_t working_area_phys;			/* physical address */
 	uint32_t working_area_size;			/* size in bytes */
 	uint32_t backup_working_area;		/* whether the content of the working area has to be preserved */
 	struct working_area *working_areas;/* list of allocated working areas */
@@ -353,7 +353,7 @@ int target_unregister_trace_callback(
  * yet it is possible to detect error conditions.
  */
 int target_poll(struct target *target);
-int target_resume(struct target *target, int current, uint32_t address,
+int target_resume(struct target *target, int current, target_addr_t address,
 		int handle_breakpoints, int debug_execution);
 int target_halt(struct target *target);
 int target_call_event_callbacks(struct target *target, enum target_event event);
@@ -474,7 +474,7 @@ int target_get_gdb_reg_list(struct target *target,
  * This routine is a wrapper for target->type->step.
  */
 int target_step(struct target *target,
-		int current, uint32_t address, int handle_breakpoints);
+		int current, target_addr_t address, int handle_breakpoints);
 /**
  * Run an algorithm on the @a target given.
  *
@@ -527,9 +527,9 @@ int target_run_flash_async_algorithm(struct target *target,
  * This routine is a wrapper for target->type->read_memory.
  */
 int target_read_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 int target_read_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 /**
  * Write @a count items of @a size bytes to the memory of @a target at
  * the @a address given. @a address must be aligned to @a size
@@ -548,9 +548,9 @@ int target_read_phys_memory(struct target *target,
  * This routine is wrapper for target->type->write_memory.
  */
 int target_write_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 int target_write_phys_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint32_t count, const uint8_t *buffer);
 
 /*
  * Write to target memory using the virtual address.
@@ -577,13 +577,13 @@ int target_write_phys_memory(struct target *target,
  * peripheral registers which do not support byte operations.
  */
 int target_write_buffer(struct target *target,
-		uint32_t address, uint32_t size, const uint8_t *buffer);
+		target_addr_t address, uint32_t size, const uint8_t *buffer);
 int target_read_buffer(struct target *target,
-		uint32_t address, uint32_t size, uint8_t *buffer);
+		target_addr_t address, uint32_t size, uint8_t *buffer);
 int target_checksum_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t *crc);
+		target_addr_t address, uint32_t size, uint32_t *crc);
 int target_blank_check_memory(struct target *target,
-		uint32_t address, uint32_t size, uint32_t *blank, uint8_t erased_value);
+		target_addr_t address, uint32_t size, uint32_t *blank, uint8_t erased_value);
 int target_wait_state(struct target *target, enum target_state state, int ms);
 
 /**
@@ -659,14 +659,19 @@ void target_buffer_set_u64_array(struct target *target, uint8_t *buffer, uint32_
 void target_buffer_set_u32_array(struct target *target, uint8_t *buffer, uint32_t count, const uint32_t *srcbuf);
 void target_buffer_set_u16_array(struct target *target, uint8_t *buffer, uint32_t count, const uint16_t *srcbuf);
 
-int target_read_u64(struct target *target, uint64_t address, uint64_t *value);
-int target_read_u32(struct target *target, uint32_t address, uint32_t *value);
-int target_read_u16(struct target *target, uint32_t address, uint16_t *value);
-int target_read_u8(struct target *target, uint32_t address, uint8_t *value);
-int target_write_u64(struct target *target, uint64_t address, uint64_t value);
-int target_write_u32(struct target *target, uint32_t address, uint32_t value);
-int target_write_u16(struct target *target, uint32_t address, uint16_t value);
-int target_write_u8(struct target *target, uint32_t address, uint8_t value);
+int target_read_u64(struct target *target, target_addr_t address, uint64_t *value);
+int target_read_u32(struct target *target, target_addr_t address, uint32_t *value);
+int target_read_u16(struct target *target, target_addr_t address, uint16_t *value);
+int target_read_u8(struct target *target, target_addr_t address, uint8_t *value);
+int target_write_u64(struct target *target, target_addr_t address, uint64_t value);
+int target_write_u32(struct target *target, target_addr_t address, uint32_t value);
+int target_write_u16(struct target *target, target_addr_t address, uint16_t value);
+int target_write_u8(struct target *target, target_addr_t address, uint8_t value);
+
+int target_write_phys_u64(struct target *target, target_addr_t address, uint64_t value);
+int target_write_phys_u32(struct target *target, target_addr_t address, uint32_t value);
+int target_write_phys_u16(struct target *target, target_addr_t address, uint16_t value);
+int target_write_phys_u8(struct target *target, target_addr_t address, uint8_t value);
 
 /* Issues USER() statements with target state information */
 int target_arch_state(struct target *target);
diff --git a/src/target/target_type.h b/src/target/target_type.h
index 2473c62..34e2778 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -53,11 +53,10 @@ struct target_type {
 
 	/* halt will log a warning, but return ERROR_OK if the target is already halted. */
 	int (*halt)(struct target *target);
-	int (*resume)(struct target *target, int current, uint32_t address,
+	int (*resume)(struct target *target, int current, target_addr_t address,
 			int handle_breakpoints, int debug_execution);
-	int (*step)(struct target *target, int current, uint32_t address,
+	int (*step)(struct target *target, int current, target_addr_t address,
 			int handle_breakpoints);
-
 	/* target reset control. assert reset can be invoked when OpenOCD and
 	 * the target is out of sync.
 	 *
@@ -111,26 +110,26 @@ struct target_type {
 	 * Target memory read callback.  Do @b not call this function
 	 * directly, use target_read_memory() instead.
 	 */
-	int (*read_memory)(struct target *target, uint32_t address,
+	int (*read_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 	/**
 	 * Target memory write callback.  Do @b not call this function
 	 * directly, use target_write_memory() instead.
 	 */
-	int (*write_memory)(struct target *target, uint32_t address,
+	int (*write_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
 
 	/* Default implementation will do some fancy alignment to improve performance, target can override */
-	int (*read_buffer)(struct target *target, uint32_t address,
+	int (*read_buffer)(struct target *target, target_addr_t address,
 			uint32_t size, uint8_t *buffer);
 
 	/* Default implementation will do some fancy alignment to improve performance, target can override */
-	int (*write_buffer)(struct target *target, uint32_t address,
+	int (*write_buffer)(struct target *target, target_addr_t address,
 			uint32_t size, const uint8_t *buffer);
 
-	int (*checksum_memory)(struct target *target, uint32_t address,
+	int (*checksum_memory)(struct target *target, target_addr_t address,
 			uint32_t count, uint32_t *checksum);
-	int (*blank_check_memory)(struct target *target, uint32_t address,
+	int (*blank_check_memory)(struct target *target, target_addr_t address,
 			uint32_t count, uint32_t *blank, uint8_t erased_value);
 
 	/*
@@ -175,15 +174,15 @@ struct target_type {
 	 */
 	int (*run_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
-			struct reg_param *reg_param, uint32_t entry_point,
-			uint32_t exit_point, int timeout_ms, void *arch_info);
+			struct reg_param *reg_param, target_addr_t entry_point,
+			target_addr_t exit_point, int timeout_ms, void *arch_info);
 	int (*start_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
-			struct reg_param *reg_param, uint32_t entry_point,
-			uint32_t exit_point, void *arch_info);
+			struct reg_param *reg_param, target_addr_t entry_point,
+			target_addr_t exit_point, void *arch_info);
 	int (*wait_algorithm)(struct target *target, int num_mem_params,
 			struct mem_param *mem_params, int num_reg_params,
-			struct reg_param *reg_param, uint32_t exit_point,
+			struct reg_param *reg_param, target_addr_t exit_point,
 			int timeout_ms, void *arch_info);
 
 	const struct command_registration *commands;
@@ -233,7 +232,7 @@ struct target_type {
 	/* translate from virtual to physical address. Default implementation is successful
 	 * no-op(i.e. virtual==physical).
 	 */
-	int (*virt2phys)(struct target *target, uint32_t address, uint32_t *physical);
+	int (*virt2phys)(struct target *target, target_addr_t address, target_addr_t *physical);
 
 	/* read directly from physical memory. caches are bypassed and untouched.
 	 *
@@ -243,13 +242,13 @@ struct target_type {
 	 *
 	 * Default implementation is to call read_memory.
 	 */
-	int (*read_phys_memory)(struct target *target, uint32_t phys_address,
+	int (*read_phys_memory)(struct target *target, target_addr_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 
 	/*
 	 * same as read_phys_memory, except that it writes...
 	 */
-	int (*write_phys_memory)(struct target *target, uint32_t phys_address,
+	int (*write_phys_memory)(struct target *target, target_addr_t phys_address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
 
 	int (*mmu)(struct target *target, int *enabled);
diff --git a/src/target/x86_32_common.c b/src/target/x86_32_common.c
index c21167e..34f92ea 100644
--- a/src/target/x86_32_common.c
+++ b/src/target/x86_32_common.c
@@ -48,8 +48,8 @@ static int read_mem(struct target *t, uint32_t size,
 			uint32_t addr, uint8_t *buf);
 static int write_mem(struct target *t, uint32_t size,
 			uint32_t addr, const uint8_t *buf);
-static int calcaddr_physfromlin(struct target *t, uint32_t addr,
-			uint32_t *physaddr);
+static int calcaddr_physfromlin(struct target *t, target_addr_t addr,
+			target_addr_t *physaddr);
 static int read_phys_mem(struct target *t, uint32_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 static int write_phys_mem(struct target *t, uint32_t phys_address,
@@ -113,7 +113,7 @@ int x86_32_common_mmu(struct target *t, int *enabled)
 	return ERROR_OK;
 }
 
-int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physical)
+int x86_32_common_virt2phys(struct target *t, target_addr_t address, target_addr_t *physical)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 
@@ -135,7 +135,7 @@ int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physic
 	} else {
 		/* target halted in protected mode */
 		if (calcaddr_physfromlin(t, address, physical) != ERROR_OK) {
-			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
+			LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 					__func__, address);
 			return ERROR_FAIL;
 		}
@@ -143,7 +143,7 @@ int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physic
 	return ERROR_OK;
 }
 
-int x86_32_common_read_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_read_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
@@ -226,7 +226,7 @@ static int read_phys_mem(struct target *t, uint32_t phys_address,
 	return retval;
 }
 
-int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_write_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
@@ -235,7 +235,7 @@ int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
 
 	check_not_halted(t);
 	if (!count || !buffer || !phys_address) {
-		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
+		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 				__func__, count, buffer, phys_address);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
@@ -444,7 +444,7 @@ static int write_mem(struct target *t, uint32_t size,
 	return retval;
 }
 
-int calcaddr_physfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
+int calcaddr_physfromlin(struct target *t, target_addr_t addr, target_addr_t *physaddr)
 {
 	uint8_t entry_buffer[8];
 
@@ -568,16 +568,16 @@ int calcaddr_physfromlin(struct target *t, uint32_t addr, uint32_t *physaddr)
 	return ERROR_OK;
 }
 
-int x86_32_common_read_memory(struct target *t, uint32_t addr,
+int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, uint8_t *buf)
 {
 	int retval = ERROR_OK;
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
-	LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
+	LOG_DEBUG("addr=" TARGET_ADDR_FMT ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 			addr, size, count, buf);
 	check_not_halted(t);
 	if (!count || !buf || !addr) {
-		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
+		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 				__func__, count, buf, addr);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
@@ -591,9 +591,10 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 			LOG_ERROR("%s could not disable paging", __func__);
 			return retval;
 		}
-		uint32_t physaddr = 0;
+		target_addr_t physaddr = 0;
 		if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
-			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32, __func__, addr);
+			LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
+					  __func__, addr);
 			retval = ERROR_FAIL;
 		}
 		/* TODO: !!! Watch out for page boundaries
@@ -603,7 +604,8 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 
 		if (retval == ERROR_OK
 			&& x86_32_common_read_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to read memory from physical address 0x%08" PRIx32, __func__, physaddr);
+			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) */
@@ -615,7 +617,8 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 	} else {
 		/* paging is off - linear address is physical address */
 		if (x86_32_common_read_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to read memory from address 0%08" PRIx32, __func__, addr);
+			LOG_ERROR("%s failed to read memory from address " TARGET_ADDR_FMT,
+					  __func__, addr);
 			retval = ERROR_FAIL;
 		}
 	}
@@ -623,16 +626,16 @@ int x86_32_common_read_memory(struct target *t, uint32_t addr,
 	return retval;
 }
 
-int x86_32_common_write_memory(struct target *t, uint32_t addr,
+int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, const uint8_t *buf)
 {
 	int retval = ERROR_OK;
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
-	LOG_DEBUG("addr=0x%08" PRIx32 ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
+	LOG_DEBUG("addr=" TARGET_ADDR_FMT ", size=%" PRIu32 ", count=0x%" PRIx32 ", buf=%p",
 			addr, size, count, buf);
 	check_not_halted(t);
 	if (!count || !buf || !addr) {
-		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=0x%08" PRIx32,
+		LOG_ERROR("%s invalid params count=0x%" PRIx32 ", buf=%p, addr=" TARGET_ADDR_FMT,
 					__func__, count, buf, addr);
 		return ERROR_COMMAND_ARGUMENT_INVALID;
 	}
@@ -645,9 +648,9 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 			LOG_ERROR("%s could not disable paging", __func__);
 			return retval;
 		}
-		uint32_t physaddr = 0;
+		target_addr_t physaddr = 0;
 		if (calcaddr_physfromlin(t, addr, &physaddr) != ERROR_OK) {
-			LOG_ERROR("%s failed to calculate physical address from 0x%08" PRIx32,
+			LOG_ERROR("%s failed to calculate physical address from " TARGET_ADDR_FMT,
 					__func__, addr);
 			retval = ERROR_FAIL;
 		}
@@ -657,7 +660,7 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 		 */
 		if (retval == ERROR_OK
 			&& x86_32_common_write_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to write memory to physical address 0x%08" PRIx32,
+			LOG_ERROR("%s failed to write memory to physical address " TARGET_ADDR_FMT,
 					__func__, physaddr);
 			retval = ERROR_FAIL;
 		}
@@ -671,7 +674,7 @@ int x86_32_common_write_memory(struct target *t, uint32_t addr,
 
 		/* paging is off - linear address is physical address */
 		if (x86_32_common_write_phys_mem(t, addr, size, count, buf) != ERROR_OK) {
-			LOG_ERROR("%s failed to write memory to address 0x%08" PRIx32,
+			LOG_ERROR("%s failed to write memory to address " TARGET_ADDR_FMT,
 					__func__, addr);
 			retval = ERROR_FAIL;
 		}
@@ -852,7 +855,7 @@ int x86_32_common_remove_watchpoint(struct target *t, struct watchpoint *wp)
 
 int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp)
 {
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (check_not_halted(t))
 		return ERROR_TARGET_NOT_HALTED;
 	/* set_breakpoint() will return ERROR_TARGET_RESOURCE_NOT_AVAILABLE if all
@@ -863,7 +866,7 @@ int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp)
 
 int x86_32_common_remove_breakpoint(struct target *t, struct breakpoint *bp)
 {
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (check_not_halted(t))
 		return ERROR_TARGET_NOT_HALTED;
 	if (bp->set)
@@ -1003,7 +1006,7 @@ static int unset_hwbp(struct target *t, struct breakpoint *bp)
 	debug_reg_list[hwbp_num].used = 0;
 	debug_reg_list[hwbp_num].bp_value = 0;
 
-	LOG_USER("%s hardware breakpoint %" PRIu32 " removed from 0x%08" PRIx32 " (hwreg=%d)",
+	LOG_USER("%s hardware breakpoint %" PRIu32 " removed from " TARGET_ADDR_FMT " (hwreg=%d)",
 			__func__, bp->unique_id, bp->address, hwbp_num);
 	return ERROR_OK;
 }
@@ -1012,7 +1015,7 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	LOG_DEBUG("id %" PRIx32, bp->unique_id);
-	uint32_t physaddr;
+	target_addr_t physaddr;
 	uint8_t opcode = SW_BP_OPCODE;
 	uint8_t readback;
 
@@ -1032,7 +1035,7 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 		return ERROR_FAIL;
 
 	if (readback != SW_BP_OPCODE) {
-		LOG_ERROR("%s software breakpoint error at 0x%08" PRIx32 ", check memory",
+		LOG_ERROR("%s software breakpoint error at " TARGET_ADDR_FMT ", check memory",
 				__func__, bp->address);
 		LOG_ERROR("%s readback=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
 				__func__, readback, *bp->orig_instr);
@@ -1059,7 +1062,7 @@ static int set_swbp(struct target *t, struct breakpoint *bp)
 			addto = addto->next;
 		addto->next = new_patch;
 	}
-	LOG_USER("%s software breakpoint %" PRIu32 " set at 0x%08" PRIx32,
+	LOG_USER("%s software breakpoint %" PRIu32 " set at " TARGET_ADDR_FMT,
 			__func__, bp->unique_id, bp->address);
 	return ERROR_OK;
 }
@@ -1068,7 +1071,7 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	LOG_DEBUG("id %" PRIx32, bp->unique_id);
-	uint32_t physaddr;
+	target_addr_t physaddr;
 	uint8_t current_instr;
 
 	/* check that user program has not modified breakpoint instruction */
@@ -1081,7 +1084,7 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 		if (write_phys_mem(t, physaddr, 1, 1, bp->orig_instr))
 			return ERROR_FAIL;
 	} else {
-		LOG_ERROR("%s software breakpoint remove error at 0x%08" PRIx32 ", check memory",
+		LOG_ERROR("%s software breakpoint remove error at " TARGET_ADDR_FMT ", check memory",
 				__func__, bp->address);
 		LOG_ERROR("%s current=0x%02" PRIx8 " orig=0x%02" PRIx8 "",
 				__func__, current_instr, *bp->orig_instr);
@@ -1107,7 +1110,7 @@ static int unset_swbp(struct target *t, struct breakpoint *bp)
 		}
 	}
 
-	LOG_USER("%s software breakpoint %" PRIu32 " removed from 0x%08" PRIx32,
+	LOG_USER("%s software breakpoint %" PRIu32 " removed from " TARGET_ADDR_FMT,
 			__func__, bp->unique_id, bp->address);
 	return ERROR_OK;
 }
@@ -1116,7 +1119,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 {
 	int error = ERROR_OK;
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (bp->set) {
 		LOG_ERROR("breakpoint already set");
 		return error;
@@ -1124,7 +1127,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 	if (bp->type == BKPT_HARD) {
 		error = set_hwbp(t, bp);
 		if (error != ERROR_OK) {
-			LOG_ERROR("%s error setting hardware breakpoint at 0x%08" PRIx32,
+			LOG_ERROR("%s error setting hardware breakpoint at " TARGET_ADDR_FMT,
 					__func__, bp->address);
 			return error;
 		}
@@ -1132,7 +1135,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 		if (x86_32->sw_bpts_supported(t)) {
 			error = set_swbp(t, bp);
 			if (error != ERROR_OK) {
-				LOG_ERROR("%s error setting software breakpoint at 0x%08" PRIx32,
+				LOG_ERROR("%s error setting software breakpoint at " TARGET_ADDR_FMT,
 						__func__, bp->address);
 				return error;
 			}
@@ -1147,7 +1150,7 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 
 static int unset_breakpoint(struct target *t, struct breakpoint *bp)
 {
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, bp->type, bp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, bp->type, bp->address);
 	if (!bp->set) {
 		LOG_WARNING("breakpoint not set");
 		return ERROR_OK;
@@ -1155,13 +1158,13 @@ static int unset_breakpoint(struct target *t, struct breakpoint *bp)
 
 	if (bp->type == BKPT_HARD) {
 		if (unset_hwbp(t, bp) != ERROR_OK) {
-			LOG_ERROR("%s error removing hardware breakpoint at 0x%08" PRIx32,
+			LOG_ERROR("%s error removing hardware breakpoint at " TARGET_ADDR_FMT,
 					__func__, bp->address);
 			return ERROR_FAIL;
 		}
 	} else {
 		if (unset_swbp(t, bp) != ERROR_OK) {
-			LOG_ERROR("%s error removing software breakpoint at 0x%08" PRIx32,
+			LOG_ERROR("%s error removing software breakpoint at " TARGET_ADDR_FMT,
 					__func__, bp->address);
 			return ERROR_FAIL;
 		}
@@ -1175,7 +1178,7 @@ static int set_watchpoint(struct target *t, struct watchpoint *wp)
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
 	int wp_num = 0;
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, wp->rw, wp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, wp->rw, wp->address);
 
 	if (wp->set) {
 		LOG_ERROR("%s watchpoint already set", __func__);
@@ -1220,7 +1223,7 @@ static int set_watchpoint(struct target *t, struct watchpoint *wp)
 	wp->set = wp_num + 1;
 	debug_reg_list[wp_num].used = 1;
 	debug_reg_list[wp_num].bp_value = wp->address;
-	LOG_USER("'%s' watchpoint %d set at 0x%08" PRIx32 " with length %" PRIu32 " (hwreg=%d)",
+	LOG_USER("'%s' watchpoint %d set at " TARGET_ADDR_FMT " with length %" PRIu32 " (hwreg=%d)",
 			wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
 			"write" : wp->rw == WPT_ACCESS ? "access" : "?",
 			wp->unique_id, wp->address, wp->length, wp_num);
@@ -1231,7 +1234,7 @@ static int unset_watchpoint(struct target *t, struct watchpoint *wp)
 {
 	struct x86_32_common *x86_32 = target_to_x86_32(t);
 	struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
-	LOG_DEBUG("type=%d, addr=0x%08" PRIx32, wp->rw, wp->address);
+	LOG_DEBUG("type=%d, addr=" TARGET_ADDR_FMT, wp->rw, wp->address);
 	if (!wp->set) {
 		LOG_WARNING("watchpoint not set");
 		return ERROR_OK;
@@ -1249,7 +1252,7 @@ static int unset_watchpoint(struct target *t, struct watchpoint *wp)
 	debug_reg_list[wp_num].bp_value = 0;
 	wp->set = 0;
 
-	LOG_USER("'%s' watchpoint %d removed from 0x%08" PRIx32 " with length %" PRIu32 " (hwreg=%d)",
+	LOG_USER("'%s' watchpoint %d removed from " TARGET_ADDR_FMT " with length %" PRIu32 " (hwreg=%d)",
 			wp->rw == WPT_READ ? "read" : wp->rw == WPT_WRITE ?
 			"write" : wp->rw == WPT_ACCESS ? "access" : "?",
 			wp->unique_id, wp->address, wp->length, wp_num);
diff --git a/src/target/x86_32_common.h b/src/target/x86_32_common.h
index b5877da..0aaa963 100644
--- a/src/target/x86_32_common.h
+++ b/src/target/x86_32_common.h
@@ -309,14 +309,14 @@ int x86_32_get_gdb_reg_list(struct target *t,
 int x86_32_common_init_arch_info(struct target *target,
 			struct x86_32_common *x86_32);
 int x86_32_common_mmu(struct target *t, int *enabled);
-int x86_32_common_virt2phys(struct target *t, uint32_t address, uint32_t *physical);
-int x86_32_common_read_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_virt2phys(struct target *t, target_addr_t address, target_addr_t *physical);
+int x86_32_common_read_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
-int x86_32_common_write_phys_mem(struct target *t, uint32_t phys_address,
+int x86_32_common_write_phys_mem(struct target *t, target_addr_t phys_address,
 			uint32_t size, uint32_t count, const uint8_t *buffer);
-int x86_32_common_read_memory(struct target *t, uint32_t addr,
+int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, uint8_t *buf);
-int x86_32_common_write_memory(struct target *t, uint32_t addr,
+int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 			uint32_t size, uint32_t count, const uint8_t *buf);
 int x86_32_common_read_io(struct target *t, uint32_t addr,
 			uint32_t size, uint8_t *buf);
diff --git a/src/target/xscale.c b/src/target/xscale.c
index e54033f..8fe8a2c 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -59,7 +59,7 @@
 
 /* forward declarations */
 static int xscale_resume(struct target *, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution);
+	target_addr_t address, int handle_breakpoints, int debug_execution);
 static int xscale_debug_entry(struct target *);
 static int xscale_restore_banked(struct target *);
 static int xscale_get_reg(struct reg *reg);
@@ -1120,7 +1120,7 @@ static void xscale_free_trace_data(struct xscale_common *xscale)
 }
 
 static int xscale_resume(struct target *target, int current,
-	uint32_t address, int handle_breakpoints, int debug_execution)
+	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
 	struct arm *arm = &xscale->arm;
@@ -1165,7 +1165,8 @@ static int xscale_resume(struct target *target, int current,
 			enum trace_mode saved_trace_mode;
 
 			/* there's a breakpoint at the current PC, we have to step over it */
-			LOG_DEBUG("unset breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("unset breakpoint at " TARGET_ADDR_FMT "",
+				breakpoint->address);
 			xscale_unset_breakpoint(target, breakpoint);
 
 			/* calculate PC of next instruction */
@@ -1222,7 +1223,8 @@ static int xscale_resume(struct target *target, int current,
 			LOG_DEBUG("disable single-step");
 			xscale_disable_single_step(target);
 
-			LOG_DEBUG("set breakpoint at 0x%8.8" PRIx32 "", breakpoint->address);
+			LOG_DEBUG("set breakpoint at " TARGET_ADDR_FMT "",
+				breakpoint->address);
 			xscale_set_breakpoint(target, breakpoint);
 		}
 	}
@@ -1384,7 +1386,7 @@ static int xscale_step_inner(struct target *target, int current,
 }
 
 static int xscale_step(struct target *target, int current,
-	uint32_t address, int handle_breakpoints)
+	target_addr_t address, int handle_breakpoints)
 {
 	struct arm *arm = target_to_arm(target);
 	struct breakpoint *breakpoint = NULL;
@@ -1778,7 +1780,7 @@ dirty:
 	return ERROR_OK;
 }
 
-static int xscale_read_memory(struct target *target, uint32_t address,
+static int xscale_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
@@ -1786,7 +1788,7 @@ static int xscale_read_memory(struct target *target, uint32_t address,
 	uint32_t i;
 	int retval;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
 		address,
 		size,
 		count);
@@ -1864,7 +1866,7 @@ static int xscale_read_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int xscale_read_phys_memory(struct target *target, uint32_t address,
+static int xscale_read_phys_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
@@ -1879,13 +1881,13 @@ static int xscale_read_phys_memory(struct target *target, uint32_t address,
 	return ERROR_FAIL;
 }
 
-static int xscale_write_memory(struct target *target, uint32_t address,
+static int xscale_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
 	int retval;
 
-	LOG_DEBUG("address: 0x%8.8" PRIx32 ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
+	LOG_DEBUG("address: " TARGET_ADDR_FMT ", size: 0x%8.8" PRIx32 ", count: 0x%8.8" PRIx32,
 		address,
 		size,
 		count);
@@ -1963,7 +1965,7 @@ static int xscale_write_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
-static int xscale_write_phys_memory(struct target *target, uint32_t address,
+static int xscale_write_phys_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
@@ -3093,7 +3095,7 @@ COMMAND_HANDLER(xscale_handle_cache_info_command)
 }
 
 static int xscale_virt2phys(struct target *target,
-	uint32_t virtual, uint32_t *physical)
+	target_addr_t virtual, target_addr_t *physical)
 {
 	struct xscale_common *xscale = target_to_xscale(target);
 	uint32_t cb;
-- 
cgit v1.1


From 9cbfc9feb35ce0fcc9bb67a4d83b02100ee65f88 Mon Sep 17 00:00:00 2001
From: David Ung <davidu@nvidia.com>
Date: Fri, 3 Apr 2015 16:55:59 -0700
Subject: arm_dpm: Add new state ARM_STATE_AARCH64

Add new enum ARM_STATE_AARCH64 to the list of possible states.

Change-Id: I3cb2df70f8d5803a63d8374bf3eb75de988e24f8
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/arm.h      | 1 +
 src/target/arm_dpm.c  | 4 ++++
 src/target/cortex_a.c | 3 +++
 3 files changed, 8 insertions(+)

(limited to 'src/target')

diff --git a/src/target/arm.h b/src/target/arm.h
index 77a2f7b..5713fc0 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -78,6 +78,7 @@ enum arm_state {
 	ARM_STATE_THUMB,
 	ARM_STATE_JAZELLE,
 	ARM_STATE_THUMB_EE,
+	ARM_STATE_AARCH64,
 };
 
 #define ARM_COMMON_MAGIC 0x0A450A45
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 62c6175..3b18719 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -165,6 +165,9 @@ static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 					/* core-specific ... ? */
 					LOG_WARNING("Jazelle PC adjustment unknown");
 					break;
+				case ARM_STATE_AARCH64:
+					LOG_ERROR("AARCH64: 32bit read requested");
+					break;
 			}
 			break;
 		default:
@@ -905,6 +908,7 @@ void arm_dpm_report_wfar(struct arm_dpm *dpm, uint32_t addr)
 			addr -= 4;
 			break;
 		case ARM_STATE_JAZELLE:
+		case ARM_STATE_AARCH64:
 			/* ?? */
 			break;
 	}
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 62ac361..1cf6656 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -992,6 +992,9 @@ static int cortex_a_internal_restore(struct target *target, int current,
 		case ARM_STATE_JAZELLE:
 			LOG_ERROR("How do I resume into Jazelle state??");
 			return ERROR_FAIL;
+		case ARM_STATE_AARCH64:
+			LOG_ERROR("Shoudn't be in AARCH64 state");
+			return ERROR_FAIL;
 	}
 	LOG_DEBUG("resume pc = 0x%08" PRIx32, resume_pc);
 	buf_set_u32(arm->pc->value, 0, 32, resume_pc);
-- 
cgit v1.1


From 84a0bb4a3c3c883b22f95abc7f1428faef3936f1 Mon Sep 17 00:00:00 2001
From: David Ung <davidu@nvidia.com>
Date: Fri, 16 Jan 2015 18:04:06 -0800
Subject: arm_dpm: Add 64bit register handling.

Add various function to read/write ARMv8 registers.

Change-Id: I16f2829bdd0e87b050a51e414ff675d5c21bcbae
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/arm_dpm.c | 333 ++++++++++++++++++++++++++++++++++++++-------------
 src/target/arm_dpm.h |  10 ++
 2 files changed, 257 insertions(+), 86 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 3b18719..0c84be5 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -130,11 +130,11 @@ int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	return retval;
 }
 
-/* just read the register -- rely on the core mode being right */
-static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+
+static int dpm_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	uint32_t value;
-	int retval;
+	int retval = ERROR_FAIL;
 
 	switch (regnum) {
 		case 0 ... 14:
@@ -190,10 +190,57 @@ static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	return retval;
 }
 
-/* just write the register -- rely on the core mode being right */
-static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+static int dpm_read_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
-	int retval;
+	uint64_t value;
+	uint32_t i;
+	int retval = ERROR_FAIL;
+
+	switch (regnum) {
+		case 0 ... 30:
+			i = 0xd5130400 + regnum; /* msr dbgdtr_el0,reg */
+			retval = dpm->instr_read_data_dcc_64(dpm, i, &value);
+			break;
+		case 31: /* SP */
+			i = 0x910003e0;
+			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
+			break;
+		case 32: /* PC */
+			i = 0xd53b4520;
+			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
+			break;
+		case 33: /* CPSR */
+			i = 0xd53b4500;
+			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
+			break;
+
+		default:
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		buf_set_u64(r->value, 0, 64, value);
+		r->valid = true;
+		r->dirty = false;
+		LOG_DEBUG("READ: %s, %16.16llx", r->name, (long long)value);
+	}
+
+	return retval;
+}
+
+
+/* just read the register -- rely on the core mode being right */
+static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	if (r->size == 64)
+		return dpm_read_reg64(dpm, r, regnum);
+	else
+		return dpm_read_reg32(dpm, r, regnum);
+}
+
+static int dpm_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval = ERROR_FAIL;
 	uint32_t value = buf_get_u32(r->value, 0, 32);
 
 	switch (regnum) {
@@ -243,19 +290,46 @@ static int dpm_write_pc_core_state(struct arm_dpm *dpm, struct reg *r)
 	return dpm->instr_write_data_r0(dpm, ARMV4_5_BX(0), value);
 }
 
-/**
- * Read basic registers of the the current context:  R0 to R15, and CPSR;
- * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
- * In normal operation this is called on entry to halting debug state,
- * possibly after some other operations supporting restore of debug state
- * or making sure the CPU is fully idle (drain write buffer, etc).
- */
-int arm_dpm_read_current_registers(struct arm_dpm *dpm)
+static int dpm_write_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval = ERROR_FAIL;
+	uint32_t i;
+	uint64_t value = buf_get_u64(r->value, 0, 64);
+
+	switch (regnum) {
+		case 0 ... 30:
+			i = 0xd5330400 + regnum;
+			retval = dpm->instr_write_data_dcc(dpm, i, value);
+			break;
+
+		default:
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->dirty = false;
+		LOG_DEBUG("WRITE: %s, %16.16llx", r->name, (unsigned long long)value);
+	}
+
+	return retval;
+}
+
+/* just write the register -- rely on the core mode being right */
+static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	if (r->size == 64)
+		return dpm_write_reg64(dpm, r, regnum);
+	else
+		return dpm_write_reg32(dpm, r, regnum);
+}
+
+static int arm_dpm_read_current_registers_i(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
-	uint32_t cpsr;
+	uint32_t cpsr, instr, core_regs;
 	int retval;
 	struct reg *r;
+	enum arm_state core_state = arm->core_state;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
@@ -270,16 +344,22 @@ int arm_dpm_read_current_registers(struct arm_dpm *dpm)
 	}
 	r->dirty = true;
 
-	retval = dpm->instr_read_data_r0(dpm, ARMV4_5_MRS(0, 0), &cpsr);
+	if (core_state == ARM_STATE_AARCH64)
+		instr = 0xd53b4500;  /* mrs x0, dspsr_el0 */
+	else
+		instr = ARMV4_5_MRS(0, 0);
+	retval = dpm->instr_read_data_r0(dpm, instr, &cpsr);
 	if (retval != ERROR_OK)
 		goto fail;
 
 	/* update core mode and state, plus shadow mapping for R8..R14 */
 	arm_set_cpsr(arm, cpsr);
 
+	core_regs = arm->core_cache->num_regs;
+
 	/* REVISIT we can probably avoid reading R1..R14, saving time... */
-	for (unsigned i = 1; i < 16; i++) {
-		r = arm_reg_current(arm, i);
+	for (unsigned i = 1; i < core_regs; i++) {
+		r = dpm->arm_reg_current(arm, i);
 		if (r->valid)
 			continue;
 
@@ -300,6 +380,23 @@ fail:
 	return retval;
 }
 
+/**
+ * Read basic registers of the the current context:  R0 to R15, and CPSR;
+ * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
+ * In normal operation this is called on entry to halting debug state,
+ * possibly after some other operations supporting restore of debug state
+ * or making sure the CPU is fully idle (drain write buffer, etc).
+ */
+int arm_dpm_read_current_registers(struct arm_dpm *dpm)
+{
+	return arm_dpm_read_current_registers_i(dpm);
+}
+
+int arm_dpm_read_current_registers_64(struct arm_dpm *dpm)
+{
+	return arm_dpm_read_current_registers_i(dpm);
+}
+
 /* Avoid needless I/O ... leave breakpoints and watchpoints alone
  * unless they're removed, or need updating because of single-stepping
  * or running debugger code.
@@ -348,59 +445,14 @@ done:
 
 static int dpm_add_breakpoint(struct target *target, struct breakpoint *bp);
 
-/**
- * Writes all modified core registers for all processor modes.  In normal
- * operation this is called on exit from halting debug state.
- *
- * @param dpm: represents the processor
- * @param bpwp: true ensures breakpoints and watchpoints are set,
- *	false ensures they are cleared
- */
-int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
+
+static int arm_dpm_write_dirty_registers_32(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
 	struct reg_cache *cache = arm->core_cache;
 	int retval;
 	bool did_write;
 
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/* If we're managing hardware breakpoints for this core, enable
-	 * or disable them as requested.
-	 *
-	 * REVISIT We don't yet manage them for ANY cores.  Eventually
-	 * we should be able to assume we handle them; but until then,
-	 * cope with the hand-crafted breakpoint code.
-	 */
-	if (arm->target->type->add_breakpoint == dpm_add_breakpoint) {
-		for (unsigned i = 0; i < dpm->nbp; i++) {
-			struct dpm_bp *dbp = dpm->dbp + i;
-			struct breakpoint *bp = dbp->bp;
-
-			retval = dpm_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
-					bp ? &bp->set : NULL);
-			if (retval != ERROR_OK)
-				goto done;
-		}
-	}
-
-	/* enable/disable watchpoints */
-	for (unsigned i = 0; i < dpm->nwp; i++) {
-		struct dpm_wp *dwp = dpm->dwp + i;
-		struct watchpoint *wp = dwp->wp;
-
-		retval = dpm_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
-				wp ? &wp->set : NULL);
-		if (retval != ERROR_OK)
-			goto done;
-	}
-
-	/* NOTE:  writes to breakpoint and watchpoint registers might
-	 * be queued, and need (efficient/batched) flushing later.
-	 */
-
 	/* Scan the registers until we find one that's both dirty and
 	 * eligible for flushing.  Flush that and everything else that
 	 * shares the same core mode setting.  Typically this won't
@@ -436,20 +488,20 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 
 				/* cope with special cases */
 				switch (regnum) {
-					case 8 ... 12:
-						/* r8..r12 "anything but FIQ" case;
-						 * we "know" core mode is accurate
-						 * since we haven't changed it yet
-						 */
-						if (arm->core_mode == ARM_MODE_FIQ
-							&& ARM_MODE_ANY
-							!= mode)
-							tmode = ARM_MODE_USR;
-						break;
-					case 16:
-						/* SPSR */
-						regnum++;
-						break;
+				case 8 ... 12:
+					/* r8..r12 "anything but FIQ" case;
+					 * we "know" core mode is accurate
+					 * since we haven't changed it yet
+					 */
+					if (arm->core_mode == ARM_MODE_FIQ
+					    && ARM_MODE_ANY
+					    != mode)
+						tmode = ARM_MODE_USR;
+					break;
+				case 16:
+					/* SPSR */
+					regnum++;
+					break;
 				}
 
 				/* REVISIT error checks */
@@ -463,8 +515,8 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 				continue;
 
 			retval = dpm_write_reg(dpm,
-					&cache->reg_list[i],
-					regnum);
+					       &cache->reg_list[i],
+					       regnum);
 			if (retval != ERROR_OK)
 				goto done;
 		}
@@ -504,6 +556,106 @@ int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	cache->reg_list[0].dirty = false;
 
+done:
+	return retval;
+}
+
+static int arm_dpm_write_dirty_registers_64(struct arm_dpm *dpm)
+{
+	struct arm *arm = dpm->arm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+
+	/* Scan the registers until we find one that's both dirty and
+	 * eligible for flushing.  Flush that and everything else that
+	 * shares the same core mode setting.  Typically this won't
+	 * actually find anything to do...
+	 */
+
+	/* check everything except our scratch register R0 */
+	for (unsigned i = 1; i < 32; i++) {
+		struct arm_reg *r;
+		unsigned regnum;
+
+		if (!cache->reg_list[i].dirty)
+			continue;
+
+		r = cache->reg_list[i].arch_info;
+		regnum = r->num;
+		retval = dpm_write_reg(dpm,
+				       &cache->reg_list[i],
+				       regnum);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+	/* flush R0 -- it's *very* dirty by now */
+	retval = dpm_write_reg(dpm, &cache->reg_list[0], 0);
+	if (retval != ERROR_OK)
+		goto done;
+	cache->reg_list[0].dirty = false;
+
+done:
+	return retval;
+}
+
+/**
+ * Writes all modified core registers for all processor modes.  In normal
+ * operation this is called on exit from halting debug state.
+ *
+ * @param dpm: represents the processor
+ * @param bpwp: true ensures breakpoints and watchpoints are set,
+ *	false ensures they are cleared
+ */
+int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
+{
+	struct arm *arm = dpm->arm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* If we're managing hardware breakpoints for this core, enable
+	 * or disable them as requested.
+	 *
+	 * REVISIT We don't yet manage them for ANY cores.  Eventually
+	 * we should be able to assume we handle them; but until then,
+	 * cope with the hand-crafted breakpoint code.
+	 */
+	if (arm->target->type->add_breakpoint == dpm_add_breakpoint) {
+		for (unsigned i = 0; i < dpm->nbp; i++) {
+			struct dpm_bp *dbp = dpm->dbp + i;
+			struct breakpoint *bp = dbp->bp;
+
+			retval = dpm_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
+					bp ? &bp->set : NULL);
+			if (retval != ERROR_OK)
+				goto done;
+		}
+	}
+
+	/* enable/disable watchpoints */
+	for (unsigned i = 0; i < dpm->nwp; i++) {
+		struct dpm_wp *dwp = dpm->dwp + i;
+		struct watchpoint *wp = dwp->wp;
+
+		retval = dpm_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
+				wp ? &wp->set : NULL);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+	/* NOTE:  writes to breakpoint and watchpoint registers might
+	 * be queued, and need (efficient/batched) flushing later.
+	 */
+
+	if (cache->reg_list[0].size == 64)
+		retval = arm_dpm_write_dirty_registers_64(dpm);
+	else
+		retval = arm_dpm_write_dirty_registers_32(dpm);
+
 	/* (void) */ dpm->finish(dpm);
 done:
 	return retval;
@@ -973,8 +1125,8 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 
 	/* register access setup */
 	arm->full_context = arm_dpm_full_context;
-	arm->read_core_reg = arm_dpm_read_core_reg;
-	arm->write_core_reg = arm_dpm_write_core_reg;
+	arm->read_core_reg = arm->read_core_reg ? : arm_dpm_read_core_reg;
+	arm->write_core_reg = arm->write_core_reg ? : arm_dpm_write_core_reg;
 
 	/* avoid duplicating the register cache */
 	if (arm->core_cache == NULL) {
@@ -999,12 +1151,21 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	target->type->add_watchpoint = dpm_add_watchpoint;
 	target->type->remove_watchpoint = dpm_remove_watchpoint;
 
+
+	if (dpm->arm_reg_current == 0)
+		dpm->arm_reg_current = arm_reg_current;
+
 	/* FIXME add vector catch support */
 
-	dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
-	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
+		dpm->nwp = 1 + ((dpm->didr >> 28) & 0xf);
+	} else {
+		dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
+		dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
+	}
 
-	dpm->nwp = 1 + ((dpm->didr >> 28) & 0xf);
+	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
 	dpm->dwp = calloc(dpm->nwp, sizeof *dpm->dwp);
 
 	if (!dpm->dbp || !dpm->dwp) {
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index bbcae78..9cc0304 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -86,10 +86,19 @@ struct arm_dpm {
 	int (*instr_read_data_dcc)(struct arm_dpm *,
 			uint32_t opcode, uint32_t *data);
 
+	int (*instr_read_data_dcc_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t *data);
+
 	/** Runs one instruction, reading data from r0 after execution. */
 	int (*instr_read_data_r0)(struct arm_dpm *,
 			uint32_t opcode, uint32_t *data);
 
+	int (*instr_read_data_r0_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t *data);
+
+	struct reg *(*arm_reg_current)(struct arm *arm,
+			unsigned regnum);
+
 	/* BREAKPOINT/WATCHPOINT SUPPORT */
 
 	/**
@@ -131,6 +140,7 @@ int arm_dpm_setup(struct arm_dpm *dpm);
 int arm_dpm_initialize(struct arm_dpm *dpm);
 
 int arm_dpm_read_current_registers(struct arm_dpm *);
+int arm_dpm_read_current_registers_64(struct arm_dpm *);
 int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
 
 
-- 
cgit v1.1


From d376f7f51831ca8816bb4aca00076b0668462775 Mon Sep 17 00:00:00 2001
From: David Ung <davidu@nvidia.com>
Date: Thu, 15 Jan 2015 17:22:20 -0800
Subject: aarch64: Add ARMv8 AARCH64 support files

Add new AARCH64 target and ARMv8 support files.
This is an instantiation from the cortex_a files but modified to support
64bit ARMv8. Not all features are complete, notably breakpts and single
stepping are not yet implemented.
Currently it lets you halt of the processors, resume, dump cpu
registers,
read/write memory and getting a stack trace with gdb.

> halt
invalid mode value encountered 5
target state: halted
unrecognized psr mode: 0x5
target halted in ARM state due to debug-request, current mode:
UNRECOGNIZED
cpsr: 0x600001c5 pc: 0x00093528
MMU: disabled, D-Cache: disabled, I-Cache: disabled

> targets
    TargetName         Type       Endian TapName            State
--  ------------------ ---------- ------ ------------------ ------------
 0* cpu0               aarch64    little cpu.dap            halted

> reg
===== arm v8 registers
(0) r0 (/64): 0x00000000FFFFFFED (dirty)
(1) r1 (/64): 0x00000000F76E4000
(2) r2 (/64): 0x0000000000000000
(3) r3 (/64): 0x0000000000010000
(4) r4 (/64): 0xFFFFFFC06E2939E1
(5) r5 (/64): 0x0000000000000018
(6) r6 (/64): 0x003A699CFB3C8480
(7) r7 (/64): 0x0000000053555555
(8) r8 (/64): 0x00FFFFFFFFFFFFFF
(9) r9 (/64): 0x000000001FFEF992
(10) r10 (/64): 0x0000000000000001
(11) r11 (/64): 0x0000000000000000
(12) r12 (/64): 0x00000000000000F0
(13) r13 (/64): 0x00000000EFDFEAC8
(14) r14 (/64): 0x00000000F6DDA659
(15) r15 (/64): 0x0000000000000000
(16) r16 (/64): 0xFFFFFFC0000F9094
(17) r17 (/64): 0x0000000000000000
(18) r18 (/64): 0x0000000000000000
(19) r19 (/64): 0xFFFFFFC00087C000
(20) r20 (/64): 0x0000000000000002
(21) r21 (/64): 0xFFFFFFC000867C28
(22) r22 (/64): 0xFFFFFFC000916A52
(23) r23 (/64): 0xFFFFFFC00116D8B0
(24) r24 (/64): 0xFFFFFFC000774A0C
(25) r25 (/64): 0x000000008007B000
(26) r26 (/64): 0x000000008007D000
(27) r27 (/64): 0xFFFFFFC000080450
(28) r28 (/64): 0x0000004080000000
(29) r29 (/64): 0xFFFFFFC00087FF20
(30) r30 (/64): 0xFFFFFFC000085114
(31) sp (/64): 0xFFFFFFC00087FF20
(32) pc (/64): 0xFFFFFFC000093528
(33) xPSR (/64): 0x00000000600001C5

And from gdb

(gdb) bt
 #0  cpu_do_idle () at /mnt/host/source/src/third_party/kernel/3.14/arch/arm64/mm/proc.S:87
 #1  0xffffffc000085114 in arch_cpu_idle () at /mnt/host/source/src/third_party/kernel/3.14/arch/arm64/kernel/process.c:107
 #2  0x0000000000000000 in ?? ()

Change-Id: Iccb1d15c7d8ace7b9e811dac3c9757ced4d0f618
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/Makefile.am |   10 +
 src/target/aarch64.c   | 2601 ++++++++++++++++++++++++++++++++++++++++++++++++
 src/target/aarch64.h   |   78 ++
 src/target/arm.h       |    5 +
 src/target/arm_dpm.c   |   30 +-
 src/target/arm_dpm.h   |    7 +
 src/target/armv8.c     |  774 ++++++++++++++
 src/target/armv8.h     |  201 ++++
 src/target/target.c    |    4 +
 9 files changed, 3701 insertions(+), 9 deletions(-)
 create mode 100644 src/target/aarch64.c
 create mode 100644 src/target/aarch64.h
 create mode 100644 src/target/armv8.c
 create mode 100644 src/target/armv8.h

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index eb0d62e..a17496c 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -26,6 +26,10 @@ noinst_LTLIBRARIES += %D%/libtarget.la
 	%D%/dsp5680xx.c \
 	%D%/hla_target.c
 
+if TARGET64
+%C%_libtarget_la_SOURCES +=$(ARMV8_SRC)
+endif
+
 TARGET_CORE_SRC = \
 	%D%/algorithm.c \
 	%D%/register.c \
@@ -69,6 +73,10 @@ ARMV7_SRC = \
 	%D%/cortex_a.c \
 	%D%/ls1_sap.c
 
+ARMV8_SRC = \
+	%D%/aarch64.c \
+	%D%/armv8.c
+
 ARM_DEBUG_SRC = \
 	%D%/arm_dpm.c \
 	%D%/arm_jtag.c \
@@ -146,6 +154,7 @@ INTEL_IA32_SRC = \
 	%D%/armv7a.h \
 	%D%/armv7m.h \
 	%D%/armv7m_trace.h \
+	%D%/armv8.h \
 	%D%/avrt.h \
 	%D%/dsp563xx.h \
 	%D%/dsp563xx_once.h \
@@ -153,6 +162,7 @@ INTEL_IA32_SRC = \
 	%D%/breakpoints.h \
 	%D%/cortex_m.h \
 	%D%/cortex_a.h \
+	%D%/aarch64.h \
 	%D%/embeddedice.h \
 	%D%/etb.h \
 	%D%/etm.h \
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
new file mode 100644
index 0000000..8ac64eb
--- /dev/null
+++ b/src/target/aarch64.c
@@ -0,0 +1,2601 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "breakpoints.h"
+#include "aarch64.h"
+#include "register.h"
+#include "target_request.h"
+#include "target_type.h"
+#include "arm_opcodes.h"
+#include <helper/time_support.h>
+
+static int aarch64_poll(struct target *target);
+static int aarch64_debug_entry(struct target *target);
+static int aarch64_restore_context(struct target *target, bool bpwp);
+static int aarch64_set_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode);
+static int aarch64_set_context_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode);
+static int aarch64_set_hybrid_breakpoint(struct target *target,
+	struct breakpoint *breakpoint);
+static int aarch64_unset_breakpoint(struct target *target,
+	struct breakpoint *breakpoint);
+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_ab_memory(struct target *target,
+	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
+static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data);
+
+static int aarch64_restore_system_control_reg(struct target *target)
+{
+	int retval = ERROR_OK;
+
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	if (aarch64->system_control_reg != aarch64->system_control_reg_curr) {
+		aarch64->system_control_reg_curr = aarch64->system_control_reg;
+		retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
+						     0xd5181000,
+						     aarch64->system_control_reg);
+	}
+
+	return retval;
+}
+
+/*  check address before aarch64_apb read write access with mmu on
+ *  remove apb predictible data abort */
+static int aarch64_check_address(struct target *target, uint32_t address)
+{
+	/* TODO */
+	return ERROR_OK;
+}
+/*  modify system_control_reg in order to enable or disable mmu for :
+ *  - virt2phys address conversion
+ *  - read or write memory in phys or virt address */
+static int aarch64_mmu_modify(struct target *target, int enable)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	int retval = ERROR_OK;
+
+	if (enable) {
+		/*  if mmu enabled at target stop and mmu not enable */
+		if (!(aarch64->system_control_reg & 0x1U)) {
+			LOG_ERROR("trying to enable mmu on target stopped with mmu disable");
+			return ERROR_FAIL;
+		}
+		if (!(aarch64->system_control_reg_curr & 0x1U)) {
+			aarch64->system_control_reg_curr |= 0x1U;
+			retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
+							     0xd5181000,
+							     aarch64->system_control_reg_curr);
+		}
+	} else {
+		if (aarch64->system_control_reg_curr & 0x4U) {
+			/*  data cache is active */
+			aarch64->system_control_reg_curr &= ~0x4U;
+			/* flush data cache armv7 function to be called */
+			if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache)
+				armv8->armv8_mmu.armv8_cache.flush_all_data_cache(target);
+		}
+		if ((aarch64->system_control_reg_curr & 0x1U)) {
+			aarch64->system_control_reg_curr &= ~0x1U;
+			retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
+							     0xd5181000,
+							     aarch64->system_control_reg_curr);
+		}
+	}
+	return retval;
+}
+
+/*
+ * Basic debug access, very low level assumes state is saved
+ */
+static int aarch64_init_debug_access(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+	uint32_t dummy;
+
+	LOG_DEBUG(" ");
+
+	/* Unlocking the debug registers for modification
+	 * The debugport might be uninitialised so try twice */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			     armv8->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
+	if (retval != ERROR_OK) {
+		/* try again */
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			     armv8->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
+		if (retval == ERROR_OK)
+			LOG_USER("Locking debug access failed on first, but succeeded on second try.");
+	}
+	if (retval != ERROR_OK)
+		return retval;
+	/* Clear Sticky Power Down status Bit in PRSR to enable access to
+	   the registers in the Core Power Domain */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_PRSR, &dummy);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Enabling of instruction execution in debug mode is done in debug_entry code */
+
+	/* Resync breakpoint registers */
+
+	/* Since this is likely called from init or reset, update target state information*/
+	return aarch64_poll(target);
+}
+
+/* To reduce needless round-trips, pass in a pointer to the current
+ * DSCR value.  Initialize it to zero if you just need to know the
+ * value on return from this function; or DSCR_INSTR_COMP if you
+ * happen to know that no instruction is pending.
+ */
+static int aarch64_exec_opcode(struct target *target,
+	uint32_t opcode, uint32_t *dscr_p)
+{
+	uint32_t dscr;
+	int retval;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	dscr = dscr_p ? *dscr_p : 0;
+
+	LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
+
+	/* Wait for InstrCompl bit to be set */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_INSTR_COMP) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
+			return retval;
+		}
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_write_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_ITR, opcode);
+	if (retval != ERROR_OK)
+		return retval;
+
+	then = timeval_ms();
+	do {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read DSCR register");
+			return retval;
+		}
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
+			return ERROR_FAIL;
+		}
+	} while ((dscr & DSCR_INSTR_COMP) == 0);	/* Wait for InstrCompl bit to be set */
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+
+/* Write to memory mapped registers directly with no cache or mmu handling */
+static int aarch64_dap_write_memap_register_u32(struct target *target,
+	uint32_t address,
+	uint32_t value)
+{
+	int retval;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap, address, value);
+
+	return retval;
+}
+
+/*
+ * AARCH64 implementation of Debug Programmer's Model
+ *
+ * NOTE the invariant:  these routines return with DSCR_INSTR_COMP set,
+ * so there's no need to poll for it before executing an instruction.
+ *
+ * NOTE that in several of these cases the "stall" mode might be useful.
+ * It'd let us queue a few operations together... prepare/finish might
+ * be the places to enable/disable that mode.
+ */
+
+static inline struct aarch64_common *dpm_to_a8(struct arm_dpm *dpm)
+{
+	return container_of(dpm, struct aarch64_common, armv8_common.dpm);
+}
+
+static int aarch64_write_dcc(struct aarch64_common *a8, uint32_t data)
+{
+	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
+	return mem_ap_write_u32(a8->armv8_common.debug_ap,
+				a8->armv8_common.debug_base + CPUDBG_DTRRX, data);
+}
+
+static int aarch64_write_dcc_64(struct aarch64_common *a8, uint64_t data)
+{
+	int ret;
+	LOG_DEBUG("write DCC 0x%08" PRIx32, (unsigned)data);
+	LOG_DEBUG("write DCC 0x%08" PRIx32, (unsigned)(data >> 32));
+	ret = mem_ap_write_u32(a8->armv8_common.debug_ap,
+			       a8->armv8_common.debug_base + CPUDBG_DTRRX, data);
+	ret += mem_ap_write_u32(a8->armv8_common.debug_ap,
+				a8->armv8_common.debug_base + CPUDBG_DTRTX, data >> 32);
+	return ret;
+}
+
+static int aarch64_read_dcc(struct aarch64_common *a8, uint32_t *data,
+	uint32_t *dscr_p)
+{
+	uint32_t dscr = DSCR_INSTR_COMP;
+	int retval;
+
+	if (dscr_p)
+		dscr = *dscr_p;
+
+	/* Wait for DTRRXfull */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
+		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
+				a8->armv8_common.debug_base + CPUDBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for read dcc");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
+					    a8->armv8_common.debug_base + CPUDBG_DTRTX,
+					    data);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+static int aarch64_read_dcc_64(struct aarch64_common *a8, uint64_t *data,
+	uint32_t *dscr_p)
+{
+	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t higher;
+	int retval;
+
+	if (dscr_p)
+		dscr = *dscr_p;
+
+	/* Wait for DTRRXfull */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
+		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
+				a8->armv8_common.debug_base + CPUDBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for read dcc");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
+					    a8->armv8_common.debug_base + CPUDBG_DTRTX,
+					    (uint32_t *)data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
+					    a8->armv8_common.debug_base + CPUDBG_DTRRX,
+					    &higher);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*data = *(uint32_t *)data | (uint64_t)higher << 32;
+	LOG_DEBUG("read DCC 0x%16.16" PRIx64, *data);
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+
+static int aarch64_dpm_prepare(struct arm_dpm *dpm)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t dscr;
+	int retval;
+
+	/* set up invariant:  INSTR_COMP is set after ever DPM operation */
+	long long then = timeval_ms();
+	for (;; ) {
+		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
+				a8->armv8_common.debug_base + CPUDBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((dscr & DSCR_INSTR_COMP) != 0)
+			break;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for dpm prepare");
+			return ERROR_FAIL;
+		}
+	}
+
+	/* this "should never happen" ... */
+	if (dscr & DSCR_DTR_RX_FULL) {
+		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
+		/* Clear DCCRX */
+		retval = aarch64_exec_opcode(
+				a8->armv8_common.arm.target,
+				0xd5130400,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int aarch64_dpm_finish(struct arm_dpm *dpm)
+{
+	/* REVISIT what could be done here? */
+	return ERROR_OK;
+}
+
+static int aarch64_instr_write_data_dcc(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	int retval;
+	uint32_t dscr = DSCR_INSTR_COMP;
+
+	retval = aarch64_write_dcc(a8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+}
+
+static int aarch64_instr_write_data_dcc_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	int retval;
+	uint32_t dscr = DSCR_INSTR_COMP;
+
+	retval = aarch64_write_dcc_64(a8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+}
+
+static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t dscr = DSCR_INSTR_COMP;
+	int retval;
+
+	retval = aarch64_write_dcc(a8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			0xd5330500,
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* then the opcode, taking data from R0 */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+
+	return retval;
+}
+
+static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t dscr = DSCR_INSTR_COMP;
+	int retval;
+
+	retval = aarch64_write_dcc_64(a8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			0xd5330400,
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* then the opcode, taking data from R0 */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+
+	return retval;
+}
+
+static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
+{
+	struct target *target = dpm->arm->target;
+	uint32_t dscr = DSCR_INSTR_COMP;
+
+	/* "Prefetch flush" after modifying execution status in CPSR */
+	return aarch64_exec_opcode(target,
+			ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
+			&dscr);
+}
+
+static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	int retval;
+	uint32_t dscr = DSCR_INSTR_COMP;
+
+	/* the opcode, writing data to DCC */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return aarch64_read_dcc(a8, data, &dscr);
+}
+
+static int aarch64_instr_read_data_dcc_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t *data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	int retval;
+	uint32_t dscr = DSCR_INSTR_COMP;
+
+	/* the opcode, writing data to DCC */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return aarch64_read_dcc_64(a8, data, &dscr);
+}
+
+static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t dscr = DSCR_INSTR_COMP;
+	int retval;
+
+	/* the opcode, writing data to R0 */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write R0 to DCC */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			0xd5130400,  /* msr dbgdtr_el0, x0 */
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return aarch64_read_dcc(a8, data, &dscr);
+}
+
+static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t *data)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t dscr = DSCR_INSTR_COMP;
+	int retval;
+
+	/* the opcode, writing data to R0 */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write R0 to DCC */
+	retval = aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			0xd5130400,  /* msr dbgdtr_el0, x0 */
+			&dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return aarch64_read_dcc_64(a8, data, &dscr);
+}
+
+static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
+	uint32_t addr, uint32_t control)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t vr = a8->armv8_common.debug_base;
+	uint32_t cr = a8->armv8_common.debug_base;
+	int retval;
+
+	switch (index_t) {
+		case 0 ... 15:	/* breakpoints */
+			vr += CPUDBG_BVR_BASE;
+			cr += CPUDBG_BCR_BASE;
+			break;
+		case 16 ... 31:	/* watchpoints */
+			vr += CPUDBG_WVR_BASE;
+			cr += CPUDBG_WCR_BASE;
+			index_t -= 16;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	vr += 4 * index_t;
+	cr += 4 * index_t;
+
+	LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
+		(unsigned) vr, (unsigned) cr);
+
+	retval = aarch64_dap_write_memap_register_u32(dpm->arm->target,
+			vr, addr);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(dpm->arm->target,
+			cr, control);
+	return retval;
+}
+
+static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
+{
+	return ERROR_OK;
+
+#if 0
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t cr;
+
+	switch (index_t) {
+		case 0 ... 15:
+			cr = a8->armv8_common.debug_base + CPUDBG_BCR_BASE;
+			break;
+		case 16 ... 31:
+			cr = a8->armv8_common.debug_base + CPUDBG_WCR_BASE;
+			index_t -= 16;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	cr += 4 * index_t;
+
+	LOG_DEBUG("A8: bpwp disable, cr %08x", (unsigned) cr);
+
+	/* clear control register */
+	return aarch64_dap_write_memap_register_u32(dpm->arm->target, cr, 0);
+#endif
+}
+
+static int aarch64_dpm_setup(struct aarch64_common *a8, uint32_t debug)
+{
+	struct arm_dpm *dpm = &a8->armv8_common.dpm;
+	int retval;
+
+	dpm->arm = &a8->armv8_common.arm;
+	dpm->didr = debug;
+
+	dpm->prepare = aarch64_dpm_prepare;
+	dpm->finish = aarch64_dpm_finish;
+
+	dpm->instr_write_data_dcc = aarch64_instr_write_data_dcc;
+	dpm->instr_write_data_dcc_64 = aarch64_instr_write_data_dcc_64;
+	dpm->instr_write_data_r0 = aarch64_instr_write_data_r0;
+	dpm->instr_write_data_r0_64 = aarch64_instr_write_data_r0_64;
+	dpm->instr_cpsr_sync = aarch64_instr_cpsr_sync;
+
+	dpm->instr_read_data_dcc = aarch64_instr_read_data_dcc;
+	dpm->instr_read_data_dcc_64 = aarch64_instr_read_data_dcc_64;
+	dpm->instr_read_data_r0 = aarch64_instr_read_data_r0;
+	dpm->instr_read_data_r0_64 = aarch64_instr_read_data_r0_64;
+
+	dpm->arm_reg_current = armv8_reg_current;
+
+	dpm->bpwp_enable = aarch64_bpwp_enable;
+	dpm->bpwp_disable = aarch64_bpwp_disable;
+
+	retval = arm_dpm_setup(dpm);
+	if (retval == ERROR_OK)
+		retval = arm_dpm_initialize(dpm);
+
+	return retval;
+}
+static struct target *get_aarch64(struct target *target, int32_t coreid)
+{
+	struct target_list *head;
+	struct target *curr;
+
+	head = target->head;
+	while (head != (struct target_list *)NULL) {
+		curr = head->target;
+		if ((curr->coreid == coreid) && (curr->state == TARGET_HALTED))
+			return curr;
+		head = head->next;
+	}
+	return target;
+}
+static int aarch64_halt(struct target *target);
+
+static int aarch64_halt_smp(struct target *target)
+{
+	int retval = 0;
+	struct target_list *head;
+	struct target *curr;
+	head = target->head;
+	while (head != (struct target_list *)NULL) {
+		curr = head->target;
+		if ((curr != target) && (curr->state != TARGET_HALTED))
+			retval += aarch64_halt(curr);
+		head = head->next;
+	}
+	return retval;
+}
+
+static int update_halt_gdb(struct target *target)
+{
+	int retval = 0;
+	if (target->gdb_service && target->gdb_service->core[0] == -1) {
+		target->gdb_service->target = target;
+		target->gdb_service->core[0] = target->coreid;
+		retval += aarch64_halt_smp(target);
+	}
+	return retval;
+}
+
+/*
+ * Cortex-A8 Run control
+ */
+
+static int aarch64_poll(struct target *target)
+{
+	int retval = ERROR_OK;
+	uint32_t dscr;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	enum target_state prev_target_state = target->state;
+	/*  toggle to another core is done by gdb as follow */
+	/*  maint packet J core_id */
+	/*  continue */
+	/*  the next polling trigger an halt event sent to gdb */
+	if ((target->state == TARGET_HALTED) && (target->smp) &&
+		(target->gdb_service) &&
+		(target->gdb_service->target == NULL)) {
+		target->gdb_service->target =
+			get_aarch64(target, target->gdb_service->core[1]);
+		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+		return retval;
+	}
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+	aarch64->cpudbg_dscr = dscr;
+
+	if (DSCR_RUN_MODE(dscr) == (DSCR_CORE_HALTED | DSCR_CORE_RESTARTED)) {
+		if (prev_target_state != TARGET_HALTED) {
+			/* We have a halting debug event */
+			LOG_DEBUG("Target halted");
+			target->state = TARGET_HALTED;
+			if ((prev_target_state == TARGET_RUNNING)
+				|| (prev_target_state == TARGET_UNKNOWN)
+				|| (prev_target_state == TARGET_RESET)) {
+				retval = aarch64_debug_entry(target);
+				if (retval != ERROR_OK)
+					return retval;
+				if (target->smp) {
+					retval = update_halt_gdb(target);
+					if (retval != ERROR_OK)
+						return retval;
+				}
+				target_call_event_callbacks(target,
+					TARGET_EVENT_HALTED);
+			}
+			if (prev_target_state == TARGET_DEBUG_RUNNING) {
+				LOG_DEBUG(" ");
+
+				retval = aarch64_debug_entry(target);
+				if (retval != ERROR_OK)
+					return retval;
+				if (target->smp) {
+					retval = update_halt_gdb(target);
+					if (retval != ERROR_OK)
+						return retval;
+				}
+
+				target_call_event_callbacks(target,
+					TARGET_EVENT_DEBUG_HALTED);
+			}
+		}
+	} else if (DSCR_RUN_MODE(dscr) == DSCR_CORE_RESTARTED)
+		target->state = TARGET_RUNNING;
+	else {
+		LOG_DEBUG("Unknown target state dscr = 0x%08" PRIx32, dscr);
+		target->state = TARGET_UNKNOWN;
+	}
+
+	return retval;
+}
+
+static int aarch64_halt(struct target *target)
+{
+	int retval = ERROR_OK;
+	uint32_t dscr;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0, 1);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0, &dscr);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x140, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x140, 6);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x140, &dscr);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0xa0, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0xa0, 5);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0xa0, &dscr);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0xa4, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0xa4, 2);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0xa4, &dscr);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x20, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x20, 4);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x20, &dscr);
+
+	/*
+	 * enter halting debug mode
+	 */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+#	/* STATUS */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x134, &dscr);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x1c, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x1c, 1);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x1c, &dscr);
+
+
+	long long then = timeval_ms();
+	for (;; ) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((dscr & DSCR_CORE_HALTED) != 0)
+			break;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for halt");
+			return ERROR_FAIL;
+		}
+	}
+
+	target->debug_reason = DBG_REASON_DBGRQ;
+
+	return ERROR_OK;
+}
+
+static int aarch64_internal_restore(struct target *target, int current,
+	uint64_t *address, int handle_breakpoints, int debug_execution)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int retval;
+	uint64_t resume_pc;
+
+	if (!debug_execution)
+		target_free_all_working_areas(target);
+
+	/* current = 1: continue on current pc, otherwise continue at <address> */
+	resume_pc = buf_get_u64(arm->pc->value, 0, 64);
+	if (!current)
+		resume_pc = *address;
+	else
+		*address = resume_pc;
+
+	/* Make sure that the Armv7 gdb thumb fixups does not
+	 * kill the return address
+	 */
+	switch (arm->core_state) {
+		case ARM_STATE_ARM:
+		case ARM_STATE_AARCH64:
+			resume_pc &= 0xFFFFFFFFFFFFFFFC;
+			break;
+		case ARM_STATE_THUMB:
+		case ARM_STATE_THUMB_EE:
+			/* When the return address is loaded into PC
+			 * bit 0 must be 1 to stay in Thumb state
+			 */
+			resume_pc |= 0x1;
+			break;
+		case ARM_STATE_JAZELLE:
+			LOG_ERROR("How do I resume into Jazelle state??");
+			return ERROR_FAIL;
+	}
+	LOG_DEBUG("resume pc = 0x%16" PRIx64, resume_pc);
+	buf_set_u64(arm->pc->value, 0, 64, resume_pc);
+	arm->pc->dirty = 1;
+	arm->pc->valid = 1;
+#if 0
+	/* restore dpm_mode at system halt */
+	dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+#endif
+	/* called it now before restoring context because it uses cpu
+	 * register r0 for restoring system control register */
+	retval = aarch64_restore_system_control_reg(target);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_restore_context(target, handle_breakpoints);
+	if (retval != ERROR_OK)
+		return retval;
+	target->debug_reason = DBG_REASON_NOTHALTED;
+	target->state = TARGET_RUNNING;
+
+	/* registers are now invalid */
+	register_cache_invalidate(arm->core_cache);
+
+#if 0
+	/* the front-end may request us not to handle breakpoints */
+	if (handle_breakpoints) {
+		/* Single step past breakpoint at current address */
+		breakpoint = breakpoint_find(target, resume_pc);
+		if (breakpoint) {
+			LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
+			cortex_m3_unset_breakpoint(target, breakpoint);
+			cortex_m3_single_step_core(target);
+			cortex_m3_set_breakpoint(target, breakpoint);
+		}
+	}
+#endif
+
+	return retval;
+}
+
+static int aarch64_internal_restart(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int retval;
+	uint32_t dscr;
+	/*
+	 * * Restart core and wait for it to be started.  Clear ITRen and sticky
+	 * * exception flags: see ARMv7 ARM, C5.9.
+	 *
+	 * REVISIT: for single stepping, we probably want to
+	 * disable IRQs by default, with optional override...
+	 */
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if ((dscr & DSCR_INSTR_COMP) == 0)
+		LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, dscr & ~DSCR_ITR_EN);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DRCR, DRCR_RESTART |
+			DRCR_CLEAR_EXCEPTIONS);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x10, 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x10000 + 0x1c, 2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	long long then = timeval_ms();
+	for (;; ) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((dscr & DSCR_CORE_RESTARTED) != 0)
+			break;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for resume");
+			return ERROR_FAIL;
+		}
+	}
+
+	target->debug_reason = DBG_REASON_NOTHALTED;
+	target->state = TARGET_RUNNING;
+
+	/* registers are now invalid */
+	register_cache_invalidate(arm->core_cache);
+
+	return ERROR_OK;
+}
+
+static int aarch64_restore_smp(struct target *target, int handle_breakpoints)
+{
+	int retval = 0;
+	struct target_list *head;
+	struct target *curr;
+	uint64_t address;
+	head = target->head;
+	while (head != (struct target_list *)NULL) {
+		curr = head->target;
+		if ((curr != target) && (curr->state != TARGET_RUNNING)) {
+			/*  resume current address , not in step mode */
+			retval += aarch64_internal_restore(curr, 1, &address,
+					handle_breakpoints, 0);
+			retval += aarch64_internal_restart(curr);
+		}
+		head = head->next;
+
+	}
+	return retval;
+}
+
+static int aarch64_resume(struct target *target, int current,
+	target_addr_t address, int handle_breakpoints, int debug_execution)
+{
+	int retval = 0;
+	uint64_t resume_addr;
+
+	if (address) {
+		LOG_DEBUG("resuming with custom address not supported");
+		return ERROR_FAIL;
+	}
+
+	/* dummy resume for smp toggle in order to reduce gdb impact  */
+	if ((target->smp) && (target->gdb_service->core[1] != -1)) {
+		/*   simulate a start and halt of target */
+		target->gdb_service->target = NULL;
+		target->gdb_service->core[0] = target->gdb_service->core[1];
+		/*  fake resume at next poll we play the  target core[1], see poll*/
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+		return 0;
+	}
+	aarch64_internal_restore(target, current, &resume_addr, handle_breakpoints, debug_execution);
+	if (target->smp) {
+		target->gdb_service->core[0] = -1;
+		retval = aarch64_restore_smp(target, handle_breakpoints);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+	aarch64_internal_restart(target);
+
+	if (!debug_execution) {
+		target->state = TARGET_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+		LOG_DEBUG("target resumed at 0x%" PRIx64, resume_addr);
+	} else {
+		target->state = TARGET_DEBUG_RUNNING;
+		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
+		LOG_DEBUG("target debug resumed at 0x%" PRIx64, resume_addr);
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_debug_entry(struct target *target)
+{
+	uint32_t dscr;
+	int retval = ERROR_OK;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	LOG_DEBUG("dscr = 0x%08" PRIx32, aarch64->cpudbg_dscr);
+
+	/* REVISIT surely we should not re-read DSCR !! */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* REVISIT see A8 TRM 12.11.4 steps 2..3 -- make sure that any
+	 * imprecise data aborts get discarded by issuing a Data
+	 * Synchronization Barrier:  ARMV4_5_MCR(15, 0, 0, 7, 10, 4).
+	 */
+
+	/* Enable the ITR execution once we are in debug mode */
+	dscr |= DSCR_ITR_EN;
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Examine debug reason */
+	arm_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
+
+	/* save address of instruction that triggered the watchpoint? */
+	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
+		uint32_t wfar;
+
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_WFAR,
+				&wfar);
+		if (retval != ERROR_OK)
+			return retval;
+		arm_dpm_report_wfar(&armv8->dpm, wfar);
+	}
+
+	retval = arm_dpm_read_current_registers_64(&armv8->dpm);
+
+	if (armv8->post_debug_entry) {
+		retval = armv8->post_debug_entry(target);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int aarch64_post_debug_entry(struct target *target)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct armv8_mmu_common *armv8_mmu = &armv8->armv8_mmu;
+	uint32_t sctlr_el1 = 0;
+	int retval;
+
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DRCR, 1<<2);
+	retval = aarch64_instr_read_data_r0(armv8->arm.dpm,
+					    0xd5381000, &sctlr_el1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("sctlr_el1 = %#8.8x", sctlr_el1);
+	aarch64->system_control_reg = sctlr_el1;
+	aarch64->system_control_reg_curr = sctlr_el1;
+	aarch64->curr_mode = armv8->arm.core_mode;
+
+	armv8_mmu->mmu_enabled = sctlr_el1 & 0x1U ? 1 : 0;
+	armv8_mmu->armv8_cache.d_u_cache_enabled = sctlr_el1 & 0x4U ? 1 : 0;
+	armv8_mmu->armv8_cache.i_cache_enabled = sctlr_el1 & 0x1000U ? 1 : 0;
+
+#if 0
+	if (armv8->armv8_mmu.armv8_cache.ctype == -1)
+		armv8_identify_cache(target);
+#endif
+
+	return ERROR_OK;
+}
+
+static int aarch64_step(struct target *target, int current, target_addr_t address,
+	int handle_breakpoints)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	struct breakpoint *breakpoint = NULL;
+	struct breakpoint stepbreakpoint;
+	struct reg *r;
+	int retval;
+
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* current = 1: continue on current pc, otherwise continue at <address> */
+	r = arm->pc;
+	if (!current)
+		buf_set_u64(r->value, 0, 64, address);
+	else
+		address = buf_get_u64(r->value, 0, 64);
+
+	/* The front-end may request us not to handle breakpoints.
+	 * But since Cortex-A8 uses breakpoint for single step,
+	 * we MUST handle breakpoints.
+	 */
+	handle_breakpoints = 1;
+	if (handle_breakpoints) {
+		breakpoint = breakpoint_find(target, address);
+		if (breakpoint)
+			aarch64_unset_breakpoint(target, breakpoint);
+	}
+
+	/* Setup single step breakpoint */
+	stepbreakpoint.address = address;
+	stepbreakpoint.length = 4;
+	stepbreakpoint.type = BKPT_HARD;
+	stepbreakpoint.set = 0;
+
+	/* Break on IVA mismatch */
+	aarch64_set_breakpoint(target, &stepbreakpoint, 0x04);
+
+	target->debug_reason = DBG_REASON_SINGLESTEP;
+
+	retval = aarch64_resume(target, 1, address, 0, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	long long then = timeval_ms();
+	while (target->state != TARGET_HALTED) {
+		retval = aarch64_poll(target);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("timeout waiting for target halt");
+			return ERROR_FAIL;
+		}
+	}
+
+	aarch64_unset_breakpoint(target, &stepbreakpoint);
+
+	target->debug_reason = DBG_REASON_BREAKPOINT;
+
+	if (breakpoint)
+		aarch64_set_breakpoint(target, breakpoint, 0);
+
+	if (target->state != TARGET_HALTED)
+		LOG_DEBUG("target stepped");
+
+	return ERROR_OK;
+}
+
+static int aarch64_restore_context(struct target *target, bool bpwp)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	LOG_DEBUG(" ");
+
+	if (armv8->pre_restore_context)
+		armv8->pre_restore_context(target);
+
+	return arm_dpm_write_dirty_registers(&armv8->dpm, bpwp);
+
+	return ERROR_OK;
+}
+
+/*
+ * Cortex-A8 Breakpoint and watchpoint functions
+ */
+
+/* Setup hardware Breakpoint Register Pair */
+static int aarch64_set_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode)
+{
+	int retval;
+	int brp_i = 0;
+	uint32_t control;
+	uint8_t byte_addr_select = 0x0F;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD) {
+		while (brp_list[brp_i].used && (brp_i < aarch64->brp_num))
+			brp_i++;
+		if (brp_i >= aarch64->brp_num) {
+			LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+			return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+		}
+		breakpoint->set = brp_i + 1;
+		if (breakpoint->length == 2)
+			byte_addr_select = (3 << (breakpoint->address & 0x02));
+		control = ((matchmode & 0x7) << 20)
+			| (byte_addr_select << 5)
+			| (3 << 1) | 1;
+		brp_list[brp_i].used = 1;
+		brp_list[brp_i].value = (breakpoint->address & 0xFFFFFFFC);
+		brp_list[brp_i].control = control;
+		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+				+ CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+				brp_list[brp_i].value);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+				+ CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+				brp_list[brp_i].control);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
+			brp_list[brp_i].control,
+			brp_list[brp_i].value);
+	} else if (breakpoint->type == BKPT_SOFT) {
+		uint8_t code[4];
+		if (breakpoint->length == 2)
+			buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11));
+		else
+			buf_set_u32(code, 0, 32, ARMV5_BKPT(0x11));
+		retval = target_read_memory(target,
+				breakpoint->address & 0xFFFFFFFE,
+				breakpoint->length, 1,
+				breakpoint->orig_instr);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = target_write_memory(target,
+				breakpoint->address & 0xFFFFFFFE,
+				breakpoint->length, 1, code);
+		if (retval != ERROR_OK)
+			return retval;
+		breakpoint->set = 0x11;	/* Any nice value but 0 */
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_set_context_breakpoint(struct target *target,
+	struct breakpoint *breakpoint, uint8_t matchmode)
+{
+	int retval = ERROR_FAIL;
+	int brp_i = 0;
+	uint32_t control;
+	uint8_t byte_addr_select = 0x0F;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return retval;
+	}
+	/*check available context BRPs*/
+	while ((brp_list[brp_i].used ||
+		(brp_list[brp_i].type != BRP_CONTEXT)) && (brp_i < aarch64->brp_num))
+		brp_i++;
+
+	if (brp_i >= aarch64->brp_num) {
+		LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+		return ERROR_FAIL;
+	}
+
+	breakpoint->set = brp_i + 1;
+	control = ((matchmode & 0x7) << 20)
+		| (byte_addr_select << 5)
+		| (3 << 1) | 1;
+	brp_list[brp_i].used = 1;
+	brp_list[brp_i].value = (breakpoint->asid);
+	brp_list[brp_i].control = control;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+			brp_list[brp_i].value);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+			brp_list[brp_i].control);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
+		brp_list[brp_i].control,
+		brp_list[brp_i].value);
+	return ERROR_OK;
+
+}
+
+static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	int retval = ERROR_FAIL;
+	int brp_1 = 0;	/* holds the contextID pair */
+	int brp_2 = 0;	/* holds the IVA pair */
+	uint32_t control_CTX, control_IVA;
+	uint8_t CTX_byte_addr_select = 0x0F;
+	uint8_t IVA_byte_addr_select = 0x0F;
+	uint8_t CTX_machmode = 0x03;
+	uint8_t IVA_machmode = 0x01;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (breakpoint->set) {
+		LOG_WARNING("breakpoint already set");
+		return retval;
+	}
+	/*check available context BRPs*/
+	while ((brp_list[brp_1].used ||
+		(brp_list[brp_1].type != BRP_CONTEXT)) && (brp_1 < aarch64->brp_num))
+		brp_1++;
+
+	printf("brp(CTX) found num: %d\n", brp_1);
+	if (brp_1 >= aarch64->brp_num) {
+		LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+		return ERROR_FAIL;
+	}
+
+	while ((brp_list[brp_2].used ||
+		(brp_list[brp_2].type != BRP_NORMAL)) && (brp_2 < aarch64->brp_num))
+		brp_2++;
+
+	printf("brp(IVA) found num: %d\n", brp_2);
+	if (brp_2 >= aarch64->brp_num) {
+		LOG_ERROR("ERROR Can not find free Breakpoint Register Pair");
+		return ERROR_FAIL;
+	}
+
+	breakpoint->set = brp_1 + 1;
+	breakpoint->linked_BRP = brp_2;
+	control_CTX = ((CTX_machmode & 0x7) << 20)
+		| (brp_2 << 16)
+		| (0 << 14)
+		| (CTX_byte_addr_select << 5)
+		| (3 << 1) | 1;
+	brp_list[brp_1].used = 1;
+	brp_list[brp_1].value = (breakpoint->asid);
+	brp_list[brp_1].control = control_CTX;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUDBG_BVR_BASE + 4 * brp_list[brp_1].BRPn,
+			brp_list[brp_1].value);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUDBG_BCR_BASE + 4 * brp_list[brp_1].BRPn,
+			brp_list[brp_1].control);
+	if (retval != ERROR_OK)
+		return retval;
+
+	control_IVA = ((IVA_machmode & 0x7) << 20)
+		| (brp_1 << 16)
+		| (IVA_byte_addr_select << 5)
+		| (3 << 1) | 1;
+	brp_list[brp_2].used = 1;
+	brp_list[brp_2].value = (breakpoint->address & 0xFFFFFFFC);
+	brp_list[brp_2].control = control_IVA;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUDBG_BVR_BASE + 4 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUDBG_BCR_BASE + 4 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].control);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	int retval;
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct aarch64_brp *brp_list = aarch64->brp_list;
+
+	if (!breakpoint->set) {
+		LOG_WARNING("breakpoint not set");
+		return ERROR_OK;
+	}
+
+	if (breakpoint->type == BKPT_HARD) {
+		if ((breakpoint->address != 0) && (breakpoint->asid != 0)) {
+			int brp_i = breakpoint->set - 1;
+			int brp_j = breakpoint->linked_BRP;
+			if ((brp_i < 0) || (brp_i >= aarch64->brp_num)) {
+				LOG_DEBUG("Invalid BRP number in breakpoint");
+				return ERROR_OK;
+			}
+			LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
+				brp_list[brp_i].control, brp_list[brp_i].value);
+			brp_list[brp_i].used = 0;
+			brp_list[brp_i].value = 0;
+			brp_list[brp_i].control = 0;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUDBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].control);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUDBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+			if ((brp_j < 0) || (brp_j >= aarch64->brp_num)) {
+				LOG_DEBUG("Invalid BRP number in breakpoint");
+				return ERROR_OK;
+			}
+			LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, brp_j,
+				brp_list[brp_j].control, brp_list[brp_j].value);
+			brp_list[brp_j].used = 0;
+			brp_list[brp_j].value = 0;
+			brp_list[brp_j].control = 0;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUDBG_BCR_BASE + 16 * brp_list[brp_j].BRPn,
+					brp_list[brp_j].control);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUDBG_BVR_BASE + 16 * brp_list[brp_j].BRPn,
+					brp_list[brp_j].value);
+			if (retval != ERROR_OK)
+				return retval;
+			breakpoint->linked_BRP = 0;
+			breakpoint->set = 0;
+			return ERROR_OK;
+
+		} else {
+			int brp_i = breakpoint->set - 1;
+			if ((brp_i < 0) || (brp_i >= aarch64->brp_num)) {
+				LOG_DEBUG("Invalid BRP number in breakpoint");
+				return ERROR_OK;
+			}
+			LOG_DEBUG("rbp %i control 0x%0" PRIx32 " value 0x%0" PRIx64, brp_i,
+				brp_list[brp_i].control, brp_list[brp_i].value);
+			brp_list[brp_i].used = 0;
+			brp_list[brp_i].value = 0;
+			brp_list[brp_i].control = 0;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].control);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+					brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+			breakpoint->set = 0;
+			return ERROR_OK;
+		}
+	} else {
+		/* restore original instruction (kept in target endianness) */
+		if (breakpoint->length == 4) {
+			retval = target_write_memory(target,
+					breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+					4, 1, breakpoint->orig_instr);
+			if (retval != ERROR_OK)
+				return retval;
+		} else {
+			retval = target_write_memory(target,
+					breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+					2, 1, breakpoint->orig_instr);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+	breakpoint->set = 0;
+
+	return ERROR_OK;
+}
+
+static int aarch64_add_breakpoint(struct target *target,
+	struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
+		LOG_INFO("no hardware breakpoint available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+		aarch64->brp_num_available--;
+
+	return aarch64_set_breakpoint(target, breakpoint, 0x00);	/* Exact match */
+}
+
+static int aarch64_add_context_breakpoint(struct target *target,
+	struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
+		LOG_INFO("no hardware breakpoint available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+		aarch64->brp_num_available--;
+
+	return aarch64_set_context_breakpoint(target, breakpoint, 0x02);	/* asid match */
+}
+
+static int aarch64_add_hybrid_breakpoint(struct target *target,
+	struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	if ((breakpoint->type == BKPT_HARD) && (aarch64->brp_num_available < 1)) {
+		LOG_INFO("no hardware breakpoint available");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	if (breakpoint->type == BKPT_HARD)
+		aarch64->brp_num_available--;
+
+	return aarch64_set_hybrid_breakpoint(target, breakpoint);	/* ??? */
+}
+
+
+static int aarch64_remove_breakpoint(struct target *target, struct breakpoint *breakpoint)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+#if 0
+/* It is perfectly possible to remove breakpoints while the target is running */
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+#endif
+
+	if (breakpoint->set) {
+		aarch64_unset_breakpoint(target, breakpoint);
+		if (breakpoint->type == BKPT_HARD)
+			aarch64->brp_num_available++;
+	}
+
+	return ERROR_OK;
+}
+
+/*
+ * Cortex-A8 Reset functions
+ */
+
+static int aarch64_assert_reset(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	LOG_DEBUG(" ");
+
+	/* FIXME when halt is requested, make it work somehow... */
+
+	/* Issue some kind of warm reset. */
+	if (target_has_event_action(target, TARGET_EVENT_RESET_ASSERT))
+		target_handle_event(target, TARGET_EVENT_RESET_ASSERT);
+	else if (jtag_get_reset_config() & RESET_HAS_SRST) {
+		/* REVISIT handle "pulls" cases, if there's
+		 * hardware that needs them to work.
+		 */
+		jtag_add_reset(0, 1);
+	} else {
+		LOG_ERROR("%s: how to reset?", target_name(target));
+		return ERROR_FAIL;
+	}
+
+	/* registers are now invalid */
+	register_cache_invalidate(armv8->arm.core_cache);
+
+	target->state = TARGET_RESET;
+
+	return ERROR_OK;
+}
+
+static int aarch64_deassert_reset(struct target *target)
+{
+	int retval;
+
+	LOG_DEBUG(" ");
+
+	/* be certain SRST is off */
+	jtag_add_reset(0, 0);
+
+	retval = aarch64_poll(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->reset_halt) {
+		if (target->state != TARGET_HALTED) {
+			LOG_WARNING("%s: ran after reset and before halt ...",
+				target_name(target));
+			retval = target_halt(target);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_write_apb_ab_memory(struct target *target,
+	uint64_t address, uint32_t size,
+	uint32_t count, const uint8_t *buffer)
+{
+	/* write memory through APB-AP */
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	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;
+	uint32_t i = 0;
+
+	LOG_DEBUG("Writing APB-AP memory address 0x%" 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 R0 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;
+
+	/*  clear any abort  */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap, armv8->debug_base + CPUDBG_DRCR, 1<<2);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* This algorithm comes from either :
+	 * Cortex-A8 TRM Example 12-25
+	 * Cortex-R4 TRM Example 11-26
+	 * (slight differences)
+	 */
+
+	/* 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_ab_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_ab_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 + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_w;
+
+	/* Set DTR mode to Normal*/
+	dscr = (dscr & ~DSCR_EXT_DCC_MASK) | DSCR_EXT_DCC_NON_BLOCKING;
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_w;
+
+	if (size > 4) {
+		LOG_WARNING("reading size >4 bytes not yet supported");
+		goto error_unset_dtr_w;
+	}
+
+	retval = aarch64_instr_write_data_dcc_64(arm->dpm, 0xd5330401, address+4);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_w;
+
+	dscr = DSCR_INSTR_COMP;
+	while (i < count * size) {
+		uint32_t val;
+
+		memcpy(&val, &buffer[i], size);
+		retval = aarch64_instr_write_data_dcc(arm->dpm, 0xd5330500, val);
+		if (retval != ERROR_OK)
+			goto error_unset_dtr_w;
+
+		retval = aarch64_exec_opcode(target, 0xb81fc020, &dscr);
+		if (retval != ERROR_OK)
+			goto error_unset_dtr_w;
+
+		retval = aarch64_exec_opcode(target, 0x91001021, &dscr);
+		if (retval != ERROR_OK)
+			goto error_unset_dtr_w;
+
+		i += 4;
+	}
+
+	/* Check for sticky abort flags in the DSCR */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_w;
+	if (dscr & (DSCR_STICKY_ABORT_PRECISE | DSCR_STICKY_ABORT_IMPRECISE)) {
+		/* Abort occurred - clear it and exit */
+		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
+		mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUDBG_DRCR, 1<<2);
+		goto error_free_buff_w;
+	}
+
+	/* 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 + CPUDBG_DSCR, &dscr);
+	dscr = (dscr & ~DSCR_EXT_DCC_MASK) | DSCR_EXT_DCC_NON_BLOCKING;
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, dscr);
+error_free_buff_w:
+	LOG_ERROR("error");
+	free(tmp_buff);
+	return ERROR_FAIL;
+}
+
+static int aarch64_read_apb_ab_memory(struct target *target,
+	target_addr_t address, uint32_t size,
+	uint32_t count, uint8_t *buffer)
+{
+	/* read memory through APB-AP */
+
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	struct reg *reg;
+	uint32_t dscr, val;
+	uint8_t *tmp_buff = NULL;
+	uint32_t i = 0;
+
+	LOG_DEBUG("Reading APB-AP memory address 0x%" TARGET_PRIxADDR " size %"	PRIu32 " count%"  PRIu32,
+			  address, size, count);
+	if (target->state != TARGET_HALTED) {
+		LOG_WARNING("target not halted");
+		return ERROR_TARGET_NOT_HALTED;
+	}
+
+	/* Mark register R0 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, 0);
+	reg->dirty = true;
+
+	/*  clear any abort  */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+		armv8->debug_base + CPUDBG_DRCR, 1<<2);
+	if (retval != ERROR_OK)
+		goto error_free_buff_r;
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_r;
+
+	if (size > 4) {
+		LOG_WARNING("reading size >4 bytes not yet supported");
+		goto error_unset_dtr_r;
+	}
+
+	while (i < count * size) {
+
+		retval = aarch64_instr_write_data_dcc_64(arm->dpm, 0xd5330400, address+4);
+		if (retval != ERROR_OK)
+			goto error_unset_dtr_r;
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+
+		dscr = DSCR_INSTR_COMP;
+		retval = aarch64_exec_opcode(target, 0xb85fc000, &dscr);
+		if (retval != ERROR_OK)
+			goto error_unset_dtr_r;
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, &dscr);
+
+		retval = aarch64_instr_read_data_dcc(arm->dpm, 0xd5130400, &val);
+		if (retval != ERROR_OK)
+			goto error_unset_dtr_r;
+		memcpy(&buffer[i], &val, size);
+		i += 4;
+		address += 4;
+	}
+
+	/* Clear any sticky error */
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+		armv8->debug_base + CPUDBG_DRCR, 1<<2);
+
+	/* Done */
+	return ERROR_OK;
+
+error_unset_dtr_r:
+	LOG_WARNING("DSCR = 0x%" PRIx32, dscr);
+	/* Todo: Unset DTR mode */
+
+error_free_buff_r:
+	LOG_ERROR("error");
+	free(tmp_buff);
+
+	/* Clear any sticky error */
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+		armv8->debug_base + CPUDBG_DRCR, 1<<2);
+
+	return ERROR_FAIL;
+}
+
+static int aarch64_read_phys_memory(struct target *target,
+	target_addr_t address, uint32_t size,
+	uint32_t count, uint8_t *buffer)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	uint8_t apsel = swjdp->apsel;
+	LOG_DEBUG("Reading memory at real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32,
+		address, size, count);
+
+	if (count && buffer) {
+
+		if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
+
+			/* read memory through AHB-AP */
+			retval = mem_ap_read_buf(armv8->memory_ap, buffer, size, count, address);
+		} else {
+			/* read memory through APB-AP */
+			retval = aarch64_mmu_modify(target, 0);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_read_apb_ab_memory(target, address, size, count, buffer);
+		}
+	}
+	return retval;
+}
+
+static int aarch64_read_memory(struct target *target, target_addr_t address,
+	uint32_t size, uint32_t count, uint8_t *buffer)
+{
+	int mmu_enabled = 0;
+	target_addr_t virt, phys;
+	int retval;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	uint8_t apsel = swjdp->apsel;
+
+	/* aarch64 handles unaligned memory access */
+	LOG_DEBUG("Reading memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
+		size, count);
+
+	/* determine if MMU was enabled on target stop */
+	if (!armv8->is_armv7r) {
+		retval = aarch64_mmu(target, &mmu_enabled);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
+		if (mmu_enabled) {
+			virt = address;
+			retval = aarch64_virt2phys(target, virt, &phys);
+			if (retval != ERROR_OK)
+				return retval;
+
+			LOG_DEBUG("Reading at virtual address. Translating v:0x%" TARGET_PRIxADDR " to r:0x%" TARGET_PRIxADDR,
+				  virt, phys);
+			address = phys;
+		}
+		retval = aarch64_read_phys_memory(target, address, size, count,
+						  buffer);
+	} else {
+		if (mmu_enabled) {
+			retval = aarch64_check_address(target, address);
+			if (retval != ERROR_OK)
+				return retval;
+			/* enable MMU as we could have disabled it for phys
+			   access */
+			retval = aarch64_mmu_modify(target, 1);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+		retval = aarch64_read_apb_ab_memory(target, address, size,
+						    count, buffer);
+	}
+	return retval;
+}
+
+static int aarch64_write_phys_memory(struct target *target,
+	target_addr_t address, uint32_t size,
+	uint32_t count, const uint8_t *buffer)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+	uint8_t apsel = swjdp->apsel;
+
+	LOG_DEBUG("Writing memory to real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
+		size, count);
+
+	if (count && buffer) {
+
+		if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
+
+			/* write memory through AHB-AP */
+			retval = mem_ap_write_buf(armv8->memory_ap, buffer, size, count, address);
+		} else {
+
+			/* write memory through APB-AP */
+			if (!armv8->is_armv7r) {
+				retval = aarch64_mmu_modify(target, 0);
+				if (retval != ERROR_OK)
+					return retval;
+			}
+			return aarch64_write_apb_ab_memory(target, address, size, count, buffer);
+		}
+	}
+
+
+	/* REVISIT this op is generic ARMv7-A/R stuff */
+	if (retval == ERROR_OK && target->state == TARGET_HALTED) {
+		struct arm_dpm *dpm = armv8->arm.dpm;
+
+		retval = dpm->prepare(dpm);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* The Cache handling will NOT work with MMU active, the
+		 * wrong addresses will be invalidated!
+		 *
+		 * For both ICache and DCache, walk all cache lines in the
+		 * address range. Cortex-A8 has fixed 64 byte line length.
+		 *
+		 * REVISIT per ARMv7, these may trigger watchpoints ...
+		 */
+
+		/* invalidate I-Cache */
+		if (armv8->armv8_mmu.armv8_cache.i_cache_enabled) {
+			/* ICIMVAU - Invalidate Cache single entry
+			 * with MVA to PoU
+			 *      MCR p15, 0, r0, c7, c5, 1
+			 */
+			for (uint32_t cacheline = address;
+				cacheline < address + size * count;
+				cacheline += 64) {
+				retval = dpm->instr_write_data_r0(dpm,
+						ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
+						cacheline);
+				if (retval != ERROR_OK)
+					return retval;
+			}
+		}
+
+		/* invalidate D-Cache */
+		if (armv8->armv8_mmu.armv8_cache.d_u_cache_enabled) {
+			/* DCIMVAC - Invalidate data Cache line
+			 * with MVA to PoC
+			 *      MCR p15, 0, r0, c7, c6, 1
+			 */
+			for (uint32_t cacheline = address;
+				cacheline < address + size * count;
+				cacheline += 64) {
+				retval = dpm->instr_write_data_r0(dpm,
+						ARMV4_5_MCR(15, 0, 0, 7, 6, 1),
+						cacheline);
+				if (retval != ERROR_OK)
+					return retval;
+			}
+		}
+
+		/* (void) */ dpm->finish(dpm);
+	}
+
+	return retval;
+}
+
+static int aarch64_write_memory(struct target *target, target_addr_t address,
+	uint32_t size, uint32_t count, const uint8_t *buffer)
+{
+	int mmu_enabled = 0;
+	target_addr_t virt, phys;
+	int retval;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	uint8_t apsel = swjdp->apsel;
+
+	/* aarch64 handles unaligned memory access */
+	LOG_DEBUG("Writing memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32
+		  "; count %" PRId32, address, size, count);
+
+	/* determine if MMU was enabled on target stop */
+	if (!armv8->is_armv7r) {
+		retval = aarch64_mmu(target, &mmu_enabled);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
+		LOG_DEBUG("Writing memory to address 0x%" TARGET_PRIxADDR "; size %"
+			  PRId32 "; count %" PRId32, address, size, count);
+		if (mmu_enabled) {
+			virt = address;
+			retval = aarch64_virt2phys(target, virt, &phys);
+			if (retval != ERROR_OK)
+				return retval;
+
+			LOG_DEBUG("Writing to virtual address. Translating v:0x%"
+				  TARGET_PRIxADDR " to r:0x%" TARGET_PRIxADDR, virt, phys);
+			address = phys;
+		}
+		retval = aarch64_write_phys_memory(target, address, size,
+				count, buffer);
+	} else {
+		if (mmu_enabled) {
+			retval = aarch64_check_address(target, address);
+			if (retval != ERROR_OK)
+				return retval;
+			/* enable MMU as we could have disabled it for phys access */
+			retval = aarch64_mmu_modify(target, 1);
+			if (retval != ERROR_OK)
+				return retval;
+		}
+		retval = aarch64_write_apb_ab_memory(target, address, size, count, buffer);
+	}
+	return retval;
+}
+
+static int aarch64_handle_target_request(void *priv)
+{
+	struct target *target = priv;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+
+	if (!target_was_examined(target))
+		return ERROR_OK;
+	if (!target->dbg_msg_enabled)
+		return ERROR_OK;
+
+	if (target->state == TARGET_RUNNING) {
+		uint32_t request;
+		uint32_t dscr;
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+
+		/* check if we have data */
+		while ((dscr & DSCR_DTR_TX_FULL) && (retval == ERROR_OK)) {
+			retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUDBG_DTRTX, &request);
+			if (retval == ERROR_OK) {
+				target_request(target, request);
+				retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+						armv8->debug_base + CPUDBG_DSCR, &dscr);
+			}
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_examine_first(struct target *target)
+{
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	int retval = ERROR_OK;
+	uint32_t pfr, debug, ctypr, ttypr, cpuid;
+	int i;
+
+	/* We do one extra read to ensure DAP is configured,
+	 * we call ahbap_debugport_init(swjdp) instead
+	 */
+	retval = dap_dp_init(swjdp);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Search for the APB-AB - it is needed for access to debug registers */
+	retval = dap_find_ap(swjdp, AP_TYPE_APB_AP, &armv8->debug_ap);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not find APB-AP for debug access");
+		return retval;
+	}
+
+	retval = mem_ap_init(armv8->debug_ap);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Could not initialize the APB-AP");
+		return retval;
+	}
+
+	armv8->debug_ap->memaccess_tck = 80;
+
+	/* Search for the AHB-AB */
+	armv8->memory_ap_available = false;
+	retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv8->memory_ap);
+	if (retval == ERROR_OK) {
+		retval = mem_ap_init(armv8->memory_ap);
+		if (retval == ERROR_OK)
+			armv8->memory_ap_available = true;
+	}
+	if (retval != ERROR_OK) {
+		/* AHB-AP not found or unavailable - use the CPU */
+		LOG_DEBUG("No AHB-AP available for memory access");
+	}
+
+
+	if (!target->dbgbase_set) {
+		uint32_t dbgbase;
+		/* Get ROM Table base */
+		uint32_t apid;
+		int32_t coreidx = target->coreid;
+		retval = dap_get_debugbase(armv8->debug_ap, &dbgbase, &apid);
+		if (retval != ERROR_OK)
+			return retval;
+		/* Lookup 0x15 -- Processor DAP */
+		retval = dap_lookup_cs_component(armv8->debug_ap, dbgbase, 0x15,
+				&armv8->debug_base, &coreidx);
+		if (retval != ERROR_OK)
+			return retval;
+		LOG_DEBUG("Detected core %" PRId32 " dbgbase: %08" PRIx32,
+			  coreidx, armv8->debug_base);
+	} else
+		armv8->debug_base = target->dbgbase;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x300, 0);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "oslock");
+		return retval;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x88, &cpuid);
+	LOG_DEBUG("0x88 = %x", cpuid);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x314, &cpuid);
+	LOG_DEBUG("0x314 = %x", cpuid);
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + 0x310, &cpuid);
+	LOG_DEBUG("0x310 = %x", cpuid);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_CPUID, &cpuid);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "CPUID");
+		return retval;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_CTYPR, &ctypr);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "CTYPR");
+		return retval;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_TTYPR, &ttypr);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "TTYPR");
+		return retval;
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + ID_AA64PFR0_EL1, &pfr);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
+		return retval;
+	}
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + ID_AA64DFR0_EL1, &debug);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
+		return retval;
+	}
+
+	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
+	LOG_DEBUG("ctypr = 0x%08" PRIx32, ctypr);
+	LOG_DEBUG("ttypr = 0x%08" PRIx32, ttypr);
+	LOG_DEBUG("ID_AA64PFR0_EL1 = 0x%08" PRIx32, pfr);
+	LOG_DEBUG("ID_AA64DFR0_EL1 = 0x%08" PRIx32, debug);
+
+	armv8->arm.core_type = ARM_MODE_MON;
+	armv8->arm.core_state = ARM_STATE_AARCH64;
+	retval = aarch64_dpm_setup(aarch64, debug);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Setup Breakpoint Register Pairs */
+	aarch64->brp_num = ((debug >> 12) & 0x0F) + 1;
+	aarch64->brp_num_context = ((debug >> 28) & 0x0F) + 1;
+
+	/* hack - no context bpt support yet */
+	aarch64->brp_num_context = 0;
+
+	aarch64->brp_num_available = aarch64->brp_num;
+	aarch64->brp_list = calloc(aarch64->brp_num, sizeof(struct aarch64_brp));
+	for (i = 0; i < aarch64->brp_num; i++) {
+		aarch64->brp_list[i].used = 0;
+		if (i < (aarch64->brp_num-aarch64->brp_num_context))
+			aarch64->brp_list[i].type = BRP_NORMAL;
+		else
+			aarch64->brp_list[i].type = BRP_CONTEXT;
+		aarch64->brp_list[i].value = 0;
+		aarch64->brp_list[i].control = 0;
+		aarch64->brp_list[i].BRPn = i;
+	}
+
+	LOG_DEBUG("Configured %i hw breakpoints", aarch64->brp_num);
+
+	target_set_examined(target);
+	return ERROR_OK;
+}
+
+static int aarch64_examine(struct target *target)
+{
+	int retval = ERROR_OK;
+
+	/* don't re-probe hardware after each reset */
+	if (!target_was_examined(target))
+		retval = aarch64_examine_first(target);
+
+	/* Configure core debug access */
+	if (retval == ERROR_OK)
+		retval = aarch64_init_debug_access(target);
+
+	return retval;
+}
+
+/*
+ *	Cortex-A8 target creation and initialization
+ */
+
+static int aarch64_init_target(struct command_context *cmd_ctx,
+	struct target *target)
+{
+	/* examine_first() does a bunch of this */
+	return ERROR_OK;
+}
+
+static int aarch64_init_arch_info(struct target *target,
+	struct aarch64_common *aarch64, struct jtag_tap *tap)
+{
+	struct armv8_common *armv8 = &aarch64->armv8_common;
+	struct adiv5_dap *dap = armv8->arm.dap;
+
+	armv8->arm.dap = dap;
+
+	/* Setup struct aarch64_common */
+	aarch64->common_magic = AARCH64_COMMON_MAGIC;
+	/*  tap has no dap initialized */
+	if (!tap->dap) {
+		tap->dap = dap_init();
+
+		/* Leave (only) generic DAP stuff for debugport_init() */
+		tap->dap->tap = tap;
+	}
+
+	armv8->arm.dap = tap->dap;
+
+	aarch64->fast_reg_read = 0;
+
+	/* register arch-specific functions */
+	armv8->examine_debug_reason = NULL;
+
+	armv8->post_debug_entry = aarch64_post_debug_entry;
+
+	armv8->pre_restore_context = NULL;
+
+	armv8->armv8_mmu.read_physical_memory = aarch64_read_phys_memory;
+
+	/* REVISIT v7a setup should be in a v7a-specific routine */
+	armv8_init_arch_info(target, armv8);
+	target_register_timer_callback(aarch64_handle_target_request, 1, 1, target);
+
+	return ERROR_OK;
+}
+
+static int aarch64_target_create(struct target *target, Jim_Interp *interp)
+{
+	struct aarch64_common *aarch64 = calloc(1, sizeof(struct aarch64_common));
+
+	aarch64->armv8_common.is_armv7r = false;
+
+	return aarch64_init_arch_info(target, aarch64, target->tap);
+}
+
+static int aarch64_mmu(struct target *target, int *enabled)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("%s: target not halted", __func__);
+		return ERROR_TARGET_INVALID;
+	}
+
+	*enabled = target_to_aarch64(target)->armv8_common.armv8_mmu.mmu_enabled;
+	return ERROR_OK;
+}
+
+static int aarch64_virt2phys(struct target *target, target_addr_t virt,
+			     target_addr_t *phys)
+{
+	int retval = ERROR_FAIL;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct adiv5_dap *swjdp = armv8->arm.dap;
+	uint8_t apsel = swjdp->apsel;
+	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
+		uint32_t ret;
+		retval = armv8_mmu_translate_va(target,
+				virt, &ret);
+		if (retval != ERROR_OK)
+			goto done;
+		*phys = ret;
+	} else {/*  use this method if armv8->memory_ap not selected
+		 *  mmu must be enable in order to get a correct translation */
+		retval = aarch64_mmu_modify(target, 1);
+		if (retval != ERROR_OK)
+			goto done;
+		retval = armv8_mmu_translate_va_pa(target, virt,  phys, 1);
+	}
+done:
+	return retval;
+}
+
+COMMAND_HANDLER(aarch64_handle_cache_info_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	return armv8_handle_cache_info_command(CMD_CTX,
+			&armv8->armv8_mmu.armv8_cache);
+}
+
+
+COMMAND_HANDLER(aarch64_handle_dbginit_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	if (!target_was_examined(target)) {
+		LOG_ERROR("target not examined yet");
+		return ERROR_FAIL;
+	}
+
+	return aarch64_init_debug_access(target);
+}
+COMMAND_HANDLER(aarch64_handle_smp_off_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	/* check target is an smp target */
+	struct target_list *head;
+	struct target *curr;
+	head = target->head;
+	target->smp = 0;
+	if (head != (struct target_list *)NULL) {
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			curr->smp = 0;
+			head = head->next;
+		}
+		/*  fixes the target display to the debugger */
+		target->gdb_service->target = target;
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(aarch64_handle_smp_on_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct target_list *head;
+	struct target *curr;
+	head = target->head;
+	if (head != (struct target_list *)NULL) {
+		target->smp = 1;
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			curr->smp = 1;
+			head = head->next;
+		}
+	}
+	return ERROR_OK;
+}
+
+COMMAND_HANDLER(aarch64_handle_smp_gdb_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	int retval = ERROR_OK;
+	struct target_list *head;
+	head = target->head;
+	if (head != (struct target_list *)NULL) {
+		if (CMD_ARGC == 1) {
+			int coreid = 0;
+			COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], coreid);
+			if (ERROR_OK != retval)
+				return retval;
+			target->gdb_service->core[1] = coreid;
+
+		}
+		command_print(CMD_CTX, "gdb coreid  %" PRId32 " -> %" PRId32, target->gdb_service->core[0]
+			, target->gdb_service->core[1]);
+	}
+	return ERROR_OK;
+}
+
+static const struct command_registration aarch64_exec_command_handlers[] = {
+	{
+		.name = "cache_info",
+		.handler = aarch64_handle_cache_info_command,
+		.mode = COMMAND_EXEC,
+		.help = "display information about target caches",
+		.usage = "",
+	},
+	{
+		.name = "dbginit",
+		.handler = aarch64_handle_dbginit_command,
+		.mode = COMMAND_EXEC,
+		.help = "Initialize core debug",
+		.usage = "",
+	},
+	{	.name = "smp_off",
+		.handler = aarch64_handle_smp_off_command,
+		.mode = COMMAND_EXEC,
+		.help = "Stop smp handling",
+		.usage = "",
+	},
+	{
+		.name = "smp_on",
+		.handler = aarch64_handle_smp_on_command,
+		.mode = COMMAND_EXEC,
+		.help = "Restart smp handling",
+		.usage = "",
+	},
+	{
+		.name = "smp_gdb",
+		.handler = aarch64_handle_smp_gdb_command,
+		.mode = COMMAND_EXEC,
+		.help = "display/fix current core played to gdb",
+		.usage = "",
+	},
+
+
+	COMMAND_REGISTRATION_DONE
+};
+static const struct command_registration aarch64_command_handlers[] = {
+	{
+		.chain = arm_command_handlers,
+	},
+	{
+		.chain = armv8_command_handlers,
+	},
+	{
+		.name = "cortex_a",
+		.mode = COMMAND_ANY,
+		.help = "Cortex-A command group",
+		.usage = "",
+		.chain = aarch64_exec_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct target_type aarch64_target = {
+	.name = "aarch64",
+
+	.poll = aarch64_poll,
+	.arch_state = armv8_arch_state,
+
+	.halt = aarch64_halt,
+	.resume = aarch64_resume,
+	.step = aarch64_step,
+
+	.assert_reset = aarch64_assert_reset,
+	.deassert_reset = aarch64_deassert_reset,
+
+	/* REVISIT allow exporting VFP3 registers ... */
+	.get_gdb_reg_list = armv8_get_gdb_reg_list,
+
+	.read_memory = aarch64_read_memory,
+	.write_memory = aarch64_write_memory,
+
+	.checksum_memory = arm_checksum_memory,
+	.blank_check_memory = arm_blank_check_memory,
+
+	.run_algorithm = armv4_5_run_algorithm,
+
+	.add_breakpoint = aarch64_add_breakpoint,
+	.add_context_breakpoint = aarch64_add_context_breakpoint,
+	.add_hybrid_breakpoint = aarch64_add_hybrid_breakpoint,
+	.remove_breakpoint = aarch64_remove_breakpoint,
+	.add_watchpoint = NULL,
+	.remove_watchpoint = NULL,
+
+	.commands = aarch64_command_handlers,
+	.target_create = aarch64_target_create,
+	.init_target = aarch64_init_target,
+	.examine = aarch64_examine,
+
+	.read_phys_memory = aarch64_read_phys_memory,
+	.write_phys_memory = aarch64_write_phys_memory,
+	.mmu = aarch64_mmu,
+	.virt2phys = aarch64_virt2phys,
+};
diff --git a/src/target/aarch64.h b/src/target/aarch64.h
new file mode 100644
index 0000000..513f06d
--- /dev/null
+++ b/src/target/aarch64.h
@@ -0,0 +1,78 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ ***************************************************************************/
+
+#ifndef AARCH64_H
+#define AARCH64_H
+
+#include "armv8.h"
+
+#define AARCH64_COMMON_MAGIC 0x411fc082
+
+#define CPUDBG_CPUID	0xD00
+#define CPUDBG_CTYPR	0xD04
+#define CPUDBG_TTYPR	0xD0C
+#define ID_AA64PFR0_EL1	0xD20
+#define ID_AA64DFR0_EL1	0xD28
+#define CPUDBG_LOCKACCESS 0xFB0
+#define CPUDBG_LOCKSTATUS 0xFB4
+
+#define BRP_NORMAL 0
+#define BRP_CONTEXT 1
+
+#define AARCH64_PADDRDBG_CPU_SHIFT 13
+
+struct aarch64_brp {
+	int used;
+	int type;
+	target_addr_t value;
+	uint32_t control;
+	uint8_t BRPn;
+};
+
+struct aarch64_common {
+	int common_magic;
+
+	/* Context information */
+	uint32_t cpudbg_dscr;
+
+	uint32_t system_control_reg;
+	uint32_t system_control_reg_curr;
+
+	enum arm_mode curr_mode;
+
+
+	/* Breakpoint register pairs */
+	int brp_num_context;
+	int brp_num;
+	int brp_num_available;
+	struct aarch64_brp *brp_list;
+
+	/* Use aarch64_read_regs_through_mem for fast register reads */
+	int fast_reg_read;
+
+	struct armv8_common armv8_common;
+
+};
+
+static inline struct aarch64_common *
+target_to_aarch64(struct target *target)
+{
+	return container_of(target->arch_info, struct aarch64_common, armv8_common.arm);
+}
+
+#endif /* AARCH64_H */
diff --git a/src/target/arm.h b/src/target/arm.h
index 5713fc0..e4f2038 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -218,6 +218,7 @@ struct arm_reg {
 };
 
 struct reg_cache *arm_build_reg_cache(struct target *target, struct arm *arm);
+struct reg_cache *armv8_build_reg_cache(struct target *target);
 
 extern const struct command_registration arm_command_handlers[];
 
@@ -225,6 +226,9 @@ int arm_arch_state(struct target *target);
 int arm_get_gdb_reg_list(struct target *target,
 		struct reg **reg_list[], int *reg_list_size,
 		enum target_register_class reg_class);
+int armv8_get_gdb_reg_list(struct target *target,
+		struct reg **reg_list[], int *reg_list_size,
+		enum target_register_class reg_class);
 
 int arm_init_arch_info(struct target *target, struct arm *arm);
 
@@ -249,6 +253,7 @@ int arm_blank_check_memory(struct target *target,
 
 void arm_set_cpsr(struct arm *arm, uint32_t cpsr);
 struct reg *arm_reg_current(struct arm *arm, unsigned regnum);
+struct reg *armv8_reg_current(struct arm *arm, unsigned regnum);
 
 extern struct reg arm_gdb_dummy_fp_reg;
 extern struct reg arm_gdb_dummy_fps_reg;
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 0c84be5..55f253e 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -299,10 +299,15 @@ static int dpm_write_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	switch (regnum) {
 		case 0 ... 30:
 			i = 0xd5330400 + regnum;
-			retval = dpm->instr_write_data_dcc(dpm, i, value);
+			retval = dpm->instr_write_data_dcc_64(dpm, i, value);
+			break;
+		case 32: /* PC */
+			i = 0xd51b4520;
+			retval = dpm->instr_write_data_r0_64(dpm, i, value);
 			break;
-
 		default:
+			LOG_DEBUG("register %s (%16.16llx) not defined", r->name,
+				  (unsigned long long)value);
 			break;
 	}
 
@@ -354,6 +359,9 @@ static int arm_dpm_read_current_registers_i(struct arm_dpm *dpm)
 
 	/* update core mode and state, plus shadow mapping for R8..R14 */
 	arm_set_cpsr(arm, cpsr);
+	if (core_state == ARM_STATE_AARCH64)
+		/* arm_set_cpsr changes core_state, restore it for now */
+		arm->core_state = ARM_STATE_AARCH64;
 
 	core_regs = arm->core_cache->num_regs;
 
@@ -573,7 +581,7 @@ static int arm_dpm_write_dirty_registers_64(struct arm_dpm *dpm)
 	 */
 
 	/* check everything except our scratch register R0 */
-	for (unsigned i = 1; i < 32; i++) {
+	for (unsigned i = 1; i <= 32; i++) {
 		struct arm_reg *r;
 		unsigned regnum;
 
@@ -1119,7 +1127,7 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
 	struct target *target = arm->target;
-	struct reg_cache *cache;
+	struct reg_cache *cache = 0;
 
 	arm->dpm = dpm;
 
@@ -1128,13 +1136,17 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	arm->read_core_reg = arm->read_core_reg ? : arm_dpm_read_core_reg;
 	arm->write_core_reg = arm->write_core_reg ? : arm_dpm_write_core_reg;
 
-	/* avoid duplicating the register cache */
-	if (arm->core_cache == NULL) {
-		cache = arm_build_reg_cache(target, arm);
+	if (arm->core_cache != NULL) {
+		if (arm->core_state == ARM_STATE_AARCH64) {
+			cache = armv8_build_reg_cache(target);
+			target->reg_cache = cache;
+		} else {
+			cache = arm_build_reg_cache(target, arm);
+			*register_get_last_cache_p(&target->reg_cache) = cache;
+		}
+
 		if (!cache)
 			return ERROR_FAIL;
-
-		*register_get_last_cache_p(&target->reg_cache) = cache;
 	}
 
 	/* coprocessor access setup */
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index 9cc0304..5b16e64 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -73,10 +73,17 @@ struct arm_dpm {
 	int (*instr_write_data_dcc)(struct arm_dpm *,
 			uint32_t opcode, uint32_t data);
 
+	int (*instr_write_data_dcc_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t data);
+
 	/** Runs one instruction, writing data to R0 before execution. */
 	int (*instr_write_data_r0)(struct arm_dpm *,
 			uint32_t opcode, uint32_t data);
 
+	/** Runs one instruction, writing data to R0 before execution. */
+	int (*instr_write_data_r0_64)(struct arm_dpm *,
+			uint32_t opcode, uint64_t data);
+
 	/** Optional core-specific operation invoked after CPSR writes. */
 	int (*instr_cpsr_sync)(struct arm_dpm *dpm);
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
new file mode 100644
index 0000000..902d290
--- /dev/null
+++ b/src/target/armv8.c
@@ -0,0 +1,774 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <helper/replacements.h>
+
+#include "armv8.h"
+#include "arm_disassembler.h"
+
+#include "register.h"
+#include <helper/binarybuffer.h>
+#include <helper/command.h>
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "arm_opcodes.h"
+#include "target.h"
+#include "target_type.h"
+
+static int armv8_read_core_reg(struct target *target, struct reg *r,
+	int num, enum arm_mode mode)
+{
+	uint64_t reg_value;
+	int retval;
+	struct arm_reg *armv8_core_reg;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	assert(num < (int)armv8->arm.core_cache->num_regs);
+
+	armv8_core_reg = armv8->arm.core_cache->reg_list[num].arch_info;
+	retval = armv8->load_core_reg_u64(target,
+			armv8_core_reg->num, &reg_value);
+
+	buf_set_u64(armv8->arm.core_cache->reg_list[num].value, 0, 64, reg_value);
+	armv8->arm.core_cache->reg_list[num].valid = 1;
+	armv8->arm.core_cache->reg_list[num].dirty = 0;
+
+	return retval;
+}
+
+#if 0
+static int armv8_write_core_reg(struct target *target, struct reg *r,
+	int num, enum arm_mode mode, target_addr_t value)
+{
+	int retval;
+	struct arm_reg *armv8_core_reg;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	assert(num < (int)armv8->arm.core_cache->num_regs);
+
+	armv8_core_reg = armv8->arm.core_cache->reg_list[num].arch_info;
+	retval = armv8->store_core_reg_u64(target,
+					    armv8_core_reg->num,
+					    value);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("JTAG failure");
+		armv8->arm.core_cache->reg_list[num].dirty = armv8->arm.core_cache->reg_list[num].valid;
+		return ERROR_JTAG_DEVICE_ERROR;
+	}
+
+	LOG_DEBUG("write core reg %i value 0x%" PRIx64 "", num, value);
+	armv8->arm.core_cache->reg_list[num].valid = 1;
+	armv8->arm.core_cache->reg_list[num].dirty = 0;
+
+	return ERROR_OK;
+}
+#endif
+
+static void armv8_show_fault_registers(struct target *target)
+{
+	/* TODO */
+}
+
+static int armv8_read_ttbcr(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t ttbcr;
+	int retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+	/*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
+			&ttbcr);
+	if (retval != ERROR_OK)
+		goto done;
+	armv8->armv8_mmu.ttbr1_used = ((ttbcr & 0x7) != 0) ? 1 : 0;
+	armv8->armv8_mmu.ttbr0_mask  = 7 << (32 - ((ttbcr & 0x7)));
+#if 0
+	LOG_INFO("ttb1 %s ,ttb0_mask %x",
+		armv8->armv8_mmu.ttbr1_used ? "used" : "not used",
+		armv8->armv8_mmu.ttbr0_mask);
+#endif
+	if (armv8->armv8_mmu.ttbr1_used == 1) {
+		LOG_INFO("SVC access above %" PRIx32,
+			 (uint32_t)(0xffffffff & armv8->armv8_mmu.ttbr0_mask));
+		armv8->armv8_mmu.os_border = 0xffffffff & armv8->armv8_mmu.ttbr0_mask;
+	} else {
+		/*  fix me , default is hard coded LINUX border  */
+		armv8->armv8_mmu.os_border = 0xc0000000;
+	}
+done:
+	dpm->finish(dpm);
+	return retval;
+}
+
+
+/*  method adapted to cortex A : reused arm v4 v5 method*/
+int armv8_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val)
+{
+	uint32_t first_lvl_descriptor = 0x0;
+	uint32_t second_lvl_descriptor = 0x0;
+	int retval;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t ttb = 0;	/*  default ttb0 */
+	if (armv8->armv8_mmu.ttbr1_used == -1)
+		armv8_read_ttbcr(target);
+	if ((armv8->armv8_mmu.ttbr1_used) &&
+		(va > (0xffffffff & armv8->armv8_mmu.ttbr0_mask))) {
+		/*  select ttb 1 */
+		ttb = 1;
+	}
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/*  MRC p15,0,<Rt>,c2,c0,ttb */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 2, 0, ttb),
+			&ttb);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = armv8->armv8_mmu.read_physical_memory(target,
+			(ttb & 0xffffc000) | ((va & 0xfff00000) >> 18),
+			4, 1, (uint8_t *)&first_lvl_descriptor);
+	if (retval != ERROR_OK)
+		return retval;
+	first_lvl_descriptor = target_buffer_get_u32(target, (uint8_t *)
+			&first_lvl_descriptor);
+	/*  reuse armv4_5 piece of code, specific armv8 changes may come later */
+	LOG_DEBUG("1st lvl desc: %8.8" PRIx32 "", first_lvl_descriptor);
+
+	if ((first_lvl_descriptor & 0x3) == 0) {
+		LOG_ERROR("Address translation failure");
+		return ERROR_TARGET_TRANSLATION_FAULT;
+	}
+
+
+	if ((first_lvl_descriptor & 0x3) == 2) {
+		/* section descriptor */
+		*val = (first_lvl_descriptor & 0xfff00000) | (va & 0x000fffff);
+		return ERROR_OK;
+	}
+
+	if ((first_lvl_descriptor & 0x3) == 1) {
+		/* coarse page table */
+		retval = armv8->armv8_mmu.read_physical_memory(target,
+				(first_lvl_descriptor & 0xfffffc00) | ((va & 0x000ff000) >> 10),
+				4, 1, (uint8_t *)&second_lvl_descriptor);
+		if (retval != ERROR_OK)
+			return retval;
+	} else if ((first_lvl_descriptor & 0x3) == 3)   {
+		/* fine page table */
+		retval = armv8->armv8_mmu.read_physical_memory(target,
+				(first_lvl_descriptor & 0xfffff000) | ((va & 0x000ffc00) >> 8),
+				4, 1, (uint8_t *)&second_lvl_descriptor);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	second_lvl_descriptor = target_buffer_get_u32(target, (uint8_t *)
+			&second_lvl_descriptor);
+
+	LOG_DEBUG("2nd lvl desc: %8.8" PRIx32 "", second_lvl_descriptor);
+
+	if ((second_lvl_descriptor & 0x3) == 0) {
+		LOG_ERROR("Address translation failure");
+		return ERROR_TARGET_TRANSLATION_FAULT;
+	}
+
+	if ((second_lvl_descriptor & 0x3) == 1) {
+		/* large page descriptor */
+		*val = (second_lvl_descriptor & 0xffff0000) | (va & 0x0000ffff);
+		return ERROR_OK;
+	}
+
+	if ((second_lvl_descriptor & 0x3) == 2) {
+		/* small page descriptor */
+		*val = (second_lvl_descriptor & 0xfffff000) | (va & 0x00000fff);
+		return ERROR_OK;
+	}
+
+	if ((second_lvl_descriptor & 0x3) == 3) {
+		*val = (second_lvl_descriptor & 0xfffffc00) | (va & 0x000003ff);
+		return ERROR_OK;
+	}
+
+	/* should not happen */
+	LOG_ERROR("Address translation failure");
+	return ERROR_TARGET_TRANSLATION_FAULT;
+
+done:
+	return retval;
+}
+
+/*  V8 method VA TO PA  */
+int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
+	target_addr_t *val, int meminfo)
+{
+	return ERROR_OK;
+}
+
+static int armv8_handle_inner_cache_info_command(struct command_context *cmd_ctx,
+	struct armv8_cache_common *armv8_cache)
+{
+	if (armv8_cache->ctype == -1) {
+		command_print(cmd_ctx, "cache not yet identified");
+		return ERROR_OK;
+	}
+
+	command_print(cmd_ctx,
+		"D-Cache: linelen %" PRIi32 ", associativity %" PRIi32 ", nsets %" PRIi32 ", cachesize %" PRId32 " KBytes",
+		armv8_cache->d_u_size.linelen,
+		armv8_cache->d_u_size.associativity,
+		armv8_cache->d_u_size.nsets,
+		armv8_cache->d_u_size.cachesize);
+
+	command_print(cmd_ctx,
+		"I-Cache: linelen %" PRIi32 ", associativity %" PRIi32 ", nsets %" PRIi32 ", cachesize %" PRId32 " KBytes",
+		armv8_cache->i_size.linelen,
+		armv8_cache->i_size.associativity,
+		armv8_cache->i_size.nsets,
+		armv8_cache->i_size.cachesize);
+
+	return ERROR_OK;
+}
+
+static int _armv8_flush_all_data(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct armv8_cachesize *d_u_size =
+		&(armv8->armv8_mmu.armv8_cache.d_u_size);
+	int32_t c_way, c_index = d_u_size->index;
+	int retval;
+	/*  check that cache data is on at target halt */
+	if (!armv8->armv8_mmu.armv8_cache.d_u_cache_enabled) {
+		LOG_INFO("flushed not performed :cache not on at target halt");
+		return ERROR_OK;
+	}
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+	do {
+		c_way = d_u_size->way;
+		do {
+			uint32_t value = (c_index << d_u_size->index_shift)
+				| (c_way << d_u_size->way_shift);
+			/*  DCCISW */
+			/* LOG_INFO ("%d %d %x",c_way,c_index,value); */
+			retval = dpm->instr_write_data_r0(dpm,
+					ARMV4_5_MCR(15, 0, 0, 7, 14, 2),
+					value);
+			if (retval != ERROR_OK)
+				goto done;
+			c_way -= 1;
+		} while (c_way >= 0);
+		c_index -= 1;
+	} while (c_index >= 0);
+	return retval;
+done:
+	LOG_ERROR("flushed failed");
+	dpm->finish(dpm);
+	return retval;
+}
+
+static int  armv8_flush_all_data(struct target *target)
+{
+	int retval = ERROR_FAIL;
+	/*  check that armv8_cache is correctly identify */
+	struct armv8_common *armv8 = target_to_armv8(target);
+	if (armv8->armv8_mmu.armv8_cache.ctype == -1) {
+		LOG_ERROR("trying to flush un-identified cache");
+		return retval;
+	}
+
+	if (target->smp) {
+		/*  look if all the other target have been flushed in order to flush level
+		 *  2 */
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			if (curr->state == TARGET_HALTED) {
+				LOG_INFO("Wait flushing data l1 on core %" PRId32, curr->coreid);
+				retval = _armv8_flush_all_data(curr);
+			}
+			head = head->next;
+		}
+	} else
+		retval = _armv8_flush_all_data(target);
+	return retval;
+}
+
+int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
+	struct armv8_cache_common *armv8_cache)
+{
+	if (armv8_cache->ctype == -1) {
+		command_print(cmd_ctx, "cache not yet identified");
+		return ERROR_OK;
+	}
+
+	if (armv8_cache->display_cache_info)
+		armv8_cache->display_cache_info(cmd_ctx, armv8_cache);
+	return ERROR_OK;
+}
+
+/*  retrieve core id cluster id  */
+static int armv8_read_mpidr(struct target *target)
+{
+	int retval = ERROR_FAIL;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t mpidr;
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+	/* MRC p15,0,<Rd>,c0,c0,5; read Multiprocessor ID register*/
+
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 0, 0, 0, 0, 5),
+			&mpidr);
+	if (retval != ERROR_OK)
+		goto done;
+	if (mpidr & 1<<31) {
+		armv8->multi_processor_system = (mpidr >> 30) & 1;
+		armv8->cluster_id = (mpidr >> 8) & 0xf;
+		armv8->cpu_id = mpidr & 0x3;
+		LOG_INFO("%s cluster %x core %x %s", target_name(target),
+			armv8->cluster_id,
+			armv8->cpu_id,
+			armv8->multi_processor_system == 0 ? "multi core" : "mono core");
+
+	} else
+		LOG_ERROR("mpdir not in multiprocessor format");
+
+done:
+	dpm->finish(dpm);
+	return retval;
+
+
+}
+
+int armv8_identify_cache(struct target *target)
+{
+	/*  read cache descriptor */
+	int retval = ERROR_FAIL;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t cache_selected, clidr;
+	uint32_t cache_i_reg, cache_d_reg;
+	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
+	if (!armv8->is_armv7r)
+		armv8_read_ttbcr(target);
+	retval = dpm->prepare(dpm);
+
+	if (retval != ERROR_OK)
+		goto done;
+	/*  retrieve CLIDR
+	 *  mrc	p15, 1, r0, c0, c0, 1		@ read clidr */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 1, 0, 0, 0, 1),
+			&clidr);
+	if (retval != ERROR_OK)
+		goto done;
+	clidr = (clidr & 0x7000000) >> 23;
+	LOG_INFO("number of cache level %" PRIx32, (uint32_t)(clidr / 2));
+	if ((clidr / 2) > 1) {
+		/* FIXME not supported present in cortex A8 and later */
+		/*  in cortex A7, A15 */
+		LOG_ERROR("cache l2 present :not supported");
+	}
+	/*  retrieve selected cache
+	 *  MRC p15, 2,<Rd>, c0, c0, 0; Read CSSELR */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+			&cache_selected);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = armv8->arm.mrc(target, 15,
+			2, 0,	/* op1, op2 */
+			0, 0,	/* CRn, CRm */
+			&cache_selected);
+	if (retval != ERROR_OK)
+		goto done;
+	/* select instruction cache
+	 *  MCR p15, 2,<Rd>, c0, c0, 0; Write CSSELR
+	 *  [0]  : 1 instruction cache selection , 0 data cache selection */
+	retval = dpm->instr_write_data_r0(dpm,
+			ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+			1);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* read CCSIDR
+	 * MRC P15,1,<RT>,C0, C0,0 ;on cortex A9 read CCSIDR
+	 * [2:0] line size  001 eight word per line
+	 * [27:13] NumSet 0x7f 16KB, 0xff 32Kbytes, 0x1ff 64Kbytes */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
+			&cache_i_reg);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/*  select data cache*/
+	retval = dpm->instr_write_data_r0(dpm,
+			ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+			0);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
+			&cache_d_reg);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/*  restore selected cache  */
+	dpm->instr_write_data_r0(dpm,
+		ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+		cache_selected);
+
+	if (retval != ERROR_OK)
+		goto done;
+	dpm->finish(dpm);
+
+	/* put fake type */
+	cache->d_u_size.linelen = 16 << (cache_d_reg & 0x7);
+	cache->d_u_size.cachesize = (((cache_d_reg >> 13) & 0x7fff)+1)/8;
+	cache->d_u_size.nsets = (cache_d_reg >> 13) & 0x7fff;
+	cache->d_u_size.associativity = ((cache_d_reg >> 3) & 0x3ff) + 1;
+	/*  compute info for set way operation on cache */
+	cache->d_u_size.index_shift = (cache_d_reg & 0x7) + 4;
+	cache->d_u_size.index = (cache_d_reg >> 13) & 0x7fff;
+	cache->d_u_size.way = ((cache_d_reg >> 3) & 0x3ff);
+	cache->d_u_size.way_shift = cache->d_u_size.way + 1;
+	{
+		int i = 0;
+		while (((cache->d_u_size.way_shift >> i) & 1) != 1)
+			i++;
+		cache->d_u_size.way_shift = 32-i;
+	}
+#if 0
+	LOG_INFO("data cache index %d << %d, way %d << %d",
+			cache->d_u_size.index, cache->d_u_size.index_shift,
+			cache->d_u_size.way,
+			cache->d_u_size.way_shift);
+
+	LOG_INFO("data cache %d bytes %d KBytes asso %d ways",
+			cache->d_u_size.linelen,
+			cache->d_u_size.cachesize,
+			cache->d_u_size.associativity);
+#endif
+	cache->i_size.linelen = 16 << (cache_i_reg & 0x7);
+	cache->i_size.associativity = ((cache_i_reg >> 3) & 0x3ff) + 1;
+	cache->i_size.nsets = (cache_i_reg >> 13) & 0x7fff;
+	cache->i_size.cachesize = (((cache_i_reg >> 13) & 0x7fff)+1)/8;
+	/*  compute info for set way operation on cache */
+	cache->i_size.index_shift = (cache_i_reg & 0x7) + 4;
+	cache->i_size.index = (cache_i_reg >> 13) & 0x7fff;
+	cache->i_size.way = ((cache_i_reg >> 3) & 0x3ff);
+	cache->i_size.way_shift = cache->i_size.way + 1;
+	{
+		int i = 0;
+		while (((cache->i_size.way_shift >> i) & 1) != 1)
+			i++;
+		cache->i_size.way_shift = 32-i;
+	}
+#if 0
+	LOG_INFO("instruction cache index %d << %d, way %d << %d",
+			cache->i_size.index, cache->i_size.index_shift,
+			cache->i_size.way, cache->i_size.way_shift);
+
+	LOG_INFO("instruction cache %d bytes %d KBytes asso %d ways",
+			cache->i_size.linelen,
+			cache->i_size.cachesize,
+			cache->i_size.associativity);
+#endif
+	/*  if no l2 cache initialize l1 data cache flush function function */
+	if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache == NULL) {
+		armv8->armv8_mmu.armv8_cache.display_cache_info =
+			armv8_handle_inner_cache_info_command;
+		armv8->armv8_mmu.armv8_cache.flush_all_data_cache =
+			armv8_flush_all_data;
+	}
+	armv8->armv8_mmu.armv8_cache.ctype = 0;
+
+done:
+	dpm->finish(dpm);
+	armv8_read_mpidr(target);
+	return retval;
+
+}
+
+int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
+{
+	struct arm *arm = &armv8->arm;
+	arm->arch_info = armv8;
+	target->arch_info = &armv8->arm;
+	/*  target is useful in all function arm v4 5 compatible */
+	armv8->arm.target = target;
+	armv8->arm.common_magic = ARM_COMMON_MAGIC;
+	armv8->common_magic = ARMV8_COMMON_MAGIC;
+
+	arm->read_core_reg = armv8_read_core_reg;
+#if 0
+	arm->write_core_reg = armv8_write_core_reg;
+#endif
+
+	armv8->armv8_mmu.armv8_cache.l2_cache = NULL;
+	armv8->armv8_mmu.armv8_cache.ctype = -1;
+	armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL;
+	armv8->armv8_mmu.armv8_cache.display_cache_info = NULL;
+	return ERROR_OK;
+}
+
+int armv8_arch_state(struct target *target)
+{
+	static const char * const state[] = {
+		"disabled", "enabled"
+	};
+
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+
+	if (armv8->common_magic != ARMV8_COMMON_MAGIC) {
+		LOG_ERROR("BUG: called for a non-Armv8 target");
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	arm_arch_state(target);
+
+	if (armv8->is_armv7r) {
+		LOG_USER("D-Cache: %s, I-Cache: %s",
+			state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
+			state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
+	} else {
+		LOG_USER("MMU: %s, D-Cache: %s, I-Cache: %s",
+			state[armv8->armv8_mmu.mmu_enabled],
+			state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
+			state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
+	}
+
+	if (arm->core_mode == ARM_MODE_ABT)
+		armv8_show_fault_registers(target);
+	if (target->debug_reason == DBG_REASON_WATCHPOINT)
+		LOG_USER("Watchpoint triggered at PC %#08x",
+			(unsigned) armv8->dpm.wp_pc);
+
+	return ERROR_OK;
+}
+
+static const struct {
+	unsigned id;
+	const char *name;
+	unsigned bits;
+	enum reg_type type;
+	const char *group;
+	const char *feature;
+} armv8_regs[] = {
+	{ ARMV8_R0,  "x0",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R1,  "x1",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R2,  "x2",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R3,  "x3",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R4,  "x4",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R5,  "x5",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R6,  "x6",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R7,  "x7",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R8,  "x8",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R9,  "x9",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R10, "x10", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R11, "x11", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R12, "x12", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R13, "x13", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R14, "x14", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R15, "x15", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R16, "x16", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R17, "x17", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R18, "x18", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R19, "x19", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R20, "x20", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R21, "x21", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R22, "x22", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R23, "x23", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R24, "x24", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R25, "x25", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R26, "x26", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R27, "x27", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R28, "x28", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R29, "x29", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R30, "x30", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_R31, "sp", 64, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_PC,  "pc", 64, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_xPSR, "CPSR", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+};
+
+#define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
+
+
+static int armv8_get_core_reg(struct reg *reg)
+{
+	int retval;
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	retval = arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
+
+	return retval;
+}
+
+static int armv8_set_core_reg(struct reg *reg, uint8_t *buf)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	uint64_t value = buf_get_u64(buf, 0, 64);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	buf_set_u64(reg->value, 0, 64, value);
+	reg->dirty = 1;
+	reg->valid = 1;
+
+	return ERROR_OK;
+}
+
+static const struct reg_arch_type armv8_reg_type = {
+	.get = armv8_get_core_reg,
+	.set = armv8_set_core_reg,
+};
+
+/** Builds cache of architecturally defined registers.  */
+struct reg_cache *armv8_build_reg_cache(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int num_regs = ARMV8_NUM_REGS;
+	struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
+	struct reg_cache *cache = malloc(sizeof(struct reg_cache));
+	struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
+	struct arm_reg *arch_info = calloc(num_regs, sizeof(struct arm_reg));
+	struct reg_feature *feature;
+	int i;
+
+	/* Build the process context cache */
+	cache->name = "arm v8 registers";
+	cache->next = NULL;
+	cache->reg_list = reg_list;
+	cache->num_regs = num_regs;
+	(*cache_p) = cache;
+
+	for (i = 0; i < num_regs; i++) {
+		arch_info[i].num = armv8_regs[i].id;
+		arch_info[i].target = target;
+		arch_info[i].arm = arm;
+
+		reg_list[i].name = armv8_regs[i].name;
+		reg_list[i].size = armv8_regs[i].bits;
+		reg_list[i].value = calloc(1, 4);
+		reg_list[i].dirty = 0;
+		reg_list[i].valid = 0;
+		reg_list[i].type = &armv8_reg_type;
+		reg_list[i].arch_info = &arch_info[i];
+
+		reg_list[i].group = armv8_regs[i].group;
+		reg_list[i].number = i;
+		reg_list[i].exist = true;
+		reg_list[i].caller_save = true;	/* gdb defaults to true */
+
+		feature = calloc(1, sizeof(struct reg_feature));
+		if (feature) {
+			feature->name = armv8_regs[i].feature;
+			reg_list[i].feature = feature;
+		} else
+			LOG_ERROR("unable to allocate feature list");
+
+		reg_list[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
+		if (reg_list[i].reg_data_type)
+			reg_list[i].reg_data_type->type = armv8_regs[i].type;
+		else
+			LOG_ERROR("unable to allocate reg type list");
+	}
+
+	arm->cpsr = reg_list + ARMV8_xPSR;
+	arm->pc = reg_list + ARMV8_PC;
+	arm->core_cache = cache;
+
+	return cache;
+}
+
+struct reg *armv8_reg_current(struct arm *arm, unsigned regnum)
+{
+	struct reg *r;
+
+	if (regnum > 33)
+		return NULL;
+
+	r = arm->core_cache->reg_list + regnum;
+	return r;
+}
+
+const struct command_registration armv8_command_handlers[] = {
+	{
+		.chain = dap_command_handlers,
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+
+int armv8_get_gdb_reg_list(struct target *target,
+	struct reg **reg_list[], int *reg_list_size,
+	enum target_register_class reg_class)
+{
+	struct arm *arm = target_to_arm(target);
+	int i;
+
+	switch (reg_class) {
+	case REG_CLASS_GENERAL:
+	case REG_CLASS_ALL:
+		*reg_list_size = 34;
+		*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+		for (i = 0; i < *reg_list_size; i++)
+				(*reg_list)[i] = armv8_reg_current(arm, i);
+
+		return ERROR_OK;
+		break;
+
+	default:
+		LOG_ERROR("not a valid register class type in query.");
+		return ERROR_FAIL;
+		break;
+	}
+}
diff --git a/src/target/armv8.h b/src/target/armv8.h
new file mode 100644
index 0000000..080237d
--- /dev/null
+++ b/src/target/armv8.h
@@ -0,0 +1,201 @@
+/***************************************************************************
+ *   Copyright (C) 2015 by David Ung                                       *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ARMV8_H
+#define OPENOCD_TARGET_ARMV8_H
+
+#include "arm_adi_v5.h"
+#include "arm.h"
+#include "armv4_5_mmu.h"
+#include "armv4_5_cache.h"
+#include "arm_dpm.h"
+
+enum {
+	ARMV8_R0,
+	ARMV8_R1,
+	ARMV8_R2,
+	ARMV8_R3,
+	ARMV8_R4,
+	ARMV8_R5,
+	ARMV8_R6,
+	ARMV8_R7,
+	ARMV8_R8,
+	ARMV8_R9,
+	ARMV8_R10,
+	ARMV8_R11,
+	ARMV8_R12,
+	ARMV8_R13,
+	ARMV8_R14,
+	ARMV8_R15,
+	ARMV8_R16,
+	ARMV8_R17,
+	ARMV8_R18,
+	ARMV8_R19,
+	ARMV8_R20,
+	ARMV8_R21,
+	ARMV8_R22,
+	ARMV8_R23,
+	ARMV8_R24,
+	ARMV8_R25,
+	ARMV8_R26,
+	ARMV8_R27,
+	ARMV8_R28,
+	ARMV8_R29,
+	ARMV8_R30,
+	ARMV8_R31,
+
+	ARMV8_PC = 32,
+	ARMV8_xPSR = 33,
+
+	ARMV8_LAST_REG,
+};
+
+
+#define ARMV8_COMMON_MAGIC 0x0A450AAA
+
+/* VA to PA translation operations opc2 values*/
+#define V2PCWPR  0
+#define V2PCWPW  1
+#define V2PCWUR  2
+#define V2PCWUW  3
+#define V2POWPR  4
+#define V2POWPW  5
+#define V2POWUR  6
+#define V2POWUW  7
+/*   L210/L220 cache controller support */
+struct armv8_l2x_cache {
+	uint32_t base;
+	uint32_t way;
+};
+
+struct armv8_cachesize {
+	uint32_t level_num;
+	/*  cache dimensionning */
+	uint32_t linelen;
+	uint32_t associativity;
+	uint32_t nsets;
+	uint32_t cachesize;
+	/* info for set way operation on cache */
+	uint32_t index;
+	uint32_t index_shift;
+	uint32_t way;
+	uint32_t way_shift;
+};
+
+struct armv8_cache_common {
+	int ctype;
+	struct armv8_cachesize d_u_size;	/* data cache */
+	struct armv8_cachesize i_size;		/* instruction cache */
+	int i_cache_enabled;
+	int d_u_cache_enabled;
+	/* l2 external unified cache if some */
+	void *l2_cache;
+	int (*flush_all_data_cache)(struct target *target);
+	int (*display_cache_info)(struct command_context *cmd_ctx,
+			struct armv8_cache_common *armv8_cache);
+};
+
+struct armv8_mmu_common {
+	/* following field mmu working way */
+	int32_t ttbr1_used; /*  -1 not initialized, 0 no ttbr1 1 ttbr1 used and  */
+	uint32_t ttbr0_mask;/*  masked to be used  */
+	uint32_t os_border;
+
+	int (*read_physical_memory)(struct target *target, target_addr_t address,
+			uint32_t size, uint32_t count, uint8_t *buffer);
+	struct armv8_cache_common armv8_cache;
+	uint32_t mmu_enabled;
+};
+
+struct armv8_common {
+	struct arm arm;
+	int common_magic;
+	struct reg_cache *core_cache;
+
+	/* Core Debug Unit */
+	struct arm_dpm dpm;
+	uint32_t debug_base;
+	struct adiv5_ap *debug_ap;
+	struct adiv5_ap *memory_ap;
+	bool memory_ap_available;
+	/* mdir */
+	uint8_t multi_processor_system;
+	uint8_t cluster_id;
+	uint8_t cpu_id;
+	bool is_armv7r;
+
+	/* cache specific to V7 Memory Management Unit compatible with v4_5*/
+	struct armv8_mmu_common armv8_mmu;
+
+	/* Direct processor core register read and writes */
+	int (*load_core_reg_u64)(struct target *target, uint32_t num, uint64_t *value);
+	int (*store_core_reg_u64)(struct target *target, uint32_t num, uint64_t value);
+
+	int (*examine_debug_reason)(struct target *target);
+	int (*post_debug_entry)(struct target *target);
+
+	void (*pre_restore_context)(struct target *target);
+};
+
+static inline struct armv8_common *
+target_to_armv8(struct target *target)
+{
+	return container_of(target->arch_info, struct armv8_common, arm);
+}
+
+/* register offsets from armv8.debug_base */
+
+#define CPUDBG_WFAR		0x018
+/* PCSR at 0x084 -or- 0x0a0 -or- both ... based on flags in DIDR */
+#define CPUDBG_DSCR		0x088
+#define CPUDBG_DRCR		0x090
+#define CPUDBG_PRCR		0x310
+#define CPUDBG_PRSR		0x314
+
+#define CPUDBG_DTRRX		0x080
+#define CPUDBG_ITR		0x084
+#define CPUDBG_DTRTX		0x08c
+
+#define CPUDBG_BVR_BASE		0x400
+#define CPUDBG_BCR_BASE		0x408
+#define CPUDBG_WVR_BASE		0x180
+#define CPUDBG_WCR_BASE		0x1C0
+#define CPUDBG_VCR		0x01C
+
+#define CPUDBG_OSLAR		0x300
+#define CPUDBG_OSLSR		0x304
+#define CPUDBG_OSSRR		0x308
+#define CPUDBG_ECR		0x024
+
+#define CPUDBG_DSCCR		0x028
+
+#define CPUDBG_AUTHSTATUS	0xFB8
+
+int armv8_arch_state(struct target *target);
+int armv8_identify_cache(struct target *target);
+int armv8_init_arch_info(struct target *target, struct armv8_common *armv8);
+int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
+		target_addr_t *val, int meminfo);
+int armv8_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val);
+
+int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
+		struct armv8_cache_common *armv8_cache);
+
+extern const struct command_registration armv8_command_handlers[];
+
+#endif
diff --git a/src/target/target.c b/src/target/target.c
index 40baf8e..5c2164f 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -88,6 +88,7 @@ extern struct target_type dragonite_target;
 extern struct target_type xscale_target;
 extern struct target_type cortexm_target;
 extern struct target_type cortexa_target;
+extern struct target_type aarch64_target;
 extern struct target_type cortexr4_target;
 extern struct target_type arm11_target;
 extern struct target_type ls1_sap_target;
@@ -135,6 +136,9 @@ static struct target_type *target_types[] = {
 	&or1k_target,
 	&quark_x10xx_target,
 	&quark_d20xx_target,
+#if BUILD_TARGET64
+	&aarch64_target,
+#endif
 	NULL,
 };
 
-- 
cgit v1.1


From 236c9966dd77b7d5d678a71d467492048f347a9b Mon Sep 17 00:00:00 2001
From: pierre Kuo <vichy.kuo@gmail.com>
Date: Tue, 17 Mar 2015 12:44:04 -0700
Subject: aarch64: Add hardware breakpoint support

Enable the use of hardware breakpoint on AARCH64.

Change-Id: I59caaa6d92ac60278af8938625b1790a1787372f
Signed-off-by: pierre Kuo <vichy.kuo@gmail.com>
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 39 +++++++++++++++++++--------------------
 1 file changed, 19 insertions(+), 20 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8ac64eb..d991a31 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1254,6 +1254,7 @@ static int aarch64_set_breakpoint(struct target *target,
 	}
 
 	if (breakpoint->type == BKPT_HARD) {
+		int64_t bpt_value;
 		while (brp_list[brp_i].used && (brp_i < aarch64->brp_num))
 			brp_i++;
 		if (brp_i >= aarch64->brp_num) {
@@ -1264,38 +1265,46 @@ static int aarch64_set_breakpoint(struct target *target,
 		if (breakpoint->length == 2)
 			byte_addr_select = (3 << (breakpoint->address & 0x02));
 		control = ((matchmode & 0x7) << 20)
+			| (1 << 13)
 			| (byte_addr_select << 5)
 			| (3 << 1) | 1;
 		brp_list[brp_i].used = 1;
-		brp_list[brp_i].value = (breakpoint->address & 0xFFFFFFFC);
+		brp_list[brp_i].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
 		brp_list[brp_i].control = control;
+		bpt_value = brp_list[brp_i].value;
+
+		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+				+ CPUDBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+				(uint32_t)(bpt_value & 0xFFFFFFFF));
+		if (retval != ERROR_OK)
+			return retval;
 		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-				+ CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
-				brp_list[brp_i].value);
+				+ CPUDBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+				(uint32_t)(bpt_value >> 32));
 		if (retval != ERROR_OK)
 			return retval;
+
 		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-				+ CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+				+ CPUDBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 				brp_list[brp_i].control);
 		if (retval != ERROR_OK)
 			return retval;
 		LOG_DEBUG("brp %i control 0x%0" PRIx32 " value 0x%" TARGET_PRIxADDR, brp_i,
 			brp_list[brp_i].control,
 			brp_list[brp_i].value);
+
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
-		if (breakpoint->length == 2)
-			buf_set_u32(code, 0, 32, ARMV5_T_BKPT(0x11));
-		else
-			buf_set_u32(code, 0, 32, ARMV5_BKPT(0x11));
+		buf_set_u32(code, 0, 32, 0xD4400000);
+
 		retval = target_read_memory(target,
-				breakpoint->address & 0xFFFFFFFE,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1,
 				breakpoint->orig_instr);
 		if (retval != ERROR_OK)
 			return retval;
 		retval = target_write_memory(target,
-				breakpoint->address & 0xFFFFFFFE,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1, code);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1465,11 +1474,6 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 					brp_list[brp_i].control);
 			if (retval != ERROR_OK)
 				return retval;
-			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUDBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
-					brp_list[brp_i].value);
-			if (retval != ERROR_OK)
-				return retval;
 			if ((brp_j < 0) || (brp_j >= aarch64->brp_num)) {
 				LOG_DEBUG("Invalid BRP number in breakpoint");
 				return ERROR_OK;
@@ -1484,11 +1488,6 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 					brp_list[brp_j].control);
 			if (retval != ERROR_OK)
 				return retval;
-			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUDBG_BVR_BASE + 16 * brp_list[brp_j].BRPn,
-					brp_list[brp_j].value);
-			if (retval != ERROR_OK)
-				return retval;
 			breakpoint->linked_BRP = 0;
 			breakpoint->set = 0;
 			return ERROR_OK;
-- 
cgit v1.1


From 5ee67ce024fcbdb0292dce1384b41ac0d6f30426 Mon Sep 17 00:00:00 2001
From: David Ung <davidu@nvidia.com>
Date: Thu, 23 Apr 2015 13:22:13 -0700
Subject: aarch64: Enable halting debug mode on breakpoint set

Ensure that we allow halting debug mode after setting breakpoint

Change-Id: I6f0d7a4a4775a93c133fb1ec31dfe3324d9f7395
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 12 ++++++++++++
 1 file changed, 12 insertions(+)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d991a31..c6354c2 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1247,6 +1247,7 @@ static int aarch64_set_breakpoint(struct target *target,
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	struct aarch64_brp *brp_list = aarch64->brp_list;
+	uint32_t dscr;
 
 	if (breakpoint->set) {
 		LOG_WARNING("breakpoint already set");
@@ -1311,6 +1312,17 @@ static int aarch64_set_breakpoint(struct target *target,
 		breakpoint->set = 0x11;	/* Any nice value but 0 */
 	}
 
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUDBG_DSCR, &dscr);
+	/* Ensure that halting debug mode is enable */
+	dscr = dscr | DSCR_HALT_DBG_MODE;
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+					 armv8->debug_base + CPUDBG_DSCR, dscr);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Failed to set DSCR.HDE");
+		return retval;
+	}
+
 	return ERROR_OK;
 }
 
-- 
cgit v1.1


From 13d13b2e2a5ed12bcc8f2f0dc44bb2beface4946 Mon Sep 17 00:00:00 2001
From: pierre Kuo <vichy.kuo@gmail.com>
Date: Thu, 23 Apr 2015 14:44:27 -0700
Subject: aarch64: Add instruction stepping support using hardware step

Use AARCH64's hardware step event to do stepping.

Change-Id: I2d029ceeadd381913d0c3355c8787b11dacff7f7
Signed-off-by: pierre Kuo <vichy.kuo@gmail.com>
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 49 +++++++++++++------------------------------------
 src/target/armv8.h   |  1 +
 2 files changed, 14 insertions(+), 36 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index c6354c2..276f4a3 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1150,45 +1150,23 @@ 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 arm *arm = &armv8->arm;
-	struct breakpoint *breakpoint = NULL;
-	struct breakpoint stepbreakpoint;
-	struct reg *r;
 	int retval;
+	uint32_t tmp;
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	/* current = 1: continue on current pc, otherwise continue at <address> */
-	r = arm->pc;
-	if (!current)
-		buf_set_u64(r->value, 0, 64, address);
-	else
-		address = buf_get_u64(r->value, 0, 64);
-
-	/* The front-end may request us not to handle breakpoints.
-	 * But since Cortex-A8 uses breakpoint for single step,
-	 * we MUST handle breakpoints.
-	 */
-	handle_breakpoints = 1;
-	if (handle_breakpoints) {
-		breakpoint = breakpoint_find(target, address);
-		if (breakpoint)
-			aarch64_unset_breakpoint(target, breakpoint);
-	}
-
-	/* Setup single step breakpoint */
-	stepbreakpoint.address = address;
-	stepbreakpoint.length = 4;
-	stepbreakpoint.type = BKPT_HARD;
-	stepbreakpoint.set = 0;
-
-	/* Break on IVA mismatch */
-	aarch64_set_breakpoint(target, &stepbreakpoint, 0x04);
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DECR, &tmp);
+	if (retval != ERROR_OK)
+		return retval;
 
-	target->debug_reason = DBG_REASON_SINGLESTEP;
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DECR, (tmp|0x4));
+	if (retval != ERROR_OK)
+		return retval;
 
 	retval = aarch64_resume(target, 1, address, 0, 0);
 	if (retval != ERROR_OK)
@@ -1205,12 +1183,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		}
 	}
 
-	aarch64_unset_breakpoint(target, &stepbreakpoint);
-
 	target->debug_reason = DBG_REASON_BREAKPOINT;
-
-	if (breakpoint)
-		aarch64_set_breakpoint(target, breakpoint, 0);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DECR, (tmp&(~0x4)));
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (target->state != TARGET_HALTED)
 		LOG_DEBUG("target stepped");
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 080237d..58e4228 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -161,6 +161,7 @@ target_to_armv8(struct target *target)
 /* register offsets from armv8.debug_base */
 
 #define CPUDBG_WFAR		0x018
+#define CPUDBG_DECR		0x024
 /* PCSR at 0x084 -or- 0x0a0 -or- both ... based on flags in DIDR */
 #define CPUDBG_DSCR		0x088
 #define CPUDBG_DRCR		0x090
-- 
cgit v1.1


From a12c15e21f663550de518595e606633bf82b5d04 Mon Sep 17 00:00:00 2001
From: David Ung <davidu@nvidia.com>
Date: Thu, 23 Apr 2015 14:49:01 -0700
Subject: aarch64: Enable resuming with address

Enable resuming to an address.

Change-Id: I29c7d3b56f6cbf8b3cd02c93733fc96f45000af3
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 276f4a3..a2b020b 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1023,12 +1023,7 @@ static int aarch64_resume(struct target *target, int current,
 	target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	int retval = 0;
-	uint64_t resume_addr;
-
-	if (address) {
-		LOG_DEBUG("resuming with custom address not supported");
-		return ERROR_FAIL;
-	}
+	uint64_t addr = address;
 
 	/* dummy resume for smp toggle in order to reduce gdb impact  */
 	if ((target->smp) && (target->gdb_service->core[1] != -1)) {
@@ -1039,7 +1034,8 @@ static int aarch64_resume(struct target *target, int current,
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 		return 0;
 	}
-	aarch64_internal_restore(target, current, &resume_addr, handle_breakpoints, debug_execution);
+	aarch64_internal_restore(target, current, &addr, handle_breakpoints,
+				 debug_execution);
 	if (target->smp) {
 		target->gdb_service->core[0] = -1;
 		retval = aarch64_restore_smp(target, handle_breakpoints);
@@ -1051,11 +1047,11 @@ static int aarch64_resume(struct target *target, int current,
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-		LOG_DEBUG("target resumed at 0x%" PRIx64, resume_addr);
+		LOG_DEBUG("target resumed at 0x%" PRIu64, addr);
 	} else {
 		target->state = TARGET_DEBUG_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
-		LOG_DEBUG("target debug resumed at 0x%" PRIx64, resume_addr);
+		LOG_DEBUG("target debug resumed at 0x%" PRIu64, addr);
 	}
 
 	return ERROR_OK;
-- 
cgit v1.1


From 6b554b3b0e34fe0a1d37a1584baf3c3e32a76b57 Mon Sep 17 00:00:00 2001
From: David Ung <davidu@nvidia.com>
Date: Mon, 20 Apr 2015 13:14:43 -0700
Subject: aarch64: Correct target state for hardware step

When using hardware step for doing stepping, the existing DSCR records
the event as external debug request.  This will generate a SIGINT event
to GDB and causes it to stop the stepping process.
For aarch64, read DESR to check if the event is a hardware step and set
state to DBG_REASON_SINGLESTEP.
With this patch, GDB can now do source level stepping.

Change-Id: I1d06f819578c74b3ac17376c67f882adddea1f52
Signed-off-by: David Ung <david.ung.42@gmail.com>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 13 +++++++++++--
 src/target/armv8.h   |  1 +
 2 files changed, 12 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index a2b020b..78d1973 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1063,6 +1063,7 @@ static int aarch64_debug_entry(struct target *target)
 	int retval = ERROR_OK;
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = target_to_armv8(target);
+	uint32_t tmp;
 
 	LOG_DEBUG("dscr = 0x%08" PRIx32, aarch64->cpudbg_dscr);
 
@@ -1086,6 +1087,10 @@ static int aarch64_debug_entry(struct target *target)
 
 	/* Examine debug reason */
 	arm_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
+	mem_ap_read_atomic_u32(armv8->debug_ap,
+				   armv8->debug_base + CPUDBG_DESR, &tmp);
+	if ((tmp & 0x7) == 0x4)
+		target->debug_reason = DBG_REASON_SINGLESTEP;
 
 	/* save address of instruction that triggered the watchpoint? */
 	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
@@ -1164,12 +1169,16 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	if (retval != ERROR_OK)
 		return retval;
 
+	target->debug_reason = DBG_REASON_SINGLESTEP;
 	retval = aarch64_resume(target, 1, address, 0, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
 	long long then = timeval_ms();
 	while (target->state != TARGET_HALTED) {
+		mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DESR, &tmp);
+		LOG_DEBUG("DESR = %#x", tmp);
 		retval = aarch64_poll(target);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1179,13 +1188,13 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		}
 	}
 
-	target->debug_reason = DBG_REASON_BREAKPOINT;
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUDBG_DECR, (tmp&(~0x4)));
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (target->state != TARGET_HALTED)
+	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+	if (target->state == TARGET_HALTED)
 		LOG_DEBUG("target stepped");
 
 	return ERROR_OK;
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 58e4228..47e3668 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -161,6 +161,7 @@ target_to_armv8(struct target *target)
 /* register offsets from armv8.debug_base */
 
 #define CPUDBG_WFAR		0x018
+#define CPUDBG_DESR		0x020
 #define CPUDBG_DECR		0x024
 /* PCSR at 0x084 -or- 0x0a0 -or- both ... based on flags in DIDR */
 #define CPUDBG_DSCR		0x088
-- 
cgit v1.1


From ced4108d26de948b293cc2bbe51f64dca64795e2 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 2 Sep 2016 10:38:08 +0200
Subject: aarch64: add symbolic definitions for armv8 opcodes

To replace hexadecimal constants with descriptive names and increase
flexibility.

Change-Id: I6f7b6f045866ed8b9360f54b640ecdb307eebc51
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/Makefile.am     |   1 +
 src/target/armv8_opcodes.h | 126 +++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 127 insertions(+)
 create mode 100644 src/target/armv8_opcodes.h

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index a17496c..e235776 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -155,6 +155,7 @@ INTEL_IA32_SRC = \
 	%D%/armv7m.h \
 	%D%/armv7m_trace.h \
 	%D%/armv8.h \
+	%D%/armv8_opcodes.h \
 	%D%/avrt.h \
 	%D%/dsp563xx.h \
 	%D%/dsp563xx_once.h \
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
new file mode 100644
index 0000000..4f696db
--- /dev/null
+++ b/src/target/armv8_opcodes.h
@@ -0,0 +1,126 @@
+/*
+ * Copyright (C) 2015 by pierrr kuo
+ * vichy.kuo@gmail.com
+ * 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.
+ *
+ */
+#ifndef OPENOCD_TARGET_ARMV8_OPCODES_H
+#define OPENOCD_TARGET_ARMV8_OPCODES_H
+
+#include "arm_opcodes.h"
+
+/* ARM V8 Move from system register to general purpose register
+ * R = 1: SPSR R = 0: CPSR
+ * Rn: target register
+ */
+#define SYSTEM_CUREL			0b1100001000010010
+#define SYSTEM_CUREL_MASK		0xC0
+#define SYSTEM_CUREL_SHIFT		6
+#define SYSTEM_CUREL_EL0		0x0
+#define SYSTEM_CUREL_EL1		0x1
+#define SYSTEM_CUREL_EL2		0x2
+#define SYSTEM_CUREL_EL3		0x3
+#define SYSTEM_CUREL_NONCH		0xF
+#define SYSTEM_AARCH64			0x1
+
+#define SYSTEM_AAR64_MODE_EL0t	0x0
+#define SYSTEM_AAR64_MODE_EL1t	0x4
+#define SYSTEM_AAR64_MODE_EL1h	0x5
+#define SYSTEM_AAR64_MODE_EL2t	0x8
+#define SYSTEM_AAR64_MODE_EL2h	0x9
+#define SYSTEM_AAR64_MODE_EL3t	0xC
+#define SYSTEM_AAR64_MODE_EL3h	0xd
+
+#define SYSTEM_DAIF			0b1101101000010001
+#define SYSTEM_DAIF_MASK		0x3C0
+#define SYSTEM_DAIF_SHIFT		6
+
+#define SYSTEM_ELR_EL1			0b1100001000000001
+#define SYSTEM_ELR_EL2			0b1110001000000001
+#define SYSTEM_ELR_EL3			0b1111001000000001
+
+#define SYSTEM_FPCR			0b1101101000100000
+#define SYSTEM_FPSR			0b1101101000100001
+#define SYSTEM_DAIF			0b1101101000010001
+#define SYSTEM_NZCV			0b1101101000010000
+#define SYSTEM_SP_EL0			0b1100001000001000
+#define SYSTEM_SP_EL1			0b1110001000001000
+#define SYSTEM_SP_EL2			0b1111001000001000
+#define SYSTEM_SP_SEL			0b1100001000010000
+#define SYSTEM_SPSR_ABT			0b1110001000011001
+#define SYSTEM_SPSR_FIQ			0b1110001000011011
+#define SYSTEM_SPSR_IRQ			0b1110001000011000
+#define SYSTEM_SPSR_UND			0b1110001000011010
+
+#define SYSTEM_SPSR_EL1			0b1100001000000000
+#define SYSTEM_SPSR_EL2			0b1110001000000000
+#define SYSTEM_SPSR_EL3			0b1111001000000000
+
+#define SYSTEM_ISR_EL1			0b1100011000001000
+
+
+#define SYSTEM_DBG_DSPSR_EL0    0b1101101000101000
+#define SYSTEM_DBG_DLR_EL0		0b1101101000101001
+#define SYSTEM_DBG_DTRRX_EL0	0b1001100000101000
+#define SYSTEM_DBG_DTRTX_EL0	0b1001100000101000
+#define SYSTEM_DBG_DBGDTR_EL0	0b1001100000100000
+
+#define SYSTEM_CCSIDR			0b1100100000000000
+#define SYSTEM_CLIDR			0b1100100000000001
+#define SYSTEM_CSSELR			0b1101000000000000
+#define SYSTEM_CTYPE			0b1101100000000001
+
+#define SYSTEM_DCCISW			0b0100001111110010
+#define SYSTEM_DCCSW			0b0100001111010010
+#define SYSTEM_ICIVAU			0b0101101110101001
+#define SYSTEM_DCCVAU			0b0101101111011001
+
+#define SYSTEM_MPIDR			0b1100000000000101
+
+#define SYSTEM_TCR_EL1			0b1100000100000010
+#define SYSTEM_TCR_EL2			0b1110000100000010
+#define SYSTEM_TCR_EL3			0b1111000100000010
+
+#define SYSTEM_TTBR0_EL1		0b1100000100000000
+#define SYSTEM_TTBR0_EL2		0b1110000100000000
+#define SYSTEM_TTBR0_EL3		0b1111000100000000
+#define SYSTEM_TTBR1_EL1		0b1100000100000001
+
+
+
+#define ARMV8_MRS_DSPSR(Rt)	(0xd53b4500 | (Rt))
+#define ARMV8_MSR_DSPSR(Rt)	(0xd51b4500 | (Rt))
+#define ARMV8_MRS_DLR(Rt)	(0xd53b4520 | (Rt))
+#define ARMV8_MSR_DLR(Rt)	(0xd51b4520 | (Rt))
+
+/* T32 ITR format */
+#define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
+
+#define ARMV8_DCPS1(IM)	(0xd4a00001 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS2(IM)	(0xd4a00002 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS3(IM)	(0xd4a00003 | (((IM) & 0xFFFF) << 5))
+
+#define DSB_SY				0xd5033F9F
+#define ARMV8_MRS(System, Rt)	(0xd5300000 | ((System) << 5) | (Rt))
+/* ARM V8 Move to system register. */
+#define ARMV8_MSR_GP(System, Rt) \
+	(0xd5100000 | ((System) << 5) | (Rt))
+/* ARM V8 Move immediate to process state field. */
+#define ARMV8_MSR_IM(Op1, CRm, Op2) \
+	(0xd500401f | ((Op1) << 16)  | ((CRm) << 8) | ((Op2) << 5))
+#define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
+#define ARMV8_MOVFSP_64(Rt) ((1 << 31) | 0x11000000 | (0x1f << 5) | (Rt))
+#define ARMV8_MOVTSP_64(Rt) ((1 << 31) | 0x11000000 | (Rt << 5) | (0x1F))
+#define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
+#define ARMV8_MOVTSP_32(Rt) (0x11000000 | (Rt << 5) | (0x1F))
+
+
+#endif /* __ARM_OPCODES_H */
-- 
cgit v1.1


From f2a271e2643e8a57881d5db7aceb21fc36140bc2 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sat, 3 Sep 2016 16:35:59 +0200
Subject: aarch64: fix reading of MPIDR

read MPIDR register through correct MSR instruction.

Change-Id: I7e2d00c2871191c4168b177a7a809443b0db4c82
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 902d290..e516518 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -33,6 +33,7 @@
 #include <string.h>
 #include <unistd.h>
 
+#include "armv8_opcodes.h"
 #include "arm_opcodes.h"
 #include "target.h"
 #include "target_type.h"
@@ -351,7 +352,7 @@ static int armv8_read_mpidr(struct target *target)
 	/* MRC p15,0,<Rd>,c0,c0,5; read Multiprocessor ID register*/
 
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 0, 0, 0, 0, 5),
+			ARMV8_MRS(SYSTEM_MPIDR, 0),
 			&mpidr);
 	if (retval != ERROR_OK)
 		goto done;
-- 
cgit v1.1


From df32af234cae1f24b0a0f3abf4b50859f9d26740 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sat, 3 Sep 2016 17:12:18 +0200
Subject: target: add -ctibase config option in addition to -dbgbase

Some vendors don't fully populate the ROM table, e.g. BCM2357 (used in
Raspberry Pi 3) doesn't list CTI, however it is mandatory for halting
an ARMv8 core and therefore it's always present (and required),
regardless of the ROM table listing it or not.

Change-Id: Ia18a4f1b5b931ccd19805b188ebf737c837c6b54
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/target.c | 17 ++++++++++++++++-
 src/target/target.h |  5 +++++
 2 files changed, 21 insertions(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index 5c2164f..ee302ee 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -4445,6 +4445,7 @@ enum target_cfg_param {
 	TCFG_COREID,
 	TCFG_CHAIN_POSITION,
 	TCFG_DBGBASE,
+	TCFG_CTIBASE,
 	TCFG_RTOS,
 	TCFG_DEFER_EXAMINE,
 };
@@ -4460,6 +4461,7 @@ static Jim_Nvp nvp_config_opts[] = {
 	{ .name = "-coreid",           .value = TCFG_COREID },
 	{ .name = "-chain-position",   .value = TCFG_CHAIN_POSITION },
 	{ .name = "-dbgbase",          .value = TCFG_DBGBASE },
+	{ .name = "-ctibase",          .value = TCFG_CTIBASE },
 	{ .name = "-rtos",             .value = TCFG_RTOS },
 	{ .name = "-defer-examine",    .value = TCFG_DEFER_EXAMINE },
 	{ .name = NULL, .value = -1 }
@@ -4726,7 +4728,20 @@ no_params:
 			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->dbgbase));
 			/* loop for more */
 			break;
-
+		case TCFG_CTIBASE:
+			if (goi->isconfigure) {
+				e = Jim_GetOpt_Wide(goi, &w);
+				if (e != JIM_OK)
+					return e;
+				target->ctibase = (uint32_t)w;
+				target->ctibase_set = true;
+			} else {
+				if (goi->argc != 0)
+					goto no_params;
+			}
+			Jim_SetResult(goi->interp, Jim_NewIntObj(goi->interp, target->ctibase));
+			/* loop for more */
+			break;
 		case TCFG_RTOS:
 			/* RTOS */
 			{
diff --git a/src/target/target.h b/src/target/target.h
index 41c9333..76630b9 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -185,6 +185,11 @@ struct target {
 	uint32_t dbgbase;					/* Really a Cortex-A specific option, but there is no
 										 * system in place to support target specific options
 										 * currently. */
+
+	 bool ctibase_set;					 /* By default the debug base is not set */
+	 uint32_t ctibase;					 /* Really a Cortex-A specific option, but there is no
+										  * system in place to support target specific options
+										  * currently. */
 	struct rtos *rtos;					/* Instance of Real Time Operating System support */
 	bool rtos_auto_detect;				/* A flag that indicates that the RTOS has been specified as "auto"
 										 * and must be detected when symbols are offered */
-- 
cgit v1.1


From a3b9e12aecc3021bc3022eca93d07f7d11255b3b Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sat, 3 Sep 2016 23:20:58 +0200
Subject: aarch64: introduce dpm extension for ARMv8

Add or move ARMv8 related dpm function to their own source module

Change-Id: Id93d50be0b8635bd40ddb2a74fe8746ff840d736
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/Makefile.am |   2 +
 src/target/aarch64.c   |  30 +-
 src/target/arm.h       |  21 ++
 src/target/arm_dpm.c   | 324 ++++------------
 src/target/arm_dpm.h   |   5 +-
 src/target/armv8.c     | 174 +++++++++
 src/target/armv8.h     |   4 +-
 src/target/armv8_dpm.c | 985 +++++++++++++++++++++++++++++++++++++++++++++++++
 src/target/armv8_dpm.h | 115 ++++++
 9 files changed, 1400 insertions(+), 260 deletions(-)
 create mode 100644 src/target/armv8_dpm.c
 create mode 100644 src/target/armv8_dpm.h

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index e235776..c20ac0f 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -74,6 +74,7 @@ ARMV7_SRC = \
 	%D%/ls1_sap.c
 
 ARMV8_SRC = \
+	%D%/armv8_dpm.c \
 	%D%/aarch64.c \
 	%D%/armv8.c
 
@@ -155,6 +156,7 @@ INTEL_IA32_SRC = \
 	%D%/armv7m.h \
 	%D%/armv7m_trace.h \
 	%D%/armv8.h \
+	%D%/armv8_dpm.h \
 	%D%/armv8_opcodes.h \
 	%D%/avrt.h \
 	%D%/dsp563xx.h \
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 78d1973..be02eb8 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -376,6 +376,18 @@ static int aarch64_dpm_finish(struct arm_dpm *dpm)
 	return ERROR_OK;
 }
 
+static int aarch64_instr_execute(struct arm_dpm *dpm,
+	uint32_t opcode)
+{
+	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	uint32_t dscr = DSCR_ITE;
+
+	return aarch64_exec_opcode(
+			a8->armv8_common.arm.target,
+			opcode,
+			&dscr);
+}
+
 static int aarch64_instr_write_data_dcc(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t data)
 {
@@ -638,6 +650,7 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint32_t debug)
 	dpm->prepare = aarch64_dpm_prepare;
 	dpm->finish = aarch64_dpm_finish;
 
+	dpm->instr_execute = aarch64_instr_execute;
 	dpm->instr_write_data_dcc = aarch64_instr_write_data_dcc;
 	dpm->instr_write_data_dcc_64 = aarch64_instr_write_data_dcc_64;
 	dpm->instr_write_data_r0 = aarch64_instr_write_data_r0;
@@ -654,9 +667,9 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint32_t debug)
 	dpm->bpwp_enable = aarch64_bpwp_enable;
 	dpm->bpwp_disable = aarch64_bpwp_disable;
 
-	retval = arm_dpm_setup(dpm);
+	retval = armv8_dpm_setup(dpm);
 	if (retval == ERROR_OK)
-		retval = arm_dpm_initialize(dpm);
+		retval = armv8_dpm_initialize(dpm);
 
 	return retval;
 }
@@ -879,6 +892,8 @@ static int aarch64_internal_restore(struct target *target, int current,
 	 */
 	switch (arm->core_state) {
 		case ARM_STATE_ARM:
+			resume_pc &= 0xFFFFFFFC;
+			break;
 		case ARM_STATE_AARCH64:
 			resume_pc &= 0xFFFFFFFFFFFFFFFC;
 			break;
@@ -897,10 +912,8 @@ static int aarch64_internal_restore(struct target *target, int current,
 	buf_set_u64(arm->pc->value, 0, 64, resume_pc);
 	arm->pc->dirty = 1;
 	arm->pc->valid = 1;
-#if 0
-	/* restore dpm_mode at system halt */
-	dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
-#endif
+	dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+
 	/* called it now before restoring context because it uses cpu
 	 * register r0 for restoring system control register */
 	retval = aarch64_restore_system_control_reg(target);
@@ -1104,7 +1117,7 @@ static int aarch64_debug_entry(struct target *target)
 		arm_dpm_report_wfar(&armv8->dpm, wfar);
 	}
 
-	retval = arm_dpm_read_current_registers_64(&armv8->dpm);
+	retval = armv8_dpm_read_current_registers(&armv8->dpm);
 
 	if (armv8->post_debug_entry) {
 		retval = armv8->post_debug_entry(target);
@@ -1209,9 +1222,8 @@ static int aarch64_restore_context(struct target *target, bool bpwp)
 	if (armv8->pre_restore_context)
 		armv8->pre_restore_context(target);
 
-	return arm_dpm_write_dirty_registers(&armv8->dpm, bpwp);
+	return armv8_dpm_write_dirty_registers(&armv8->dpm, bpwp);
 
-	return ERROR_OK;
 }
 
 /*
diff --git a/src/target/arm.h b/src/target/arm.h
index e4f2038..2548038 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -66,6 +66,15 @@ 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,
+
 	ARM_MODE_ANY = -1
 };
 
@@ -180,6 +189,18 @@ struct arm {
 			uint32_t CRn, uint32_t CRm,
 			uint32_t value);
 
+	/** Read coprocessor register.  */
+	int (*mrs)(struct target *target, uint32_t op0,
+			uint32_t op1, uint32_t op2,
+			uint32_t CRn, uint32_t CRm,
+			uint32_t *value);
+
+	/** Write coprocessor register.  */
+	int (*msr)(struct target *target, uint32_t cpnum,
+			uint32_t op1, uint32_t op2,
+			uint32_t CRn, uint32_t CRm,
+			uint32_t value);
+
 	void *arch_info;
 
 	/** For targets conforming to ARM Debug Interface v5,
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 55f253e..00ebeba 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -21,6 +21,7 @@
 
 #include "arm.h"
 #include "arm_dpm.h"
+#include "armv8_dpm.h"
 #include <jtag/jtag.h>
 #include "register.h"
 #include "breakpoints.h"
@@ -130,11 +131,11 @@ int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	return retval;
 }
 
-
-static int dpm_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+/* just read the register -- rely on the core mode being right */
+static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
 	uint32_t value;
-	int retval = ERROR_FAIL;
+	int retval;
 
 	switch (regnum) {
 		case 0 ... 14:
@@ -165,8 +166,8 @@ static int dpm_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 					/* core-specific ... ? */
 					LOG_WARNING("Jazelle PC adjustment unknown");
 					break;
-				case ARM_STATE_AARCH64:
-					LOG_ERROR("AARCH64: 32bit read requested");
+				default:
+					LOG_WARNING("unknow core state");
 					break;
 			}
 			break;
@@ -190,57 +191,10 @@ static int dpm_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	return retval;
 }
 
-static int dpm_read_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
-{
-	uint64_t value;
-	uint32_t i;
-	int retval = ERROR_FAIL;
-
-	switch (regnum) {
-		case 0 ... 30:
-			i = 0xd5130400 + regnum; /* msr dbgdtr_el0,reg */
-			retval = dpm->instr_read_data_dcc_64(dpm, i, &value);
-			break;
-		case 31: /* SP */
-			i = 0x910003e0;
-			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
-			break;
-		case 32: /* PC */
-			i = 0xd53b4520;
-			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
-			break;
-		case 33: /* CPSR */
-			i = 0xd53b4500;
-			retval = dpm->instr_read_data_r0_64(dpm, i, &value);
-			break;
-
-		default:
-			break;
-	}
-
-	if (retval == ERROR_OK) {
-		buf_set_u64(r->value, 0, 64, value);
-		r->valid = true;
-		r->dirty = false;
-		LOG_DEBUG("READ: %s, %16.16llx", r->name, (long long)value);
-	}
-
-	return retval;
-}
-
-
-/* just read the register -- rely on the core mode being right */
-static int dpm_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
-{
-	if (r->size == 64)
-		return dpm_read_reg64(dpm, r, regnum);
-	else
-		return dpm_read_reg32(dpm, r, regnum);
-}
-
-static int dpm_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+/* just write the register -- rely on the core mode being right */
+static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
-	int retval = ERROR_FAIL;
+	int retval;
 	uint32_t value = buf_get_u32(r->value, 0, 32);
 
 	switch (regnum) {
@@ -290,51 +244,19 @@ static int dpm_write_pc_core_state(struct arm_dpm *dpm, struct reg *r)
 	return dpm->instr_write_data_r0(dpm, ARMV4_5_BX(0), value);
 }
 
-static int dpm_write_reg64(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
-{
-	int retval = ERROR_FAIL;
-	uint32_t i;
-	uint64_t value = buf_get_u64(r->value, 0, 64);
-
-	switch (regnum) {
-		case 0 ... 30:
-			i = 0xd5330400 + regnum;
-			retval = dpm->instr_write_data_dcc_64(dpm, i, value);
-			break;
-		case 32: /* PC */
-			i = 0xd51b4520;
-			retval = dpm->instr_write_data_r0_64(dpm, i, value);
-			break;
-		default:
-			LOG_DEBUG("register %s (%16.16llx) not defined", r->name,
-				  (unsigned long long)value);
-			break;
-	}
-
-	if (retval == ERROR_OK) {
-		r->dirty = false;
-		LOG_DEBUG("WRITE: %s, %16.16llx", r->name, (unsigned long long)value);
-	}
-
-	return retval;
-}
-
-/* just write the register -- rely on the core mode being right */
-static int dpm_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
-{
-	if (r->size == 64)
-		return dpm_write_reg64(dpm, r, regnum);
-	else
-		return dpm_write_reg32(dpm, r, regnum);
-}
-
-static int arm_dpm_read_current_registers_i(struct arm_dpm *dpm)
+/**
+ * Read basic registers of the the current context:  R0 to R15, and CPSR;
+ * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
+ * In normal operation this is called on entry to halting debug state,
+ * possibly after some other operations supporting restore of debug state
+ * or making sure the CPU is fully idle (drain write buffer, etc).
+ */
+int arm_dpm_read_current_registers(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
-	uint32_t cpsr, instr, core_regs;
+	uint32_t cpsr;
 	int retval;
 	struct reg *r;
-	enum arm_state core_state = arm->core_state;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
@@ -349,25 +271,16 @@ static int arm_dpm_read_current_registers_i(struct arm_dpm *dpm)
 	}
 	r->dirty = true;
 
-	if (core_state == ARM_STATE_AARCH64)
-		instr = 0xd53b4500;  /* mrs x0, dspsr_el0 */
-	else
-		instr = ARMV4_5_MRS(0, 0);
-	retval = dpm->instr_read_data_r0(dpm, instr, &cpsr);
+	retval = dpm->instr_read_data_r0(dpm, ARMV4_5_MRS(0, 0), &cpsr);
 	if (retval != ERROR_OK)
 		goto fail;
 
 	/* update core mode and state, plus shadow mapping for R8..R14 */
 	arm_set_cpsr(arm, cpsr);
-	if (core_state == ARM_STATE_AARCH64)
-		/* arm_set_cpsr changes core_state, restore it for now */
-		arm->core_state = ARM_STATE_AARCH64;
-
-	core_regs = arm->core_cache->num_regs;
 
 	/* REVISIT we can probably avoid reading R1..R14, saving time... */
-	for (unsigned i = 1; i < core_regs; i++) {
-		r = dpm->arm_reg_current(arm, i);
+	for (unsigned i = 1; i < 16; i++) {
+		r = arm_reg_current(arm, i);
 		if (r->valid)
 			continue;
 
@@ -388,23 +301,6 @@ fail:
 	return retval;
 }
 
-/**
- * Read basic registers of the the current context:  R0 to R15, and CPSR;
- * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
- * In normal operation this is called on entry to halting debug state,
- * possibly after some other operations supporting restore of debug state
- * or making sure the CPU is fully idle (drain write buffer, etc).
- */
-int arm_dpm_read_current_registers(struct arm_dpm *dpm)
-{
-	return arm_dpm_read_current_registers_i(dpm);
-}
-
-int arm_dpm_read_current_registers_64(struct arm_dpm *dpm)
-{
-	return arm_dpm_read_current_registers_i(dpm);
-}
-
 /* Avoid needless I/O ... leave breakpoints and watchpoints alone
  * unless they're removed, or need updating because of single-stepping
  * or running debugger code.
@@ -453,14 +349,59 @@ done:
 
 static int dpm_add_breakpoint(struct target *target, struct breakpoint *bp);
 
-
-static int arm_dpm_write_dirty_registers_32(struct arm_dpm *dpm)
+/**
+ * Writes all modified core registers for all processor modes.  In normal
+ * operation this is called on exit from halting debug state.
+ *
+ * @param dpm: represents the processor
+ * @param bpwp: true ensures breakpoints and watchpoints are set,
+ *	false ensures they are cleared
+ */
+int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 {
 	struct arm *arm = dpm->arm;
 	struct reg_cache *cache = arm->core_cache;
 	int retval;
 	bool did_write;
 
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* If we're managing hardware breakpoints for this core, enable
+	 * or disable them as requested.
+	 *
+	 * REVISIT We don't yet manage them for ANY cores.  Eventually
+	 * we should be able to assume we handle them; but until then,
+	 * cope with the hand-crafted breakpoint code.
+	 */
+	if (arm->target->type->add_breakpoint == dpm_add_breakpoint) {
+		for (unsigned i = 0; i < dpm->nbp; i++) {
+			struct dpm_bp *dbp = dpm->dbp + i;
+			struct breakpoint *bp = dbp->bp;
+
+			retval = dpm_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
+					bp ? &bp->set : NULL);
+			if (retval != ERROR_OK)
+				goto done;
+		}
+	}
+
+	/* enable/disable watchpoints */
+	for (unsigned i = 0; i < dpm->nwp; i++) {
+		struct dpm_wp *dwp = dpm->dwp + i;
+		struct watchpoint *wp = dwp->wp;
+
+		retval = dpm_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
+				wp ? &wp->set : NULL);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+	/* NOTE:  writes to breakpoint and watchpoint registers might
+	 * be queued, and need (efficient/batched) flushing later.
+	 */
+
 	/* Scan the registers until we find one that's both dirty and
 	 * eligible for flushing.  Flush that and everything else that
 	 * shares the same core mode setting.  Typically this won't
@@ -564,106 +505,6 @@ static int arm_dpm_write_dirty_registers_32(struct arm_dpm *dpm)
 		goto done;
 	cache->reg_list[0].dirty = false;
 
-done:
-	return retval;
-}
-
-static int arm_dpm_write_dirty_registers_64(struct arm_dpm *dpm)
-{
-	struct arm *arm = dpm->arm;
-	struct reg_cache *cache = arm->core_cache;
-	int retval;
-
-	/* Scan the registers until we find one that's both dirty and
-	 * eligible for flushing.  Flush that and everything else that
-	 * shares the same core mode setting.  Typically this won't
-	 * actually find anything to do...
-	 */
-
-	/* check everything except our scratch register R0 */
-	for (unsigned i = 1; i <= 32; i++) {
-		struct arm_reg *r;
-		unsigned regnum;
-
-		if (!cache->reg_list[i].dirty)
-			continue;
-
-		r = cache->reg_list[i].arch_info;
-		regnum = r->num;
-		retval = dpm_write_reg(dpm,
-				       &cache->reg_list[i],
-				       regnum);
-		if (retval != ERROR_OK)
-			goto done;
-	}
-
-	/* flush R0 -- it's *very* dirty by now */
-	retval = dpm_write_reg(dpm, &cache->reg_list[0], 0);
-	if (retval != ERROR_OK)
-		goto done;
-	cache->reg_list[0].dirty = false;
-
-done:
-	return retval;
-}
-
-/**
- * Writes all modified core registers for all processor modes.  In normal
- * operation this is called on exit from halting debug state.
- *
- * @param dpm: represents the processor
- * @param bpwp: true ensures breakpoints and watchpoints are set,
- *	false ensures they are cleared
- */
-int arm_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
-{
-	struct arm *arm = dpm->arm;
-	struct reg_cache *cache = arm->core_cache;
-	int retval;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/* If we're managing hardware breakpoints for this core, enable
-	 * or disable them as requested.
-	 *
-	 * REVISIT We don't yet manage them for ANY cores.  Eventually
-	 * we should be able to assume we handle them; but until then,
-	 * cope with the hand-crafted breakpoint code.
-	 */
-	if (arm->target->type->add_breakpoint == dpm_add_breakpoint) {
-		for (unsigned i = 0; i < dpm->nbp; i++) {
-			struct dpm_bp *dbp = dpm->dbp + i;
-			struct breakpoint *bp = dbp->bp;
-
-			retval = dpm_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
-					bp ? &bp->set : NULL);
-			if (retval != ERROR_OK)
-				goto done;
-		}
-	}
-
-	/* enable/disable watchpoints */
-	for (unsigned i = 0; i < dpm->nwp; i++) {
-		struct dpm_wp *dwp = dpm->dwp + i;
-		struct watchpoint *wp = dwp->wp;
-
-		retval = dpm_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
-				wp ? &wp->set : NULL);
-		if (retval != ERROR_OK)
-			goto done;
-	}
-
-	/* NOTE:  writes to breakpoint and watchpoint registers might
-	 * be queued, and need (efficient/batched) flushing later.
-	 */
-
-	if (cache->reg_list[0].size == 64)
-		retval = arm_dpm_write_dirty_registers_64(dpm);
-	else
-		retval = arm_dpm_write_dirty_registers_32(dpm);
-
 	/* (void) */ dpm->finish(dpm);
 done:
 	return retval;
@@ -1133,20 +974,15 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 
 	/* register access setup */
 	arm->full_context = arm_dpm_full_context;
-	arm->read_core_reg = arm->read_core_reg ? : arm_dpm_read_core_reg;
-	arm->write_core_reg = arm->write_core_reg ? : arm_dpm_write_core_reg;
+	arm->read_core_reg = arm_dpm_read_core_reg;
+	arm->write_core_reg = arm_dpm_write_core_reg;
 
 	if (arm->core_cache != NULL) {
-		if (arm->core_state == ARM_STATE_AARCH64) {
-			cache = armv8_build_reg_cache(target);
-			target->reg_cache = cache;
-		} else {
-			cache = arm_build_reg_cache(target, arm);
-			*register_get_last_cache_p(&target->reg_cache) = cache;
-		}
-
+		cache = arm_build_reg_cache(target, arm);
 		if (!cache)
 			return ERROR_FAIL;
+
+		*register_get_last_cache_p(&target->reg_cache) = cache;
 	}
 
 	/* coprocessor access setup */
@@ -1163,20 +999,10 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	target->type->add_watchpoint = dpm_add_watchpoint;
 	target->type->remove_watchpoint = dpm_remove_watchpoint;
 
-
-	if (dpm->arm_reg_current == 0)
-		dpm->arm_reg_current = arm_reg_current;
-
 	/* FIXME add vector catch support */
 
-	if (arm->core_state == ARM_STATE_AARCH64) {
-		dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
-		dpm->nwp = 1 + ((dpm->didr >> 28) & 0xf);
-	} else {
-		dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
-		dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
-	}
-
+	dpm->nbp = 1 + ((dpm->didr >> 24) & 0xf);
+	dpm->nwp = 1 + ((dpm->didr >> 28) & 0xf);
 	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
 	dpm->dwp = calloc(dpm->nwp, sizeof *dpm->dwp);
 
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index 5b16e64..ad49b8c 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -59,7 +59,7 @@ struct arm_dpm {
 	struct arm *arm;
 
 	/** Cache of DIDR */
-	uint32_t didr;
+	uint64_t didr;
 
 	/** Invoke before a series of instruction operations */
 	int (*prepare)(struct arm_dpm *);
@@ -67,6 +67,9 @@ struct arm_dpm {
 	/** Invoke after a series of instruction operations */
 	int (*finish)(struct arm_dpm *);
 
+	/** Runs one instruction. */
+	int (*instr_execute)(struct arm_dpm *, uint32_t opcode);
+
 	/* WRITE TO CPU */
 
 	/** Runs one instruction, writing data to DCC before execution. */
diff --git a/src/target/armv8.c b/src/target/armv8.c
index e516518..537dadb 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -38,6 +38,105 @@
 #include "target.h"
 #include "target_type.h"
 
+static const char * const armv8_state_strings[] = {
+	"ARM", "Thumb", "Jazelle", "ThumbEE", "ARM64",
+};
+
+static const struct {
+	const char *name;
+	unsigned psr;
+	/* For user and system modes, these list indices for all registers.
+	 * otherwise they're just indices for the shadow registers and SPSR.
+	 */
+	unsigned short n_indices;
+	const uint8_t *indices;
+} armv8_mode_data[] = {
+	/* These special modes are currently only supported
+	 * by ARMv6M and ARMv7M profiles */
+	{
+		.name = "EL0T",
+		.psr = ARMV8_64_EL0T,
+	},
+	{
+		.name = "EL1T",
+		.psr = ARMV8_64_EL1T,
+	},
+	{
+		.name = "EL1H",
+		.psr = ARMV8_64_EL1H,
+	},
+	{
+		.name = "EL2T",
+		.psr = ARMV8_64_EL2T,
+	},
+	{
+		.name = "EL2H",
+		.psr = ARMV8_64_EL2H,
+	},
+	{
+		.name = "EL3T",
+		.psr = ARMV8_64_EL3T,
+	},
+	{
+		.name = "EL3H",
+		.psr = ARMV8_64_EL3H,
+	},
+};
+
+/** Map PSR mode bits to the name of an ARM processor operating mode. */
+const char *armv8_mode_name(unsigned psr_mode)
+{
+	for (unsigned i = 0; i < ARRAY_SIZE(armv8_mode_data); i++) {
+		if (armv8_mode_data[i].psr == psr_mode)
+			return armv8_mode_data[i].name;
+	}
+	LOG_ERROR("unrecognized psr mode: %#02x", 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_core_reg(struct target *target, struct reg *r,
 	int num, enum arm_mode mode)
 {
@@ -86,6 +185,81 @@ static int armv8_write_core_reg(struct target *target, struct reg *r,
 	return ERROR_OK;
 }
 #endif
+/**
+ * Configures host-side ARM records to reflect the specified CPSR.
+ * Later, code can use arm_reg_current() to map register numbers
+ * according to how they are exposed by this mode.
+ */
+void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
+{
+	uint32_t mode = cpsr & 0x1F;
+
+	/* NOTE:  this may be called very early, before the register
+	 * cache is set up.  We can't defend against many errors, in
+	 * particular against CPSRs that aren't valid *here* ...
+	 */
+	if (arm->cpsr) {
+		buf_set_u32(arm->cpsr->value, 0, 32, cpsr);
+		arm->cpsr->valid = 1;
+		arm->cpsr->dirty = 0;
+	}
+
+	/* 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 & (1 << 5)) {	/* T */
+			if (cpsr & (1 << 24)) { /* J */
+				LOG_WARNING("ThumbEE -- incomplete support");
+				state = ARM_STATE_THUMB_EE;
+			} else
+				state = ARM_STATE_THUMB;
+		} else {
+			if (cpsr & (1 << 24)) { /* J */
+				LOG_ERROR("Jazelle state handling is BROKEN!");
+				state = ARM_STATE_JAZELLE;
+			} else
+				state = ARM_STATE_ARM;
+		}
+	}
+	arm->core_state = state;
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		switch (mode) {
+			case SYSTEM_AAR64_MODE_EL0t:
+				arm->core_mode = ARMV8_64_EL0T;
+			break;
+			case SYSTEM_AAR64_MODE_EL1t:
+				arm->core_mode = ARMV8_64_EL0T;
+			break;
+			case SYSTEM_AAR64_MODE_EL1h:
+				arm->core_mode = ARMV8_64_EL1H;
+			break;
+			case SYSTEM_AAR64_MODE_EL2t:
+				arm->core_mode = ARMV8_64_EL2T;
+			break;
+			case SYSTEM_AAR64_MODE_EL2h:
+				arm->core_mode = ARMV8_64_EL2H;
+			break;
+			case SYSTEM_AAR64_MODE_EL3t:
+				arm->core_mode = ARMV8_64_EL3T;
+			break;
+			case SYSTEM_AAR64_MODE_EL3h:
+				arm->core_mode = ARMV8_64_EL3H;
+			break;
+			default:
+				LOG_DEBUG("unknow mode 0x%x", (unsigned) (mode));
+			break;
+		}
+	} else {
+		arm->core_mode = mode;
+	}
+
+	LOG_DEBUG("set CPSR %#8.8x: %s mode, %s state", (unsigned) cpsr,
+		armv8_mode_name(arm->core_mode),
+		armv8_state_strings[arm->core_state]);
+}
 
 static void armv8_show_fault_registers(struct target *target)
 {
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 47e3668..ccb205c 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -23,7 +23,7 @@
 #include "arm.h"
 #include "armv4_5_mmu.h"
 #include "armv4_5_cache.h"
-#include "arm_dpm.h"
+#include "armv8_dpm.h"
 
 enum {
 	ARMV8_R0,
@@ -198,6 +198,8 @@ int armv8_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val);
 int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 		struct armv8_cache_common *armv8_cache);
 
+void armv8_set_cpsr(struct arm *arm, uint32_t cpsr);
+
 extern const struct command_registration armv8_command_handlers[];
 
 #endif
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
new file mode 100644
index 0000000..dc6682b
--- /dev/null
+++ b/src/target/armv8_dpm.c
@@ -0,0 +1,985 @@
+/*
+ * Copyright (C) 2009 by David Brownell
+ *
+ * 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.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "arm.h"
+#include "armv8.h"
+#include "armv8_dpm.h"
+#include <jtag/jtag.h>
+#include "register.h"
+#include "breakpoints.h"
+#include "target_type.h"
+#include "armv8_opcodes.h"
+
+
+/**
+ * @file
+ * Implements various ARM DPM operations using architectural debug registers.
+ * These routines layer over core-specific communication methods to cope with
+ * implementation differences between cores like ARM1136 and Cortex-A8.
+ *
+ * The "Debug Programmers' Model" (DPM) for ARMv6 and ARMv7 is defined by
+ * Part C (Debug Architecture) of the ARM Architecture Reference Manual,
+ * ARMv7-A and ARMv7-R edition (ARM DDI 0406B).  In OpenOCD, DPM operations
+ * are abstracted through internal programming interfaces to share code and
+ * to minimize needless differences in debug behavior between cores.
+ */
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Coprocessor support
+ */
+
+/* Read coprocessor */
+static int dpmv8_mrc(struct target *target, int cpnum,
+	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t *value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("MRC p%d, %d, r0, c%d, c%d, %d", cpnum,
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
+
+	/* read coprocessor register into R0; return via DCC */
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2)),
+			value);
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int dpmv8_mcr(struct target *target, int cpnum,
+	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("MCR p%d, %d, r0, c%d, c%d, %d", cpnum,
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
+
+	/* read DCC into r0; then write coprocessor register from R0 */
+	retval = dpm->instr_write_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2)),
+			value);
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int dpmv8_mrs(struct target *target, uint32_t op0,
+	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t *value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval;
+	uint32_t op_code;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+	op_code = ((op0 & 0x3) << 19 | (op1 & 0x7) << 16 | (CRn & 0xF) << 12 |\
+				(CRm & 0xF) << 8 | (op2 & 0x7) << 5);
+	op_code >>= 5;
+	LOG_DEBUG("MRS p%d, %d, r0, c%d, c%d, %d", (int)op0,
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
+	/* read coprocessor register into R0; return via DCC */
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV8_MRS(op_code, 0),
+			value);
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int dpmv8_msr(struct target *target, uint32_t op0,
+	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
+	uint32_t value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval;
+	uint32_t op_code;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	op_code = ((op0 & 0x3) << 19 | (op1 & 0x7) << 16 | (CRn & 0xF) << 12 |\
+				(CRm & 0xF) << 8 | (op2 & 0x7) << 5);
+	op_code >>= 5;
+	LOG_DEBUG("MSR p%d, %d, r0, c%d, c%d, %d", (int)op0,
+		(int) op1, (int) CRn,
+		(int) CRm, (int) op2);
+
+	/* read DCC into r0; then write coprocessor register from R0 */
+	retval = dpm->instr_write_data_r0(dpm,
+			ARMV8_MSR_GP(op_code, 0),
+			value);
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Register access utilities
+ */
+
+/* Toggles between recorded core mode (USR, SVC, etc) and a temporary one.
+ * Routines *must* restore the original mode before returning!!
+ */
+int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
+{
+	int retval;
+	uint32_t cpsr;
+
+	/* restore previous mode */
+	if (mode == ARM_MODE_ANY)
+		cpsr = buf_get_u32(dpm->arm->cpsr->value, 0, 32);
+
+	/* else force to the specified mode */
+	else
+		cpsr = mode >> 4;
+
+	switch ((cpsr & 0xC) >> 2) {
+			case SYSTEM_CUREL_EL1:
+				retval = dpm->instr_execute(dpm, ARMV8_DCPS1(11));
+				if (retval != ERROR_OK)
+					return retval;
+				break;
+			case SYSTEM_CUREL_EL2:
+				retval = dpm->instr_execute(dpm, ARMV8_DCPS2(11));
+				if (retval != ERROR_OK)
+					return retval;
+				break;
+			break;
+			case SYSTEM_CUREL_EL3:
+				retval = dpm->instr_execute(dpm, ARMV8_DCPS3(11));
+				if (retval != ERROR_OK)
+					return retval;
+				break;
+			break;
+			default:
+				LOG_DEBUG("unknow mode 0x%x", (unsigned) ((cpsr & 0xC) >> 2));
+				break;
+	}
+
+
+	retval = dpm->instr_write_data_r0(dpm, ARMV8_MSR_DSPSR(0), cpsr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (dpm->instr_cpsr_sync)
+		retval = dpm->instr_cpsr_sync(dpm);
+
+	return retval;
+}
+
+/* just read the register -- rely on the core mode being right */
+static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	uint32_t value;
+	uint64_t value_64;
+	int retval = ERROR_FAIL;
+
+	switch (regnum) {
+		case 0 ... 30:
+			retval = dpm->instr_read_data_dcc_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum),
+				&value_64);
+			break;
+		case 31:
+			retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MOVFSP_64(0),
+				&value_64);
+			break;
+		case 32:
+			retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS_DLR(0),
+				&value_64);
+			break;
+		case 33:
+			retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_DSPSR(0),
+				&value);
+		default:
+			LOG_DEBUG("READ: %s fail", r->name);
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->valid = true;
+		r->dirty = false;
+		buf_set_u64(r->value, 0, 32, value_64);
+		if (r->size == 64)
+			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
+		else
+			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+	}
+	return retval;
+}
+
+/* just write the register -- rely on the core mode being right */
+static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval = ERROR_FAIL;
+	uint32_t value = 0xFFFFFFFF;
+	uint64_t value_64 = 0xFFFFFFFFFFFFFFFF;
+
+	switch (regnum) {
+		case 0 ... 30:
+			value_64 = buf_get_u64(r->value, 0, 64);
+			retval = dpm->instr_write_data_dcc_64(dpm,
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
+				value_64);
+			break;
+		case 31:
+			value_64 = buf_get_u64(r->value, 0, 64);
+			retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MOVTSP_64(0),
+				value_64);
+			break;
+		case 32:
+			value_64 = buf_get_u64(r->value, 0, 64);
+			retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_DLR(0),
+				value_64);
+			break;
+		case 33:
+			value = buf_get_u32(r->value, 0, 32);
+			retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_DSPSR(0),
+				value);
+		default:
+			LOG_DEBUG("write: %s fail", r->name);
+			break;
+	}
+
+
+	if (retval == ERROR_OK) {
+		r->dirty = false;
+		if (r->size == 64)
+			LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long) value_64);
+		else
+			LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value);
+	}
+
+	return retval;
+}
+
+/**
+ * Read basic registers of the the current context:  R0 to R15, and CPSR;
+ * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
+ * In normal operation this is called on entry to halting debug state,
+ * possibly after some other operations supporting restore of debug state
+ * or making sure the CPU is fully idle (drain write buffer, etc).
+ */
+int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
+{
+	struct arm *arm = dpm->arm;
+	uint32_t cpsr;
+	int retval;
+	struct reg *r;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* read R0 first (it's used for scratch), then CPSR */
+	r = arm->core_cache->reg_list + 0;
+	if (!r->valid) {
+		retval = dpmv8_read_reg(dpm, r, 0);
+		if (retval != ERROR_OK)
+			goto fail;
+	}
+	r->dirty = true;
+	/* read cpsr to r0 and get it back */
+	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS_DSPSR(0), &cpsr);
+	if (retval != ERROR_OK)
+		goto fail;
+
+	/* update core mode and state, plus shadow mapping for R8..R14 */
+	armv8_set_cpsr(arm, cpsr);
+
+	/* REVISIT we can probably avoid reading R1..R14, saving time... */
+	for (unsigned i = 1; i < arm->core_cache->num_regs ; i++) {
+		r = armv8_reg_current(arm, i);
+		if (r->valid)
+			continue;
+
+		retval = dpmv8_read_reg(dpm, r, i);
+		if (retval != ERROR_OK)
+			goto fail;
+	}
+
+	/* NOTE: SPSR ignored (if it's even relevant). */
+
+	/* REVISIT the debugger can trigger various exceptions.  See the
+	 * ARMv7A architecture spec, section C5.7, for more info about
+	 * what defenses are needed; v6 debug has the most issues.
+	 */
+
+fail:
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+/* Avoid needless I/O ... leave breakpoints and watchpoints alone
+ * unless they're removed, or need updating because of single-stepping
+ * or running debugger code.
+ */
+static int dpmv8_maybe_update_bpwp(struct arm_dpm *dpm, bool bpwp,
+	struct dpm_bpwp *xp, int *set_p)
+{
+	int retval = ERROR_OK;
+	bool disable;
+
+	if (!set_p) {
+		if (!xp->dirty)
+			goto done;
+		xp->dirty = false;
+		/* removed or startup; we must disable it */
+		disable = true;
+	} else if (bpwp) {
+		if (!xp->dirty)
+			goto done;
+		/* disabled, but we must set it */
+		xp->dirty = disable = false;
+		*set_p = true;
+	} else {
+		if (!*set_p)
+			goto done;
+		/* set, but we must temporarily disable it */
+		xp->dirty = disable = true;
+		*set_p = false;
+	}
+
+	if (disable)
+		retval = dpm->bpwp_disable(dpm, xp->number);
+	else
+		retval = dpm->bpwp_enable(dpm, xp->number,
+				xp->address, xp->control);
+
+	if (retval != ERROR_OK)
+		LOG_ERROR("%s: can't %s HW %spoint %d",
+			disable ? "disable" : "enable",
+			target_name(dpm->arm->target),
+			(xp->number < 16) ? "break" : "watch",
+			xp->number & 0xf);
+done:
+	return retval;
+}
+
+static int dpmv8_add_breakpoint(struct target *target, struct breakpoint *bp);
+
+/**
+ * Writes all modified core registers for all processor modes.  In normal
+ * operation this is called on exit from halting debug state.
+ *
+ * @param dpm: represents the processor
+ * @param bpwp: true ensures breakpoints and watchpoints are set,
+ *	false ensures they are cleared
+ */
+int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
+{
+	struct arm *arm = dpm->arm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* If we're managing hardware breakpoints for this core, enable
+	 * or disable them as requested.
+	 *
+	 * REVISIT We don't yet manage them for ANY cores.  Eventually
+	 * we should be able to assume we handle them; but until then,
+	 * cope with the hand-crafted breakpoint code.
+	 */
+	if (arm->target->type->add_breakpoint == dpmv8_add_breakpoint) {
+		for (unsigned i = 0; i < dpm->nbp; i++) {
+			struct dpm_bp *dbp = dpm->dbp + i;
+			struct breakpoint *bp = dbp->bp;
+
+			retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dbp->bpwp,
+					bp ? &bp->set : NULL);
+			if (retval != ERROR_OK)
+				goto done;
+		}
+	}
+
+	/* enable/disable watchpoints */
+	for (unsigned i = 0; i < dpm->nwp; i++) {
+		struct dpm_wp *dwp = dpm->dwp + i;
+		struct watchpoint *wp = dwp->wp;
+
+		retval = dpmv8_maybe_update_bpwp(dpm, bpwp, &dwp->bpwp,
+				wp ? &wp->set : NULL);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+	/* NOTE:  writes to breakpoint and watchpoint registers might
+	 * be queued, and need (efficient/batched) flushing later.
+	 */
+
+	/* Scan the registers until we find one that's both dirty and
+	 * eligible for flushing.  Flush that and everything else that
+	 * shares the same core mode setting.  Typically this won't
+	 * actually find anything to do...
+	 */
+
+	/* check everything except our scratch register R0 */
+	for (unsigned i = 1; i < cache->num_regs; i++) {
+		struct arm_reg *r;
+		unsigned regnum;
+
+		/* also skip PC, CPSR, and non-dirty */
+		if (i == (arm->core_cache->num_regs - 2))
+			continue;
+		if (arm->cpsr == cache->reg_list + i)
+			continue;
+		if (!cache->reg_list[i].dirty)
+			continue;
+
+		r = cache->reg_list[i].arch_info;
+		regnum = r->num;
+
+		retval = dpmv8_write_reg(dpm,
+				&cache->reg_list[i],
+				regnum);
+		if (retval != ERROR_OK)
+			goto done;
+	}
+
+
+	/* Restore original CPSR ... assuming either that we changed it,
+	 * or it's dirty.  Must write PC to ensure the return address is
+	 * defined, and must not write it before CPSR.
+	 */
+	retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+	if (retval != ERROR_OK)
+		goto done;
+	arm->cpsr->dirty = false;
+
+	retval = dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2));
+	if (retval != ERROR_OK)
+		goto done;
+	arm->pc->dirty = false;
+
+	/* flush R0 -- it's *very* dirty by now */
+	retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
+	if (retval != ERROR_OK)
+		goto done;
+	cache->reg_list[0].dirty = false;
+
+	/* (void) */ dpm->finish(dpm);
+done:
+	return retval;
+}
+
+/*
+ * Standard ARM register accessors ... there are three methods
+ * in "struct arm", to support individual read/write and bulk read
+ * of registers.
+ */
+
+static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
+	int regnum, enum arm_mode mode)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = target_to_arm(target)->dpm;
+	int retval;
+	int max = arm->core_cache->num_regs;
+
+	if (regnum < 0 || regnum > max)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* REVISIT what happens if we try to read SPSR in a core mode
+	 * which has no such register?
+	 */
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_read_reg(dpm, r, regnum);
+	if (retval != ERROR_OK)
+		goto fail;
+
+fail:
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
+	int regnum, enum arm_mode mode, uint8_t *value)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = target_to_arm(target)->dpm;
+	int retval;
+	int max = arm->core_cache->num_regs;
+
+	if (regnum < 0 || regnum > max)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	/* REVISIT what happens if we try to write SPSR in a core mode
+	 * which has no such register?
+	 */
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_write_reg(dpm, r, regnum);
+	/* always clean up, regardless of error */
+
+	/* (void) */ dpm->finish(dpm);
+	return retval;
+}
+
+static int armv8_dpm_full_context(struct target *target)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	struct reg_cache *cache = arm->core_cache;
+	int retval;
+	bool did_read;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	do {
+		enum arm_mode mode = ARM_MODE_ANY;
+
+		did_read = false;
+
+		/* We "know" arm_dpm_read_current_registers() was called so
+		 * the unmapped registers (R0..R7, PC, AND CPSR) and some
+		 * view of R8..R14 are current.  We also "know" oddities of
+		 * register mapping: special cases for R8..R12 and SPSR.
+		 *
+		 * Pick some mode with unread registers and read them all.
+		 * Repeat until done.
+		 */
+		for (unsigned i = 0; i < cache->num_regs; i++) {
+			struct arm_reg *r;
+
+			if (cache->reg_list[i].valid)
+				continue;
+			r = cache->reg_list[i].arch_info;
+
+			/* may need to pick a mode and set CPSR */
+			if (!did_read) {
+				did_read = true;
+				mode = r->mode;
+
+				/* For regular (ARM_MODE_ANY) R8..R12
+				 * in case we've entered debug state
+				 * in FIQ mode we need to patch mode.
+				 */
+				if (mode != ARM_MODE_ANY)
+					retval = dpmv8_modeswitch(dpm, mode);
+				else
+					retval = dpmv8_modeswitch(dpm, ARM_MODE_USR);
+
+				if (retval != ERROR_OK)
+					goto done;
+			}
+			if (r->mode != mode)
+				continue;
+
+			/* CPSR was read, so "R16" must mean SPSR */
+			retval = dpmv8_read_reg(dpm,
+					&cache->reg_list[i],
+					(r->num == 16) ? 17 : r->num);
+			if (retval != ERROR_OK)
+				goto done;
+		}
+
+	} while (did_read);
+
+	retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+	/* (void) */ dpm->finish(dpm);
+done:
+	return retval;
+}
+
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Breakpoint and Watchpoint support.
+ *
+ * Hardware {break,watch}points are usually left active, to minimize
+ * debug entry/exit costs.  When they are set or cleared, it's done in
+ * batches.  Also, DPM-conformant hardware can update debug registers
+ * regardless of whether the CPU is running or halted ... though that
+ * fact isn't currently leveraged.
+ */
+
+static int dpmv8_bpwp_setup(struct arm_dpm *dpm, struct dpm_bpwp *xp,
+	uint32_t addr, uint32_t length)
+{
+	uint32_t control;
+
+	control = (1 << 0)	/* enable */
+		| (3 << 1);	/* both user and privileged access */
+
+	/* Match 1, 2, or all 4 byte addresses in this word.
+	 *
+	 * FIXME:  v7 hardware allows lengths up to 2 GB for BP and WP.
+	 * Support larger length, when addr is suitably aligned.  In
+	 * particular, allow watchpoints on 8 byte "double" values.
+	 *
+	 * REVISIT allow watchpoints on unaligned 2-bit values; and on
+	 * v7 hardware, unaligned 4-byte ones too.
+	 */
+	switch (length) {
+		case 1:
+			control |= (1 << (addr & 3)) << 5;
+			break;
+		case 2:
+			/* require 2-byte alignment */
+			if (!(addr & 1)) {
+				control |= (3 << (addr & 2)) << 5;
+				break;
+			}
+		/* FALL THROUGH */
+		case 4:
+			/* require 4-byte alignment */
+			if (!(addr & 3)) {
+				control |= 0xf << 5;
+				break;
+			}
+		/* FALL THROUGH */
+		default:
+			LOG_ERROR("unsupported {break,watch}point length/alignment");
+			return ERROR_COMMAND_SYNTAX_ERROR;
+	}
+
+	/* other shared control bits:
+	 * bits 15:14 == 0 ... both secure and nonsecure states (v6.1+ only)
+	 * bit 20 == 0 ... not linked to a context ID
+	 * bit 28:24 == 0 ... not ignoring N LSBs (v7 only)
+	 */
+
+	xp->address = addr & ~3;
+	xp->control = control;
+	xp->dirty = true;
+
+	LOG_DEBUG("BPWP: addr %8.8" PRIx32 ", control %" PRIx32 ", number %d",
+		xp->address, control, xp->number);
+
+	/* hardware is updated in write_dirty_registers() */
+	return ERROR_OK;
+}
+
+static int dpmv8_add_breakpoint(struct target *target, struct breakpoint *bp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+	if (bp->length < 2)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	if (!dpm->bpwp_enable)
+		return retval;
+
+	/* FIXME we need a generic solution for software breakpoints. */
+	if (bp->type == BKPT_SOFT)
+		LOG_DEBUG("using HW bkpt, not SW...");
+
+	for (unsigned i = 0; i < dpm->nbp; i++) {
+		if (!dpm->dbp[i].bp) {
+			retval = dpmv8_bpwp_setup(dpm, &dpm->dbp[i].bpwp,
+					bp->address, bp->length);
+			if (retval == ERROR_OK)
+				dpm->dbp[i].bp = bp;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+static int dpmv8_remove_breakpoint(struct target *target, struct breakpoint *bp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned i = 0; i < dpm->nbp; i++) {
+		if (dpm->dbp[i].bp == bp) {
+			dpm->dbp[i].bp = NULL;
+			dpm->dbp[i].bpwp.dirty = true;
+
+			/* hardware is updated in write_dirty_registers() */
+			retval = ERROR_OK;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+static int dpmv8_watchpoint_setup(struct arm_dpm *dpm, unsigned index_t,
+	struct watchpoint *wp)
+{
+	int retval;
+	struct dpm_wp *dwp = dpm->dwp + index_t;
+	uint32_t control;
+
+	/* this hardware doesn't support data value matching or masking */
+	if (wp->value || wp->mask != ~(uint32_t)0) {
+		LOG_DEBUG("watchpoint values and masking not supported");
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+	}
+
+	retval = dpmv8_bpwp_setup(dpm, &dwp->bpwp, wp->address, wp->length);
+	if (retval != ERROR_OK)
+		return retval;
+
+	control = dwp->bpwp.control;
+	switch (wp->rw) {
+		case WPT_READ:
+			control |= 1 << 3;
+			break;
+		case WPT_WRITE:
+			control |= 2 << 3;
+			break;
+		case WPT_ACCESS:
+			control |= 3 << 3;
+			break;
+	}
+	dwp->bpwp.control = control;
+
+	dpm->dwp[index_t].wp = wp;
+
+	return retval;
+}
+
+static int dpmv8_add_watchpoint(struct target *target, struct watchpoint *wp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
+	if (dpm->bpwp_enable) {
+		for (unsigned i = 0; i < dpm->nwp; i++) {
+			if (!dpm->dwp[i].wp) {
+				retval = dpmv8_watchpoint_setup(dpm, i, wp);
+				break;
+			}
+		}
+	}
+
+	return retval;
+}
+
+static int dpmv8_remove_watchpoint(struct target *target, struct watchpoint *wp)
+{
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = arm->dpm;
+	int retval = ERROR_COMMAND_SYNTAX_ERROR;
+
+	for (unsigned i = 0; i < dpm->nwp; i++) {
+		if (dpm->dwp[i].wp == wp) {
+			dpm->dwp[i].wp = NULL;
+			dpm->dwp[i].bpwp.dirty = true;
+
+			/* hardware is updated in write_dirty_registers() */
+			retval = ERROR_OK;
+			break;
+		}
+	}
+
+	return retval;
+}
+
+void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
+{
+	switch (dpm->arm->core_state) {
+		case ARM_STATE_ARM:
+		case ARM_STATE_AARCH64:
+			addr -= 8;
+			break;
+		case ARM_STATE_THUMB:
+		case ARM_STATE_THUMB_EE:
+			addr -= 4;
+			break;
+		case ARM_STATE_JAZELLE:
+			/* ?? */
+			break;
+		default:
+			LOG_DEBUG("Unknow core_state");
+			break;
+	}
+	dpm->wp_pc = addr;
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Other debug and support utilities
+ */
+
+void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
+{
+	struct target *target = dpm->arm->target;
+
+	dpm->dscr = dscr;
+
+	/* Examine debug reason */
+	switch (DSCR_ENTRY(dscr)) {
+		/* FALL THROUGH -- assume a v6 core in abort mode */
+		case DSCRV8_ENTRY_HLT:	/* HALT request from debugger */
+		case DSCRV8_ENTRY_EXT_DEBUG:	/* EDBGRQ */
+			target->debug_reason = DBG_REASON_DBGRQ;
+			break;
+		case DSCRV8_ENTRY_HALT_STEP_EXECLU:	/* HALT step */
+		case DSCRV8_ENTRY_BKPT:	/* SW BKPT */
+		case DSCRV8_ENTRY_RESET_CATCH:	/* Reset catch */
+		case DSCRV8_ENTRY_OS_UNLOCK:  /*OS unlock catch*/
+		case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
+		case DSCRV8_ENTRY_HALT_STEP_NORMAL: /* Halt step*/
+		case DSCRV8_ENTRY_SW_ACCESS_DBG: /*SW access dbg register*/
+			target->debug_reason = DBG_REASON_BREAKPOINT;
+			break;
+		case DSCRV8_ENTRY_WATCHPOINT:	/* asynch watchpoint */
+			target->debug_reason = DBG_REASON_WATCHPOINT;
+			break;
+		default:
+			target->debug_reason = DBG_REASON_UNDEFINED;
+			break;
+	}
+
+}
+
+/*----------------------------------------------------------------------*/
+
+/*
+ * Setup and management support.
+ */
+
+/**
+ * Hooks up this DPM to its associated target; call only once.
+ * Initially this only covers the register cache.
+ *
+ * Oh, and watchpoints.  Yeah.
+ */
+int armv8_dpm_setup(struct arm_dpm *dpm)
+{
+	struct arm *arm = dpm->arm;
+	struct target *target = arm->target;
+	struct reg_cache *cache;
+	arm->dpm = dpm;
+
+	/* register access setup */
+	arm->full_context = armv8_dpm_full_context;
+	arm->read_core_reg = armv8_dpm_read_core_reg;
+	arm->write_core_reg = armv8_dpm_write_core_reg;
+
+	cache = armv8_build_reg_cache(target);
+	if (!cache)
+		return ERROR_FAIL;
+
+	/* coprocessor access setup */
+	arm->mrc = dpmv8_mrc;
+	arm->mcr = dpmv8_mcr;
+	arm->mrs = dpmv8_mrs;
+	arm->msr = dpmv8_msr;
+	/* breakpoint setup -- optional until it works everywhere */
+	if (!target->type->add_breakpoint) {
+		target->type->add_breakpoint = dpmv8_add_breakpoint;
+		target->type->remove_breakpoint = dpmv8_remove_breakpoint;
+	}
+
+	/* watchpoint setup */
+	target->type->add_watchpoint = dpmv8_add_watchpoint;
+	target->type->remove_watchpoint = dpmv8_remove_watchpoint;
+
+	/* FIXME add vector catch support */
+
+	dpm->nbp = 1 + ((dpm->didr >> 12) & 0xf);
+	dpm->dbp = calloc(dpm->nbp, sizeof *dpm->dbp);
+
+	dpm->nwp = 1 + ((dpm->didr >> 20) & 0xf);
+	dpm->dwp = calloc(dpm->nwp, sizeof *dpm->dwp);
+
+	if (!dpm->dbp || !dpm->dwp) {
+		free(dpm->dbp);
+		free(dpm->dwp);
+		return ERROR_FAIL;
+	}
+
+	LOG_INFO("%s: hardware has %d breakpoints, %d watchpoints",
+		target_name(target), dpm->nbp, dpm->nwp);
+
+	/* REVISIT ... and some of those breakpoints could match
+	 * execution context IDs...
+	 */
+
+	return ERROR_OK;
+}
+
+/**
+ * Reinitializes DPM state at the beginning of a new debug session
+ * or after a reset which may have affected the debug module.
+ */
+int armv8_dpm_initialize(struct arm_dpm *dpm)
+{
+	/* Disable all breakpoints and watchpoints at startup. */
+	if (dpm->bpwp_disable) {
+		unsigned i;
+
+		for (i = 0; i < dpm->nbp; i++) {
+			dpm->dbp[i].bpwp.number = i;
+			(void) dpm->bpwp_disable(dpm, i);
+		}
+		for (i = 0; i < dpm->nwp; i++) {
+			dpm->dwp[i].bpwp.number = 16 + i;
+			(void) dpm->bpwp_disable(dpm, 16 + i);
+		}
+	} else
+		LOG_WARNING("%s: can't disable breakpoints and watchpoints",
+			target_name(dpm->arm->target));
+
+	return ERROR_OK;
+}
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
new file mode 100644
index 0000000..416f4df
--- /dev/null
+++ b/src/target/armv8_dpm.h
@@ -0,0 +1,115 @@
+/*
+ * Copyright (C) 2009 by David Brownell
+ *
+ * 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.
+ */
+
+#ifndef OPENOCD_TARGET_ARMV8_DPM_H
+#define OPENOCD_TARGET_ARMV8_DPM_H
+
+#include "arm_dpm.h"
+
+/* forward-declare struct armv8_common */
+struct armv8_common;
+
+/**
+ * This wraps an implementation of DPM primitives.  Each interface
+ * provider supplies a structure like this, which is the glue between
+ * upper level code and the lower level hardware access.
+ *
+ * It is a PRELIMINARY AND INCOMPLETE set of primitives, starting with
+ * support for CPU register access.
+ */
+int armv8_dpm_setup(struct arm_dpm *dpm);
+int armv8_dpm_initialize(struct arm_dpm *dpm);
+
+int armv8_dpm_read_current_registers(struct arm_dpm *);
+int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
+
+
+int armv8_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);
+
+void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
+
+/* DSCR bits; see ARMv7a arch spec section C10.3.1.
+ * Not all v7 bits are valid in v6.
+ */
+#define DSCR_DEBUG_STATUS_MASK		(0x1F <<  0)
+#define DSCR_ERR					(0x1 <<  6)
+#define DSCR_SYS_ERROR_PEND			(0x1 <<  7)
+#define DSCR_CUR_EL					(0x3 <<  8)
+#define DSCR_EL_STATUS_MASK			(0xF << 10)
+#define DSCR_HDE					(0x1 << 14)
+#define DSCR_SDD					(0x1 << 16)
+#define DSCR_NON_SECURE             (0x1 << 18)
+#define DSCR_MA						(0x1 << 20)
+#define DSCR_TDA					(0x1 << 21)
+#define DSCR_INTDIS_MASK			(0x3 << 22)
+#define DSCR_ITE					(0x1 << 24)
+#define DSCR_PIPE_ADVANCE           (0x1 << 25)
+#define DSCR_TXU					(0x1 << 26)
+#define DSCR_RTO					(0x1 << 27) /* bit 28 is reserved */
+#define DSCR_ITO					(0x1 << 28)
+#define DSCR_DTR_TX_FULL            (0x1 << 29)
+#define DSCR_DTR_RX_FULL            (0x1 << 30) /* bit 31 is reserved */
+
+
+
+/* Methods of entry into debug mode */
+#define DSCRV8_ENTRY_NON_DEBUG			(0x2)
+#define DSCRV8_ENTRY_RESTARTING			(0x1)
+#define DSCRV8_ENTRY_BKPT				(0x7)
+#define DSCRV8_ENTRY_EXT_DEBUG			(0x13)
+#define DSCRV8_ENTRY_HALT_STEP_NORMAL	(0x1B)
+#define DSCRV8_ENTRY_HALT_STEP_EXECLU	(0x1F)
+#define DSCRV8_ENTRY_OS_UNLOCK			(0x23)
+#define DSCRV8_ENTRY_RESET_CATCH		(0x27)
+#define DSCRV8_ENTRY_WATCHPOINT			(0x2B)
+#define DSCRV8_ENTRY_HLT				(0x2F)
+#define DSCRV8_ENTRY_SW_ACCESS_DBG		(0x33)
+#define DSCRV8_ENTRY_EXCEPTION_CATCH	(0x37)
+#define DSCRV8_ENTRY_HALT_STEP			(0x3B)
+#define DSCRV8_HALT_MASK			(0x3C)
+
+/*DRCR registers*/
+#define DRCR_CSE				(1 << 2)
+#define DRCR_CSPA				(1 << 3)
+#define DRCR_CBRRQ				(1 << 4)
+
+
+/* DTR modes */
+#define DSCR_EXT_DCC_NON_BLOCKING     (0x0 << 20)
+#define DSCR_EXT_DCC_STALL_MODE       (0x1 << 20)
+#define DSCR_EXT_DCC_FAST_MODE        (0x2 << 20)  /* bits 22, 23 are reserved */
+
+
+/* DRCR (debug run control register) bits */
+#define DRCR_HALT				(1 << 0)
+#define DRCR_RESTART			(1 << 1)
+#define DRCR_CLEAR_EXCEPTIONS	(1 << 2)
+
+/* PRCR (processor debug status register) bits */
+#define PRSR_PU					(1 << 0)
+#define PRSR_SPD				(1 << 1)
+#define PRSR_RESET				(1 << 2)
+#define PRSR_SR					(1 << 3)
+#define PRSR_HALT				(1 << 4)
+#define PRSR_OSLK				(1 << 5)
+#define PRSR_DLK				(1 << 6)
+#define PRSR_EDAD				(1 << 7)
+#define PRSR_SDAD				(1 << 8)
+#define PRSR_EPMAD				(1 << 9)
+#define PRSR_SPMAD				(1 << 10)
+#define PRSR_SDR				(1 << 11)
+
+void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
+
+#endif /* OPENOCD_TARGET_ARM_DPM_H */
-- 
cgit v1.1


From db97bb4a9dbc210b599dc5acc58696ea22eee0da Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 1 Sep 2016 21:14:46 +0200
Subject: aarch64: make DCC read/write functions operate on struct armv8_common

Change the signature of aarch64_(read|write)_dcc[_64] to take a
"struct armv8_common *" as the context to operate on. No functional
change.

Change-Id: Ie501113f65ea22aff2eee173ec717f6908a63494
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 61 ++++++++++++++++++++++++++--------------------------
 1 file changed, 31 insertions(+), 30 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index be02eb8..93db496 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -234,26 +234,26 @@ static inline struct aarch64_common *dpm_to_a8(struct arm_dpm *dpm)
 	return container_of(dpm, struct aarch64_common, armv8_common.dpm);
 }
 
-static int aarch64_write_dcc(struct aarch64_common *a8, uint32_t data)
+static int aarch64_write_dcc(struct armv8_common *armv8, uint32_t data)
 {
 	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
-	return mem_ap_write_u32(a8->armv8_common.debug_ap,
-				a8->armv8_common.debug_base + CPUDBG_DTRRX, data);
+	return mem_ap_write_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DTRRX, data);
 }
 
-static int aarch64_write_dcc_64(struct aarch64_common *a8, uint64_t data)
+static int aarch64_write_dcc_64(struct armv8_common *armv8, uint64_t data)
 {
 	int ret;
-	LOG_DEBUG("write DCC 0x%08" PRIx32, (unsigned)data);
-	LOG_DEBUG("write DCC 0x%08" PRIx32, (unsigned)(data >> 32));
-	ret = mem_ap_write_u32(a8->armv8_common.debug_ap,
-			       a8->armv8_common.debug_base + CPUDBG_DTRRX, data);
-	ret += mem_ap_write_u32(a8->armv8_common.debug_ap,
-				a8->armv8_common.debug_base + CPUDBG_DTRTX, data >> 32);
+	LOG_DEBUG("write DCC Low word0x%08" PRIx32, (unsigned)data);
+	LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
+	ret = mem_ap_write_u32(armv8->debug_ap,
+			       armv8->debug_base + CPUDBG_DTRRX, data);
+	ret += mem_ap_write_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DTRTX, data >> 32);
 	return ret;
 }
 
-static int aarch64_read_dcc(struct aarch64_common *a8, uint32_t *data,
+static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
 	uint32_t *dscr_p)
 {
 	uint32_t dscr = DSCR_INSTR_COMP;
@@ -265,8 +265,8 @@ static int aarch64_read_dcc(struct aarch64_common *a8, uint32_t *data,
 	/* Wait for DTRRXfull */
 	long long then = timeval_ms();
 	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-				a8->armv8_common.debug_base + CPUDBG_DSCR,
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR,
 				&dscr);
 		if (retval != ERROR_OK)
 			return retval;
@@ -276,8 +276,8 @@ static int aarch64_read_dcc(struct aarch64_common *a8, uint32_t *data,
 		}
 	}
 
-	retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-					    a8->armv8_common.debug_base + CPUDBG_DTRTX,
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUDBG_DTRTX,
 					    data);
 	if (retval != ERROR_OK)
 		return retval;
@@ -288,7 +288,8 @@ static int aarch64_read_dcc(struct aarch64_common *a8, uint32_t *data,
 
 	return retval;
 }
-static int aarch64_read_dcc_64(struct aarch64_common *a8, uint64_t *data,
+
+static int aarch64_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 	uint32_t *dscr_p)
 {
 	uint32_t dscr = DSCR_INSTR_COMP;
@@ -301,8 +302,8 @@ static int aarch64_read_dcc_64(struct aarch64_common *a8, uint64_t *data,
 	/* Wait for DTRRXfull */
 	long long then = timeval_ms();
 	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-				a8->armv8_common.debug_base + CPUDBG_DSCR,
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR,
 				&dscr);
 		if (retval != ERROR_OK)
 			return retval;
@@ -312,14 +313,14 @@ static int aarch64_read_dcc_64(struct aarch64_common *a8, uint64_t *data,
 		}
 	}
 
-	retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-					    a8->armv8_common.debug_base + CPUDBG_DTRTX,
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUDBG_DTRTX,
 					    (uint32_t *)data);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-					    a8->armv8_common.debug_base + CPUDBG_DTRRX,
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUDBG_DTRRX,
 					    &higher);
 	if (retval != ERROR_OK)
 		return retval;
@@ -395,7 +396,7 @@ static int aarch64_instr_write_data_dcc(struct arm_dpm *dpm,
 	int retval;
 	uint32_t dscr = DSCR_INSTR_COMP;
 
-	retval = aarch64_write_dcc(a8, data);
+	retval = aarch64_write_dcc(&a8->armv8_common, data);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -412,7 +413,7 @@ static int aarch64_instr_write_data_dcc_64(struct arm_dpm *dpm,
 	int retval;
 	uint32_t dscr = DSCR_INSTR_COMP;
 
-	retval = aarch64_write_dcc_64(a8, data);
+	retval = aarch64_write_dcc_64(&a8->armv8_common, data);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -429,7 +430,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 	uint32_t dscr = DSCR_INSTR_COMP;
 	int retval;
 
-	retval = aarch64_write_dcc(a8, data);
+	retval = aarch64_write_dcc(&a8->armv8_common, data);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -456,7 +457,7 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 	uint32_t dscr = DSCR_INSTR_COMP;
 	int retval;
 
-	retval = aarch64_write_dcc_64(a8, data);
+	retval = aarch64_write_dcc_64(&a8->armv8_common, data);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -502,7 +503,7 @@ static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return aarch64_read_dcc(a8, data, &dscr);
+	return aarch64_read_dcc(&a8->armv8_common, data, &dscr);
 }
 
 static int aarch64_instr_read_data_dcc_64(struct arm_dpm *dpm,
@@ -520,7 +521,7 @@ static int aarch64_instr_read_data_dcc_64(struct arm_dpm *dpm,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return aarch64_read_dcc_64(a8, data, &dscr);
+	return aarch64_read_dcc_64(&a8->armv8_common, data, &dscr);
 }
 
 static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
@@ -546,7 +547,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return aarch64_read_dcc(a8, data, &dscr);
+	return aarch64_read_dcc(&a8->armv8_common, data, &dscr);
 }
 
 static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
@@ -572,7 +573,7 @@ static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
 	if (retval != ERROR_OK)
 		return retval;
 
-	return aarch64_read_dcc_64(a8, data, &dscr);
+	return aarch64_read_dcc_64(&a8->armv8_common, data, &dscr);
 }
 
 static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
-- 
cgit v1.1


From 53573f7860cf33d7ef01608f4894f68de7ab8c73 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 15 Sep 2016 12:12:25 +0200
Subject: aarch64: Implement MA mode for bulk memory reads and writes

- 64bit addresses are supported
- Aarch32 state is supported

Change-Id: I8c37fa166954d09195d08c6963b8017194e350f5
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 249 ++++++++++++++++++++++++++++++++++-----------------
 1 file changed, 165 insertions(+), 84 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 93db496..33c7daa 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -26,7 +26,7 @@
 #include "register.h"
 #include "target_request.h"
 #include "target_type.h"
-#include "arm_opcodes.h"
+#include "armv8_opcodes.h"
 #include <helper/time_support.h>
 
 static int aarch64_poll(struct target *target);
@@ -43,7 +43,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_ab_memory(struct target *target,
+static int aarch64_read_apb_ap_memory(struct target *target,
 	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t data);
@@ -1673,7 +1673,7 @@ static int aarch64_deassert_reset(struct target *target)
 	return ERROR_OK;
 }
 
-static int aarch64_write_apb_ab_memory(struct target *target,
+static int aarch64_write_apb_ap_memory(struct target *target,
 	uint64_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
@@ -1688,7 +1688,6 @@ static int aarch64_write_apb_ab_memory(struct target *target,
 	struct reg *reg;
 	uint32_t dscr;
 	uint8_t *tmp_buff = NULL;
-	uint32_t i = 0;
 
 	LOG_DEBUG("Writing APB-AP memory address 0x%" PRIx64 " size %"  PRIu32 " count%"  PRIu32,
 			  address, size, count);
@@ -1711,15 +1710,13 @@ static int aarch64_write_apb_ab_memory(struct target *target,
 	reg->dirty = true;
 
 	/*  clear any abort  */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap, armv8->debug_base + CPUDBG_DRCR, 1<<2);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DRCR, DRCR_CSE);
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* This algorithm comes from either :
-	 * Cortex-A8 TRM Example 12-25
-	 * Cortex-R4 TRM Example 11-26
-	 * (slight differences)
-	 */
+
+	/* 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.
@@ -1732,7 +1729,7 @@ static int aarch64_write_apb_ab_memory(struct target *target,
 		/* First bytes not aligned - read the 32 bit word to avoid corrupting
 		 * the other bytes in the word.
 		 */
-		retval = aarch64_read_apb_ab_memory(target, (address & ~0x3), 4, 1, tmp_buff);
+		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3), 4, 1, tmp_buff);
 		if (retval != ERROR_OK)
 			goto error_free_buff_w;
 	}
@@ -1743,7 +1740,7 @@ static int aarch64_write_apb_ab_memory(struct target *target,
 
 		/* Read the last word to avoid corruption during 32 bit write */
 		int mem_offset = (total_u32-1) * 4;
-		retval = aarch64_read_apb_ab_memory(target, (address & ~0x3) + mem_offset, 4, 1, &tmp_buff[mem_offset]);
+		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;
 	}
@@ -1759,48 +1756,54 @@ static int aarch64_write_apb_ab_memory(struct target *target,
 	if (retval != ERROR_OK)
 		goto error_free_buff_w;
 
-	/* Set DTR mode to Normal*/
-	dscr = (dscr & ~DSCR_EXT_DCC_MASK) | DSCR_EXT_DCC_NON_BLOCKING;
+	/* Set Normal access mode  */
+	dscr = (dscr & ~DSCR_MA);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUDBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_free_buff_w;
 
-	if (size > 4) {
-		LOG_WARNING("reading size >4 bytes not yet supported");
-		goto error_unset_dtr_w;
-	}
+	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 */
+		retval += aarch64_write_dcc_64(armv8, address & ~0x3ULL);
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
+		retval += aarch64_exec_opcode(target,
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+	} else {
+		/* Write R0 with value 'address' using write procedure */
+		/* Step 1.a+b - Write the address for read access into DBGDTRRX */
+		retval += aarch64_write_dcc(armv8, address & ~0x3ULL);
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
+		retval += aarch64_exec_opcode(target,
+				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), &dscr);
 
-	retval = aarch64_instr_write_data_dcc_64(arm->dpm, 0xd5330401, address+4);
+	}
+	/* Step 1.d   - Change DCC to memory mode */
+	dscr = dscr | DSCR_MA;
+	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, dscr);
 	if (retval != ERROR_OK)
 		goto error_unset_dtr_w;
 
-	dscr = DSCR_INSTR_COMP;
-	while (i < count * size) {
-		uint32_t val;
-
-		memcpy(&val, &buffer[i], size);
-		retval = aarch64_instr_write_data_dcc(arm->dpm, 0xd5330500, val);
-		if (retval != ERROR_OK)
-			goto error_unset_dtr_w;
-
-		retval = aarch64_exec_opcode(target, 0xb81fc020, &dscr);
-		if (retval != ERROR_OK)
-			goto error_unset_dtr_w;
 
-		retval = aarch64_exec_opcode(target, 0x91001021, &dscr);
-		if (retval != ERROR_OK)
-			goto error_unset_dtr_w;
+	/* Step 2.a   - Do the write */
+	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
+					tmp_buff, 4, total_u32, armv8->debug_base + CPUDBG_DTRRX);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_w;
 
-		i += 4;
-	}
+	/* 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 + CPUDBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		goto error_unset_dtr_w;
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUDBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		goto error_free_buff_w;
-	if (dscr & (DSCR_STICKY_ABORT_PRECISE | DSCR_STICKY_ABORT_IMPRECISE)) {
+	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -1816,7 +1819,7 @@ error_unset_dtr_w:
 	/* Unset DTR mode */
 	mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUDBG_DSCR, &dscr);
-	dscr = (dscr & ~DSCR_EXT_DCC_MASK) | DSCR_EXT_DCC_NON_BLOCKING;
+	dscr = (dscr & ~DSCR_MA);
 	mem_ap_write_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUDBG_DSCR, dscr);
 error_free_buff_w:
@@ -1825,19 +1828,23 @@ error_free_buff_w:
 	return ERROR_FAIL;
 }
 
-static int aarch64_read_apb_ab_memory(struct target *target,
+static int aarch64_read_apb_ap_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
 	/* read memory through APB-AP */
-
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
 	struct armv8_common *armv8 = target_to_armv8(target);
 	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, val;
+	uint32_t dscr;
 	uint8_t *tmp_buff = NULL;
-	uint32_t i = 0;
+	uint8_t *u8buf_ptr;
+	uint32_t value;
 
 	LOG_DEBUG("Reading APB-AP memory address 0x%" TARGET_PRIxADDR " size %"	PRIu32 " count%"  PRIu32,
 			  address, size, count);
@@ -1846,72 +1853,146 @@ static int aarch64_read_apb_ab_memory(struct target *target,
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	/* Mark register R0 as dirty, as it will be used
+	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
+	/* Mark register X0, X1 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;
 
-	/*  clear any abort  */
+	/*	clear any abort  */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-		armv8->debug_base + CPUDBG_DRCR, 1<<2);
+				armv8->debug_base + CPUDBG_DRCR, DRCR_CSE);
 	if (retval != ERROR_OK)
 		goto error_free_buff_r;
 
+	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+
+	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
+
+	/* Set Normal access mode  */
+	dscr = (dscr & ~DSCR_MA);
+	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUDBG_DSCR, dscr);
+
+	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 */
+		retval += aarch64_write_dcc_64(armv8, address & ~0x3ULL);
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
+		retval += aarch64_exec_opcode(target, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval += aarch64_exec_opcode(target, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+		/* Step 1.e - Change DCC to memory mode */
+		dscr = dscr | DSCR_MA;
+		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, dscr);
+		/* Step 1.f - read DBGDTRTX and discard the value */
+		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DTRTX, &value);
+	} else {
+		/* Write R0 with value 'address' using write procedure */
+		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
+		retval += aarch64_write_dcc(armv8, address & ~0x3ULL);
+		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
+		retval += aarch64_exec_opcode(target,
+				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), &dscr);
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval += aarch64_exec_opcode(target,
+				T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)), &dscr);
+		/* Step 1.e - Change DCC to memory mode */
+		dscr = dscr | DSCR_MA;
+		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, dscr);
+		/* Step 1.f - read DBGDTRTX and discard the value */
+		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DTRTX, &value);
+
+	}
 	if (retval != ERROR_OK)
 		goto error_unset_dtr_r;
 
-	if (size > 4) {
-		LOG_WARNING("reading size >4 bytes not yet supported");
-		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;
 
-	while (i < count * size) {
+		/* 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;
 
-		retval = aarch64_instr_write_data_dcc_64(arm->dpm, 0xd5330400, address+4);
-		if (retval != ERROR_OK)
-			goto error_unset_dtr_r;
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+	/* 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.
+	 */
 
-		dscr = DSCR_INSTR_COMP;
-		retval = aarch64_exec_opcode(target, 0xb85fc000, &dscr);
-		if (retval != ERROR_OK)
+	/* 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 + CPUDBG_DTRTX);
+	if (retval != ERROR_OK)
 			goto error_unset_dtr_r;
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
 
-		retval = aarch64_instr_read_data_dcc(arm->dpm, 0xd5130400, &val);
-		if (retval != ERROR_OK)
-			goto error_unset_dtr_r;
-		memcpy(&buffer[i], &val, size);
-		i += 4;
-		address += 4;
+	/* Step 3.a - set DTR access mode back to Normal mode	*/
+	dscr = (dscr & ~DSCR_MA);
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUDBG_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 + CPUDBG_DTRTX, &value);
+	memcpy(u8buf_ptr + (total_u32-1) * 4, &value, 4);
+
+	/* Check for sticky abort flags in the DSCR */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		goto error_free_buff_r;
+	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
+		/* Abort occurred - clear it and exit */
+		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
+		mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUDBG_DRCR, DRCR_CSE);
+		goto error_free_buff_r;
 	}
 
-	/* Clear any sticky error */
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-		armv8->debug_base + CPUDBG_DRCR, 1<<2);
+	/* 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);
+	}
 
 	/* Done */
 	return ERROR_OK;
 
 error_unset_dtr_r:
-	LOG_WARNING("DSCR = 0x%" PRIx32, dscr);
-	/* Todo: Unset DTR mode */
-
+	/* Unset DTR mode */
+	mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, &dscr);
+	dscr = (dscr & ~DSCR_MA);
+	mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUDBG_DSCR, dscr);
 error_free_buff_r:
 	LOG_ERROR("error");
 	free(tmp_buff);
-
-	/* Clear any sticky error */
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-		armv8->debug_base + CPUDBG_DRCR, 1<<2);
-
 	return ERROR_FAIL;
 }
 
@@ -1937,7 +2018,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_ab_memory(target, address, size, count, buffer);
+			retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
 		}
 	}
 	return retval;
@@ -1988,7 +2069,7 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 			if (retval != ERROR_OK)
 				return retval;
 		}
-		retval = aarch64_read_apb_ab_memory(target, address, size,
+		retval = aarch64_read_apb_ap_memory(target, address, size,
 						    count, buffer);
 	}
 	return retval;
@@ -2020,7 +2101,7 @@ static int aarch64_write_phys_memory(struct target *target,
 				if (retval != ERROR_OK)
 					return retval;
 			}
-			return aarch64_write_apb_ab_memory(target, address, size, count, buffer);
+			return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
 		}
 	}
 
@@ -2128,7 +2209,7 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 			if (retval != ERROR_OK)
 				return retval;
 		}
-		retval = aarch64_write_apb_ab_memory(target, address, size, count, buffer);
+		retval = aarch64_write_apb_ap_memory(target, address, size, count, buffer);
 	}
 	return retval;
 }
-- 
cgit v1.1


From b085570de9550bf50938d57f50465d07ba3ed87b Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sat, 17 Sep 2016 21:11:38 +0200
Subject: aarch64: deconflict debug register names

CPUDBG_ -> CPUV8_DBG_ for armv8 debug registers.

Change-Id: I3d24cc209309fa9bbeb5c3e6c88a572383c9360e
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 200 +++++++++++++++++++++++++--------------------------
 src/target/armv8.h   |  58 ++++++++-------
 2 files changed, 131 insertions(+), 127 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 33c7daa..84e6db3 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -125,11 +125,11 @@ static int aarch64_init_debug_access(struct target *target)
 	/* Unlocking the debug registers for modification
 	 * The debugport might be uninitialised so try twice */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			     armv8->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
+			     armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
 	if (retval != ERROR_OK) {
 		/* try again */
 		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			     armv8->debug_base + CPUDBG_LOCKACCESS, 0xC5ACCE55);
+			     armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
 		if (retval == ERROR_OK)
 			LOG_USER("Locking debug access failed on first, but succeeded on second try.");
 	}
@@ -138,7 +138,7 @@ static int aarch64_init_debug_access(struct target *target)
 	/* Clear Sticky Power Down status Bit in PRSR to enable access to
 	   the registers in the Core Power Domain */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_PRSR, &dummy);
+			armv8->debug_base + CPUV8_DBG_PRSR, &dummy);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -152,7 +152,7 @@ static int aarch64_init_debug_access(struct target *target)
 
 /* To reduce needless round-trips, pass in a pointer to the current
  * DSCR value.  Initialize it to zero if you just need to know the
- * value on return from this function; or DSCR_INSTR_COMP if you
+ * value on return from this function; or DSCR_ITE if you
  * happen to know that no instruction is pending.
  */
 static int aarch64_exec_opcode(struct target *target,
@@ -167,9 +167,9 @@ static int aarch64_exec_opcode(struct target *target,
 
 	/* Wait for InstrCompl bit to be set */
 	long long then = timeval_ms();
-	while ((dscr & DSCR_INSTR_COMP) == 0) {
+	while ((dscr & DSCR_ITE) == 0) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
 			return retval;
@@ -181,14 +181,14 @@ static int aarch64_exec_opcode(struct target *target,
 	}
 
 	retval = mem_ap_write_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_ITR, opcode);
+			armv8->debug_base + CPUV8_DBG_ITR, opcode);
 	if (retval != ERROR_OK)
 		return retval;
 
 	then = timeval_ms();
 	do {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK) {
 			LOG_ERROR("Could not read DSCR register");
 			return retval;
@@ -197,7 +197,7 @@ static int aarch64_exec_opcode(struct target *target,
 			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
 			return ERROR_FAIL;
 		}
-	} while ((dscr & DSCR_INSTR_COMP) == 0);	/* Wait for InstrCompl bit to be set */
+	} while ((dscr & DSCR_ITE) == 0);	/* Wait for InstrCompl bit to be set */
 
 	if (dscr_p)
 		*dscr_p = dscr;
@@ -221,7 +221,7 @@ static int aarch64_dap_write_memap_register_u32(struct target *target,
 /*
  * AARCH64 implementation of Debug Programmer's Model
  *
- * NOTE the invariant:  these routines return with DSCR_INSTR_COMP set,
+ * NOTE the invariant:  these routines return with DSCR_ITE set,
  * so there's no need to poll for it before executing an instruction.
  *
  * NOTE that in several of these cases the "stall" mode might be useful.
@@ -238,7 +238,7 @@ static int aarch64_write_dcc(struct armv8_common *armv8, uint32_t data)
 {
 	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
 	return mem_ap_write_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DTRRX, data);
+				armv8->debug_base + CPUV8_DBG_DTRRX, data);
 }
 
 static int aarch64_write_dcc_64(struct armv8_common *armv8, uint64_t data)
@@ -247,16 +247,16 @@ static int aarch64_write_dcc_64(struct armv8_common *armv8, uint64_t data)
 	LOG_DEBUG("write DCC Low word0x%08" PRIx32, (unsigned)data);
 	LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
 	ret = mem_ap_write_u32(armv8->debug_ap,
-			       armv8->debug_base + CPUDBG_DTRRX, data);
+			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
 	ret += mem_ap_write_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DTRTX, data >> 32);
+				armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
 	return ret;
 }
 
 static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
 	uint32_t *dscr_p)
 {
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	if (dscr_p)
@@ -266,7 +266,7 @@ static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
 	long long then = timeval_ms();
 	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR,
+				armv8->debug_base + CPUV8_DBG_DSCR,
 				&dscr);
 		if (retval != ERROR_OK)
 			return retval;
@@ -277,7 +277,7 @@ static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					    armv8->debug_base + CPUDBG_DTRTX,
+					    armv8->debug_base + CPUV8_DBG_DTRTX,
 					    data);
 	if (retval != ERROR_OK)
 		return retval;
@@ -292,7 +292,7 @@ static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
 static int aarch64_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 	uint32_t *dscr_p)
 {
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 	uint32_t higher;
 	int retval;
 
@@ -303,7 +303,7 @@ static int aarch64_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 	long long then = timeval_ms();
 	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR,
+				armv8->debug_base + CPUV8_DBG_DSCR,
 				&dscr);
 		if (retval != ERROR_OK)
 			return retval;
@@ -314,13 +314,13 @@ static int aarch64_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					    armv8->debug_base + CPUDBG_DTRTX,
+					    armv8->debug_base + CPUV8_DBG_DTRTX,
 					    (uint32_t *)data);
 	if (retval != ERROR_OK)
 		return retval;
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					    armv8->debug_base + CPUDBG_DTRRX,
+					    armv8->debug_base + CPUV8_DBG_DTRRX,
 					    &higher);
 	if (retval != ERROR_OK)
 		return retval;
@@ -344,11 +344,11 @@ static int aarch64_dpm_prepare(struct arm_dpm *dpm)
 	long long then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-				a8->armv8_common.debug_base + CPUDBG_DSCR,
+				a8->armv8_common.debug_base + CPUV8_DBG_DSCR,
 				&dscr);
 		if (retval != ERROR_OK)
 			return retval;
-		if ((dscr & DSCR_INSTR_COMP) != 0)
+		if ((dscr & DSCR_ITE) != 0)
 			break;
 		if (timeval_ms() > then + 1000) {
 			LOG_ERROR("Timeout waiting for dpm prepare");
@@ -394,7 +394,7 @@ static int aarch64_instr_write_data_dcc(struct arm_dpm *dpm,
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
 	int retval;
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 
 	retval = aarch64_write_dcc(&a8->armv8_common, data);
 	if (retval != ERROR_OK)
@@ -411,7 +411,7 @@ static int aarch64_instr_write_data_dcc_64(struct arm_dpm *dpm,
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
 	int retval;
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 
 	retval = aarch64_write_dcc_64(&a8->armv8_common, data);
 	if (retval != ERROR_OK)
@@ -427,7 +427,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t data)
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	retval = aarch64_write_dcc(&a8->armv8_common, data);
@@ -454,7 +454,7 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t data)
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	retval = aarch64_write_dcc_64(&a8->armv8_common, data);
@@ -480,7 +480,7 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
 {
 	struct target *target = dpm->arm->target;
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 
 	/* "Prefetch flush" after modifying execution status in CPSR */
 	return aarch64_exec_opcode(target,
@@ -493,7 +493,7 @@ static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
 	int retval;
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 
 	/* the opcode, writing data to DCC */
 	retval = aarch64_exec_opcode(
@@ -511,7 +511,7 @@ static int aarch64_instr_read_data_dcc_64(struct arm_dpm *dpm,
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
 	int retval;
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 
 	/* the opcode, writing data to DCC */
 	retval = aarch64_exec_opcode(
@@ -528,7 +528,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *data)
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to R0 */
@@ -554,7 +554,7 @@ static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t *data)
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_INSTR_COMP;
+	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to R0 */
@@ -586,12 +586,12 @@ static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
 
 	switch (index_t) {
 		case 0 ... 15:	/* breakpoints */
-			vr += CPUDBG_BVR_BASE;
-			cr += CPUDBG_BCR_BASE;
+			vr += CPUV8_DBG_BVR_BASE;
+			cr += CPUV8_DBG_BCR_BASE;
 			break;
 		case 16 ... 31:	/* watchpoints */
-			vr += CPUDBG_WVR_BASE;
-			cr += CPUDBG_WCR_BASE;
+			vr += CPUV8_DBG_WVR_BASE;
+			cr += CPUV8_DBG_WCR_BASE;
 			index_t -= 16;
 			break;
 		default:
@@ -617,15 +617,15 @@ static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 	return ERROR_OK;
 
 #if 0
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
+	struct aarch64_common *a = dpm_to_a8(dpm);
 	uint32_t cr;
 
 	switch (index_t) {
 		case 0 ... 15:
-			cr = a8->armv8_common.debug_base + CPUDBG_BCR_BASE;
+			cr = a->armv8_common.debug_base + CPUV8_DBG_BCR_BASE;
 			break;
 		case 16 ... 31:
-			cr = a8->armv8_common.debug_base + CPUDBG_WCR_BASE;
+			cr = a->armv8_common.debug_base + CPUV8_DBG_WCR_BASE;
 			index_t -= 16;
 			break;
 		default:
@@ -633,7 +633,7 @@ static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 	}
 	cr += 4 * index_t;
 
-	LOG_DEBUG("A8: bpwp disable, cr %08x", (unsigned) cr);
+	LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
 
 	/* clear control register */
 	return aarch64_dap_write_memap_register_u32(dpm->arm->target, cr, 0);
@@ -740,7 +740,7 @@ static int aarch64_poll(struct target *target)
 		return retval;
 	}
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 	aarch64->cpudbg_dscr = dscr;
@@ -835,7 +835,7 @@ static int aarch64_halt(struct target *target)
 	 * enter halting debug mode
 	 */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -854,7 +854,7 @@ static int aarch64_halt(struct target *target)
 	long long then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
 		if ((dscr & DSCR_CORE_HALTED) != 0)
@@ -961,20 +961,20 @@ static int aarch64_internal_restart(struct target *target)
 	 */
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if ((dscr & DSCR_INSTR_COMP) == 0)
+	if ((dscr & DSCR_ITE) == 0)
 		LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, dscr & ~DSCR_ITR_EN);
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr & ~DSCR_ITR_EN);
 	if (retval != ERROR_OK)
 		return retval;
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DRCR, DRCR_RESTART |
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_RESTART |
 			DRCR_CLEAR_EXCEPTIONS);
 	if (retval != ERROR_OK)
 		return retval;
@@ -992,7 +992,7 @@ static int aarch64_internal_restart(struct target *target)
 	long long then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
 		if ((dscr & DSCR_CORE_RESTARTED) != 0)
@@ -1083,7 +1083,7 @@ static int aarch64_debug_entry(struct target *target)
 
 	/* REVISIT surely we should not re-read DSCR !! */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1095,14 +1095,14 @@ static int aarch64_debug_entry(struct target *target)
 	/* Enable the ITR execution once we are in debug mode */
 	dscr |= DSCR_ITR_EN;
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Examine debug reason */
 	arm_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
 	mem_ap_read_atomic_u32(armv8->debug_ap,
-				   armv8->debug_base + CPUDBG_DESR, &tmp);
+				   armv8->debug_base + CPUV8_DBG_EDESR, &tmp);
 	if ((tmp & 0x7) == 0x4)
 		target->debug_reason = DBG_REASON_SINGLESTEP;
 
@@ -1111,7 +1111,7 @@ static int aarch64_debug_entry(struct target *target)
 		uint32_t wfar;
 
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_WFAR,
+				armv8->debug_base + CPUV8_DBG_WFAR0,
 				&wfar);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1138,7 +1138,7 @@ static int aarch64_post_debug_entry(struct target *target)
 	int retval;
 
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DRCR, 1<<2);
+				armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
 	retval = aarch64_instr_read_data_r0(armv8->arm.dpm,
 					    0xd5381000, &sctlr_el1);
 	if (retval != ERROR_OK)
@@ -1174,12 +1174,12 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DECR, &tmp);
+			armv8->debug_base + CPUV8_DBG_EDECR, &tmp);
 	if (retval != ERROR_OK)
 		return retval;
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DECR, (tmp|0x4));
+			armv8->debug_base + CPUV8_DBG_EDECR, (tmp|0x4));
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1191,7 +1191,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	long long then = timeval_ms();
 	while (target->state != TARGET_HALTED) {
 		mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DESR, &tmp);
+			armv8->debug_base + CPUV8_DBG_EDESR, &tmp);
 		LOG_DEBUG("DESR = %#x", tmp);
 		retval = aarch64_poll(target);
 		if (retval != ERROR_OK)
@@ -1203,7 +1203,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	}
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DECR, (tmp&(~0x4)));
+			armv8->debug_base + CPUV8_DBG_EDECR, (tmp&(~0x4)));
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1270,18 +1270,18 @@ static int aarch64_set_breakpoint(struct target *target,
 		bpt_value = brp_list[brp_i].value;
 
 		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-				+ CPUDBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+				+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
 				(uint32_t)(bpt_value & 0xFFFFFFFF));
 		if (retval != ERROR_OK)
 			return retval;
 		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-				+ CPUDBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+				+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
 				(uint32_t)(bpt_value >> 32));
 		if (retval != ERROR_OK)
 			return retval;
 
 		retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-				+ CPUDBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+				+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 				brp_list[brp_i].control);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1308,11 +1308,11 @@ static int aarch64_set_breakpoint(struct target *target,
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUDBG_DSCR, &dscr);
+					armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	/* Ensure that halting debug mode is enable */
-	dscr = dscr | DSCR_HALT_DBG_MODE;
+	dscr = dscr | DSCR_HDE;
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-					 armv8->debug_base + CPUDBG_DSCR, dscr);
+					 armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Failed to set DSCR.HDE");
 		return retval;
@@ -1354,12 +1354,12 @@ static int aarch64_set_context_breakpoint(struct target *target,
 	brp_list[brp_i].value = (breakpoint->asid);
 	brp_list[brp_i].control = control;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
 			brp_list[brp_i].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
 			brp_list[brp_i].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1420,12 +1420,12 @@ static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoin
 	brp_list[brp_1].value = (breakpoint->asid);
 	brp_list[brp_1].control = control_CTX;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUDBG_BVR_BASE + 4 * brp_list[brp_1].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_1].BRPn,
 			brp_list[brp_1].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUDBG_BCR_BASE + 4 * brp_list[brp_1].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_1].BRPn,
 			brp_list[brp_1].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1438,12 +1438,12 @@ static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoin
 	brp_list[brp_2].value = (breakpoint->address & 0xFFFFFFFC);
 	brp_list[brp_2].control = control_IVA;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUDBG_BVR_BASE + 4 * brp_list[brp_2].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_2].BRPn,
 			brp_list[brp_2].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUDBG_BCR_BASE + 4 * brp_list[brp_2].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_2].BRPn,
 			brp_list[brp_2].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1477,7 +1477,7 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 			brp_list[brp_i].value = 0;
 			brp_list[brp_i].control = 0;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUDBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].control);
 			if (retval != ERROR_OK)
 				return retval;
@@ -1491,7 +1491,7 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 			brp_list[brp_j].value = 0;
 			brp_list[brp_j].control = 0;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUDBG_BCR_BASE + 16 * brp_list[brp_j].BRPn,
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_j].BRPn,
 					brp_list[brp_j].control);
 			if (retval != ERROR_OK)
 				return retval;
@@ -1511,12 +1511,12 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 			brp_list[brp_i].value = 0;
 			brp_list[brp_i].control = 0;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUDBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].control);
 			if (retval != ERROR_OK)
 				return retval;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUDBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].value);
 			if (retval != ERROR_OK)
 				return retval;
@@ -1711,7 +1711,7 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 
 	/*  clear any abort  */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DRCR, DRCR_CSE);
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1752,14 +1752,14 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, &dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		goto error_free_buff_w;
 
 	/* Set Normal access mode  */
 	dscr = (dscr & ~DSCR_MA);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		/* Write X0 with value 'address' using write procedure */
@@ -1780,34 +1780,34 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	/* Step 1.d   - Change DCC to memory mode */
 	dscr = dscr | DSCR_MA;
 	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 	if (retval != ERROR_OK)
 		goto error_unset_dtr_w;
 
 
 	/* Step 2.a   - Do the write */
 	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
-					tmp_buff, 4, total_u32, armv8->debug_base + CPUDBG_DTRRX);
+					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 + CPUDBG_DSCR, dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 	if (retval != ERROR_OK)
 		goto error_unset_dtr_w;
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		goto error_free_buff_w;
 	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUDBG_DRCR, 1<<2);
+					armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
 		goto error_free_buff_w;
 	}
 
@@ -1818,10 +1818,10 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 error_unset_dtr_w:
 	/* Unset DTR mode */
 	mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	dscr = (dscr & ~DSCR_MA);
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 error_free_buff_w:
 	LOG_ERROR("error");
 	free(tmp_buff);
@@ -1867,20 +1867,20 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 
 	/*	clear any abort  */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DRCR, DRCR_CSE);
+				armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 	if (retval != ERROR_OK)
 		goto error_free_buff_r;
 
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
 	/* Set Normal access mode  */
 	dscr = (dscr & ~DSCR_MA);
 	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_DSCR, dscr);
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		/* Write X0 with value 'address' using write procedure */
@@ -1893,10 +1893,10 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		/* Step 1.e - Change DCC to memory mode */
 		dscr = dscr | DSCR_MA;
 		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, dscr);
+				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 + CPUDBG_DTRTX, &value);
+				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
@@ -1910,10 +1910,10 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		/* Step 1.e - Change DCC to memory mode */
 		dscr = dscr | DSCR_MA;
 		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, dscr);
+				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 + CPUDBG_DTRTX, &value);
+				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
 
 	}
 	if (retval != ERROR_OK)
@@ -1945,32 +1945,32 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	/* 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 + CPUDBG_DTRTX);
+									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,
-					armv8->debug_base + CPUDBG_DSCR, dscr);
+					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 + CPUDBG_DTRTX, &value);
+			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
 	memcpy(u8buf_ptr + (total_u32-1) * 4, &value, 4);
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		goto error_free_buff_r;
 	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUDBG_DRCR, DRCR_CSE);
+					armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 		goto error_free_buff_r;
 	}
 
@@ -1986,10 +1986,10 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 error_unset_dtr_r:
 	/* Unset DTR mode */
 	mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	dscr = (dscr & ~DSCR_MA);
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 error_free_buff_r:
 	LOG_ERROR("error");
 	free(tmp_buff);
@@ -2229,16 +2229,16 @@ static int aarch64_handle_target_request(void *priv)
 		uint32_t request;
 		uint32_t dscr;
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUDBG_DSCR, &dscr);
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 
 		/* check if we have data */
 		while ((dscr & DSCR_DTR_TX_FULL) && (retval == ERROR_OK)) {
 			retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUDBG_DTRTX, &request);
+					armv8->debug_base + CPUV8_DBG_DTRTX, &request);
 			if (retval == ERROR_OK) {
 				target_request(target, request);
 				retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-						armv8->debug_base + CPUDBG_DSCR, &dscr);
+						armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 			}
 		}
 	}
@@ -2310,7 +2310,7 @@ static int aarch64_examine_first(struct target *target)
 		armv8->debug_base = target->dbgbase;
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x300, 0);
+			armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "oslock");
 		return retval;
diff --git a/src/target/armv8.h b/src/target/armv8.h
index ccb205c..41edb99 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -159,34 +159,38 @@ target_to_armv8(struct target *target)
 }
 
 /* register offsets from armv8.debug_base */
+#define CPUV8_DBG_MAINID0		0xD00
+#define CPUV8_DBG_CPUFEATURE0	0xD20
+#define CPUV8_DBG_DBGFEATURE0	0xD28
+#define CPUV8_DBG_MEMFEATURE0	0xD38
+
+#define CPUV8_DBG_LOCKACCESS 0xFB0
+#define CPUV8_DBG_LOCKSTATUS 0xFB4
+
+#define CPUV8_DBG_EDESR		0x20
+#define CPUV8_DBG_EDECR		0x24
+#define CPUV8_DBG_WFAR0		0x30
+#define CPUV8_DBG_WFAR1		0x34
+#define CPUV8_DBG_DSCR		0x088
+#define CPUV8_DBG_DRCR		0x090
+#define CPUV8_DBG_PRCR		0x310
+#define CPUV8_DBG_PRSR		0x314
+
+#define CPUV8_DBG_DTRRX		0x080
+#define CPUV8_DBG_ITR		0x084
+#define CPUV8_DBG_SCR		0x088
+#define CPUV8_DBG_DTRTX		0x08c
+
+#define CPUV8_DBG_BVR_BASE	0x400
+#define CPUV8_DBG_BCR_BASE	0x408
+#define CPUV8_DBG_WVR_BASE	0x800
+#define CPUV8_DBG_WCR_BASE	0x808
+#define CPUV8_DBG_VCR		0x01C
+
+#define CPUV8_DBG_OSLAR		0x300
+
+#define CPUV8_DBG_AUTHSTATUS	0xFB8
 
-#define CPUDBG_WFAR		0x018
-#define CPUDBG_DESR		0x020
-#define CPUDBG_DECR		0x024
-/* PCSR at 0x084 -or- 0x0a0 -or- both ... based on flags in DIDR */
-#define CPUDBG_DSCR		0x088
-#define CPUDBG_DRCR		0x090
-#define CPUDBG_PRCR		0x310
-#define CPUDBG_PRSR		0x314
-
-#define CPUDBG_DTRRX		0x080
-#define CPUDBG_ITR		0x084
-#define CPUDBG_DTRTX		0x08c
-
-#define CPUDBG_BVR_BASE		0x400
-#define CPUDBG_BCR_BASE		0x408
-#define CPUDBG_WVR_BASE		0x180
-#define CPUDBG_WCR_BASE		0x1C0
-#define CPUDBG_VCR		0x01C
-
-#define CPUDBG_OSLAR		0x300
-#define CPUDBG_OSLSR		0x304
-#define CPUDBG_OSSRR		0x308
-#define CPUDBG_ECR		0x024
-
-#define CPUDBG_DSCCR		0x028
-
-#define CPUDBG_AUTHSTATUS	0xFB8
 
 int armv8_arch_state(struct target *target);
 int armv8_identify_cache(struct target *target);
-- 
cgit v1.1


From 4ba8388f2658314c0de944d088c964cb19d11050 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sat, 17 Sep 2016 21:43:15 +0200
Subject: aarch64: fix context and hybrid hardware breakpoints

Fix 64bit address setting
Fix register spacing (16 instead of 4)
Set HMC bit for all but linked context match breakpoints,
where the bit is ignored anyway

Change-Id: I48428f39154a6fe5fadc075ca918d1500a0bb241
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 27 +++++++++++++++++----------
 1 file changed, 17 insertions(+), 10 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 84e6db3..d06e668 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1348,18 +1348,19 @@ static int aarch64_set_context_breakpoint(struct target *target,
 
 	breakpoint->set = brp_i + 1;
 	control = ((matchmode & 0x7) << 20)
+		| (1 << 13)
 		| (byte_addr_select << 5)
 		| (3 << 1) | 1;
 	brp_list[brp_i].used = 1;
 	brp_list[brp_i].value = (breakpoint->asid);
 	brp_list[brp_i].control = control;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
 			brp_list[brp_i].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 			brp_list[brp_i].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1420,30 +1421,36 @@ static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoin
 	brp_list[brp_1].value = (breakpoint->asid);
 	brp_list[brp_1].control = control_CTX;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_1].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_1].BRPn,
 			brp_list[brp_1].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_1].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_1].BRPn,
 			brp_list[brp_1].control);
 	if (retval != ERROR_OK)
 		return retval;
 
 	control_IVA = ((IVA_machmode & 0x7) << 20)
 		| (brp_1 << 16)
+		| (1 << 13)
 		| (IVA_byte_addr_select << 5)
 		| (3 << 1) | 1;
 	brp_list[brp_2].used = 1;
-	brp_list[brp_2].value = (breakpoint->address & 0xFFFFFFFC);
+	brp_list[brp_2].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
 	brp_list[brp_2].control = control_IVA;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_2].BRPn,
-			brp_list[brp_2].value);
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value & 0xFFFFFFFF);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_2].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value >> 32);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_2].BRPn,
 			brp_list[brp_2].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1511,12 +1518,12 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 			brp_list[brp_i].value = 0;
 			brp_list[brp_i].control = 0;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].control);
 			if (retval != ERROR_OK)
 				return retval;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].value);
 			if (retval != ERROR_OK)
 				return retval;
-- 
cgit v1.1


From 21a5c404304bfc616de7648a3de67d91a2c43e66 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:02:36 +0200
Subject: aarch64: formalize use of CTI in halt and resume

Use configured CTI base address instead of hardcoded value, if
available.
Use symbolic constants instead of raw hex offsets.
Trim halt and resume code to what is actually necessary.

Change-Id: I4997c2bcca7cebf5ad78859a6a12abe8639594ed
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 94 +++++++++++++++++++++-------------------------------
 src/target/armv8.h   | 31 +++++++++++++++++
 2 files changed, 68 insertions(+), 57 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d06e668..4097d11 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -796,59 +796,49 @@ static int aarch64_halt(struct target *target)
 	uint32_t dscr;
 	struct armv8_common *armv8 = target_to_armv8(target);
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0, &dscr);
+	/* enable CTI*/
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0, 1);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0, &dscr);
+			armv8->cti_base + CTI_CTR, 1);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x140, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x140, 6);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x140, &dscr);
+			armv8->cti_base + CTI_GATE, 3);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa0, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa0, 5);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa0, &dscr);
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa4, &dscr);
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa4, 2);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa4, &dscr);
+			armv8->cti_base + CTI_OUTEN0, 1);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x20, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x20, 4);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x20, &dscr);
+			armv8->cti_base + CTI_OUTEN1, 2);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/*
-	 * enter halting debug mode
+	 * add HDE in halting debug mode
 	 */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-#	/* STATUS */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x134, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, 1);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, &dscr);
+			armv8->cti_base + CTI_APPPULSE, 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->cti_base + CTI_INACK, 1);
+	if (retval != ERROR_OK)
+		return retval;
 
 
 	long long then = timeval_ms();
@@ -857,7 +847,7 @@ static int aarch64_halt(struct target *target)
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
-		if ((dscr & DSCR_CORE_HALTED) != 0)
+		if ((dscr & DSCRV8_HALT_MASK) != 0)
 			break;
 		if (timeval_ms() > then + 1000) {
 			LOG_ERROR("Timeout waiting for halt");
@@ -969,23 +959,7 @@ static int aarch64_internal_restart(struct target *target)
 		LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr & ~DSCR_ITR_EN);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_RESTART |
-			DRCR_CLEAR_EXCEPTIONS);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x10, 1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, 2);
+			armv8->cti_base + CTI_APPPULSE, 2);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -995,7 +969,7 @@ static int aarch64_internal_restart(struct target *target)
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
-		if ((dscr & DSCR_CORE_RESTARTED) != 0)
+		if ((dscr & DSCR_HDE) != 0)
 			break;
 		if (timeval_ms() > then + 1000) {
 			LOG_ERROR("Timeout waiting for resume");
@@ -1061,11 +1035,11 @@ static int aarch64_resume(struct target *target, int current,
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-		LOG_DEBUG("target resumed at 0x%" PRIu64, addr);
+		LOG_DEBUG("target resumed at 0x%" PRIx64, addr);
 	} else {
 		target->state = TARGET_DEBUG_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
-		LOG_DEBUG("target debug resumed at 0x%" PRIu64, addr);
+		LOG_DEBUG("target debug resumed at 0x%" PRIx64, addr);
 	}
 
 	return ERROR_OK;
@@ -2316,6 +2290,12 @@ static int aarch64_examine_first(struct target *target)
 	} else
 		armv8->debug_base = target->dbgbase;
 
+	LOG_DEBUG("Target ctibase is 0x%x", target->ctibase);
+	if (target->ctibase == 0)
+		armv8->cti_base = target->ctibase = armv8->debug_base + 0x1000;
+	else
+		armv8->cti_base = target->ctibase;
+
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
 	if (retval != ERROR_OK) {
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 41edb99..9c6ccce 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -130,6 +130,8 @@ struct armv8_common {
 	/* Core Debug Unit */
 	struct arm_dpm dpm;
 	uint32_t debug_base;
+	uint32_t cti_base;
+
 	struct adiv5_ap *debug_ap;
 	struct adiv5_ap *memory_ap;
 	bool memory_ap_available;
@@ -191,6 +193,35 @@ target_to_armv8(struct target *target)
 
 #define CPUV8_DBG_AUTHSTATUS	0xFB8
 
+/*define CTI(cross trigger interface)*/
+#define CTI_CTR				0x0
+#define CTI_INACK			0x10
+#define CTI_APPSET			0x14
+#define CTI_APPCLEAR		0x18
+#define CTI_APPPULSE		0x1C
+#define CTI_INEN0			0x20
+#define CTI_INEN1			0x24
+#define CTI_INEN2			0x28
+#define CTI_INEN3			0x2C
+#define CTI_INEN4			0x30
+#define CTI_INEN5			0x34
+#define CTI_INEN6			0x38
+#define CTI_INEN7			0x3C
+#define CTI_OUTEN0			0xA0
+#define CTI_OUTEN1			0xA4
+#define CTI_OUTEN2			0xA8
+#define CTI_OUTEN3			0xAC
+#define CTI_OUTEN4			0xB0
+#define CTI_OUTEN5			0xB4
+#define CTI_OUTEN6			0xB8
+#define CTI_OUTEN7			0xBC
+#define CTI_TRIN_STATUS		0x130
+#define CTI_TROUT_STATUS	0x134
+#define CTI_CHIN_STATUS		0x138
+#define CTI_CHOU_STATUS		0x13C
+#define CTI_GATE			0x140
+#define CTI_UNLOCK			0xFB0
+
 
 int armv8_arch_state(struct target *target);
 int armv8_identify_cache(struct target *target);
-- 
cgit v1.1


From f28d5ee71fcbcd296d85e236013e16790732c3af Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:06:42 +0200
Subject: aarch64: fix error recovery in aarch64_dpm_prepare

Flush DTRRX with a dummy read if it's full, clear sticky errors
by writing CSE bit to EDRCR register.

Change-Id: Ia42ae9d3859ba6cbe892d48584e21acdd4e25c84
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 4097d11..fcf600d 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -360,10 +360,14 @@ static int aarch64_dpm_prepare(struct arm_dpm *dpm)
 	if (dscr & DSCR_DTR_RX_FULL) {
 		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
 		/* Clear DCCRX */
-		retval = aarch64_exec_opcode(
-				a8->armv8_common.arm.target,
-				0xd5130400,
-				&dscr);
+		retval = mem_ap_read_u32(a8->armv8_common.debug_ap,
+			a8->armv8_common.debug_base + CPUV8_DBG_DTRRX, &dscr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Clear sticky error */
+		retval = mem_ap_write_u32(a8->armv8_common.debug_ap,
+			a8->armv8_common.debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 		if (retval != ERROR_OK)
 			return retval;
 	}
-- 
cgit v1.1


From 044e4b81080ad80815ce56636dfec9ecc5df5b13 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:10:55 +0200
Subject: aarch64: fix accesses to SCTLR_ELn register

The system control register has several instances, depending on the
exception level. Make sure to access always access the correct one.

Change-Id: I9e867f4dbd9625762042f20ed905064ea4e3270f
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 169 +++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 142 insertions(+), 27 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index fcf600d..09e828e 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -57,11 +57,41 @@ static int aarch64_restore_system_control_reg(struct target *target)
 
 	if (aarch64->system_control_reg != aarch64->system_control_reg_curr) {
 		aarch64->system_control_reg_curr = aarch64->system_control_reg;
-		retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
-						     0xd5181000,
-						     aarch64->system_control_reg);
+		/* LOG_INFO("cp15_control_reg: %8.8" PRIx32, cortex_v8->cp15_control_reg); */
+
+		switch (armv8->arm.core_mode) {
+			case ARMV8_64_EL0T:
+			case ARMV8_64_EL1T:
+			case ARMV8_64_EL1H:
+				retval = armv8->arm.msr(target, 3, /*op 0*/
+						0, 1,	/* op1, op2 */
+						0, 0,	/* CRn, CRm */
+						aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+			break;
+			case ARMV8_64_EL2T:
+			case ARMV8_64_EL2H:
+				retval = armv8->arm.msr(target, 3, /*op 0*/
+						4, 1,	/* op1, op2 */
+						0, 0,	/* CRn, CRm */
+						aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+			break;
+			case ARMV8_64_EL3H:
+			case ARMV8_64_EL3T:
+				retval = armv8->arm.msr(target, 3, /*op 0*/
+						6, 1,	/* op1, op2 */
+						0, 0,	/* CRn, CRm */
+						aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+			break;
+			default:
+				LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+			}
 	}
-
 	return retval;
 }
 
@@ -82,16 +112,45 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	int retval = ERROR_OK;
 
 	if (enable) {
-		/*  if mmu enabled at target stop and mmu not enable */
+		/*	if mmu enabled at target stop and mmu not enable */
 		if (!(aarch64->system_control_reg & 0x1U)) {
 			LOG_ERROR("trying to enable mmu on target stopped with mmu disable");
 			return ERROR_FAIL;
 		}
 		if (!(aarch64->system_control_reg_curr & 0x1U)) {
 			aarch64->system_control_reg_curr |= 0x1U;
-			retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
-							     0xd5181000,
-							     aarch64->system_control_reg_curr);
+			switch (armv8->arm.core_mode) {
+				case ARMV8_64_EL0T:
+				case ARMV8_64_EL1T:
+				case ARMV8_64_EL1H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							0, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+				break;
+				case ARMV8_64_EL2T:
+				case ARMV8_64_EL2H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							4, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+				break;
+				case ARMV8_64_EL3H:
+				case ARMV8_64_EL3T:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							6, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+				break;
+				default:
+					LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+			}
 		}
 	} else {
 		if (aarch64->system_control_reg_curr & 0x4U) {
@@ -103,9 +162,39 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 		}
 		if ((aarch64->system_control_reg_curr & 0x1U)) {
 			aarch64->system_control_reg_curr &= ~0x1U;
-			retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
-							     0xd5181000,
-							     aarch64->system_control_reg_curr);
+			switch (armv8->arm.core_mode) {
+				case ARMV8_64_EL0T:
+				case ARMV8_64_EL1T:
+				case ARMV8_64_EL1H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							0, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+					break;
+				case ARMV8_64_EL2T:
+				case ARMV8_64_EL2H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							4, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+					break;
+				case ARMV8_64_EL3H:
+				case ARMV8_64_EL3T:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							6, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+					break;
+				default:
+					LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+					break;
+			}
 		}
 	}
 	return retval;
@@ -1111,31 +1200,57 @@ static int aarch64_post_debug_entry(struct target *target)
 {
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
-	struct armv8_mmu_common *armv8_mmu = &armv8->armv8_mmu;
-	uint32_t sctlr_el1 = 0;
 	int retval;
 
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
-	retval = aarch64_instr_read_data_r0(armv8->arm.dpm,
-					    0xd5381000, &sctlr_el1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	LOG_DEBUG("sctlr_el1 = %#8.8x", sctlr_el1);
-	aarch64->system_control_reg = sctlr_el1;
-	aarch64->system_control_reg_curr = sctlr_el1;
-	aarch64->curr_mode = armv8->arm.core_mode;
-
-	armv8_mmu->mmu_enabled = sctlr_el1 & 0x1U ? 1 : 0;
-	armv8_mmu->armv8_cache.d_u_cache_enabled = sctlr_el1 & 0x4U ? 1 : 0;
-	armv8_mmu->armv8_cache.i_cache_enabled = sctlr_el1 & 0x1000U ? 1 : 0;
+				    armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
+	switch (armv8->arm.core_mode) {
+		case ARMV8_64_EL0T:
+		case ARMV8_64_EL1T:
+		case ARMV8_64_EL1H:
+			retval = armv8->arm.mrs(target, 3, /*op 0*/
+					0, 0,	/* op1, op2 */
+					1, 0,	/* CRn, CRm */
+					&aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+		break;
+		case ARMV8_64_EL2T:
+		case ARMV8_64_EL2H:
+			retval = armv8->arm.mrs(target, 3, /*op 0*/
+					4, 0,	/* op1, op2 */
+					1, 0,	/* CRn, CRm */
+					&aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+		break;
+		case ARMV8_64_EL3H:
+		case ARMV8_64_EL3T:
+			retval = armv8->arm.mrs(target, 3, /*op 0*/
+					6, 0,	/* op1, op2 */
+					1, 0,	/* CRn, CRm */
+					&aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+		break;
+		default:
+			LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+	}
+	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
+	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
 #if 0
 	if (armv8->armv8_mmu.armv8_cache.ctype == -1)
 		armv8_identify_cache(target);
 #endif
 
+	armv8->armv8_mmu.mmu_enabled =
+			(aarch64->system_control_reg & 0x1U) ? 1 : 0;
+	armv8->armv8_mmu.armv8_cache.d_u_cache_enabled =
+		(aarch64->system_control_reg & 0x4U) ? 1 : 0;
+	armv8->armv8_mmu.armv8_cache.i_cache_enabled =
+		(aarch64->system_control_reg & 0x1000U) ? 1 : 0;
+	aarch64->curr_mode = armv8->arm.core_mode;
 	return ERROR_OK;
 }
 
-- 
cgit v1.1


From 391109505fff97772c70f7dda02865e7a9863007 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:15:15 +0200
Subject: aarch64: use symbolic opcodes instead of hex values

Use opcode definitions from armv8_opcodes.h where appropriate

Change-Id: Iead33fb8e62eb2dd2419ef8932f7d46c087f51a8
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 09e828e..8ddc226 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -529,7 +529,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5330500,
+			ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -556,7 +556,7 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5330400,
+			ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0),
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -577,7 +577,7 @@ static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
 
 	/* "Prefetch flush" after modifying execution status in CPSR */
 	return aarch64_exec_opcode(target,
-			ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
+			DSB_SY,
 			&dscr);
 }
 
@@ -635,7 +635,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 	/* write R0 to DCC */
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5130400,  /* msr dbgdtr_el0, x0 */
+			ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),  /* msr dbgdtr_el0, x0 */
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -661,7 +661,7 @@ static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
 	/* write R0 to DCC */
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5130400,  /* msr dbgdtr_el0, x0 */
+			ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0),  /* msr dbgdtr_el0, x0 */
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1384,8 +1384,7 @@ static int aarch64_set_breakpoint(struct target *target,
 
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
-		buf_set_u32(code, 0, 32, 0xD4400000);
-
+		buf_set_u32(code, 0, 32, ARMV8_BKPT(0x11));
 		retval = target_read_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1,
-- 
cgit v1.1


From d8abda4bd828f40fc76613f4d809d86f4c6f1c97 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:34:03 +0200
Subject: aarch64: fix entry into debug state

- armv8 EDSCR has no ITR_EN bit, ITR is always enabled. Writes to this
  bit are ignored but we should not do them anyway
- use dpmv8 function to report the reason for debug entry
- WFAR is a 64bit register

Change-Id: I07b81ecf105ceb7c3ae2f764bb408eb973c1d1de
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 34 +++++++++++++++-------------------
 src/target/arm_dpm.h |  2 +-
 2 files changed, 16 insertions(+), 20 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8ddc226..ef73afd 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1140,49 +1140,45 @@ static int aarch64_resume(struct target *target, int current,
 
 static int aarch64_debug_entry(struct target *target)
 {
-	uint32_t dscr;
 	int retval = ERROR_OK;
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = target_to_armv8(target);
-	uint32_t tmp;
 
 	LOG_DEBUG("dscr = 0x%08" PRIx32, aarch64->cpudbg_dscr);
 
-	/* REVISIT surely we should not re-read DSCR !! */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
 	/* REVISIT see A8 TRM 12.11.4 steps 2..3 -- make sure that any
 	 * imprecise data aborts get discarded by issuing a Data
 	 * Synchronization Barrier:  ARMV4_5_MCR(15, 0, 0, 7, 10, 4).
 	 */
 
-	/* Enable the ITR execution once we are in debug mode */
-	dscr |= DSCR_ITR_EN;
+	/* make sure to clear all sticky errors */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Examine debug reason */
-	arm_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
-	mem_ap_read_atomic_u32(armv8->debug_ap,
-				   armv8->debug_base + CPUV8_DBG_EDESR, &tmp);
-	if ((tmp & 0x7) == 0x4)
-		target->debug_reason = DBG_REASON_SINGLESTEP;
+	armv8_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
 
 	/* save address of instruction that triggered the watchpoint? */
 	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
-		uint32_t wfar;
+		uint32_t tmp;
+		uint64_t wfar = 0;
 
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_WFAR1,
+				&tmp);
+		if (retval != ERROR_OK)
+			return retval;
+		wfar = tmp;
+		wfar = (wfar << 32);
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_WFAR0,
-				&wfar);
+				&tmp);
 		if (retval != ERROR_OK)
 			return retval;
-		arm_dpm_report_wfar(&armv8->dpm, wfar);
+		wfar |= tmp;
+		armv8_dpm_report_wfar(&armv8->dpm, wfar);
 	}
 
 	retval = armv8_dpm_read_current_registers(&armv8->dpm);
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index ad49b8c..27badf4 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -138,7 +138,7 @@ struct arm_dpm {
 	struct dpm_wp *dwp;
 
 	/** Address of the instruction which triggered a watchpoint. */
-	uint32_t wp_pc;
+	target_addr_t wp_pc;
 
 	/** Recent value of DSCR. */
 	uint32_t dscr;
-- 
cgit v1.1


From 7eb95b1d72c0e4aea3499caf33312f8846bab262 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:43:27 +0200
Subject: aarch64: fix reading of translation table registers

Correctly access and parse aarch64 ttbcr.

Change-Id: I1b1652791a6b5200f58033925286292d838e8410
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 103 +++++++++++++++++++++++++++++++++++++++++++++++------
 src/target/armv8.h |   8 ++++-
 2 files changed, 100 insertions(+), 11 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 537dadb..5a96d96 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -266,31 +266,114 @@ static void armv8_show_fault_registers(struct target *target)
 	/* TODO */
 }
 
+static uint8_t armv8_pa_size(uint32_t ps)
+{
+	uint8_t ret = 0;
+	switch (ps) {
+		case 0:
+			ret = 32;
+			break;
+		case 1:
+			ret = 36;
+			break;
+		case 2:
+			ret = 40;
+			break;
+		case 3:
+			ret = 42;
+			break;
+		case 4:
+			ret = 44;
+			break;
+		case 5:
+			ret = 48;
+			break;
+		default:
+			LOG_INFO("Unknow physicall address size");
+			break;
+	}
+	return ret;
+}
+
 static int armv8_read_ttbcr(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct arm *arm = &armv8->arm;
 	uint32_t ttbcr;
+	uint64_t ttbcr_64;
+
 	int retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		goto done;
-	/*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
-			&ttbcr);
+
+	/* claaer ttrr1_used and ttbr0_mask */
+	memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
+	memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
+
+	switch (arm->core_mode) {
+		case ARMV8_64_EL3H:
+		case ARMV8_64_EL3T:
+			retval = dpm->instr_read_data_r0(dpm,
+					ARMV8_MRS(SYSTEM_TCR_EL3, 0),
+					&ttbcr);
+			retval += dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TTBR0_EL3, 0),
+					&armv8->ttbr_base);
+			if (retval != ERROR_OK)
+				goto done;
+			armv8->va_size = 64 - (ttbcr & 0x3F);
+			armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+			armv8->page_size = (ttbcr >> 14) & 3;
+			break;
+		case ARMV8_64_EL2T:
+		case ARMV8_64_EL2H:
+			retval = dpm->instr_read_data_r0(dpm,
+					ARMV8_MRS(SYSTEM_TCR_EL2, 0),
+					&ttbcr);
+			retval += dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TTBR0_EL2, 0),
+					&armv8->ttbr_base);
+			if (retval != ERROR_OK)
+				goto done;
+			armv8->va_size = 64 - (ttbcr & 0x3F);
+			armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+			armv8->page_size = (ttbcr >> 14) & 3;
+			break;
+		case ARMV8_64_EL0T:
+		case ARMV8_64_EL1T:
+		case ARMV8_64_EL1H:
+			retval = dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TCR_EL1, 0),
+					&ttbcr_64);
+			armv8->va_size = 64 - (ttbcr_64 & 0x3F);
+			armv8->pa_size = armv8_pa_size((ttbcr_64 >> 32) & 7);
+			armv8->page_size = (ttbcr_64 >> 14) & 3;
+			armv8->armv8_mmu.ttbr1_used = (((ttbcr_64 >> 16) & 0x3F) != 0) ? 1 : 0;
+			armv8->armv8_mmu.ttbr0_mask  = 0x0000FFFFFFFFFFFF;
+			retval += dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TTBR0_EL1 | (armv8->armv8_mmu.ttbr1_used), 0),
+					&armv8->ttbr_base);
+			if (retval != ERROR_OK)
+				goto done;
+			break;
+		default:
+			LOG_ERROR("unknow core state");
+			retval = ERROR_FAIL;
+			break;
+	}
 	if (retval != ERROR_OK)
 		goto done;
-	armv8->armv8_mmu.ttbr1_used = ((ttbcr & 0x7) != 0) ? 1 : 0;
-	armv8->armv8_mmu.ttbr0_mask  = 7 << (32 - ((ttbcr & 0x7)));
+
 #if 0
-	LOG_INFO("ttb1 %s ,ttb0_mask %x",
+	LOG_INFO("ttb1 %s ,ttb0_mask %llx",
 		armv8->armv8_mmu.ttbr1_used ? "used" : "not used",
 		armv8->armv8_mmu.ttbr0_mask);
 #endif
 	if (armv8->armv8_mmu.ttbr1_used == 1) {
-		LOG_INFO("SVC access above %" PRIx32,
-			 (uint32_t)(0xffffffff & armv8->armv8_mmu.ttbr0_mask));
-		armv8->armv8_mmu.os_border = 0xffffffff & armv8->armv8_mmu.ttbr0_mask;
+		LOG_INFO("TTBR0 access above %" PRIx64,
+			 (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
+		armv8->armv8_mmu.os_border = armv8->armv8_mmu.ttbr0_mask;
 	} else {
 		/*  fix me , default is hard coded LINUX border  */
 		armv8->armv8_mmu.os_border = 0xc0000000;
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 9c6ccce..b9e3f12 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -113,7 +113,7 @@ struct armv8_cache_common {
 struct armv8_mmu_common {
 	/* following field mmu working way */
 	int32_t ttbr1_used; /*  -1 not initialized, 0 no ttbr1 1 ttbr1 used and  */
-	uint32_t ttbr0_mask;/*  masked to be used  */
+	uint64_t ttbr0_mask;/*  masked to be used  */
 	uint32_t os_border;
 
 	int (*read_physical_memory)(struct target *target, target_addr_t address,
@@ -141,6 +141,12 @@ struct armv8_common {
 	uint8_t cpu_id;
 	bool is_armv7r;
 
+	/* armv8 aarch64 need below information for page translation */
+	uint8_t va_size;
+	uint8_t pa_size;
+	uint32_t page_size;
+	uint64_t ttbr_base;
+
 	/* cache specific to V7 Memory Management Unit compatible with v4_5*/
 	struct armv8_mmu_common armv8_mmu;
 
-- 
cgit v1.1


From 4314624669fbeb5b4aed7ae33f9a031e022383c5 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 11:49:57 +0200
Subject: aarch64: fix cache identification

Use correct instructions to access CLIDR, CSSELR and CCSIDR.

Change-Id: I319b96c03a44fdb59fcb18a00f816f6af0261f0a
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c |  2 --
 src/target/armv8.c   | 44 ++++++++++++++++++--------------------------
 2 files changed, 18 insertions(+), 28 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index ef73afd..c2cd441 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1235,10 +1235,8 @@ static int aarch64_post_debug_entry(struct target *target)
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
-#if 0
 	if (armv8->armv8_mmu.armv8_cache.ctype == -1)
 		armv8_identify_cache(target);
-#endif
 
 	armv8->armv8_mmu.mmu_enabled =
 			(aarch64->system_control_reg & 0x1U) ? 1 : 0;
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 5a96d96..fda51ef 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -634,23 +634,22 @@ done:
 
 int armv8_identify_cache(struct target *target)
 {
-	/*  read cache descriptor */
+	/*	read cache descriptor */
 	int retval = ERROR_FAIL;
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	uint32_t cache_selected, clidr;
 	uint32_t cache_i_reg, cache_d_reg;
 	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
-	if (!armv8->is_armv7r)
-		armv8_read_ttbcr(target);
+	armv8_read_ttbcr(target);
 	retval = dpm->prepare(dpm);
 
 	if (retval != ERROR_OK)
 		goto done;
-	/*  retrieve CLIDR
-	 *  mrc	p15, 1, r0, c0, c0, 1		@ read clidr */
+	/*	retrieve CLIDR
+	 *	mrc p15, 1, r0, c0, c0, 1		@ read clidr */
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 1, 0, 0, 0, 1),
+			ARMV8_MRS(SYSTEM_CLIDR, 0),
 			&clidr);
 	if (retval != ERROR_OK)
 		goto done;
@@ -658,58 +657,51 @@ int armv8_identify_cache(struct target *target)
 	LOG_INFO("number of cache level %" PRIx32, (uint32_t)(clidr / 2));
 	if ((clidr / 2) > 1) {
 		/* FIXME not supported present in cortex A8 and later */
-		/*  in cortex A7, A15 */
+		/*	in cortex A7, A15 */
 		LOG_ERROR("cache l2 present :not supported");
 	}
-	/*  retrieve selected cache
-	 *  MRC p15, 2,<Rd>, c0, c0, 0; Read CSSELR */
+	/*	retrieve selected cache*/
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+			ARMV8_MRS(SYSTEM_CSSELR, 0),
 			&cache_selected);
 	if (retval != ERROR_OK)
 		goto done;
 
-	retval = armv8->arm.mrc(target, 15,
-			2, 0,	/* op1, op2 */
-			0, 0,	/* CRn, CRm */
-			&cache_selected);
-	if (retval != ERROR_OK)
-		goto done;
+
 	/* select instruction cache
-	 *  MCR p15, 2,<Rd>, c0, c0, 0; Write CSSELR
-	 *  [0]  : 1 instruction cache selection , 0 data cache selection */
+	 *	[0]  : 1 instruction cache selection , 0 data cache selection */
 	retval = dpm->instr_write_data_r0(dpm,
-			ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+			ARMV8_MRS(SYSTEM_CSSELR, 0),
 			1);
 	if (retval != ERROR_OK)
 		goto done;
 
 	/* read CCSIDR
 	 * MRC P15,1,<RT>,C0, C0,0 ;on cortex A9 read CCSIDR
-	 * [2:0] line size  001 eight word per line
+	 * [2:0] line size	001 eight word per line
 	 * [27:13] NumSet 0x7f 16KB, 0xff 32Kbytes, 0x1ff 64Kbytes */
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
+			ARMV8_MRS(SYSTEM_CCSIDR, 0),
 			&cache_i_reg);
 	if (retval != ERROR_OK)
 		goto done;
 
-	/*  select data cache*/
+	/*	select data cache*/
 	retval = dpm->instr_write_data_r0(dpm,
-			ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+			ARMV8_MRS(SYSTEM_CSSELR, 0),
 			0);
 	if (retval != ERROR_OK)
 		goto done;
 
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
+			ARMV8_MRS(SYSTEM_CCSIDR, 0),
 			&cache_d_reg);
 	if (retval != ERROR_OK)
 		goto done;
 
-	/*  restore selected cache  */
+	/*	restore selected cache	*/
 	dpm->instr_write_data_r0(dpm,
-		ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+		ARMV8_MRS(SYSTEM_CSSELR, 0),
 		cache_selected);
 
 	if (retval != ERROR_OK)
-- 
cgit v1.1


From 42574b3a10ca8b10e8b84b84c6427a3b9d09fe13 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:16:19 +0200
Subject: aarch64: correct breakpoint register offset

armv8 breakpoint register spacing is 16, not 4 as in armv7-a

Change-Id: I0d49d06878a0c9dab35cde478064e5366f01a8e0
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 11 ++++-------
 1 file changed, 4 insertions(+), 7 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index c2cd441..4d4b461 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -690,8 +690,8 @@ static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
 		default:
 			return ERROR_FAIL;
 	}
-	vr += 4 * index_t;
-	cr += 4 * index_t;
+	vr += 16 * index_t;
+	cr += 16 * index_t;
 
 	LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
 		(unsigned) vr, (unsigned) cr);
@@ -707,9 +707,6 @@ static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
 
 static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 {
-	return ERROR_OK;
-
-#if 0
 	struct aarch64_common *a = dpm_to_a8(dpm);
 	uint32_t cr;
 
@@ -724,13 +721,13 @@ static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 		default:
 			return ERROR_FAIL;
 	}
-	cr += 4 * index_t;
+	cr += 16 * index_t;
 
 	LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
 
 	/* clear control register */
 	return aarch64_dap_write_memap_register_u32(dpm->arm->target, cr, 0);
-#endif
+
 }
 
 static int aarch64_dpm_setup(struct aarch64_common *a8, uint32_t debug)
-- 
cgit v1.1


From 667c1d0d16784d77c430f3f2a773ad2da4b72ea0 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:17:41 +0200
Subject: aarch64: clear breakpoint value register on removal

Not only null control but also value of the breakpoint when it is
removed.

Change-Id: Id99c7e3644729c64e563f1fa8b0577f350be6a98
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 27 +++++++++++++++++++++++++++
 1 file changed, 27 insertions(+)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 4d4b461..68de65d 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1571,6 +1571,16 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 					brp_list[brp_i].control);
 			if (retval != ERROR_OK)
 				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
+					(uint32_t)brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+					(uint32_t)brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
 			if ((brp_j < 0) || (brp_j >= aarch64->brp_num)) {
 				LOG_DEBUG("Invalid BRP number in breakpoint");
 				return ERROR_OK;
@@ -1585,6 +1595,17 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 					brp_list[brp_j].control);
 			if (retval != ERROR_OK)
 				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_j].BRPn,
+					(uint32_t)brp_list[brp_j].value);
+			if (retval != ERROR_OK)
+				return retval;
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_j].BRPn,
+					(uint32_t)brp_list[brp_j].value);
+			if (retval != ERROR_OK)
+				return retval;
+
 			breakpoint->linked_BRP = 0;
 			breakpoint->set = 0;
 			return ERROR_OK;
@@ -1610,6 +1631,12 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 					brp_list[brp_i].value);
 			if (retval != ERROR_OK)
 				return retval;
+
+			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+					+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_i].BRPn,
+					(uint32_t)brp_list[brp_i].value);
+			if (retval != ERROR_OK)
+				return retval;
 			breakpoint->set = 0;
 			return ERROR_OK;
 		}
-- 
cgit v1.1


From 03861123d7ea0f031643afc0faa87027c58cbd11 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:18:47 +0200
Subject: aarch64: remove armv7-a virt-to-phys code

Page table layout in aarch64 is very different from armv7-a layout.
Remove the incorrect handling, to be replaced correct armv8 code in a
later patch

Change-Id: I64c728a72a24f9f4177726ccc07a02a8ca0d56ce
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c |   8 ++---
 src/target/armv8.c   | 100 +++++++--------------------------------------------
 src/target/armv8.h   |  12 +++++++
 3 files changed, 27 insertions(+), 93 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 68de65d..a2ed8b5 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2623,12 +2623,8 @@ static int aarch64_virt2phys(struct target *target, target_addr_t virt,
 		if (retval != ERROR_OK)
 			goto done;
 		*phys = ret;
-	} else {/*  use this method if armv8->memory_ap not selected
-		 *  mmu must be enable in order to get a correct translation */
-		retval = aarch64_mmu_modify(target, 1);
-		if (retval != ERROR_OK)
-			goto done;
-		retval = armv8_mmu_translate_va_pa(target, virt,  phys, 1);
+	} else {
+		LOG_ERROR("AAR64 processor not support translate va to pa");
 	}
 done:
 	return retval;
diff --git a/src/target/armv8.c b/src/target/armv8.c
index fda51ef..f7c6e31 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -383,104 +383,30 @@ done:
 	return retval;
 }
 
+static int armv8_4K_translate(struct target *target,  uint32_t va, uint32_t *val)
+{
+	LOG_ERROR("4K page Address translation need to add");
+	return ERROR_FAIL;
+}
+
 
 /*  method adapted to cortex A : reused arm v4 v5 method*/
 int armv8_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val)
 {
-	uint32_t first_lvl_descriptor = 0x0;
-	uint32_t second_lvl_descriptor = 0x0;
-	int retval;
+	int retval = ERROR_FAIL;
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
-	uint32_t ttb = 0;	/*  default ttb0 */
-	if (armv8->armv8_mmu.ttbr1_used == -1)
-		armv8_read_ttbcr(target);
-	if ((armv8->armv8_mmu.ttbr1_used) &&
-		(va > (0xffffffff & armv8->armv8_mmu.ttbr0_mask))) {
-		/*  select ttb 1 */
-		ttb = 1;
-	}
+
 	retval = dpm->prepare(dpm);
+	retval += armv8_read_ttbcr(target);
 	if (retval != ERROR_OK)
 		goto done;
-
-	/*  MRC p15,0,<Rt>,c2,c0,ttb */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 0, 0, 2, 0, ttb),
-			&ttb);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = armv8->armv8_mmu.read_physical_memory(target,
-			(ttb & 0xffffc000) | ((va & 0xfff00000) >> 18),
-			4, 1, (uint8_t *)&first_lvl_descriptor);
-	if (retval != ERROR_OK)
-		return retval;
-	first_lvl_descriptor = target_buffer_get_u32(target, (uint8_t *)
-			&first_lvl_descriptor);
-	/*  reuse armv4_5 piece of code, specific armv8 changes may come later */
-	LOG_DEBUG("1st lvl desc: %8.8" PRIx32 "", first_lvl_descriptor);
-
-	if ((first_lvl_descriptor & 0x3) == 0) {
-		LOG_ERROR("Address translation failure");
-		return ERROR_TARGET_TRANSLATION_FAULT;
-	}
-
-
-	if ((first_lvl_descriptor & 0x3) == 2) {
-		/* section descriptor */
-		*val = (first_lvl_descriptor & 0xfff00000) | (va & 0x000fffff);
-		return ERROR_OK;
-	}
-
-	if ((first_lvl_descriptor & 0x3) == 1) {
-		/* coarse page table */
-		retval = armv8->armv8_mmu.read_physical_memory(target,
-				(first_lvl_descriptor & 0xfffffc00) | ((va & 0x000ff000) >> 10),
-				4, 1, (uint8_t *)&second_lvl_descriptor);
-		if (retval != ERROR_OK)
-			return retval;
-	} else if ((first_lvl_descriptor & 0x3) == 3)   {
-		/* fine page table */
-		retval = armv8->armv8_mmu.read_physical_memory(target,
-				(first_lvl_descriptor & 0xfffff000) | ((va & 0x000ffc00) >> 8),
-				4, 1, (uint8_t *)&second_lvl_descriptor);
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	second_lvl_descriptor = target_buffer_get_u32(target, (uint8_t *)
-			&second_lvl_descriptor);
-
-	LOG_DEBUG("2nd lvl desc: %8.8" PRIx32 "", second_lvl_descriptor);
-
-	if ((second_lvl_descriptor & 0x3) == 0) {
-		LOG_ERROR("Address translation failure");
-		return ERROR_TARGET_TRANSLATION_FAULT;
-	}
-
-	if ((second_lvl_descriptor & 0x3) == 1) {
-		/* large page descriptor */
-		*val = (second_lvl_descriptor & 0xffff0000) | (va & 0x0000ffff);
-		return ERROR_OK;
-	}
-
-	if ((second_lvl_descriptor & 0x3) == 2) {
-		/* small page descriptor */
-		*val = (second_lvl_descriptor & 0xfffff000) | (va & 0x00000fff);
-		return ERROR_OK;
-	}
-
-	if ((second_lvl_descriptor & 0x3) == 3) {
-		*val = (second_lvl_descriptor & 0xfffffc00) | (va & 0x000003ff);
-		return ERROR_OK;
-	}
-
-	/* should not happen */
-	LOG_ERROR("Address translation failure");
-	return ERROR_TARGET_TRANSLATION_FAULT;
+	if (armv8->page_size == 0)
+		return armv8_4K_translate(target, va, val);
 
 done:
-	return retval;
+	dpm->finish(dpm);
+	return ERROR_FAIL;
 }
 
 /*  V8 method VA TO PA  */
diff --git a/src/target/armv8.h b/src/target/armv8.h
index b9e3f12..884d39a 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -228,6 +228,18 @@ target_to_armv8(struct target *target)
 #define CTI_GATE			0x140
 #define CTI_UNLOCK			0xFB0
 
+#define PAGE_SIZE_4KB				0x1000
+#define PAGE_SIZE_4KB_LEVEL0_BITS	39
+#define PAGE_SIZE_4KB_LEVEL1_BITS	30
+#define PAGE_SIZE_4KB_LEVEL2_BITS	21
+#define PAGE_SIZE_4KB_LEVEL3_BITS	12
+
+#define PAGE_SIZE_4KB_LEVEL0_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL0_BITS)
+#define PAGE_SIZE_4KB_LEVEL1_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL1_BITS)
+#define PAGE_SIZE_4KB_LEVEL2_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL2_BITS)
+#define PAGE_SIZE_4KB_LEVEL3_MASK	((0x1FFULL) << PAGE_SIZE_4KB_LEVEL3_BITS)
+
+#define PAGE_SIZE_4KB_TRBBASE_MASK	0xFFFFFFFFF000
 
 int armv8_arch_state(struct target *target);
 int armv8_identify_cache(struct target *target);
-- 
cgit v1.1


From f442a530fa14651cf417829e866b877449f29892 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:22:14 +0200
Subject: aarch64: use symbolic constant for register count

Aarch64 has 34 registers, but use ARMV8_LAST_REG instead of
raw integer constant.

Change-Id: I86481899ade74f27fc90eff9f367d444c03e535e
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index f7c6e31..06000c1 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -908,7 +908,7 @@ struct reg *armv8_reg_current(struct arm *arm, unsigned regnum)
 {
 	struct reg *r;
 
-	if (regnum > 33)
+	if (regnum > (ARMV8_LAST_REG - 1))
 		return NULL;
 
 	r = arm->core_cache->reg_list + regnum;
@@ -933,14 +933,13 @@ int armv8_get_gdb_reg_list(struct target *target,
 	switch (reg_class) {
 	case REG_CLASS_GENERAL:
 	case REG_CLASS_ALL:
-		*reg_list_size = 34;
+		*reg_list_size = ARMV8_LAST_REG;
 		*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 
-		for (i = 0; i < *reg_list_size; i++)
+		for (i = 0; i < ARMV8_LAST_REG; i++)
 				(*reg_list)[i] = armv8_reg_current(arm, i);
 
 		return ERROR_OK;
-		break;
 
 	default:
 		LOG_ERROR("not a valid register class type in query.");
-- 
cgit v1.1


From b273ec93c67895e376ce54b6c8086202696d0d81 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:23:27 +0200
Subject: aarch64: correct display for aarch64 state

Aarch64 state has different PSTATE and exception level model.
Correct the printout e.g. in poll command.

Change-Id: I1820fd1836c7076ae0aa405fa335fd1a14a2e5b3
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 40 +++++++++++++++++++++++++++++-----------
 1 file changed, 29 insertions(+), 11 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 06000c1..d6b7dc8 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -724,6 +724,27 @@ int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 	return ERROR_OK;
 }
 
+int armv8_aarch64_state(struct target *target)
+{
+	struct arm *arm = target_to_arm(target);
+
+	if (arm->common_magic != ARM_COMMON_MAGIC) {
+		LOG_ERROR("BUG: called for a non-ARM target");
+		return ERROR_FAIL;
+	}
+
+	LOG_USER("target halted in %s state due to %s, current mode: %s\n"
+		"cpsr: 0x%8.8" PRIx32 " pc: 0x%" PRIx64 "%s",
+		armv8_state_strings[arm->core_state],
+		debug_reason_name(target),
+		armv8_mode_name(arm->core_mode),
+		buf_get_u32(arm->cpsr->value, 0, 32),
+		buf_get_u64(arm->pc->value, 0, 64),
+		arm->is_semihosting ? ", semihosting" : "");
+
+	return ERROR_OK;
+}
+
 int armv8_arch_state(struct target *target)
 {
 	static const char * const state[] = {
@@ -738,18 +759,15 @@ int armv8_arch_state(struct target *target)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 	}
 
-	arm_arch_state(target);
+	if (arm->core_state == ARM_STATE_AARCH64)
+		armv8_aarch64_state(target);
+	else
+		arm_arch_state(target);
 
-	if (armv8->is_armv7r) {
-		LOG_USER("D-Cache: %s, I-Cache: %s",
-			state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
-			state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
-	} else {
-		LOG_USER("MMU: %s, D-Cache: %s, I-Cache: %s",
-			state[armv8->armv8_mmu.mmu_enabled],
-			state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
-			state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
-	}
+	LOG_USER("MMU: %s, D-Cache: %s, I-Cache: %s",
+		state[armv8->armv8_mmu.mmu_enabled],
+		state[armv8->armv8_mmu.armv8_cache.d_u_cache_enabled],
+		state[armv8->armv8_mmu.armv8_cache.i_cache_enabled]);
 
 	if (arm->core_mode == ARM_MODE_ABT)
 		armv8_show_fault_registers(target);
-- 
cgit v1.1


From 6c096b2234b13033e7705b6148a74ed0c1923312 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:26:49 +0200
Subject: aarch64: fix first examination

properly decode debug capabilities, remove superfluous register
accesses.

Change-Id: I2cca699b515262dd2a508d7be97826eb17b9c607
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 93 ++++++++++++++++++++++++----------------------------
 1 file changed, 42 insertions(+), 51 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index a2ed8b5..ae2ecbf 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -730,7 +730,7 @@ static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
 
 }
 
-static int aarch64_dpm_setup(struct aarch64_common *a8, uint32_t debug)
+static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 {
 	struct arm_dpm *dpm = &a8->armv8_common.dpm;
 	int retval;
@@ -2368,9 +2368,12 @@ static int aarch64_examine_first(struct target *target)
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	struct adiv5_dap *swjdp = armv8->arm.dap;
-	int retval = ERROR_OK;
-	uint32_t pfr, debug, ctypr, ttypr, cpuid;
 	int i;
+	int retval = ERROR_OK;
+	uint64_t debug, ttypr;
+	uint32_t cpuid;
+	uint32_t tmp0, tmp1;
+	debug = ttypr = cpuid = 0;
 
 	/* We do one extra read to ensure DAP is configured,
 	 * we call ahbap_debugport_init(swjdp) instead
@@ -2421,91 +2424,79 @@ static int aarch64_examine_first(struct target *target)
 				&armv8->debug_base, &coreidx);
 		if (retval != ERROR_OK)
 			return retval;
-		LOG_DEBUG("Detected core %" PRId32 " dbgbase: %08" PRIx32,
-			  coreidx, armv8->debug_base);
+		LOG_DEBUG("Detected core %" PRId32 " dbgbase: %08" PRIx32
+				" apid: %08" PRIx32, coreidx, armv8->debug_base, apid);
 	} else
 		armv8->debug_base = target->dbgbase;
 
-	LOG_DEBUG("Target ctibase is 0x%x", target->ctibase);
-	if (target->ctibase == 0)
-		armv8->cti_base = target->ctibase = armv8->debug_base + 0x1000;
-	else
-		armv8->cti_base = target->ctibase;
-
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "oslock");
+		LOG_DEBUG("LOCK debug access fail");
 		return retval;
 	}
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x88, &cpuid);
-	LOG_DEBUG("0x88 = %x", cpuid);
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x314, &cpuid);
-	LOG_DEBUG("0x314 = %x", cpuid);
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x310, &cpuid);
-	LOG_DEBUG("0x310 = %x", cpuid);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_CPUID, &cpuid);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "CPUID");
+		LOG_DEBUG("Examine %s failed", "oslock");
 		return retval;
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_CTYPR, &ctypr);
+			armv8->debug_base + CPUV8_DBG_MAINID0, &cpuid);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "CTYPR");
+		LOG_DEBUG("Examine %s failed", "CPUID");
 		return retval;
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUDBG_TTYPR, &ttypr);
+			armv8->debug_base + CPUV8_DBG_MEMFEATURE0, &tmp0);
+	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_MEMFEATURE0 + 4, &tmp1);
 	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "TTYPR");
+		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 + ID_AA64PFR0_EL1, &pfr);
-	if (retval != ERROR_OK) {
-		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
-		return retval;
-	}
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + ID_AA64DFR0_EL1, &debug);
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp0);
+	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp1);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
 		return retval;
 	}
+	debug |= tmp1;
+	debug = (debug << 32) | tmp0;
 
 	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
-	LOG_DEBUG("ctypr = 0x%08" PRIx32, ctypr);
-	LOG_DEBUG("ttypr = 0x%08" PRIx32, ttypr);
-	LOG_DEBUG("ID_AA64PFR0_EL1 = 0x%08" PRIx32, pfr);
-	LOG_DEBUG("ID_AA64DFR0_EL1 = 0x%08" PRIx32, debug);
+	LOG_DEBUG("ttypr = 0x%08" PRIx64, ttypr);
+	LOG_DEBUG("debug = 0x%08" PRIx64, debug);
+
+	if (target->ctibase == 0) {
+		/* assume a v8 rom table layout */
+		armv8->cti_base = target->ctibase = armv8->debug_base + 0x10000;
+		LOG_INFO("Target ctibase is not set, assuming 0x%0" PRIx32, target->ctibase);
+	} else
+		armv8->cti_base = target->ctibase;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->cti_base + CTI_UNLOCK , 0xC5ACCE55);
+	if (retval != ERROR_OK)
+		return retval;
+
 
 	armv8->arm.core_type = ARM_MODE_MON;
-	armv8->arm.core_state = ARM_STATE_AARCH64;
 	retval = aarch64_dpm_setup(aarch64, debug);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Setup Breakpoint Register Pairs */
-	aarch64->brp_num = ((debug >> 12) & 0x0F) + 1;
-	aarch64->brp_num_context = ((debug >> 28) & 0x0F) + 1;
-
-	/* hack - no context bpt support yet */
-	aarch64->brp_num_context = 0;
-
+	aarch64->brp_num = (uint32_t)((debug >> 12) & 0x0F) + 1;
+	aarch64->brp_num_context = (uint32_t)((debug >> 28) & 0x0F) + 1;
 	aarch64->brp_num_available = aarch64->brp_num;
 	aarch64->brp_list = calloc(aarch64->brp_num, sizeof(struct aarch64_brp));
 	for (i = 0; i < aarch64->brp_num; i++) {
-- 
cgit v1.1


From e8602889f9a9fce35eecc8109865fd9ae6d4d05e Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:31:29 +0200
Subject: aarch64: use correct A64 instructions for cache handling

Replace A32 MCR with proper A64 MSR opcodes

Change-Id: I64a60b17a58a26b199d2d1b2d5d91098e0c8cbd0
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 18 +++++++++---------
 src/target/armv8.c   |  3 +--
 2 files changed, 10 insertions(+), 11 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index ae2ecbf..d76da83 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2235,7 +2235,7 @@ static int aarch64_write_phys_memory(struct target *target,
 		 * wrong addresses will be invalidated!
 		 *
 		 * For both ICache and DCache, walk all cache lines in the
-		 * address range. Cortex-A8 has fixed 64 byte line length.
+		 * address range. Cortex-A has fixed 64 byte line length.
 		 *
 		 * REVISIT per ARMv7, these may trigger watchpoints ...
 		 */
@@ -2246,12 +2246,12 @@ static int aarch64_write_phys_memory(struct target *target,
 			 * with MVA to PoU
 			 *      MCR p15, 0, r0, c7, c5, 1
 			 */
-			for (uint32_t cacheline = address;
-				cacheline < address + size * count;
+			for (uint32_t cacheline = 0;
+				cacheline < size * count;
 				cacheline += 64) {
 				retval = dpm->instr_write_data_r0(dpm,
-						ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
-						cacheline);
+						ARMV8_MSR_GP(SYSTEM_ICIVAU, 0),
+						address + cacheline);
 				if (retval != ERROR_OK)
 					return retval;
 			}
@@ -2263,12 +2263,12 @@ static int aarch64_write_phys_memory(struct target *target,
 			 * with MVA to PoC
 			 *      MCR p15, 0, r0, c7, c6, 1
 			 */
-			for (uint32_t cacheline = address;
-				cacheline < address + size * count;
+			for (uint32_t cacheline = 0;
+				cacheline < size * count;
 				cacheline += 64) {
 				retval = dpm->instr_write_data_r0(dpm,
-						ARMV4_5_MCR(15, 0, 0, 7, 6, 1),
-						cacheline);
+						ARMV8_MSR_GP(SYSTEM_DCCVAU, 0),
+						address + cacheline);
 				if (retval != ERROR_OK)
 					return retval;
 			}
diff --git a/src/target/armv8.c b/src/target/armv8.c
index d6b7dc8..176c7ad 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -34,7 +34,6 @@
 #include <unistd.h>
 
 #include "armv8_opcodes.h"
-#include "arm_opcodes.h"
 #include "target.h"
 #include "target_type.h"
 
@@ -465,7 +464,7 @@ static int _armv8_flush_all_data(struct target *target)
 			/*  DCCISW */
 			/* LOG_INFO ("%d %d %x",c_way,c_index,value); */
 			retval = dpm->instr_write_data_r0(dpm,
-					ARMV4_5_MCR(15, 0, 0, 7, 14, 2),
+					ARMV8_MSR_GP(SYSTEM_DCCISW, 0),
 					value);
 			if (retval != ERROR_OK)
 				goto done;
-- 
cgit v1.1


From 4afa32ece148268a07313843c3f0a860c27f3309 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:34:21 +0200
Subject: aarch64: unify armv7-a and armv8 debug entry decoding

Make DSCR_RUN_MODE() usable for armv8 and arm7 debug

Change-Id: Ib3ba3000d5b6aa03e590f3ca4969e677474eb12c
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c  |  8 ++------
 src/target/arm_dpm.c  | 16 ++++++----------
 src/target/arm_dpm.h  | 24 ++++++++++++------------
 src/target/cortex_a.c |  6 +-----
 4 files changed, 21 insertions(+), 33 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d76da83..948a207 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -835,7 +835,7 @@ static int aarch64_poll(struct target *target)
 		return retval;
 	aarch64->cpudbg_dscr = dscr;
 
-	if (DSCR_RUN_MODE(dscr) == (DSCR_CORE_HALTED | DSCR_CORE_RESTARTED)) {
+	if (DSCR_RUN_MODE(dscr) == 0x3) {
 		if (prev_target_state != TARGET_HALTED) {
 			/* We have a halting debug event */
 			LOG_DEBUG("Target halted");
@@ -870,12 +870,8 @@ static int aarch64_poll(struct target *target)
 					TARGET_EVENT_DEBUG_HALTED);
 			}
 		}
-	} else if (DSCR_RUN_MODE(dscr) == DSCR_CORE_RESTARTED)
+	} else
 		target->state = TARGET_RUNNING;
-	else {
-		LOG_DEBUG("Unknown target state dscr = 0x%08" PRIx32, dscr);
-		target->state = TARGET_UNKNOWN;
-	}
 
 	return retval;
 }
diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index 00ebeba..f15bff7 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -930,20 +930,16 @@ void arm_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 
 	/* Examine debug reason */
 	switch (DSCR_ENTRY(dscr)) {
-		case 6:	/* Data abort (v6 only) */
-		case 7:	/* Prefetch abort (v6 only) */
-		/* FALL THROUGH -- assume a v6 core in abort mode */
-		case 0:	/* HALT request from debugger */
-		case 4:	/* EDBGRQ */
+		case DSCR_ENTRY_HALT_REQ:	/* HALT request from debugger */
+		case DSCR_ENTRY_EXT_DBG_REQ:	/* EDBGRQ */
 			target->debug_reason = DBG_REASON_DBGRQ;
 			break;
-		case 1:	/* HW breakpoint */
-		case 3:	/* SW BKPT */
-		case 5:	/* vector catch */
+		case DSCR_ENTRY_BREAKPOINT:	/* HW breakpoint */
+		case DSCR_ENTRY_BKPT_INSTR:	/* vector catch */
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
-		case 2:	/* asynch watchpoint */
-		case 10:/* precise watchpoint */
+		case DSCR_ENTRY_IMPRECISE_WATCHPT:	/* asynch watchpoint */
+		case DSCR_ENTRY_PRECISE_WATCHPT:/* precise watchpoint */
 			target->debug_reason = DBG_REASON_WATCHPOINT;
 			break;
 		default:
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index 27badf4..63467d5 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -186,21 +186,21 @@ void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);
 #define DSCR_DTR_TX_FULL            (0x1 << 29)
 #define DSCR_DTR_RX_FULL            (0x1 << 30) /* bit 31 is reserved */
 
-#define DSCR_ENTRY(dscr)            (((dscr) >> 2) & 0xf)
-#define DSCR_RUN_MODE(dscr)         ((dscr) & (DSCR_CORE_HALTED | DSCR_CORE_RESTARTED))
+#define DSCR_ENTRY(dscr)            ((dscr) & 0x3f)
+#define DSCR_RUN_MODE(dscr)         ((dscr) & 0x03)
 
 
 /* Methods of entry into debug mode */
-#define DSCR_ENTRY_HALT_REQ           (0x0 << 2)
-#define DSCR_ENTRY_BREAKPOINT         (0x1 << 2)
-#define DSCR_ENTRY_IMPRECISE_WATCHPT  (0x2 << 2)
-#define DSCR_ENTRY_BKPT_INSTR         (0x3 << 2)
-#define DSCR_ENTRY_EXT_DBG_REQ        (0x4 << 2)
-#define DSCR_ENTRY_VECT_CATCH         (0x5 << 2)
-#define DSCR_ENTRY_D_SIDE_ABORT       (0x6 << 2)  /* v6 only */
-#define DSCR_ENTRY_I_SIDE_ABORT       (0x7 << 2)  /* v6 only */
-#define DSCR_ENTRY_OS_UNLOCK          (0x8 << 2)
-#define DSCR_ENTRY_PRECISE_WATCHPT    (0xA << 2)
+#define DSCR_ENTRY_HALT_REQ           (0x03)
+#define DSCR_ENTRY_BREAKPOINT         (0x07)
+#define DSCR_ENTRY_IMPRECISE_WATCHPT  (0x0B)
+#define DSCR_ENTRY_BKPT_INSTR         (0x0F)
+#define DSCR_ENTRY_EXT_DBG_REQ        (0x13)
+#define DSCR_ENTRY_VECT_CATCH         (0x17)
+#define DSCR_ENTRY_D_SIDE_ABORT       (0x1B)  /* v6 only */
+#define DSCR_ENTRY_I_SIDE_ABORT       (0x1F)  /* v6 only */
+#define DSCR_ENTRY_OS_UNLOCK          (0x23)
+#define DSCR_ENTRY_PRECISE_WATCHPT    (0x2B)
 
 /* DTR modes */
 #define DSCR_EXT_DCC_NON_BLOCKING     (0x0 << 20)
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 1cf6656..5d90e34 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -879,12 +879,8 @@ static int cortex_a_poll(struct target *target)
 					TARGET_EVENT_DEBUG_HALTED);
 			}
 		}
-	} else if (DSCR_RUN_MODE(dscr) == DSCR_CORE_RESTARTED)
+	} else
 		target->state = TARGET_RUNNING;
-	else {
-		LOG_DEBUG("Unknown target state dscr = 0x%08" PRIx32, dscr);
-		target->state = TARGET_UNKNOWN;
-	}
 
 	return retval;
 }
-- 
cgit v1.1


From 56ab6ab1594e71b690ace9b813fb31b465693acb Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 15:36:09 +0200
Subject: aarch64: fix armv8_set_core_reg when destination is cpsr

When armv8_set_core_reg is used to set the value of
the CPSR, also update the internal architecture state.

Change-Id: I5f6a2be6fde8d91ec3352d8ba23c4aa90eb02977
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 176c7ad..ad06796 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -845,14 +845,20 @@ static int armv8_set_core_reg(struct reg *reg, uint8_t *buf)
 {
 	struct arm_reg *armv8_reg = reg->arch_info;
 	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
 	uint64_t value = buf_get_u64(buf, 0, 64);
 
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	buf_set_u64(reg->value, 0, 64, value);
+	if (reg == arm->cpsr) {
+		armv8_set_cpsr(arm, (uint32_t)value);
+	} else {
+		buf_set_u64(reg->value, 0, 64, value);
+		reg->valid = 1;
+	}
+
 	reg->dirty = 1;
-	reg->valid = 1;
 
 	return ERROR_OK;
 }
-- 
cgit v1.1


From ae7f2094c95ad4195f9136f708d5b95b75874eed Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 1 Sep 2016 22:27:28 +0200
Subject: aarch64: remove references to armv7-r

aarch64 target doesn't support the -r profile anyway.

Change-Id: Iaa470ed9f95ea495ab1bafdf401f55a1ebcefddf
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 27 +++++++++------------------
 src/target/armv8.h   |  1 -
 2 files changed, 9 insertions(+), 19 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 948a207..e215206 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2152,11 +2152,9 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 		size, count);
 
 	/* determine if MMU was enabled on target stop */
-	if (!armv8->is_armv7r) {
-		retval = aarch64_mmu(target, &mmu_enabled);
-		if (retval != ERROR_OK)
-			return retval;
-	}
+	retval = aarch64_mmu(target, &mmu_enabled);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
 		if (mmu_enabled) {
@@ -2209,16 +2207,13 @@ static int aarch64_write_phys_memory(struct target *target,
 		} else {
 
 			/* write memory through APB-AP */
-			if (!armv8->is_armv7r) {
-				retval = aarch64_mmu_modify(target, 0);
-				if (retval != ERROR_OK)
-					return retval;
-			}
+			retval = aarch64_mmu_modify(target, 0);
+			if (retval != ERROR_OK)
+				return retval;
 			return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
 		}
 	}
 
-
 	/* REVISIT this op is generic ARMv7-A/R stuff */
 	if (retval == ERROR_OK && target->state == TARGET_HALTED) {
 		struct arm_dpm *dpm = armv8->arm.dpm;
@@ -2291,11 +2286,9 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 		  "; count %" PRId32, address, size, count);
 
 	/* determine if MMU was enabled on target stop */
-	if (!armv8->is_armv7r) {
-		retval = aarch64_mmu(target, &mmu_enabled);
-		if (retval != ERROR_OK)
-			return retval;
-	}
+	retval = aarch64_mmu(target, &mmu_enabled);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
 		LOG_DEBUG("Writing memory to address 0x%" TARGET_PRIxADDR "; size %"
@@ -2580,8 +2573,6 @@ static int aarch64_target_create(struct target *target, Jim_Interp *interp)
 {
 	struct aarch64_common *aarch64 = calloc(1, sizeof(struct aarch64_common));
 
-	aarch64->armv8_common.is_armv7r = false;
-
 	return aarch64_init_arch_info(target, aarch64, target->tap);
 }
 
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 884d39a..169bcb3 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -139,7 +139,6 @@ struct armv8_common {
 	uint8_t multi_processor_system;
 	uint8_t cluster_id;
 	uint8_t cpu_id;
-	bool is_armv7r;
 
 	/* armv8 aarch64 need below information for page translation */
 	uint8_t va_size;
-- 
cgit v1.1


From 9cc8cfe1b82ee333bbc6ca29bbc94f4419c8cf83 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 15 Sep 2016 09:14:31 +0200
Subject: aarch64: fix stepping from address

The step command optionally carries a resume address. In this case,
stepping should start not at the current PC, but at the given address.

Change-Id: Id5792a3745f470cf29efa90c63d65f33d36f6b25
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index e215206..ae7f5a1 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1264,7 +1264,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		return retval;
 
 	target->debug_reason = DBG_REASON_SINGLESTEP;
-	retval = aarch64_resume(target, 1, address, 0, 0);
+	retval = aarch64_resume(target, current, address, 0, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
-- 
cgit v1.1


From 1826f0af1ca20ce3f8cc8cb0e41f66372a1922d8 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 15 Sep 2016 10:19:42 +0200
Subject: aarch64: remove code for AHB-AP support

Reduce complexity of memory access functions, anyway there are no ARMv8
platforms that actually contain an AHB-AP at all. while at it, fix
virt-to-phys function signatures to expect target_addr_t.

Change-Id: I55a369686f42993988b6323e5a77f38de12530a9
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 202 +++++++--------------------------------------------
 src/target/armv8.c   |   4 +-
 src/target/armv8.h   |   6 +-
 3 files changed, 31 insertions(+), 181 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index ae7f5a1..8c085d5 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2113,26 +2113,16 @@ static int aarch64_read_phys_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
-	struct armv8_common *armv8 = target_to_armv8(target);
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
-	struct adiv5_dap *swjdp = armv8->arm.dap;
-	uint8_t apsel = swjdp->apsel;
 	LOG_DEBUG("Reading memory at real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32,
 		address, size, count);
 
 	if (count && buffer) {
-
-		if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-
-			/* read memory through AHB-AP */
-			retval = mem_ap_read_buf(armv8->memory_ap, buffer, size, count, address);
-		} else {
-			/* read memory through APB-AP */
-			retval = aarch64_mmu_modify(target, 0);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
-		}
+		/* read memory through APB-AP */
+		retval = aarch64_mmu_modify(target, 0);
+		if (retval != ERROR_OK)
+			return retval;
+		retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
 	}
 	return retval;
 }
@@ -2141,11 +2131,7 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, uint8_t *buffer)
 {
 	int mmu_enabled = 0;
-	target_addr_t virt, phys;
 	int retval;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct adiv5_dap *swjdp = armv8->arm.dap;
-	uint8_t apsel = swjdp->apsel;
 
 	/* aarch64 handles unaligned memory access */
 	LOG_DEBUG("Reading memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
@@ -2156,116 +2142,33 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-		if (mmu_enabled) {
-			virt = address;
-			retval = aarch64_virt2phys(target, virt, &phys);
-			if (retval != ERROR_OK)
-				return retval;
-
-			LOG_DEBUG("Reading at virtual address. Translating v:0x%" TARGET_PRIxADDR " to r:0x%" TARGET_PRIxADDR,
-				  virt, phys);
-			address = phys;
-		}
-		retval = aarch64_read_phys_memory(target, address, size, count,
-						  buffer);
-	} else {
-		if (mmu_enabled) {
-			retval = aarch64_check_address(target, address);
-			if (retval != ERROR_OK)
-				return retval;
-			/* enable MMU as we could have disabled it for phys
-			   access */
-			retval = aarch64_mmu_modify(target, 1);
-			if (retval != ERROR_OK)
-				return retval;
-		}
-		retval = aarch64_read_apb_ap_memory(target, address, size,
-						    count, buffer);
+	if (mmu_enabled) {
+		retval = aarch64_check_address(target, address);
+		if (retval != ERROR_OK)
+			return retval;
+		/* enable MMU as we could have disabled it for phys access */
+		retval = aarch64_mmu_modify(target, 1);
+		if (retval != ERROR_OK)
+			return retval;
 	}
-	return retval;
+	return aarch64_read_apb_ap_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_write_phys_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct adiv5_dap *swjdp = armv8->arm.dap;
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
-	uint8_t apsel = swjdp->apsel;
 
 	LOG_DEBUG("Writing memory to real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
 		size, count);
 
 	if (count && buffer) {
-
-		if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-
-			/* write memory through AHB-AP */
-			retval = mem_ap_write_buf(armv8->memory_ap, buffer, size, count, address);
-		} else {
-
-			/* write memory through APB-AP */
-			retval = aarch64_mmu_modify(target, 0);
-			if (retval != ERROR_OK)
-				return retval;
-			return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
-		}
-	}
-
-	/* REVISIT this op is generic ARMv7-A/R stuff */
-	if (retval == ERROR_OK && target->state == TARGET_HALTED) {
-		struct arm_dpm *dpm = armv8->arm.dpm;
-
-		retval = dpm->prepare(dpm);
+		/* write memory through APB-AP */
+		retval = aarch64_mmu_modify(target, 0);
 		if (retval != ERROR_OK)
 			return retval;
-
-		/* The Cache handling will NOT work with MMU active, the
-		 * wrong addresses will be invalidated!
-		 *
-		 * For both ICache and DCache, walk all cache lines in the
-		 * address range. Cortex-A has fixed 64 byte line length.
-		 *
-		 * REVISIT per ARMv7, these may trigger watchpoints ...
-		 */
-
-		/* invalidate I-Cache */
-		if (armv8->armv8_mmu.armv8_cache.i_cache_enabled) {
-			/* ICIMVAU - Invalidate Cache single entry
-			 * with MVA to PoU
-			 *      MCR p15, 0, r0, c7, c5, 1
-			 */
-			for (uint32_t cacheline = 0;
-				cacheline < size * count;
-				cacheline += 64) {
-				retval = dpm->instr_write_data_r0(dpm,
-						ARMV8_MSR_GP(SYSTEM_ICIVAU, 0),
-						address + cacheline);
-				if (retval != ERROR_OK)
-					return retval;
-			}
-		}
-
-		/* invalidate D-Cache */
-		if (armv8->armv8_mmu.armv8_cache.d_u_cache_enabled) {
-			/* DCIMVAC - Invalidate data Cache line
-			 * with MVA to PoC
-			 *      MCR p15, 0, r0, c7, c6, 1
-			 */
-			for (uint32_t cacheline = 0;
-				cacheline < size * count;
-				cacheline += 64) {
-				retval = dpm->instr_write_data_r0(dpm,
-						ARMV8_MSR_GP(SYSTEM_DCCVAU, 0),
-						address + cacheline);
-				if (retval != ERROR_OK)
-					return retval;
-			}
-		}
-
-		/* (void) */ dpm->finish(dpm);
+		return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
 	}
 
 	return retval;
@@ -2275,11 +2178,7 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 	uint32_t size, uint32_t count, const uint8_t *buffer)
 {
 	int mmu_enabled = 0;
-	target_addr_t virt, phys;
 	int retval;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct adiv5_dap *swjdp = armv8->arm.dap;
-	uint8_t apsel = swjdp->apsel;
 
 	/* aarch64 handles unaligned memory access */
 	LOG_DEBUG("Writing memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32
@@ -2290,34 +2189,16 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-		LOG_DEBUG("Writing memory to address 0x%" TARGET_PRIxADDR "; size %"
-			  PRId32 "; count %" PRId32, address, size, count);
-		if (mmu_enabled) {
-			virt = address;
-			retval = aarch64_virt2phys(target, virt, &phys);
-			if (retval != ERROR_OK)
-				return retval;
-
-			LOG_DEBUG("Writing to virtual address. Translating v:0x%"
-				  TARGET_PRIxADDR " to r:0x%" TARGET_PRIxADDR, virt, phys);
-			address = phys;
-		}
-		retval = aarch64_write_phys_memory(target, address, size,
-				count, buffer);
-	} else {
-		if (mmu_enabled) {
-			retval = aarch64_check_address(target, address);
-			if (retval != ERROR_OK)
-				return retval;
-			/* enable MMU as we could have disabled it for phys access */
-			retval = aarch64_mmu_modify(target, 1);
-			if (retval != ERROR_OK)
-				return retval;
-		}
-		retval = aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+	if (mmu_enabled) {
+		retval = aarch64_check_address(target, address);
+		if (retval != ERROR_OK)
+			return retval;
+		/* enable MMU as we could have disabled it for phys access */
+		retval = aarch64_mmu_modify(target, 1);
+		if (retval != ERROR_OK)
+			return retval;
 	}
-	return retval;
+	return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_handle_target_request(void *priv)
@@ -2386,20 +2267,6 @@ static int aarch64_examine_first(struct target *target)
 
 	armv8->debug_ap->memaccess_tck = 80;
 
-	/* Search for the AHB-AB */
-	armv8->memory_ap_available = false;
-	retval = dap_find_ap(swjdp, AP_TYPE_AHB_AP, &armv8->memory_ap);
-	if (retval == ERROR_OK) {
-		retval = mem_ap_init(armv8->memory_ap);
-		if (retval == ERROR_OK)
-			armv8->memory_ap_available = true;
-	}
-	if (retval != ERROR_OK) {
-		/* AHB-AP not found or unavailable - use the CPU */
-		LOG_DEBUG("No AHB-AP available for memory access");
-	}
-
-
 	if (!target->dbgbase_set) {
 		uint32_t dbgbase;
 		/* Get ROM Table base */
@@ -2590,22 +2457,7 @@ static int aarch64_mmu(struct target *target, int *enabled)
 static int aarch64_virt2phys(struct target *target, target_addr_t virt,
 			     target_addr_t *phys)
 {
-	int retval = ERROR_FAIL;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct adiv5_dap *swjdp = armv8->arm.dap;
-	uint8_t apsel = swjdp->apsel;
-	if (armv8->memory_ap_available && (apsel == armv8->memory_ap->ap_num)) {
-		uint32_t ret;
-		retval = armv8_mmu_translate_va(target,
-				virt, &ret);
-		if (retval != ERROR_OK)
-			goto done;
-		*phys = ret;
-	} else {
-		LOG_ERROR("AAR64 processor not support translate va to pa");
-	}
-done:
-	return retval;
+	return armv8_mmu_translate_va(target, virt, phys);
 }
 
 COMMAND_HANDLER(aarch64_handle_cache_info_command)
diff --git a/src/target/armv8.c b/src/target/armv8.c
index ad06796..148ab6d 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -382,7 +382,7 @@ done:
 	return retval;
 }
 
-static int armv8_4K_translate(struct target *target,  uint32_t va, uint32_t *val)
+static int armv8_4K_translate(struct target *target,  target_addr_t va, target_addr_t *val)
 {
 	LOG_ERROR("4K page Address translation need to add");
 	return ERROR_FAIL;
@@ -390,7 +390,7 @@ static int armv8_4K_translate(struct target *target,  uint32_t va, uint32_t *val
 
 
 /*  method adapted to cortex A : reused arm v4 v5 method*/
-int armv8_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val)
+int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr_t *val)
 {
 	int retval = ERROR_FAIL;
 	struct armv8_common *armv8 = target_to_armv8(target);
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 169bcb3..f077045 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -131,10 +131,8 @@ struct armv8_common {
 	struct arm_dpm dpm;
 	uint32_t debug_base;
 	uint32_t cti_base;
-
 	struct adiv5_ap *debug_ap;
-	struct adiv5_ap *memory_ap;
-	bool memory_ap_available;
+
 	/* mdir */
 	uint8_t multi_processor_system;
 	uint8_t cluster_id;
@@ -245,7 +243,7 @@ int armv8_identify_cache(struct target *target);
 int armv8_init_arch_info(struct target *target, struct armv8_common *armv8);
 int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 		target_addr_t *val, int meminfo);
-int armv8_mmu_translate_va(struct target *target,  uint32_t va, uint32_t *val);
+int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr_t *val);
 
 int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 		struct armv8_cache_common *armv8_cache);
-- 
cgit v1.1


From 18ab54b82a4b5660bcd93f2481bd24e73c6a1ce6 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 15 Sep 2016 17:17:05 +0200
Subject: aarch64: fix duplication of register cache

Change-Id: Ib4422e39171f19eea3f0b5a86f9dccdbb7044265
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8_dpm.c | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index dc6682b..493ccca 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -916,9 +916,11 @@ int armv8_dpm_setup(struct arm_dpm *dpm)
 	arm->read_core_reg = armv8_dpm_read_core_reg;
 	arm->write_core_reg = armv8_dpm_write_core_reg;
 
-	cache = armv8_build_reg_cache(target);
-	if (!cache)
-		return ERROR_FAIL;
+	if (arm->core_cache == NULL) {
+		cache = armv8_build_reg_cache(target);
+		if (!cache)
+			return ERROR_FAIL;
+	}
 
 	/* coprocessor access setup */
 	arm->mrc = dpmv8_mrc;
-- 
cgit v1.1


From 11bc04e00c506346b8f35ba8b71d0598ef547f86 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 10:12:00 +0200
Subject: aarch64: fix register list

According to gdb documentation, a register "cpsr" is expected if
aarch64 features are announced. Also, the value buffer must be
capable of holding a 64bit value (8 byte, not 4)

Change-Id: I7aec4e84fa87eadb26797acd0d16c988b9852616
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 66 +++++++++++++++++++++++++++---------------------------
 1 file changed, 33 insertions(+), 33 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 148ab6d..c351dfd 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -785,42 +785,42 @@ static const struct {
 	const char *group;
 	const char *feature;
 } armv8_regs[] = {
-	{ ARMV8_R0,  "x0",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R1,  "x1",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R2,  "x2",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R3,  "x3",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R4,  "x4",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R5,  "x5",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R6,  "x6",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R7,  "x7",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R8,  "x8",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R9,  "x9",  64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R10, "x10", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R11, "x11", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R12, "x12", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R13, "x13", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R14, "x14", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R15, "x15", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R16, "x16", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R17, "x17", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R18, "x18", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R19, "x19", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R20, "x20", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R21, "x21", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R22, "x22", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R23, "x23", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R24, "x24", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R25, "x25", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R26, "x26", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R27, "x27", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R28, "x28", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R29, "x29", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R30, "x30", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R0,  "x0",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R1,  "x1",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R2,  "x2",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R3,  "x3",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R4,  "x4",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R5,  "x5",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R6,  "x6",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R7,  "x7",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R8,  "x8",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R9,  "x9",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R10, "x10", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R11, "x11", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R12, "x12", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R13, "x13", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R14, "x14", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R15, "x15", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R16, "x16", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R17, "x17", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R18, "x18", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R19, "x19", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R20, "x20", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R21, "x21", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R22, "x22", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R23, "x23", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R24, "x24", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R25, "x25", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R26, "x26", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R27, "x27", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R28, "x28", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R29, "x29", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R30, "x30", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
 
 	{ ARMV8_R31, "sp", 64, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
 	{ ARMV8_PC,  "pc", 64, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
 
-	{ ARMV8_xPSR, "CPSR", 64, REG_TYPE_INT, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_xPSR, "CPSR", 32, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" },
 };
 
 #define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
@@ -895,7 +895,7 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 
 		reg_list[i].name = armv8_regs[i].name;
 		reg_list[i].size = armv8_regs[i].bits;
-		reg_list[i].value = calloc(1, 4);
+		reg_list[i].value = calloc(1, 8);
 		reg_list[i].dirty = 0;
 		reg_list[i].valid = 0;
 		reg_list[i].type = &armv8_reg_type;
-- 
cgit v1.1


From df7069af556bd9601ec293a87e8465fb713b3a06 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 12:55:17 +0200
Subject: aarch64: report the correct reason for halting after singlestep

Don't report breakpoint as debug reason when halt is due to a
single-step event.

Change-Id: Ie6c3ca1e5427c73eb726a038301b6a29a47d1217
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c   | 22 ++++++++++------------
 src/target/armv8_dpm.c |  5 ++++-
 2 files changed, 14 insertions(+), 13 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8c085d5..b1534e3 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1246,7 +1246,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	int retval;
-	uint32_t tmp;
+	uint32_t edecr;
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
@@ -1254,25 +1254,26 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	}
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_EDECR, &tmp);
+			armv8->debug_base + CPUV8_DBG_EDECR, &edecr);
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* make sure EDECR.SS is not set when restoring the register */
+	edecr &= ~0x4;
+
+	/* set EDECR.SS to enter hardware step mode */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_EDECR, (tmp|0x4));
+			armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
 	if (retval != ERROR_OK)
 		return retval;
 
-	target->debug_reason = DBG_REASON_SINGLESTEP;
+	/* resume the target */
 	retval = aarch64_resume(target, current, address, 0, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
 	long long then = timeval_ms();
 	while (target->state != TARGET_HALTED) {
-		mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_EDESR, &tmp);
-		LOG_DEBUG("DESR = %#x", tmp);
 		retval = aarch64_poll(target);
 		if (retval != ERROR_OK)
 			return retval;
@@ -1282,15 +1283,12 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		}
 	}
 
+	/* restore EDECR */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_EDECR, (tmp&(~0x4)));
+			armv8->debug_base + CPUV8_DBG_EDECR, edecr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-	if (target->state == TARGET_HALTED)
-		LOG_DEBUG("target stepped");
-
 	return ERROR_OK;
 }
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 493ccca..08b1a20 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -874,11 +874,14 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 			target->debug_reason = DBG_REASON_DBGRQ;
 			break;
 		case DSCRV8_ENTRY_HALT_STEP_EXECLU:	/* HALT step */
+		case DSCRV8_ENTRY_HALT_STEP_NORMAL: /* Halt step*/
+		case DSCRV8_ENTRY_HALT_STEP:
+			target->debug_reason = DBG_REASON_SINGLESTEP;
+			break;
 		case DSCRV8_ENTRY_BKPT:	/* SW BKPT */
 		case DSCRV8_ENTRY_RESET_CATCH:	/* Reset catch */
 		case DSCRV8_ENTRY_OS_UNLOCK:  /*OS unlock catch*/
 		case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
-		case DSCRV8_ENTRY_HALT_STEP_NORMAL: /* Halt step*/
 		case DSCRV8_ENTRY_SW_ACCESS_DBG: /*SW access dbg register*/
 			target->debug_reason = DBG_REASON_BREAKPOINT;
 			break;
-- 
cgit v1.1


From 4246fac240eb28e5a28112b71fc0b5f911e6c922 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 13:46:08 +0200
Subject: aarch64: use correct instruction for software breakpoints

External debuggers need to use HLT, not BRK. HLT generates a halting
debug event while BRK generates a debug exception for self-hosted
debugging.

Change-Id: I24024b83668107f73a14cc75d951134917269e5c
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c       | 2 +-
 src/target/armv8_dpm.c     | 4 ++--
 src/target/armv8_opcodes.h | 3 +++
 3 files changed, 6 insertions(+), 3 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index b1534e3..d38e042 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1369,7 +1369,7 @@ static int aarch64_set_breakpoint(struct target *target,
 
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
-		buf_set_u32(code, 0, 32, ARMV8_BKPT(0x11));
+		buf_set_u32(code, 0, 32, ARMV8_HLT(0x11));
 		retval = target_read_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1,
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 08b1a20..8c0d45a 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -869,7 +869,6 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 	/* Examine debug reason */
 	switch (DSCR_ENTRY(dscr)) {
 		/* FALL THROUGH -- assume a v6 core in abort mode */
-		case DSCRV8_ENTRY_HLT:	/* HALT request from debugger */
 		case DSCRV8_ENTRY_EXT_DEBUG:	/* EDBGRQ */
 			target->debug_reason = DBG_REASON_DBGRQ;
 			break;
@@ -878,7 +877,8 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 		case DSCRV8_ENTRY_HALT_STEP:
 			target->debug_reason = DBG_REASON_SINGLESTEP;
 			break;
-		case DSCRV8_ENTRY_BKPT:	/* SW BKPT */
+		case DSCRV8_ENTRY_HLT:	/* HLT instruction (software breakpoint) */
+		case DSCRV8_ENTRY_BKPT:	/* SW BKPT (?) */
 		case DSCRV8_ENTRY_RESET_CATCH:	/* Reset catch */
 		case DSCRV8_ENTRY_OS_UNLOCK:  /*OS unlock catch*/
 		case DSCRV8_ENTRY_EXCEPTION_CATCH:  /*exception catch*/
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 4f696db..a1fb5d4 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -116,7 +116,10 @@
 /* ARM V8 Move immediate to process state field. */
 #define ARMV8_MSR_IM(Op1, CRm, Op2) \
 	(0xd500401f | ((Op1) << 16)  | ((CRm) << 8) | ((Op2) << 5))
+
 #define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
+#define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5))
+
 #define ARMV8_MOVFSP_64(Rt) ((1 << 31) | 0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_64(Rt) ((1 << 31) | 0x11000000 | (Rt << 5) | (0x1F))
 #define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
-- 
cgit v1.1


From b430d0a1520903213e0796e9fcbc5ab180f1fbe8 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 19 Sep 2016 17:04:03 +0200
Subject: aarch64: disable interrupts when stepping [WIP]

On live hardware, interrupts will happen while the core is
held for stepping. The next step will most of the time execute an
interrupt service instead of the next line of code, which is not
what you expect. Disable interrupts through DSCR before resuming
for a step, and re-enable them again after the step happened.

This should be made configurable, like on cortex_a target.

Change-Id: I94d8ffb58cf7579dedb66bc756b7eb6828b6e8e4
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d38e042..88f9b5a 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1241,6 +1241,28 @@ static int aarch64_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
+static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	uint32_t dscr;
+
+	/* Read DSCR */
+	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	dscr &= ~bit_mask;
+	/* put new value */
+	dscr |= value & bit_mask;
+
+	/* write new DSCR */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	return retval;
+}
+
 static int aarch64_step(struct target *target, int current, target_addr_t address,
 	int handle_breakpoints)
 {
@@ -1267,6 +1289,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* disable interrupts while stepping */
+	retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
+	if (retval != ERROR_OK)
+		return ERROR_OK;
+
 	/* resume the target */
 	retval = aarch64_resume(target, current, address, 0, 0);
 	if (retval != ERROR_OK)
@@ -1289,6 +1316,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* restore interrupts */
+	retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
+	if (retval != ERROR_OK)
+		return ERROR_OK;
+
 	return ERROR_OK;
 }
 
-- 
cgit v1.1


From e17d1d4dc870155010422095272d8a6c16435451 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Tue, 20 Sep 2016 11:16:30 +0200
Subject: aarch64: add cache handling functions

For now only D-Cache flush (Clean&Invalidate) and I-Cache
invalidate are implemented. That's enough for software breakpoints.

Change-Id: I8e96d645a230b51e3490403f4564e59ba6a76cf3
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/Makefile.am     |   4 +-
 src/target/armv8_cache.c   | 122 +++++++++++++++++++++++++++++++++++++++++++++
 src/target/armv8_cache.h   |  26 ++++++++++
 src/target/armv8_opcodes.h |   2 +
 4 files changed, 153 insertions(+), 1 deletion(-)
 create mode 100644 src/target/armv8_cache.c
 create mode 100644 src/target/armv8_cache.h

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index c20ac0f..0021f44 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -76,7 +76,8 @@ ARMV7_SRC = \
 ARMV8_SRC = \
 	%D%/armv8_dpm.c \
 	%D%/aarch64.c \
-	%D%/armv8.c
+	%D%/armv8.c \
+	%D%/armv8_cache.c
 
 ARM_DEBUG_SRC = \
 	%D%/arm_dpm.c \
@@ -158,6 +159,7 @@ INTEL_IA32_SRC = \
 	%D%/armv8.h \
 	%D%/armv8_dpm.h \
 	%D%/armv8_opcodes.h \
+	%D%/armv8_cache.h \
 	%D%/avrt.h \
 	%D%/dsp563xx.h \
 	%D%/dsp563xx_once.h \
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
new file mode 100644
index 0000000..fb18804
--- /dev/null
+++ b/src/target/armv8_cache.c
@@ -0,0 +1,122 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *   matthias.welwarsky@sysgo.com                                          *
+ *                                                                         *
+ *   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 "armv8_cache.h"
+#include "armv8_dpm.h"
+#include "armv8_opcodes.h"
+
+static int armv8_d_cache_sanity_check(struct armv8_common *armv8)
+{
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+
+	if (armv8_cache->d_u_cache_enabled)
+		return ERROR_OK;
+
+	return ERROR_TARGET_INVALID;
+}
+
+static int armv8_i_cache_sanity_check(struct armv8_common *armv8)
+{
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+
+	if (armv8_cache->i_cache_enabled)
+		return ERROR_OK;
+
+	return ERROR_TARGET_INVALID;
+}
+
+int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va, size_t size)
+{
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+	uint64_t linelen = armv8_cache->d_u_size.linelen;
+	target_addr_t va_line, va_end;
+	int retval;
+
+	retval = armv8_d_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	va_line = va & (-linelen);
+	va_end = va + size;
+
+	while (va_line < va_end) {
+		/* DC CIVAC */
+		/* Aarch32: DCCIMVAC: ARMV4_5_MCR(15, 0, 0, 7, 14, 1) */
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_SYS(SYSTEM_DCCIVAC, 0), va_line);
+		if (retval != ERROR_OK)
+			goto done;
+		va_line += linelen;
+	}
+
+	dpm->finish(dpm);
+	return retval;
+
+done:
+	LOG_ERROR("d-cache invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
+
+int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va, size_t size)
+{
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
+	uint64_t linelen = armv8_cache->i_size.linelen;
+	target_addr_t va_line, va_end;
+	int retval;
+
+	retval = armv8_i_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	va_line = va & (-linelen);
+	va_end = va + size;
+
+	while (va_line < va_end) {
+		/* IC IVAU - Invalidate instruction cache by VA to PoU. */
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_SYS(SYSTEM_ICIVAU, 0), va_line);
+		if (retval != ERROR_OK)
+			goto done;
+		va_line += linelen;
+	}
+
+	dpm->finish(dpm);
+	return retval;
+
+done:
+	LOG_ERROR("d-cache invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
diff --git a/src/target/armv8_cache.h b/src/target/armv8_cache.h
new file mode 100644
index 0000000..fa46e16
--- /dev/null
+++ b/src/target/armv8_cache.h
@@ -0,0 +1,26 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *   matthias.welwarsky@sysgo.com                                          *
+ *                                                                         *
+ *   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/>. *
+ ***************************************************************************/
+#ifndef OPENOCD_TARGET_ARMV8_CACHE_H_
+#define OPENOCD_TARGET_ARMV8_CACHE_H_
+
+#include "armv8.h"
+
+extern int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va, size_t size);
+extern int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va, size_t size);
+
+#endif /* OPENOCD_TARGET_ARMV8_CACHE_H_ */
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index a1fb5d4..89d7440 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -82,6 +82,7 @@
 #define SYSTEM_DCCSW			0b0100001111010010
 #define SYSTEM_ICIVAU			0b0101101110101001
 #define SYSTEM_DCCVAU			0b0101101111011001
+#define SYSTEM_DCCIVAC			0b0101101111110001
 
 #define SYSTEM_MPIDR			0b1100000000000101
 
@@ -125,5 +126,6 @@
 #define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_32(Rt) (0x11000000 | (Rt << 5) | (0x1F))
 
+#define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
 #endif /* __ARM_OPCODES_H */
-- 
cgit v1.1


From c6ed75fb0e61cb710c91c935449330ac7b441799 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Tue, 20 Sep 2016 11:29:39 +0200
Subject: aarch64: add cache handling when setting/deleting soft breakpoints

Flush D-Cache before, flush D-Cache and invalidate I-Cache after
modifying the breakpoint location.

Change-Id: Id2e2f4f2545c062de7e27275f66857357496d4ae
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 88f9b5a..1eb4d82 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -27,6 +27,7 @@
 #include "target_request.h"
 #include "target_type.h"
 #include "armv8_opcodes.h"
+#include "armv8_cache.h"
 #include <helper/time_support.h>
 
 static int aarch64_poll(struct target *target);
@@ -1401,6 +1402,7 @@ static int aarch64_set_breakpoint(struct target *target,
 
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
+
 		buf_set_u32(code, 0, 32, ARMV8_HLT(0x11));
 		retval = target_read_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
@@ -1408,11 +1410,25 @@ static int aarch64_set_breakpoint(struct target *target,
 				breakpoint->orig_instr);
 		if (retval != ERROR_OK)
 			return retval;
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
 		retval = target_write_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1, code);
 		if (retval != ERROR_OK)
 			return retval;
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		armv8_cache_i_inner_inval_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
 		breakpoint->set = 0x11;	/* Any nice value but 0 */
 	}
 
@@ -1668,6 +1684,11 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 		}
 	} else {
 		/* restore original instruction (kept in target endianness) */
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
 		if (breakpoint->length == 4) {
 			retval = target_write_memory(target,
 					breakpoint->address & 0xFFFFFFFFFFFFFFFE,
@@ -1681,6 +1702,14 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 			if (retval != ERROR_OK)
 				return retval;
 		}
+
+		armv8_cache_d_inner_flush_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
+
+		armv8_cache_i_inner_inval_virt(armv8,
+				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
+				breakpoint->length);
 	}
 	breakpoint->set = 0;
 
-- 
cgit v1.1


From 6b392dea66bbf72fef74fa8262385157a24f3750 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 29 Sep 2016 14:06:42 +0200
Subject: aarch64: update smp halt and resume to better facilitate CTI

Set up CTI so that halt and resume requests get routed to all PEs in the
SMP group.

Change-Id: Ie92cfd3fe54632e5fdc049a6bf5b24b99451a8c9
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 150 ++++++++++++++++++++++++++-------------------------
 src/target/armv8.h   |   5 ++
 2 files changed, 82 insertions(+), 73 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 1eb4d82..8d74dbd 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -212,19 +212,6 @@ static int aarch64_init_debug_access(struct target *target)
 
 	LOG_DEBUG(" ");
 
-	/* Unlocking the debug registers for modification
-	 * The debugport might be uninitialised so try twice */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			     armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
-	if (retval != ERROR_OK) {
-		/* try again */
-		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			     armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
-		if (retval == ERROR_OK)
-			LOG_USER("Locking debug access failed on first, but succeeded on second try.");
-	}
-	if (retval != ERROR_OK)
-		return retval;
 	/* Clear Sticky Power Down status Bit in PRSR to enable access to
 	   the registers in the Core Power Domain */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -232,7 +219,30 @@ static int aarch64_init_debug_access(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* Enabling of instruction execution in debug mode is done in debug_entry code */
+	/*
+	 * Static CTI configuration:
+	 * Channel 0 -> trigger outputs HALT request to PE
+	 * Channel 1 -> trigger outputs Resume request to PE
+	 * Gate all channel trigger events from entering the CTM
+	 */
+
+	/* Enable CTI */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->cti_base + CTI_CTR, 1);
+	/* By default, gate all channel triggers to and from the CTM */
+	if (retval == ERROR_OK)
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->cti_base + CTI_GATE, 0);
+	/* output halt requests to PE on channel 0 trigger */
+	if (retval == ERROR_OK)
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->cti_base + CTI_OUTEN0, CTI_CHNL(0));
+	/* output restart requests to PE on channel 1 trigger */
+	if (retval == ERROR_OK)
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->cti_base + CTI_OUTEN1, CTI_CHNL(1));
+	if (retval != ERROR_OK)
+		return retval;
 
 	/* Resync breakpoint registers */
 
@@ -783,16 +793,27 @@ static int aarch64_halt(struct target *target);
 
 static int aarch64_halt_smp(struct target *target)
 {
-	int retval = 0;
-	struct target_list *head;
-	struct target *curr;
-	head = target->head;
+	int retval = ERROR_OK;
+	struct target_list *head = target->head;
+
 	while (head != (struct target_list *)NULL) {
-		curr = head->target;
-		if ((curr != target) && (curr->state != TARGET_HALTED))
-			retval += aarch64_halt(curr);
+		struct target *curr = head->target;
+		struct armv8_common *armv8 = target_to_armv8(curr);
+
+		/* open the gate for channel 0 to let HALT requests pass to the CTM */
+		if (curr->smp)
+			retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->cti_base + CTI_GATE, CTI_CHNL(0));
+		if (retval != ERROR_OK)
+			break;
+
 		head = head->next;
 	}
+
+	/* halt the target PE */
+	if (retval == ERROR_OK)
+		retval = aarch64_halt(target);
+
 	return retval;
 }
 
@@ -883,51 +904,23 @@ static int aarch64_halt(struct target *target)
 	uint32_t dscr;
 	struct armv8_common *armv8 = target_to_armv8(target);
 
-	/* enable CTI*/
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_CTR, 1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_GATE, 3);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_OUTEN0, 1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_OUTEN1, 2);
-	if (retval != ERROR_OK)
-		return retval;
-
 	/*
 	 * add HDE in halting debug mode
 	 */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval == ERROR_OK)
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
 	if (retval != ERROR_OK)
 		return retval;
 
+	/* trigger an event on channel 0, this outputs a halt request to the PE */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_APPPULSE, 1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_INACK, 1);
+			armv8->cti_base + CTI_APPPULSE, CTI_CHNL(0));
 	if (retval != ERROR_OK)
 		return retval;
 
-
 	long long then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -1023,7 +1016,7 @@ static int aarch64_internal_restore(struct target *target, int current,
 	return retval;
 }
 
-static int aarch64_internal_restart(struct target *target)
+static int aarch64_internal_restart(struct target *target, bool slave_pe)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm *arm = &armv8->arm;
@@ -1045,22 +1038,39 @@ static int aarch64_internal_restart(struct target *target)
 	if ((dscr & DSCR_ITE) == 0)
 		LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
 
+	/* make sure to acknowledge the halt event before resuming */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_APPPULSE, 2);
+			armv8->cti_base + CTI_INACK, CTI_TRIG(HALT));
+
+	/*
+	 * open the CTI gate for channel 1 so that the restart events
+	 * get passed along to all PEs
+	 */
+	if (retval == ERROR_OK)
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->cti_base + CTI_GATE, CTI_CHNL(1));
 	if (retval != ERROR_OK)
 		return retval;
 
-	long long then = timeval_ms();
-	for (;; ) {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (!slave_pe) {
+		/* trigger an event on channel 1, generates a restart request to the PE */
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->cti_base + CTI_APPPULSE, CTI_CHNL(1));
 		if (retval != ERROR_OK)
 			return retval;
-		if ((dscr & DSCR_HDE) != 0)
-			break;
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for resume");
-			return ERROR_FAIL;
+
+		long long then = timeval_ms();
+		for (;; ) {
+			retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+			if (retval != ERROR_OK)
+				return retval;
+			if ((dscr & DSCR_HDE) != 0)
+				break;
+			if (timeval_ms() > then + 1000) {
+				LOG_ERROR("Timeout waiting for resume");
+				return ERROR_FAIL;
+			}
 		}
 	}
 
@@ -1086,7 +1096,7 @@ static int aarch64_restore_smp(struct target *target, int handle_breakpoints)
 			/*  resume current address , not in step mode */
 			retval += aarch64_internal_restore(curr, 1, &address,
 					handle_breakpoints, 0);
-			retval += aarch64_internal_restart(curr);
+			retval += aarch64_internal_restart(curr, true);
 		}
 		head = head->next;
 
@@ -1117,7 +1127,7 @@ static int aarch64_resume(struct target *target, int current,
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	aarch64_internal_restart(target);
+	aarch64_internal_restart(target, false);
 
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
@@ -2398,12 +2408,6 @@ static int aarch64_examine_first(struct target *target)
 	} else
 		armv8->cti_base = target->ctibase;
 
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_UNLOCK , 0xC5ACCE55);
-	if (retval != ERROR_OK)
-		return retval;
-
-
 	armv8->arm.core_type = ARM_MODE_MON;
 	retval = aarch64_dpm_setup(aarch64, debug);
 	if (retval != ERROR_OK)
diff --git a/src/target/armv8.h b/src/target/armv8.h
index f077045..f6859b2 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -225,6 +225,11 @@ target_to_armv8(struct target *target)
 #define CTI_GATE			0x140
 #define CTI_UNLOCK			0xFB0
 
+#define CTI_CHNL(x)			(1 << x)
+#define CTI_TRIG_HALT		0
+#define CTI_TRIG_RESUME		1
+#define CTI_TRIG(n)			(1 << CTI_TRIG_##n)
+
 #define PAGE_SIZE_4KB				0x1000
 #define PAGE_SIZE_4KB_LEVEL0_BITS	39
 #define PAGE_SIZE_4KB_LEVEL1_BITS	30
-- 
cgit v1.1


From a9931e6a3ce9672a63e05790efa167a677a36da5 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 15 Sep 2016 09:13:51 +0200
Subject: aarch64: add basic Aarch32 support

Add database for common, equivalent opcodes for Aarch32 and
Aarch64 execution states

Revisit all functions that access Aarch64 specific registers
or use Aarch64 opcodes and rewrite them to act depending on
current state of the core.

Add core register access functions for Aarch32 state

Add function to determine the core execution state without
reading DSPSR.

Change-Id: I345e9f6d682fb4ba454e4b1d16bb5e1b27570691
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/Makefile.am     |   1 +
 src/target/aarch64.c       |  43 +++++-------
 src/target/armv8.c         | 166 ++++++++++++++++++++++++++++++++++---------
 src/target/armv8.h         |   7 +-
 src/target/armv8_dpm.c     | 171 +++++++++++++++++++++++++++++++++++++++------
 src/target/armv8_opcodes.c |  66 +++++++++++++++++
 src/target/armv8_opcodes.h |  35 ++++++++--
 7 files changed, 406 insertions(+), 83 deletions(-)
 create mode 100644 src/target/armv8_opcodes.c

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index 0021f44..e936d3f 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -75,6 +75,7 @@ ARMV7_SRC = \
 
 ARMV8_SRC = \
 	%D%/armv8_dpm.c \
+	%D%/armv8_opcodes.c \
 	%D%/aarch64.c \
 	%D%/armv8.c \
 	%D%/armv8_cache.c
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 8d74dbd..824a042 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -90,7 +90,10 @@ static int aarch64_restore_system_control_reg(struct target *target)
 					return retval;
 			break;
 			default:
-				LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+				retval = armv8->arm.mcr(target, 15, 0, 0, 1, 0, aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+				break;
 			}
 	}
 	return retval;
@@ -531,6 +534,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t data)
 {
 	struct aarch64_common *a8 = dpm_to_a8(dpm);
+
 	uint32_t dscr = DSCR_ITE;
 	int retval;
 
@@ -539,9 +543,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 		return retval;
 
 	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
-			&dscr);
+			a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, READ_REG_DTRRX), &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -584,12 +586,11 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
 {
 	struct target *target = dpm->arm->target;
+	struct armv8_common *armv8 = target_to_armv8(target);
 	uint32_t dscr = DSCR_ITE;
 
 	/* "Prefetch flush" after modifying execution status in CPSR */
-	return aarch64_exec_opcode(target,
-			DSB_SY,
-			&dscr);
+	return aarch64_exec_opcode(target, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dscr);
 }
 
 static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
@@ -645,9 +646,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 
 	/* write R0 to DCC */
 	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),  /* msr dbgdtr_el0, x0 */
-			&dscr);
+			a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, WRITE_REG_DTRTX), &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -999,20 +998,6 @@ static int aarch64_internal_restore(struct target *target, int current,
 	/* registers are now invalid */
 	register_cache_invalidate(arm->core_cache);
 
-#if 0
-	/* the front-end may request us not to handle breakpoints */
-	if (handle_breakpoints) {
-		/* Single step past breakpoint at current address */
-		breakpoint = breakpoint_find(target, resume_pc);
-		if (breakpoint) {
-			LOG_DEBUG("unset breakpoint at 0x%8.8x", breakpoint->address);
-			cortex_m3_unset_breakpoint(target, breakpoint);
-			cortex_m3_single_step_core(target);
-			cortex_m3_set_breakpoint(target, breakpoint);
-		}
-	}
-#endif
-
 	return retval;
 }
 
@@ -1202,8 +1187,10 @@ static int aarch64_post_debug_entry(struct target *target)
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	int retval;
 
+	/* clear sticky errors */
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				    armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
+				    armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+
 	switch (armv8->arm.core_mode) {
 		case ARMV8_64_EL0T:
 		case ARMV8_64_EL1T:
@@ -1234,8 +1221,12 @@ static int aarch64_post_debug_entry(struct target *target)
 				return retval;
 		break;
 		default:
-			LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+			retval = armv8->arm.mrc(target, 15, 0, 0, 1, 0, &aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+			break;
 	}
+
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
diff --git a/src/target/armv8.c b/src/target/armv8.c
index c351dfd..306a06e 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -38,7 +38,7 @@
 #include "target_type.h"
 
 static const char * const armv8_state_strings[] = {
-	"ARM", "Thumb", "Jazelle", "ThumbEE", "ARM64",
+	"AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
 };
 
 static const struct {
@@ -53,6 +53,30 @@ static const struct {
 	/* These special modes are currently only supported
 	 * by ARMv6M and ARMv7M profiles */
 	{
+		.name = "USR",
+		.psr = ARM_MODE_USR,
+	},
+	{
+		.name = "FIQ",
+		.psr = ARM_MODE_FIQ,
+	},
+	{
+		.name = "IRQ",
+		.psr = ARM_MODE_IRQ,
+	},
+	{
+		.name = "SVC",
+		.psr = ARM_MODE_SVC,
+	},
+	{
+		.name = "MON",
+		.psr = ARM_MODE_MON,
+	},
+	{
+		.name = "ABT",
+		.psr = ARM_MODE_ABT,
+	},
+	{
 		.name = "EL0T",
 		.psr = ARMV8_64_EL0T,
 	},
@@ -260,9 +284,59 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 		armv8_state_strings[arm->core_state]);
 }
 
+static void armv8_show_fault_registers32(struct armv8_common *armv8)
+{
+	uint32_t dfsr, ifsr, dfar, ifar;
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	int retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return;
+
+	/* ARMV4_5_MRC(cpnum, op1, r0, CRn, CRm, op2) */
+
+	/* c5/c0 - {data, instruction} fault status registers */
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 0)),
+			&dfsr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 1)),
+			&ifsr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* c6/c0 - {data, instruction} fault address registers */
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 0)),
+			&dfar);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 2)),
+			&ifar);
+	if (retval != ERROR_OK)
+		goto done;
+
+	LOG_USER("Data fault registers        DFSR: %8.8" PRIx32
+		", DFAR: %8.8" PRIx32, dfsr, dfar);
+	LOG_USER("Instruction fault registers IFSR: %8.8" PRIx32
+		", IFAR: %8.8" PRIx32, ifsr, ifar);
+
+done:
+	/* (void) */ dpm->finish(dpm);
+}
+
 static void armv8_show_fault_registers(struct target *target)
 {
-	/* TODO */
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	if (armv8->arm.core_state != ARM_STATE_AARCH64)
+		armv8_show_fault_registers32(armv8);
 }
 
 static uint8_t armv8_pa_size(uint32_t ps)
@@ -294,6 +368,45 @@ static uint8_t armv8_pa_size(uint32_t ps)
 	return ret;
 }
 
+static int armv8_read_ttbcr32(struct target *target)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t ttbcr, ttbcr_n;
+	int retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+	/*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
+	retval = dpm->instr_read_data_r0(dpm,
+			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 2, 0, 2)),
+			&ttbcr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	LOG_DEBUG("ttbcr %" PRIx32, ttbcr);
+
+	ttbcr_n = ttbcr & 0x7;
+	armv8->armv8_mmu.ttbcr = ttbcr;
+
+	/*
+	 * ARM Architecture Reference Manual (ARMv7-A and ARMv7-Redition),
+	 * document # ARM DDI 0406C
+	 */
+	armv8->armv8_mmu.ttbr_range[0]  = 0xffffffff >> ttbcr_n;
+	armv8->armv8_mmu.ttbr_range[1] = 0xffffffff;
+	armv8->armv8_mmu.ttbr_mask[0] = 0xffffffff << (14 - ttbcr_n);
+	armv8->armv8_mmu.ttbr_mask[1] = 0xffffffff << 14;
+
+	LOG_DEBUG("ttbr1 %s, ttbr0_mask %" PRIx32 " ttbr1_mask %" PRIx32,
+		  (ttbcr_n != 0) ? "used" : "not used",
+		  armv8->armv8_mmu.ttbr_mask[0],
+		  armv8->armv8_mmu.ttbr_mask[1]);
+
+done:
+	dpm->finish(dpm);
+	return retval;
+}
+
 static int armv8_read_ttbcr(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
@@ -528,14 +641,13 @@ static int armv8_read_mpidr(struct target *target)
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	uint32_t mpidr;
+
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		goto done;
 	/* MRC p15,0,<Rd>,c0,c0,5; read Multiprocessor ID register*/
 
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_MPIDR, 0),
-			&mpidr);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
 	if (retval != ERROR_OK)
 		goto done;
 	if (mpidr & 1<<31) {
@@ -566,18 +678,21 @@ int armv8_identify_cache(struct target *target)
 	uint32_t cache_selected, clidr;
 	uint32_t cache_i_reg, cache_d_reg;
 	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
-	armv8_read_ttbcr(target);
-	retval = dpm->prepare(dpm);
+	int is_aarch64 = armv8->arm.core_state == ARM_STATE_AARCH64;
 
+	retval = is_aarch64 ? armv8_read_ttbcr(target) : armv8_read_ttbcr32(target);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		goto done;
-	/*	retrieve CLIDR
-	 *	mrc p15, 1, r0, c0, c0, 1		@ read clidr */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CLIDR, 0),
-			&clidr);
+
+	/*	retrieve CLIDR */
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CLIDR), &clidr);
 	if (retval != ERROR_OK)
 		goto done;
+
 	clidr = (clidr & 0x7000000) >> 23;
 	LOG_INFO("number of cache level %" PRIx32, (uint32_t)(clidr / 2));
 	if ((clidr / 2) > 1) {
@@ -586,18 +701,13 @@ int armv8_identify_cache(struct target *target)
 		LOG_ERROR("cache l2 present :not supported");
 	}
 	/*	retrieve selected cache*/
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CSSELR, 0),
-			&cache_selected);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CSSELR), &cache_selected);
 	if (retval != ERROR_OK)
 		goto done;
 
-
 	/* select instruction cache
 	 *	[0]  : 1 instruction cache selection , 0 data cache selection */
-	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CSSELR, 0),
-			1);
+	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 1);
 	if (retval != ERROR_OK)
 		goto done;
 
@@ -605,30 +715,21 @@ int armv8_identify_cache(struct target *target)
 	 * MRC P15,1,<RT>,C0, C0,0 ;on cortex A9 read CCSIDR
 	 * [2:0] line size	001 eight word per line
 	 * [27:13] NumSet 0x7f 16KB, 0xff 32Kbytes, 0x1ff 64Kbytes */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CCSIDR, 0),
-			&cache_i_reg);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_i_reg);
 	if (retval != ERROR_OK)
 		goto done;
 
 	/*	select data cache*/
-	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CSSELR, 0),
-			0);
+	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 0);
 	if (retval != ERROR_OK)
 		goto done;
 
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CCSIDR, 0),
-			&cache_d_reg);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_d_reg);
 	if (retval != ERROR_OK)
 		goto done;
 
 	/*	restore selected cache	*/
-	dpm->instr_write_data_r0(dpm,
-		ARMV8_MRS(SYSTEM_CSSELR, 0),
-		cache_selected);
-
+	dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), cache_selected);
 	if (retval != ERROR_OK)
 		goto done;
 	dpm->finish(dpm);
@@ -770,6 +871,7 @@ int armv8_arch_state(struct target *target)
 
 	if (arm->core_mode == ARM_MODE_ABT)
 		armv8_show_fault_registers(target);
+
 	if (target->debug_reason == DBG_REASON_WATCHPOINT)
 		LOG_USER("Watchpoint triggered at PC %#08x",
 			(unsigned) armv8->dpm.wp_pc);
diff --git a/src/target/armv8.h b/src/target/armv8.h
index f6859b2..fa3674b 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -116,6 +116,10 @@ struct armv8_mmu_common {
 	uint64_t ttbr0_mask;/*  masked to be used  */
 	uint32_t os_border;
 
+	uint32_t ttbcr;     /* cache for ttbcr register */
+	uint32_t ttbr_mask[2];
+	uint32_t ttbr_range[2];
+
 	int (*read_physical_memory)(struct target *target, target_addr_t address,
 			uint32_t size, uint32_t count, uint8_t *buffer);
 	struct armv8_cache_common armv8_cache;
@@ -133,6 +137,8 @@ struct armv8_common {
 	uint32_t cti_base;
 	struct adiv5_ap *debug_ap;
 
+	const uint32_t *opcodes;
+
 	/* mdir */
 	uint8_t multi_processor_system;
 	uint8_t cluster_id;
@@ -144,7 +150,6 @@ struct armv8_common {
 	uint32_t page_size;
 	uint64_t ttbr_base;
 
-	/* cache specific to V7 Memory Management Unit compatible with v4_5*/
 	struct armv8_mmu_common armv8_mmu;
 
 	/* Direct processor core register read and writes */
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 8c0d45a..c3d5ec4 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -164,6 +164,7 @@ static int dpmv8_msr(struct target *target, uint32_t op0,
  */
 int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
+	struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
 	int retval;
 	uint32_t cpsr;
 
@@ -199,7 +200,7 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	}
 
 
-	retval = dpm->instr_write_data_r0(dpm, ARMV8_MSR_DSPSR(0), cpsr);
+	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_DSPSR), cpsr);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -209,6 +210,86 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	return retval;
 }
 
+static int dpmv8_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	uint32_t value;
+	int retval = ERROR_FAIL;
+	bool valid = true;
+
+	switch (regnum) {
+	case 0 ... 14:
+		/* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
+		retval = dpm->instr_read_data_dcc(dpm,
+			T32_FMTITR(ARMV4_5_MCR(14, 0, regnum, 0, 5, 0)),
+			&value);
+		break;
+		case ARMV8_R31:
+			retval = dpm->instr_read_data_dcc(dpm,
+				T32_FMTITR(ARMV4_5_MCR(14, 0, 13, 0, 5, 0)),
+				&value);
+				break;
+		case ARMV8_PC:
+			retval = dpm->instr_read_data_r0(dpm,
+				T32_FMTITR(ARMV8_MRC_DLR(0)),
+				&value);
+			break;
+		case ARMV8_xPSR:
+			retval = dpm->instr_read_data_r0(dpm,
+				T32_FMTITR(ARMV8_MRC_DSPSR(0)),
+				&value);
+			break;
+		default:
+			LOG_DEBUG("READ: %s ignored", r->name);
+			retval = ERROR_OK;
+			value = 0xFFFFFFFF;
+			valid = false;
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->valid = valid;
+		r->dirty = false;
+		buf_set_u64(r->value, 0, 32, value);
+		LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+	}
+	return retval;
+}
+
+static int dpmv8_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
+{
+	int retval;
+	uint64_t value = buf_get_u64(r->value, 0, 32);
+
+	switch (regnum) {
+		case 0 ... 14:
+			/* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
+			retval = dpm->instr_write_data_dcc(dpm,
+					T32_FMTITR(ARMV4_5_MRC(14, 0, regnum, 0, 5, 0)), value);
+			break;
+		case ARMV8_PC:/* PC
+			 * read r0 from DCC; then "MOV pc, r0" */
+			retval = dpm->instr_write_data_r0(dpm,
+					T32_FMTITR(ARMV8_MCR_DLR(0)), value);
+			break;
+		case ARMV8_xPSR: /* CPSR */
+			/* read r0 from DCC, then "MCR r0, DSPSR" */
+			retval = dpm->instr_write_data_r0(dpm,
+					T32_FMTITR(ARMV8_MCR_DSPSR(0)), value);
+			break;
+		default:
+			retval = ERROR_OK;
+			LOG_DEBUG("WRITE: %s ignored", r->name);
+			break;
+	}
+
+	if (retval == ERROR_OK) {
+		r->dirty = false;
+		LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value);
+	}
+
+	return retval;
+}
+
 /* just read the register -- rely on the core mode being right */
 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
@@ -222,20 +303,21 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 				ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum),
 				&value_64);
 			break;
-		case 31:
+		case ARMV8_R31:
 			retval = dpm->instr_read_data_r0_64(dpm,
 				ARMV8_MOVFSP_64(0),
 				&value_64);
 			break;
-		case 32:
+		case ARMV8_PC:
 			retval = dpm->instr_read_data_r0_64(dpm,
 				ARMV8_MRS_DLR(0),
 				&value_64);
 			break;
-		case 33:
+		case ARMV8_xPSR:
 			retval = dpm->instr_read_data_r0(dpm,
 				ARMV8_MRS_DSPSR(0),
 				&value);
+			break;
 		default:
 			LOG_DEBUG("READ: %s fail", r->name);
 			break;
@@ -244,11 +326,13 @@ static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	if (retval == ERROR_OK) {
 		r->valid = true;
 		r->dirty = false;
-		buf_set_u64(r->value, 0, 32, value_64);
-		if (r->size == 64)
+		if (r->size == 64) {
+			buf_set_u64(r->value, 0, 64, value_64);
 			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
-		else
+		} else {
+			buf_set_u32(r->value, 0, 32, value);
 			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
+		}
 	}
 	return retval;
 }
@@ -267,23 +351,24 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
 				value_64);
 			break;
-		case 31:
+		case ARMV8_R31:
 			value_64 = buf_get_u64(r->value, 0, 64);
 			retval = dpm->instr_write_data_r0_64(dpm,
 				ARMV8_MOVTSP_64(0),
 				value_64);
 			break;
-		case 32:
+		case ARMV8_PC:
 			value_64 = buf_get_u64(r->value, 0, 64);
 			retval = dpm->instr_write_data_r0_64(dpm,
 				ARMV8_MSR_DLR(0),
 				value_64);
 			break;
-		case 33:
+		case ARMV8_xPSR:
 			value = buf_get_u32(r->value, 0, 32);
 			retval = dpm->instr_write_data_r0(dpm,
 				ARMV8_MSR_DSPSR(0),
 				value);
+			break;
 		default:
 			LOG_DEBUG("write: %s fail", r->name);
 			break;
@@ -301,6 +386,36 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 	return retval;
 }
 
+static inline enum arm_state dpm_get_core_state(uint32_t dscr)
+{
+	int el = (dscr >> 8) & 0x3;
+	int rw = (dscr >> 10) & 0xF;
+
+	LOG_DEBUG("EL:%i, RW:0x%x", el, rw);
+
+	/* DSCR.RW = 0b1111 - all EL are using AArch64 state */
+	if (rw == 0xF)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b1110 - all EL > 0 are using AArch64 state */
+	if (rw == 0xE && el > 0)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b110x - all EL > 1 are using Aarch64 state */
+	if ((rw & 0xE) == 0xC && el > 1)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b10xx - all EL > 2 are using Aarch64 state */
+	if ((rw & 0xC) == 0x8 && el > 2)
+		return ARM_STATE_AARCH64;
+
+	/* DSCR.RW = 0b0xxx - all EL are using AArch32 state */
+	if ((rw & 0x8) == 0)
+		return ARM_STATE_ARM;
+
+	return ARM_STATE_ARM;
+}
+
 /**
  * Read basic registers of the the current context:  R0 to R15, and CPSR;
  * sets the core mode (such as USR or IRQ) and state (such as ARM or Thumb).
@@ -311,7 +426,10 @@ static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
+	struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info;
+	enum arm_state core_state;
 	uint32_t cpsr;
+
 	int retval;
 	struct reg *r;
 
@@ -319,16 +437,22 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	if (retval != ERROR_OK)
 		return retval;
 
+	core_state = dpm_get_core_state(dpm->dscr);
+
+	armv8_select_opcodes(armv8, core_state);
+
 	/* read R0 first (it's used for scratch), then CPSR */
 	r = arm->core_cache->reg_list + 0;
 	if (!r->valid) {
-		retval = dpmv8_read_reg(dpm, r, 0);
+		retval = core_state == ARM_STATE_AARCH64 ?
+					dpmv8_read_reg(dpm, r, 0) : dpmv8_read_reg32(dpm, r, 0);
 		if (retval != ERROR_OK)
 			goto fail;
 	}
 	r->dirty = true;
+
 	/* read cpsr to r0 and get it back */
-	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS_DSPSR(0), &cpsr);
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
 	if (retval != ERROR_OK)
 		goto fail;
 
@@ -341,7 +465,9 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 		if (r->valid)
 			continue;
 
-		retval = dpmv8_read_reg(dpm, r, i);
+		retval = core_state == ARM_STATE_AARCH64 ?
+					dpmv8_read_reg(dpm, r, i) : dpmv8_read_reg32(dpm, r, i);
+
 		if (retval != ERROR_OK)
 			goto fail;
 	}
@@ -419,6 +545,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 	struct arm *arm = dpm->arm;
 	struct reg_cache *cache = arm->core_cache;
 	int retval;
+	bool is_aarch64 = arm->core_state == ARM_STATE_AARCH64;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
@@ -480,9 +607,8 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		r = cache->reg_list[i].arch_info;
 		regnum = r->num;
 
-		retval = dpmv8_write_reg(dpm,
-				&cache->reg_list[i],
-				regnum);
+		retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[i], regnum)
+				: dpmv8_write_reg32(dpm, &cache->reg_list[i], regnum);
 		if (retval != ERROR_OK)
 			goto done;
 	}
@@ -497,13 +623,15 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 		goto done;
 	arm->cpsr->dirty = false;
 
-	retval = dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2));
+	retval = is_aarch64 ? dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2))
+			: dpmv8_write_reg32(dpm, arm->pc, (arm->core_cache->num_regs - 2));
 	if (retval != ERROR_OK)
 		goto done;
 	arm->pc->dirty = false;
 
 	/* flush R0 -- it's *very* dirty by now */
-	retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
+	retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[0], 0)
+			: dpmv8_write_reg32(dpm, &cache->reg_list[0], 0);
 	if (retval != ERROR_OK)
 		goto done;
 	cache->reg_list[0].dirty = false;
@@ -538,7 +666,8 @@ static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = dpmv8_read_reg(dpm, r, regnum);
+	retval = arm->core_state == ARM_STATE_AARCH64 ?
+			dpmv8_read_reg(dpm, r, regnum) : dpmv8_read_reg32(dpm, r, regnum);
 	if (retval != ERROR_OK)
 		goto fail;
 
@@ -566,7 +695,9 @@ static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = dpmv8_write_reg(dpm, r, regnum);
+	retval = arm->core_state == ARM_STATE_AARCH64 ?
+			dpmv8_write_reg(dpm, r, regnum) : dpmv8_write_reg32(dpm, r, regnum);
+
 	/* always clean up, regardless of error */
 
 	/* (void) */ dpm->finish(dpm);
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
new file mode 100644
index 0000000..78b60e0
--- /dev/null
+++ b/src/target/armv8_opcodes.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2015 by Matthias Welwarsky <matthias.welwarsky@sysgo.com>
+ *
+ * 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.
+ *
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "armv8.h"
+#include "armv8_opcodes.h"
+
+static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CLIDR]	= ARMV8_MRS(SYSTEM_CLIDR, 0),
+		[READ_REG_CSSELR]	= ARMV8_MRS(SYSTEM_CSSELR, 0),
+		[READ_REG_CCSIDR]	= ARMV8_MRS(SYSTEM_CCSIDR, 0),
+		[WRITE_REG_CSSELR]	= ARMV8_MSR_GP(SYSTEM_CSSELR, 0),
+		[READ_REG_MPIDR]	= ARMV8_MRS(SYSTEM_MPIDR, 0),
+		[READ_REG_DTRRX]	= ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
+		[WRITE_REG_DTRTX]	= ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),
+		[WRITE_REG_DSPSR]	= ARMV8_MSR_DSPSR(0),
+		[READ_REG_DSPSR]	= ARMV8_MRS_DSPSR(0),
+		[ARMV8_OPC_DSB_SY]  = ARMV8_DSB_SY,
+};
+
+static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CLIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 1)),
+		[READ_REG_CSSELR]	= T32_FMTITR(ARMV4_5_MRC(15, 2, 0, 0, 0, 0)),
+		[READ_REG_CCSIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 0)),
+		[WRITE_REG_CSSELR]	= T32_FMTITR(ARMV4_5_MCR(15, 2, 0, 0, 0, 0)),
+		[READ_REG_MPIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 0, 0, 5)),
+		[READ_REG_DTRRX]	= T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)),
+		[WRITE_REG_DTRTX]	= T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)),
+		[WRITE_REG_DSPSR]	= T32_FMTITR(ARMV8_MCR_DSPSR(0)),
+		[READ_REG_DSPSR]	= T32_FMTITR(ARMV8_MRC_DSPSR(0)),
+		[ARMV8_OPC_DSB_SY]	= T32_FMTITR(ARMV8_DSB_SY_T1),
+};
+
+void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)
+{
+	if (state_is_aarch64)
+		armv8->opcodes = &a64_opcodes[0];
+	else
+		armv8->opcodes = &t32_opcodes[0];
+}
+
+uint32_t armv8_opcode(struct armv8_common *armv8, enum armv8_opcode code)
+{
+	if ((int)code >= ARMV8_OPC_NUM)
+		return -1;
+
+	return *(armv8->opcodes + code);
+}
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 89d7440..9f5ebf8 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -95,8 +95,6 @@
 #define SYSTEM_TTBR0_EL3		0b1111000100000000
 #define SYSTEM_TTBR1_EL1		0b1100000100000001
 
-
-
 #define ARMV8_MRS_DSPSR(Rt)	(0xd53b4500 | (Rt))
 #define ARMV8_MSR_DSPSR(Rt)	(0xd51b4500 | (Rt))
 #define ARMV8_MRS_DLR(Rt)	(0xd53b4520 | (Rt))
@@ -105,11 +103,23 @@
 /* T32 ITR format */
 #define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
 
+/* T32 instruction to access coprocessor registers */
+#define ARMV8_MCR_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MCR(cp, opc1, Rt, CRn, CRm, opc2)
+#define ARMV8_MRC_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MRC(cp, opc1, Rt, CRn, CRm, opc2)
+
+/* T32 instructions to access DSPSR and DLR */
+#define ARMV8_MRC_DSPSR(Rt) ARMV8_MRC_T1(15, 4, 3, 5, 0, Rt)
+#define ARMV8_MCR_DSPSR(Rt) ARMV8_MCR_T1(15, 4, 3, 5, 0, Rt)
+#define ARMV8_MRC_DLR(Rt)	ARMV8_MRC_T1(15, 4, 3, 5, 1, Rt)
+#define ARMV8_MCR_DLR(Rt)	ARMV8_MCR_T1(15, 4, 3, 5, 1, Rt)
+
 #define ARMV8_DCPS1(IM)	(0xd4a00001 | (((IM) & 0xFFFF) << 5))
 #define ARMV8_DCPS2(IM)	(0xd4a00002 | (((IM) & 0xFFFF) << 5))
 #define ARMV8_DCPS3(IM)	(0xd4a00003 | (((IM) & 0xFFFF) << 5))
 
-#define DSB_SY				0xd5033F9F
+#define ARMV8_DSB_SY				0xd5033F9F
+#define ARMV8_DSB_SY_T1				0xf3bf8f4f
+
 #define ARMV8_MRS(System, Rt)	(0xd5300000 | ((System) << 5) | (Rt))
 /* ARM V8 Move to system register. */
 #define ARMV8_MSR_GP(System, Rt) \
@@ -128,4 +138,21 @@
 
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
-#endif /* __ARM_OPCODES_H */
+enum armv8_opcode {
+	READ_REG_CLIDR,
+	READ_REG_CSSELR,
+	READ_REG_CCSIDR,
+	WRITE_REG_CSSELR,
+	READ_REG_MPIDR,
+	READ_REG_DTRRX,
+	WRITE_REG_DTRTX,
+	WRITE_REG_DSPSR,
+	READ_REG_DSPSR,
+	ARMV8_OPC_DSB_SY,
+	ARMV8_OPC_NUM,
+};
+
+extern uint32_t armv8_opcode(struct armv8_common *armv8, enum armv8_opcode);
+extern void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64);
+
+#endif /* OPENOCD_TARGET_ARMV8_OPCODES_H */
-- 
cgit v1.1


From ef263a45a272366836da10b7c42c916d43527f86 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 22 Sep 2016 21:16:31 +0200
Subject: aarch64: refactor armv8 dpm

Move all DPM related functions from aarch64.c to armv8_dpm.c.

Change-Id: I43404ff5db414ae898787a523d3219e5bee44889
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c   | 523 ++-----------------------------------------------
 src/target/armv8_dpm.c | 427 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 440 insertions(+), 510 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 824a042..1d2abc7 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -46,8 +46,6 @@ 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,
 	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
-static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
-	uint32_t opcode, uint32_t data);
 
 static int aarch64_restore_system_control_reg(struct target *target)
 {
@@ -253,61 +251,6 @@ static int aarch64_init_debug_access(struct target *target)
 	return aarch64_poll(target);
 }
 
-/* To reduce needless round-trips, pass in a pointer to the current
- * DSCR value.  Initialize it to zero if you just need to know the
- * value on return from this function; or DSCR_ITE if you
- * happen to know that no instruction is pending.
- */
-static int aarch64_exec_opcode(struct target *target,
-	uint32_t opcode, uint32_t *dscr_p)
-{
-	uint32_t dscr;
-	int retval;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	dscr = dscr_p ? *dscr_p : 0;
-
-	LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
-
-	/* Wait for InstrCompl bit to be set */
-	long long then = timeval_ms();
-	while ((dscr & DSCR_ITE) == 0) {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
-			return retval;
-		}
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
-			return ERROR_FAIL;
-		}
-	}
-
-	retval = mem_ap_write_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_ITR, opcode);
-	if (retval != ERROR_OK)
-		return retval;
-
-	then = timeval_ms();
-	do {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-		if (retval != ERROR_OK) {
-			LOG_ERROR("Could not read DSCR register");
-			return retval;
-		}
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
-			return ERROR_FAIL;
-		}
-	} while ((dscr & DSCR_ITE) == 0);	/* Wait for InstrCompl bit to be set */
-
-	if (dscr_p)
-		*dscr_p = dscr;
-
-	return retval;
-}
-
 /* Write to memory mapped registers directly with no cache or mmu handling */
 static int aarch64_dap_write_memap_register_u32(struct target *target,
 	uint32_t address,
@@ -321,425 +264,6 @@ static int aarch64_dap_write_memap_register_u32(struct target *target,
 	return retval;
 }
 
-/*
- * AARCH64 implementation of Debug Programmer's Model
- *
- * NOTE the invariant:  these routines return with DSCR_ITE set,
- * so there's no need to poll for it before executing an instruction.
- *
- * NOTE that in several of these cases the "stall" mode might be useful.
- * It'd let us queue a few operations together... prepare/finish might
- * be the places to enable/disable that mode.
- */
-
-static inline struct aarch64_common *dpm_to_a8(struct arm_dpm *dpm)
-{
-	return container_of(dpm, struct aarch64_common, armv8_common.dpm);
-}
-
-static int aarch64_write_dcc(struct armv8_common *armv8, uint32_t data)
-{
-	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
-	return mem_ap_write_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DTRRX, data);
-}
-
-static int aarch64_write_dcc_64(struct armv8_common *armv8, uint64_t data)
-{
-	int ret;
-	LOG_DEBUG("write DCC Low word0x%08" PRIx32, (unsigned)data);
-	LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
-	ret = mem_ap_write_u32(armv8->debug_ap,
-			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
-	ret += mem_ap_write_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
-	return ret;
-}
-
-static int aarch64_read_dcc(struct armv8_common *armv8, uint32_t *data,
-	uint32_t *dscr_p)
-{
-	uint32_t dscr = DSCR_ITE;
-	int retval;
-
-	if (dscr_p)
-		dscr = *dscr_p;
-
-	/* Wait for DTRRXfull */
-	long long then = timeval_ms();
-	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR,
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for read dcc");
-			return ERROR_FAIL;
-		}
-	}
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					    armv8->debug_base + CPUV8_DBG_DTRTX,
-					    data);
-	if (retval != ERROR_OK)
-		return retval;
-	LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
-
-	if (dscr_p)
-		*dscr_p = dscr;
-
-	return retval;
-}
-
-static int aarch64_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
-	uint32_t *dscr_p)
-{
-	uint32_t dscr = DSCR_ITE;
-	uint32_t higher;
-	int retval;
-
-	if (dscr_p)
-		dscr = *dscr_p;
-
-	/* Wait for DTRRXfull */
-	long long then = timeval_ms();
-	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR,
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for read dcc");
-			return ERROR_FAIL;
-		}
-	}
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					    armv8->debug_base + CPUV8_DBG_DTRTX,
-					    (uint32_t *)data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					    armv8->debug_base + CPUV8_DBG_DTRRX,
-					    &higher);
-	if (retval != ERROR_OK)
-		return retval;
-
-	*data = *(uint32_t *)data | (uint64_t)higher << 32;
-	LOG_DEBUG("read DCC 0x%16.16" PRIx64, *data);
-
-	if (dscr_p)
-		*dscr_p = dscr;
-
-	return retval;
-}
-
-static int aarch64_dpm_prepare(struct arm_dpm *dpm)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr;
-	int retval;
-
-	/* set up invariant:  INSTR_COMP is set after ever DPM operation */
-	long long then = timeval_ms();
-	for (;; ) {
-		retval = mem_ap_read_atomic_u32(a8->armv8_common.debug_ap,
-				a8->armv8_common.debug_base + CPUV8_DBG_DSCR,
-				&dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		if ((dscr & DSCR_ITE) != 0)
-			break;
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for dpm prepare");
-			return ERROR_FAIL;
-		}
-	}
-
-	/* this "should never happen" ... */
-	if (dscr & DSCR_DTR_RX_FULL) {
-		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
-		/* Clear DCCRX */
-		retval = mem_ap_read_u32(a8->armv8_common.debug_ap,
-			a8->armv8_common.debug_base + CPUV8_DBG_DTRRX, &dscr);
-		if (retval != ERROR_OK)
-			return retval;
-
-		/* Clear sticky error */
-		retval = mem_ap_write_u32(a8->armv8_common.debug_ap,
-			a8->armv8_common.debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-		if (retval != ERROR_OK)
-			return retval;
-	}
-
-	return retval;
-}
-
-static int aarch64_dpm_finish(struct arm_dpm *dpm)
-{
-	/* REVISIT what could be done here? */
-	return ERROR_OK;
-}
-
-static int aarch64_instr_execute(struct arm_dpm *dpm,
-	uint32_t opcode)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_ITE;
-
-	return aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-}
-
-static int aarch64_instr_write_data_dcc(struct arm_dpm *dpm,
-	uint32_t opcode, uint32_t data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	int retval;
-	uint32_t dscr = DSCR_ITE;
-
-	retval = aarch64_write_dcc(&a8->armv8_common, data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-}
-
-static int aarch64_instr_write_data_dcc_64(struct arm_dpm *dpm,
-	uint32_t opcode, uint64_t data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	int retval;
-	uint32_t dscr = DSCR_ITE;
-
-	retval = aarch64_write_dcc_64(&a8->armv8_common, data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-}
-
-static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
-	uint32_t opcode, uint32_t data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-
-	uint32_t dscr = DSCR_ITE;
-	int retval;
-
-	retval = aarch64_write_dcc(&a8->armv8_common, data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, READ_REG_DTRRX), &dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* then the opcode, taking data from R0 */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-
-	return retval;
-}
-
-static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
-	uint32_t opcode, uint64_t data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_ITE;
-	int retval;
-
-	retval = aarch64_write_dcc_64(&a8->armv8_common, data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0),
-			&dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* then the opcode, taking data from R0 */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-
-	return retval;
-}
-
-static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
-{
-	struct target *target = dpm->arm->target;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	uint32_t dscr = DSCR_ITE;
-
-	/* "Prefetch flush" after modifying execution status in CPSR */
-	return aarch64_exec_opcode(target, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dscr);
-}
-
-static int aarch64_instr_read_data_dcc(struct arm_dpm *dpm,
-	uint32_t opcode, uint32_t *data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	int retval;
-	uint32_t dscr = DSCR_ITE;
-
-	/* the opcode, writing data to DCC */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return aarch64_read_dcc(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_instr_read_data_dcc_64(struct arm_dpm *dpm,
-	uint32_t opcode, uint64_t *data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	int retval;
-	uint32_t dscr = DSCR_ITE;
-
-	/* the opcode, writing data to DCC */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return aarch64_read_dcc_64(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
-	uint32_t opcode, uint32_t *data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_ITE;
-	int retval;
-
-	/* the opcode, writing data to R0 */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* write R0 to DCC */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target, armv8_opcode(&a8->armv8_common, WRITE_REG_DTRTX), &dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return aarch64_read_dcc(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
-	uint32_t opcode, uint64_t *data)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t dscr = DSCR_ITE;
-	int retval;
-
-	/* the opcode, writing data to R0 */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			opcode,
-			&dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* write R0 to DCC */
-	retval = aarch64_exec_opcode(
-			a8->armv8_common.arm.target,
-			ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0),  /* msr dbgdtr_el0, x0 */
-			&dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return aarch64_read_dcc_64(&a8->armv8_common, data, &dscr);
-}
-
-static int aarch64_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
-	uint32_t addr, uint32_t control)
-{
-	struct aarch64_common *a8 = dpm_to_a8(dpm);
-	uint32_t vr = a8->armv8_common.debug_base;
-	uint32_t cr = a8->armv8_common.debug_base;
-	int retval;
-
-	switch (index_t) {
-		case 0 ... 15:	/* breakpoints */
-			vr += CPUV8_DBG_BVR_BASE;
-			cr += CPUV8_DBG_BCR_BASE;
-			break;
-		case 16 ... 31:	/* watchpoints */
-			vr += CPUV8_DBG_WVR_BASE;
-			cr += CPUV8_DBG_WCR_BASE;
-			index_t -= 16;
-			break;
-		default:
-			return ERROR_FAIL;
-	}
-	vr += 16 * index_t;
-	cr += 16 * index_t;
-
-	LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
-		(unsigned) vr, (unsigned) cr);
-
-	retval = aarch64_dap_write_memap_register_u32(dpm->arm->target,
-			vr, addr);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = aarch64_dap_write_memap_register_u32(dpm->arm->target,
-			cr, control);
-	return retval;
-}
-
-static int aarch64_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
-{
-	struct aarch64_common *a = dpm_to_a8(dpm);
-	uint32_t cr;
-
-	switch (index_t) {
-		case 0 ... 15:
-			cr = a->armv8_common.debug_base + CPUV8_DBG_BCR_BASE;
-			break;
-		case 16 ... 31:
-			cr = a->armv8_common.debug_base + CPUV8_DBG_WCR_BASE;
-			index_t -= 16;
-			break;
-		default:
-			return ERROR_FAIL;
-	}
-	cr += 16 * index_t;
-
-	LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
-
-	/* clear control register */
-	return aarch64_dap_write_memap_register_u32(dpm->arm->target, cr, 0);
-
-}
-
 static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 {
 	struct arm_dpm *dpm = &a8->armv8_common.dpm;
@@ -748,32 +272,13 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 	dpm->arm = &a8->armv8_common.arm;
 	dpm->didr = debug;
 
-	dpm->prepare = aarch64_dpm_prepare;
-	dpm->finish = aarch64_dpm_finish;
-
-	dpm->instr_execute = aarch64_instr_execute;
-	dpm->instr_write_data_dcc = aarch64_instr_write_data_dcc;
-	dpm->instr_write_data_dcc_64 = aarch64_instr_write_data_dcc_64;
-	dpm->instr_write_data_r0 = aarch64_instr_write_data_r0;
-	dpm->instr_write_data_r0_64 = aarch64_instr_write_data_r0_64;
-	dpm->instr_cpsr_sync = aarch64_instr_cpsr_sync;
-
-	dpm->instr_read_data_dcc = aarch64_instr_read_data_dcc;
-	dpm->instr_read_data_dcc_64 = aarch64_instr_read_data_dcc_64;
-	dpm->instr_read_data_r0 = aarch64_instr_read_data_r0;
-	dpm->instr_read_data_r0_64 = aarch64_instr_read_data_r0_64;
-
-	dpm->arm_reg_current = armv8_reg_current;
-
-	dpm->bpwp_enable = aarch64_bpwp_enable;
-	dpm->bpwp_disable = aarch64_bpwp_disable;
-
 	retval = armv8_dpm_setup(dpm);
 	if (retval == ERROR_OK)
 		retval = armv8_dpm_initialize(dpm);
 
 	return retval;
 }
+
 static struct target *get_aarch64(struct target *target, int32_t coreid)
 {
 	struct target_list *head;
@@ -1853,6 +1358,7 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	/* write memory through APB-AP */
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
 	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;
@@ -1937,17 +1443,15 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	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 */
-		retval += aarch64_write_dcc_64(armv8, address & ~0x3ULL);
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
-		retval += aarch64_exec_opcode(target,
-				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+		retval = dpm->instr_write_data_dcc_64(dpm,
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRX */
-		retval += aarch64_write_dcc(armv8, address & ~0x3ULL);
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-		retval += aarch64_exec_opcode(target,
-				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), &dscr);
+		dpm->instr_write_data_dcc(dpm,
+				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL);
 
 	}
 	/* Step 1.d   - Change DCC to memory mode */
@@ -2008,6 +1512,7 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	/* read memory through APB-AP */
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
 	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;
@@ -2058,11 +1563,11 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	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 */
-		retval += aarch64_write_dcc_64(armv8, address & ~0x3ULL);
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
-		retval += aarch64_exec_opcode(target, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+		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 += aarch64_exec_opcode(target, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+		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,
@@ -2073,13 +1578,11 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
-		retval += aarch64_write_dcc(armv8, address & ~0x3ULL);
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-		retval += aarch64_exec_opcode(target,
-				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), &dscr);
+		retval += dpm->instr_write_data_dcc(dpm,
+				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL);
 		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-		retval += aarch64_exec_opcode(target,
-				T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)), &dscr);
+		retval += dpm->instr_execute(dpm, T32_FMTITR(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,
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index c3d5ec4..4bb2075 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -26,6 +26,8 @@
 #include "target_type.h"
 #include "armv8_opcodes.h"
 
+#include "helper/time_support.h"
+
 
 /**
  * @file
@@ -42,6 +44,410 @@
 
 /*----------------------------------------------------------------------*/
 
+static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
+{
+	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
+	return mem_ap_write_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRRX, data);
+}
+
+static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
+{
+	int ret;
+	LOG_DEBUG("write DCC Low word 0x%08" PRIx32, (unsigned)data);
+	LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
+	ret = mem_ap_write_u32(armv8->debug_ap,
+			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
+	ret += mem_ap_write_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
+	return ret;
+}
+
+static int dpmv8_read_dcc(struct armv8_common *armv8, uint32_t *data,
+	uint32_t *dscr_p)
+{
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	if (dscr_p)
+		dscr = *dscr_p;
+
+	/* Wait for DTRRXfull */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for read dcc");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUV8_DBG_DTRTX,
+					    data);
+	if (retval != ERROR_OK)
+		return retval;
+	LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+
+static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
+	uint32_t *dscr_p)
+{
+	uint32_t dscr = DSCR_ITE;
+	uint32_t higher;
+	int retval;
+
+	if (dscr_p)
+		dscr = *dscr_p;
+
+	/* Wait for DTRRXfull */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_DTR_TX_FULL) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for read dcc");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUV8_DBG_DTRTX,
+					    (uint32_t *)data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+					    armv8->debug_base + CPUV8_DBG_DTRRX,
+					    &higher);
+	if (retval != ERROR_OK)
+		return retval;
+
+	*data = *(uint32_t *)data | (uint64_t)higher << 32;
+	LOG_DEBUG("read DCC 0x%16.16" PRIx64, *data);
+
+	if (dscr_p)
+		*dscr_p = dscr;
+
+	return retval;
+}
+
+static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr;
+	int retval;
+
+	/* set up invariant:  INSTR_COMP is set after ever DPM operation */
+	long long then = timeval_ms();
+	for (;; ) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR,
+				&dscr);
+		if (retval != ERROR_OK)
+			return retval;
+		if ((dscr & DSCR_ITE) != 0)
+			break;
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for dpm prepare");
+			return ERROR_FAIL;
+		}
+	}
+
+	/* this "should never happen" ... */
+	if (dscr & DSCR_DTR_RX_FULL) {
+		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
+		/* Clear DCCRX */
+		retval = mem_ap_read_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRRX, &dscr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Clear sticky error */
+		retval = mem_ap_write_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	return retval;
+}
+
+static int dpmv8_dpm_finish(struct arm_dpm *dpm)
+{
+	/* REVISIT what could be done here? */
+	return ERROR_OK;
+}
+
+static int dpmv8_exec_opcode(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *p_dscr)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
+
+	if (p_dscr)
+		dscr = *p_dscr;
+
+	/* Wait for InstrCompl bit to be set */
+	long long then = timeval_ms();
+	while ((dscr & DSCR_ITE) == 0) {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read DSCR register, opcode = 0x%08" PRIx32, opcode);
+			return retval;
+		}
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
+			return ERROR_FAIL;
+		}
+	}
+
+	retval = mem_ap_write_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_ITR, opcode);
+	if (retval != ERROR_OK)
+		return retval;
+
+	then = timeval_ms();
+	do {
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Could not read DSCR register");
+			return retval;
+		}
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("Timeout waiting for aarch64_exec_opcode");
+			return ERROR_FAIL;
+		}
+	} while ((dscr & DSCR_ITE) == 0);	/* Wait for InstrCompl bit to be set */
+
+	if (p_dscr)
+		*p_dscr = dscr;
+
+	return retval;
+}
+
+static int dpmv8_instr_execute(struct arm_dpm *dpm, uint32_t opcode)
+{
+	return dpmv8_exec_opcode(dpm, opcode, NULL);
+}
+
+static int dpmv8_instr_write_data_dcc(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	retval = dpmv8_write_dcc(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_exec_opcode(dpm, opcode, 0);
+}
+
+static int dpmv8_instr_write_data_dcc_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	int retval;
+
+	retval = dpmv8_write_dcc_64(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_exec_opcode(dpm, opcode, 0);
+}
+
+static int dpmv8_instr_write_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	retval = dpmv8_write_dcc(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, READ_REG_DTRRX), &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* then the opcode, taking data from R0 */
+	return dpmv8_exec_opcode(dpm, opcode, &dscr);
+}
+
+static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	retval = dpmv8_write_dcc_64(armv8, data);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* then the opcode, taking data from R0 */
+	return dpmv8_exec_opcode(dpm, opcode, &dscr);
+}
+
+static int dpmv8_instr_cpsr_sync(struct arm_dpm *dpm)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	/* "Prefetch flush" after modifying execution status in CPSR */
+	return dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), NULL);
+}
+
+static int dpmv8_instr_read_data_dcc(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	/* the opcode, writing data to DCC */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc(armv8, data, &dscr);
+}
+
+static int dpmv8_instr_read_data_dcc_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	/* the opcode, writing data to DCC */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc_64(armv8, data, &dscr);
+}
+
+static int dpmv8_instr_read_data_r0(struct arm_dpm *dpm,
+	uint32_t opcode, uint32_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	/* the opcode, writing data to R0 */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write R0 to DCC */
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc(armv8, data, &dscr);
+}
+
+static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
+	uint32_t opcode, uint64_t *data)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t dscr = DSCR_ITE;
+	int retval;
+
+	/* the opcode, writing data to R0 */
+	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* write R0 to DCC */
+	retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return dpmv8_read_dcc_64(armv8, data, &dscr);
+}
+
+#if 0
+static int dpmv8_bpwp_enable(struct arm_dpm *dpm, unsigned index_t,
+	target_addr_t addr, uint32_t control)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t vr = armv8->debug_base;
+	uint32_t cr = armv8->debug_base;
+	int retval;
+
+	switch (index_t) {
+		case 0 ... 15:	/* breakpoints */
+			vr += CPUV8_DBG_BVR_BASE;
+			cr += CPUV8_DBG_BCR_BASE;
+			break;
+		case 16 ... 31:	/* watchpoints */
+			vr += CPUV8_DBG_WVR_BASE;
+			cr += CPUV8_DBG_WCR_BASE;
+			index_t -= 16;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	vr += 16 * index_t;
+	cr += 16 * index_t;
+
+	LOG_DEBUG("A8: bpwp enable, vr %08x cr %08x",
+		(unsigned) vr, (unsigned) cr);
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap, vr, addr);
+	if (retval != ERROR_OK)
+		return retval;
+	return mem_ap_write_atomic_u32(armv8->debug_ap, cr, control);
+}
+#endif
+
+static int dpmv8_bpwp_disable(struct arm_dpm *dpm, unsigned index_t)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	uint32_t cr;
+
+	switch (index_t) {
+		case 0 ... 15:
+			cr = armv8->debug_base + CPUV8_DBG_BCR_BASE;
+			break;
+		case 16 ... 31:
+			cr = armv8->debug_base + CPUV8_DBG_WCR_BASE;
+			index_t -= 16;
+			break;
+		default:
+			return ERROR_FAIL;
+	}
+	cr += 16 * index_t;
+
+	LOG_DEBUG("A: bpwp disable, cr %08x", (unsigned) cr);
+
+	/* clear control register */
+	return mem_ap_write_atomic_u32(armv8->debug_ap, cr, 0);
+}
+
 /*
  * Coprocessor support
  */
@@ -1061,6 +1467,27 @@ int armv8_dpm_setup(struct arm_dpm *dpm)
 	arm->mcr = dpmv8_mcr;
 	arm->mrs = dpmv8_mrs;
 	arm->msr = dpmv8_msr;
+
+	dpm->prepare = dpmv8_dpm_prepare;
+	dpm->finish = dpmv8_dpm_finish;
+
+	dpm->instr_execute = dpmv8_instr_execute;
+	dpm->instr_write_data_dcc = dpmv8_instr_write_data_dcc;
+	dpm->instr_write_data_dcc_64 = dpmv8_instr_write_data_dcc_64;
+	dpm->instr_write_data_r0 = dpmv8_instr_write_data_r0;
+	dpm->instr_write_data_r0_64 = dpmv8_instr_write_data_r0_64;
+	dpm->instr_cpsr_sync = dpmv8_instr_cpsr_sync;
+
+	dpm->instr_read_data_dcc = dpmv8_instr_read_data_dcc;
+	dpm->instr_read_data_dcc_64 = dpmv8_instr_read_data_dcc_64;
+	dpm->instr_read_data_r0 = dpmv8_instr_read_data_r0;
+	dpm->instr_read_data_r0_64 = dpmv8_instr_read_data_r0_64;
+
+	dpm->arm_reg_current = armv8_reg_current;
+
+/*	dpm->bpwp_enable = dpmv8_bpwp_enable; */
+	dpm->bpwp_disable = dpmv8_bpwp_disable;
+
 	/* breakpoint setup -- optional until it works everywhere */
 	if (!target->type->add_breakpoint) {
 		target->type->add_breakpoint = dpmv8_add_breakpoint;
-- 
cgit v1.1


From b69750fd0ded4ee19861673106761627d88734e4 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 22 Sep 2016 21:29:42 +0200
Subject: aarch64: armv8 cache functions update

Update cache identification to match functionality present in
armv7a_cache.c

Change-Id: I2dc4bee80f5a22b8728334d40331c183d1406f27
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c       |   6 +-
 src/target/armv8.c         | 301 ++++++---------------------------------------
 src/target/armv8.h         |  17 ++-
 src/target/armv8_cache.c   | 299 +++++++++++++++++++++++++++++++++++++++++++-
 src/target/armv8_opcodes.h |   1 +
 5 files changed, 353 insertions(+), 271 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 1d2abc7..05abe6b 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -735,8 +735,10 @@ static int aarch64_post_debug_entry(struct target *target)
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
-	if (armv8->armv8_mmu.armv8_cache.ctype == -1)
-		armv8_identify_cache(target);
+	if (armv8->armv8_mmu.armv8_cache.info == -1) {
+		armv8_identify_cache(armv8);
+		armv8_read_mpidr(armv8);
+	}
 
 	armv8->armv8_mmu.mmu_enabled =
 			(aarch64->system_control_reg & 0x1U) ? 1 : 0;
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 306a06e..467c771 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -37,6 +37,8 @@
 #include "target.h"
 #include "target_type.h"
 
+#define __unused __attribute__((unused))
+
 static const char * const armv8_state_strings[] = {
 	"AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
 };
@@ -208,6 +210,38 @@ static int armv8_write_core_reg(struct target *target, struct reg *r,
 	return ERROR_OK;
 }
 #endif
+
+/*  retrieve core id cluster id  */
+int armv8_read_mpidr(struct armv8_common *armv8)
+{
+	int retval = ERROR_FAIL;
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t mpidr;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
+	if (retval != ERROR_OK)
+		goto done;
+	if (mpidr & 1<<31) {
+		armv8->multi_processor_system = (mpidr >> 30) & 1;
+		armv8->cluster_id = (mpidr >> 8) & 0xf;
+		armv8->cpu_id = mpidr & 0x3;
+		LOG_INFO("%s cluster %x core %x %s", target_name(armv8->arm.target),
+			armv8->cluster_id,
+			armv8->cpu_id,
+			armv8->multi_processor_system == 0 ? "multi core" : "mono core");
+
+	} else
+		LOG_ERROR("mpdir not in multiprocessor format");
+
+done:
+	dpm->finish(dpm);
+	return retval;
+}
+
 /**
  * Configures host-side ARM records to reflect the specified CPSR.
  * Later, code can use arm_reg_current() to map register numbers
@@ -368,7 +402,7 @@ static uint8_t armv8_pa_size(uint32_t ps)
 	return ret;
 }
 
-static int armv8_read_ttbcr32(struct target *target)
+static __unused int armv8_read_ttbcr32(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
@@ -528,103 +562,10 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	return ERROR_OK;
 }
 
-static int armv8_handle_inner_cache_info_command(struct command_context *cmd_ctx,
-	struct armv8_cache_common *armv8_cache)
-{
-	if (armv8_cache->ctype == -1) {
-		command_print(cmd_ctx, "cache not yet identified");
-		return ERROR_OK;
-	}
-
-	command_print(cmd_ctx,
-		"D-Cache: linelen %" PRIi32 ", associativity %" PRIi32 ", nsets %" PRIi32 ", cachesize %" PRId32 " KBytes",
-		armv8_cache->d_u_size.linelen,
-		armv8_cache->d_u_size.associativity,
-		armv8_cache->d_u_size.nsets,
-		armv8_cache->d_u_size.cachesize);
-
-	command_print(cmd_ctx,
-		"I-Cache: linelen %" PRIi32 ", associativity %" PRIi32 ", nsets %" PRIi32 ", cachesize %" PRId32 " KBytes",
-		armv8_cache->i_size.linelen,
-		armv8_cache->i_size.associativity,
-		armv8_cache->i_size.nsets,
-		armv8_cache->i_size.cachesize);
-
-	return ERROR_OK;
-}
-
-static int _armv8_flush_all_data(struct target *target)
-{
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct arm_dpm *dpm = armv8->arm.dpm;
-	struct armv8_cachesize *d_u_size =
-		&(armv8->armv8_mmu.armv8_cache.d_u_size);
-	int32_t c_way, c_index = d_u_size->index;
-	int retval;
-	/*  check that cache data is on at target halt */
-	if (!armv8->armv8_mmu.armv8_cache.d_u_cache_enabled) {
-		LOG_INFO("flushed not performed :cache not on at target halt");
-		return ERROR_OK;
-	}
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		goto done;
-	do {
-		c_way = d_u_size->way;
-		do {
-			uint32_t value = (c_index << d_u_size->index_shift)
-				| (c_way << d_u_size->way_shift);
-			/*  DCCISW */
-			/* LOG_INFO ("%d %d %x",c_way,c_index,value); */
-			retval = dpm->instr_write_data_r0(dpm,
-					ARMV8_MSR_GP(SYSTEM_DCCISW, 0),
-					value);
-			if (retval != ERROR_OK)
-				goto done;
-			c_way -= 1;
-		} while (c_way >= 0);
-		c_index -= 1;
-	} while (c_index >= 0);
-	return retval;
-done:
-	LOG_ERROR("flushed failed");
-	dpm->finish(dpm);
-	return retval;
-}
-
-static int  armv8_flush_all_data(struct target *target)
-{
-	int retval = ERROR_FAIL;
-	/*  check that armv8_cache is correctly identify */
-	struct armv8_common *armv8 = target_to_armv8(target);
-	if (armv8->armv8_mmu.armv8_cache.ctype == -1) {
-		LOG_ERROR("trying to flush un-identified cache");
-		return retval;
-	}
-
-	if (target->smp) {
-		/*  look if all the other target have been flushed in order to flush level
-		 *  2 */
-		struct target_list *head;
-		struct target *curr;
-		head = target->head;
-		while (head != (struct target_list *)NULL) {
-			curr = head->target;
-			if (curr->state == TARGET_HALTED) {
-				LOG_INFO("Wait flushing data l1 on core %" PRId32, curr->coreid);
-				retval = _armv8_flush_all_data(curr);
-			}
-			head = head->next;
-		}
-	} else
-		retval = _armv8_flush_all_data(target);
-	return retval;
-}
-
 int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 	struct armv8_cache_common *armv8_cache)
 {
-	if (armv8_cache->ctype == -1) {
+	if (armv8_cache->info == -1) {
 		command_print(cmd_ctx, "cache not yet identified");
 		return ERROR_OK;
 	}
@@ -634,174 +575,6 @@ int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 	return ERROR_OK;
 }
 
-/*  retrieve core id cluster id  */
-static int armv8_read_mpidr(struct target *target)
-{
-	int retval = ERROR_FAIL;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct arm_dpm *dpm = armv8->arm.dpm;
-	uint32_t mpidr;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		goto done;
-	/* MRC p15,0,<Rd>,c0,c0,5; read Multiprocessor ID register*/
-
-	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_MPIDR), &mpidr);
-	if (retval != ERROR_OK)
-		goto done;
-	if (mpidr & 1<<31) {
-		armv8->multi_processor_system = (mpidr >> 30) & 1;
-		armv8->cluster_id = (mpidr >> 8) & 0xf;
-		armv8->cpu_id = mpidr & 0x3;
-		LOG_INFO("%s cluster %x core %x %s", target_name(target),
-			armv8->cluster_id,
-			armv8->cpu_id,
-			armv8->multi_processor_system == 0 ? "multi core" : "mono core");
-
-	} else
-		LOG_ERROR("mpdir not in multiprocessor format");
-
-done:
-	dpm->finish(dpm);
-	return retval;
-
-
-}
-
-int armv8_identify_cache(struct target *target)
-{
-	/*	read cache descriptor */
-	int retval = ERROR_FAIL;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct arm_dpm *dpm = armv8->arm.dpm;
-	uint32_t cache_selected, clidr;
-	uint32_t cache_i_reg, cache_d_reg;
-	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
-	int is_aarch64 = armv8->arm.core_state == ARM_STATE_AARCH64;
-
-	retval = is_aarch64 ? armv8_read_ttbcr(target) : armv8_read_ttbcr32(target);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/*	retrieve CLIDR */
-	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CLIDR), &clidr);
-	if (retval != ERROR_OK)
-		goto done;
-
-	clidr = (clidr & 0x7000000) >> 23;
-	LOG_INFO("number of cache level %" PRIx32, (uint32_t)(clidr / 2));
-	if ((clidr / 2) > 1) {
-		/* FIXME not supported present in cortex A8 and later */
-		/*	in cortex A7, A15 */
-		LOG_ERROR("cache l2 present :not supported");
-	}
-	/*	retrieve selected cache*/
-	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CSSELR), &cache_selected);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/* select instruction cache
-	 *	[0]  : 1 instruction cache selection , 0 data cache selection */
-	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 1);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/* read CCSIDR
-	 * MRC P15,1,<RT>,C0, C0,0 ;on cortex A9 read CCSIDR
-	 * [2:0] line size	001 eight word per line
-	 * [27:13] NumSet 0x7f 16KB, 0xff 32Kbytes, 0x1ff 64Kbytes */
-	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_i_reg);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/*	select data cache*/
-	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), 0);
-	if (retval != ERROR_OK)
-		goto done;
-
-	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_CCSIDR), &cache_d_reg);
-	if (retval != ERROR_OK)
-		goto done;
-
-	/*	restore selected cache	*/
-	dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_CSSELR), cache_selected);
-	if (retval != ERROR_OK)
-		goto done;
-	dpm->finish(dpm);
-
-	/* put fake type */
-	cache->d_u_size.linelen = 16 << (cache_d_reg & 0x7);
-	cache->d_u_size.cachesize = (((cache_d_reg >> 13) & 0x7fff)+1)/8;
-	cache->d_u_size.nsets = (cache_d_reg >> 13) & 0x7fff;
-	cache->d_u_size.associativity = ((cache_d_reg >> 3) & 0x3ff) + 1;
-	/*  compute info for set way operation on cache */
-	cache->d_u_size.index_shift = (cache_d_reg & 0x7) + 4;
-	cache->d_u_size.index = (cache_d_reg >> 13) & 0x7fff;
-	cache->d_u_size.way = ((cache_d_reg >> 3) & 0x3ff);
-	cache->d_u_size.way_shift = cache->d_u_size.way + 1;
-	{
-		int i = 0;
-		while (((cache->d_u_size.way_shift >> i) & 1) != 1)
-			i++;
-		cache->d_u_size.way_shift = 32-i;
-	}
-#if 0
-	LOG_INFO("data cache index %d << %d, way %d << %d",
-			cache->d_u_size.index, cache->d_u_size.index_shift,
-			cache->d_u_size.way,
-			cache->d_u_size.way_shift);
-
-	LOG_INFO("data cache %d bytes %d KBytes asso %d ways",
-			cache->d_u_size.linelen,
-			cache->d_u_size.cachesize,
-			cache->d_u_size.associativity);
-#endif
-	cache->i_size.linelen = 16 << (cache_i_reg & 0x7);
-	cache->i_size.associativity = ((cache_i_reg >> 3) & 0x3ff) + 1;
-	cache->i_size.nsets = (cache_i_reg >> 13) & 0x7fff;
-	cache->i_size.cachesize = (((cache_i_reg >> 13) & 0x7fff)+1)/8;
-	/*  compute info for set way operation on cache */
-	cache->i_size.index_shift = (cache_i_reg & 0x7) + 4;
-	cache->i_size.index = (cache_i_reg >> 13) & 0x7fff;
-	cache->i_size.way = ((cache_i_reg >> 3) & 0x3ff);
-	cache->i_size.way_shift = cache->i_size.way + 1;
-	{
-		int i = 0;
-		while (((cache->i_size.way_shift >> i) & 1) != 1)
-			i++;
-		cache->i_size.way_shift = 32-i;
-	}
-#if 0
-	LOG_INFO("instruction cache index %d << %d, way %d << %d",
-			cache->i_size.index, cache->i_size.index_shift,
-			cache->i_size.way, cache->i_size.way_shift);
-
-	LOG_INFO("instruction cache %d bytes %d KBytes asso %d ways",
-			cache->i_size.linelen,
-			cache->i_size.cachesize,
-			cache->i_size.associativity);
-#endif
-	/*  if no l2 cache initialize l1 data cache flush function function */
-	if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache == NULL) {
-		armv8->armv8_mmu.armv8_cache.display_cache_info =
-			armv8_handle_inner_cache_info_command;
-		armv8->armv8_mmu.armv8_cache.flush_all_data_cache =
-			armv8_flush_all_data;
-	}
-	armv8->armv8_mmu.armv8_cache.ctype = 0;
-
-done:
-	dpm->finish(dpm);
-	armv8_read_mpidr(target);
-	return retval;
-
-}
-
 int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 {
 	struct arm *arm = &armv8->arm;
@@ -818,7 +591,7 @@ int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 #endif
 
 	armv8->armv8_mmu.armv8_cache.l2_cache = NULL;
-	armv8->armv8_mmu.armv8_cache.ctype = -1;
+	armv8->armv8_mmu.armv8_cache.info = -1;
 	armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL;
 	armv8->armv8_mmu.armv8_cache.display_cache_info = NULL;
 	return ERROR_OK;
diff --git a/src/target/armv8.h b/src/target/armv8.h
index fa3674b..2862ebd 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -97,12 +97,22 @@ struct armv8_cachesize {
 	uint32_t way_shift;
 };
 
-struct armv8_cache_common {
-	int ctype;
+/* information about one architecture cache at any level */
+struct armv8_arch_cache {
+	int ctype;				/* cache type, CLIDR encoding */
 	struct armv8_cachesize d_u_size;	/* data cache */
 	struct armv8_cachesize i_size;		/* instruction cache */
+};
+
+struct armv8_cache_common {
+	int info;
+	int loc;
+	uint32_t iminline;
+	uint32_t dminline;
+	struct armv8_arch_cache arch[6];	/* cache info, L1 - L7 */
 	int i_cache_enabled;
 	int d_u_cache_enabled;
+
 	/* l2 external unified cache if some */
 	void *l2_cache;
 	int (*flush_all_data_cache)(struct target *target);
@@ -249,7 +259,8 @@ target_to_armv8(struct target *target)
 #define PAGE_SIZE_4KB_TRBBASE_MASK	0xFFFFFFFFF000
 
 int armv8_arch_state(struct target *target);
-int armv8_identify_cache(struct target *target);
+int armv8_read_mpidr(struct armv8_common *armv8);
+int armv8_identify_cache(struct armv8_common *armv8);
 int armv8_init_arch_info(struct target *target, struct armv8_common *armv8);
 int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 		target_addr_t *val, int meminfo);
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index fb18804..f496c3c 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -24,6 +24,11 @@
 #include "armv8_dpm.h"
 #include "armv8_opcodes.h"
 
+/* CLIDR cache types */
+#define CACHE_LEVEL_HAS_UNIFIED_CACHE	0x4
+#define CACHE_LEVEL_HAS_D_CACHE		0x2
+#define CACHE_LEVEL_HAS_I_CACHE		0x1
+
 static int armv8_d_cache_sanity_check(struct armv8_common *armv8)
 {
 	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
@@ -44,11 +49,72 @@ static int armv8_i_cache_sanity_check(struct armv8_common *armv8)
 	return ERROR_TARGET_INVALID;
 }
 
+static int armv8_cache_d_inner_flush_level(struct arm_dpm *dpm, struct armv8_cachesize *size, int cl)
+{
+	int retval = ERROR_OK;
+	int32_t c_way, c_index = size->index;
+
+	LOG_DEBUG("cl %" PRId32, cl);
+	do {
+		c_way = size->way;
+		do {
+			uint32_t value = (c_index << size->index_shift)
+				| (c_way << size->way_shift) | (cl << 1);
+			/*
+			 * DC CISW - Clean and invalidate data cache
+			 * line by Set/Way.
+			 */
+			retval = dpm->instr_write_data_r0(dpm,
+					ARMV8_SYS(SYSTEM_DCCISW, 0), value);
+			if (retval != ERROR_OK)
+				goto done;
+			c_way -= 1;
+		} while (c_way >= 0);
+		c_index -= 1;
+	} while (c_index >= 0);
+
+ done:
+	return retval;
+}
+
+static int armv8_cache_d_inner_clean_inval_all(struct armv8_common *armv8)
+{
+	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	int cl;
+	int retval;
+
+	retval = armv8_d_cache_sanity_check(armv8);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	for (cl = 0; cl < cache->loc; cl++) {
+		/* skip i-only caches */
+		if (cache->arch[cl].ctype < CACHE_LEVEL_HAS_D_CACHE)
+			continue;
+
+		armv8_cache_d_inner_flush_level(dpm, &cache->arch[cl].d_u_size, cl);
+	}
+
+	retval = dpm->finish(dpm);
+	return retval;
+
+done:
+	LOG_ERROR("clean invalidate failed");
+	dpm->finish(dpm);
+
+	return retval;
+}
+
 int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va, size_t size)
 {
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
-	uint64_t linelen = armv8_cache->d_u_size.linelen;
+	uint64_t linelen = armv8_cache->dminline;
 	target_addr_t va_line, va_end;
 	int retval;
 
@@ -87,7 +153,7 @@ int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va,
 {
 	struct arm_dpm *dpm = armv8->arm.dpm;
 	struct armv8_cache_common *armv8_cache = &armv8->armv8_mmu.armv8_cache;
-	uint64_t linelen = armv8_cache->i_size.linelen;
+	uint64_t linelen = armv8_cache->iminline;
 	target_addr_t va_line, va_end;
 	int retval;
 
@@ -120,3 +186,232 @@ done:
 
 	return retval;
 }
+
+static int armv8_handle_inner_cache_info_command(struct command_context *cmd_ctx,
+	struct armv8_cache_common *armv8_cache)
+{
+	int cl;
+
+	if (armv8_cache->info == -1) {
+		command_print(cmd_ctx, "cache not yet identified");
+		return ERROR_OK;
+	}
+
+	for (cl = 0; cl < armv8_cache->loc; cl++) {
+		struct armv8_arch_cache *arch = &(armv8_cache->arch[cl]);
+
+		if (arch->ctype & 1) {
+			command_print(cmd_ctx,
+				"L%d I-Cache: linelen %" PRIi32
+				", associativity %" PRIi32
+				", nsets %" PRIi32
+				", cachesize %" PRId32 " KBytes",
+				cl+1,
+				arch->i_size.linelen,
+				arch->i_size.associativity,
+				arch->i_size.nsets,
+				arch->i_size.cachesize);
+		}
+
+		if (arch->ctype >= 2) {
+			command_print(cmd_ctx,
+				"L%d D-Cache: linelen %" PRIi32
+				", associativity %" PRIi32
+				", nsets %" PRIi32
+				", cachesize %" PRId32 " KBytes",
+				cl+1,
+				arch->d_u_size.linelen,
+				arch->d_u_size.associativity,
+				arch->d_u_size.nsets,
+				arch->d_u_size.cachesize);
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int _armv8_flush_all_data(struct target *target)
+{
+	return armv8_cache_d_inner_clean_inval_all(target_to_armv8(target));
+}
+
+static int  armv8_flush_all_data(struct target *target)
+{
+	int retval = ERROR_FAIL;
+	/*  check that armv8_cache is correctly identify */
+	struct armv8_common *armv8 = target_to_armv8(target);
+	if (armv8->armv8_mmu.armv8_cache.info == -1) {
+		LOG_ERROR("trying to flush un-identified cache");
+		return retval;
+	}
+
+	if (target->smp) {
+		/*  look if all the other target have been flushed in order to flush level
+		 *  2 */
+		struct target_list *head;
+		struct target *curr;
+		head = target->head;
+		while (head != (struct target_list *)NULL) {
+			curr = head->target;
+			if (curr->state == TARGET_HALTED) {
+				LOG_INFO("Wait flushing data l1 on core %" PRId32, curr->coreid);
+				retval = _armv8_flush_all_data(curr);
+			}
+			head = head->next;
+		}
+	} else
+		retval = _armv8_flush_all_data(target);
+	return retval;
+}
+
+static int get_cache_info(struct arm_dpm *dpm, int cl, int ct, uint32_t *cache_reg)
+{
+	int retval = ERROR_OK;
+
+	/*  select cache level */
+	retval = dpm->instr_write_data_r0(dpm,
+			ARMV8_MSR_GP(SYSTEM_CSSELR, 0),
+			(cl << 1) | (ct == 1 ? 1 : 0));
+	if (retval != ERROR_OK)
+		goto done;
+
+	retval = dpm->instr_read_data_r0(dpm,
+			ARMV8_MRS(SYSTEM_CCSIDR, 0),
+			cache_reg);
+ done:
+	return retval;
+}
+
+static struct armv8_cachesize decode_cache_reg(uint32_t cache_reg)
+{
+	struct armv8_cachesize size;
+	int i = 0;
+
+	size.linelen = 16 << (cache_reg & 0x7);
+	size.associativity = ((cache_reg >> 3) & 0x3ff) + 1;
+	size.nsets = ((cache_reg >> 13) & 0x7fff) + 1;
+	size.cachesize = size.linelen * size.associativity * size.nsets / 1024;
+
+	/*  compute info for set way operation on cache */
+	size.index_shift = (cache_reg & 0x7) + 4;
+	size.index = (cache_reg >> 13) & 0x7fff;
+	size.way = ((cache_reg >> 3) & 0x3ff);
+
+	while (((size.way << i) & 0x80000000) == 0)
+		i++;
+	size.way_shift = i;
+
+	return size;
+}
+
+int armv8_identify_cache(struct armv8_common *armv8)
+{
+	/*  read cache descriptor */
+	int retval = ERROR_FAIL;
+	struct arm_dpm *dpm = armv8->arm.dpm;
+	uint32_t csselr, clidr, ctr;
+	uint32_t cache_reg;
+	int cl, ctype;
+	struct armv8_cache_common *cache = &(armv8->armv8_mmu.armv8_cache);
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* retrieve CTR */
+	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS(SYSTEM_CTR, 0), &ctr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	cache->iminline = 4UL << (ctr & 0xf);
+	cache->dminline = 4UL << ((ctr & 0xf0000) >> 16);
+	LOG_DEBUG("ctr %" PRIx32 " ctr.iminline %" PRId32 " ctr.dminline %" PRId32,
+		 ctr, cache->iminline, cache->dminline);
+
+	/*  retrieve CLIDR */
+	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS(SYSTEM_CLIDR, 0), &clidr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	cache->loc = (clidr & 0x7000000) >> 24;
+	LOG_DEBUG("Number of cache levels to PoC %" PRId32, cache->loc);
+
+	/*  retrieve selected cache for later restore
+	 *  MRC p15, 2,<Rd>, c0, c0, 0; Read CSSELR */
+	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS(SYSTEM_CSSELR, 0), &csselr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	/* retrieve all available inner caches */
+	for (cl = 0; cl < cache->loc; clidr >>= 3, cl++) {
+
+		/* isolate cache type at current level */
+		ctype = clidr & 7;
+
+		/* skip reserved values */
+		if (ctype > CACHE_LEVEL_HAS_UNIFIED_CACHE)
+			continue;
+
+		/* separate d or unified d/i cache at this level ? */
+		if (ctype & (CACHE_LEVEL_HAS_UNIFIED_CACHE | CACHE_LEVEL_HAS_D_CACHE)) {
+			/* retrieve d-cache info */
+			retval = get_cache_info(dpm, cl, 0, &cache_reg);
+			if (retval != ERROR_OK)
+				goto done;
+			cache->arch[cl].d_u_size = decode_cache_reg(cache_reg);
+
+			LOG_DEBUG("data/unified cache index %d << %d, way %d << %d",
+					cache->arch[cl].d_u_size.index,
+					cache->arch[cl].d_u_size.index_shift,
+					cache->arch[cl].d_u_size.way,
+					cache->arch[cl].d_u_size.way_shift);
+
+			LOG_DEBUG("cacheline %d bytes %d KBytes asso %d ways",
+					cache->arch[cl].d_u_size.linelen,
+					cache->arch[cl].d_u_size.cachesize,
+					cache->arch[cl].d_u_size.associativity);
+		}
+
+		/* separate i-cache at this level ? */
+		if (ctype & CACHE_LEVEL_HAS_I_CACHE) {
+			/* retrieve i-cache info */
+			retval = get_cache_info(dpm, cl, 1, &cache_reg);
+			if (retval != ERROR_OK)
+				goto done;
+			cache->arch[cl].i_size = decode_cache_reg(cache_reg);
+
+			LOG_DEBUG("instruction cache index %d << %d, way %d << %d",
+					cache->arch[cl].i_size.index,
+					cache->arch[cl].i_size.index_shift,
+					cache->arch[cl].i_size.way,
+					cache->arch[cl].i_size.way_shift);
+
+			LOG_DEBUG("cacheline %d bytes %d KBytes asso %d ways",
+					cache->arch[cl].i_size.linelen,
+					cache->arch[cl].i_size.cachesize,
+					cache->arch[cl].i_size.associativity);
+		}
+
+		cache->arch[cl].ctype = ctype;
+	}
+
+	/*  restore selected cache  */
+	dpm->instr_write_data_r0(dpm, ARMV8_MSR_GP(SYSTEM_CSSELR, 0), csselr);
+	if (retval != ERROR_OK)
+		goto done;
+
+	armv8->armv8_mmu.armv8_cache.info = 1;
+
+	/*  if no l2 cache initialize l1 data cache flush function function */
+	if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache == NULL) {
+		armv8->armv8_mmu.armv8_cache.display_cache_info =
+			armv8_handle_inner_cache_info_command;
+		armv8->armv8_mmu.armv8_cache.flush_all_data_cache =
+			armv8_flush_all_data;
+	}
+
+done:
+	dpm->finish(dpm);
+	return retval;
+
+}
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 9f5ebf8..51dc15a 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -77,6 +77,7 @@
 #define SYSTEM_CLIDR			0b1100100000000001
 #define SYSTEM_CSSELR			0b1101000000000000
 #define SYSTEM_CTYPE			0b1101100000000001
+#define SYSTEM_CTR				0b1101100000000001
 
 #define SYSTEM_DCCISW			0b0100001111110010
 #define SYSTEM_DCCSW			0b0100001111010010
-- 
cgit v1.1


From 40ce7374d3d6c495834b41f0a98f0bd6cb620dc4 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 26 Sep 2016 11:08:11 +0200
Subject: aarch64: remove bogus os_border calculation

The artificial "os_border" doesn't exist in aarch64 state and is wrong
for aarch32 state as well. Remove it.

Change-Id: I7c673a1404b03aa78dbd505e115fa3a93f7ca05f
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 40 +++++-----------------------------------
 src/target/armv8.h |  1 -
 2 files changed, 5 insertions(+), 36 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 467c771..d644963 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -441,7 +441,7 @@ done:
 	return retval;
 }
 
-static int armv8_read_ttbcr(struct target *target)
+static __unused int armv8_read_ttbcr(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
@@ -511,48 +511,18 @@ static int armv8_read_ttbcr(struct target *target)
 	if (retval != ERROR_OK)
 		goto done;
 
-#if 0
-	LOG_INFO("ttb1 %s ,ttb0_mask %llx",
-		armv8->armv8_mmu.ttbr1_used ? "used" : "not used",
-		armv8->armv8_mmu.ttbr0_mask);
-#endif
-	if (armv8->armv8_mmu.ttbr1_used == 1) {
-		LOG_INFO("TTBR0 access above %" PRIx64,
-			 (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
-		armv8->armv8_mmu.os_border = armv8->armv8_mmu.ttbr0_mask;
-	} else {
-		/*  fix me , default is hard coded LINUX border  */
-		armv8->armv8_mmu.os_border = 0xc0000000;
-	}
+	if (armv8->armv8_mmu.ttbr1_used == 1)
+		LOG_INFO("TTBR0 access above %" PRIx64, (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
+
 done:
 	dpm->finish(dpm);
 	return retval;
 }
 
-static int armv8_4K_translate(struct target *target,  target_addr_t va, target_addr_t *val)
-{
-	LOG_ERROR("4K page Address translation need to add");
-	return ERROR_FAIL;
-}
-
-
 /*  method adapted to cortex A : reused arm v4 v5 method*/
 int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr_t *val)
 {
-	int retval = ERROR_FAIL;
-	struct armv8_common *armv8 = target_to_armv8(target);
-	struct arm_dpm *dpm = armv8->arm.dpm;
-
-	retval = dpm->prepare(dpm);
-	retval += armv8_read_ttbcr(target);
-	if (retval != ERROR_OK)
-		goto done;
-	if (armv8->page_size == 0)
-		return armv8_4K_translate(target, va, val);
-
-done:
-	dpm->finish(dpm);
-	return ERROR_FAIL;
+	return ERROR_OK;
 }
 
 /*  V8 method VA TO PA  */
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 2862ebd..07e3b41 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -124,7 +124,6 @@ struct armv8_mmu_common {
 	/* following field mmu working way */
 	int32_t ttbr1_used; /*  -1 not initialized, 0 no ttbr1 1 ttbr1 used and  */
 	uint64_t ttbr0_mask;/*  masked to be used  */
-	uint32_t os_border;
 
 	uint32_t ttbcr;     /* cache for ttbcr register */
 	uint32_t ttbr_mask[2];
-- 
cgit v1.1


From fc3ce94bba5420402ea11f37d2113b0b9e380bd0 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 26 Sep 2016 11:44:25 +0200
Subject: aarch64: slightly simplify breakpoint set function

Set HDE bit through helper function instead of manual mem_ap access.

Change-Id: I68c157870f3f3c47a875d425ade6e975d8075424
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 05abe6b..f818344 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -861,7 +861,6 @@ static int aarch64_set_breakpoint(struct target *target,
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	struct aarch64_brp *brp_list = aarch64->brp_list;
-	uint32_t dscr;
 
 	if (breakpoint->set) {
 		LOG_WARNING("breakpoint already set");
@@ -940,12 +939,8 @@ static int aarch64_set_breakpoint(struct target *target,
 		breakpoint->set = 0x11;	/* Any nice value but 0 */
 	}
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	/* Ensure that halting debug mode is enable */
-	dscr = dscr | DSCR_HDE;
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-					 armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Failed to set DSCR.HDE");
 		return retval;
-- 
cgit v1.1


From 2b56f4f656e11f50d124e13e6d66a967ca45d373 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Oct 2016 15:05:53 +0200
Subject: aarch64: provide virt2phys command

Use AT commands to translate virtual to physical addresses based on
current MMU configuration.

Change-Id: I1bbd7d674c435541b617b17022fa9f7f0f01bdab
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c       |  2 +-
 src/target/armv8.c         | 78 +++++++++++++++++++++++++++++++++++++++++++++-
 src/target/armv8.h         |  4 +++
 src/target/armv8_opcodes.h |  7 +++++
 4 files changed, 89 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index f818344..3510db2 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2011,7 +2011,7 @@ static int aarch64_mmu(struct target *target, int *enabled)
 static int aarch64_virt2phys(struct target *target, target_addr_t virt,
 			     target_addr_t *phys)
 {
-	return armv8_mmu_translate_va(target, virt, phys);
+	return armv8_mmu_translate_va_pa(target, virt, phys, 1);
 }
 
 COMMAND_HANDLER(aarch64_handle_cache_info_command)
diff --git a/src/target/armv8.c b/src/target/armv8.c
index d644963..3f3127e 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -529,7 +529,83 @@ int armv8_mmu_translate_va(struct target *target,  target_addr_t va, target_addr
 int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	target_addr_t *val, int meminfo)
 {
-	return ERROR_OK;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = target_to_arm(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	uint32_t retval;
+	uint32_t instr = 0;
+	uint64_t par;
+
+	static const char * const shared_name[] = {
+			"Non-", "UNDEFINED ", "Outer ", "Inner "
+	};
+
+	static const char * const secure_name[] = {
+			"Secure", "Not Secure"
+	};
+
+	retval = dpm->prepare(dpm);
+	if (retval != ERROR_OK)
+		return retval;
+
+	switch (armv8_curel_from_core_mode(arm)) {
+	case SYSTEM_CUREL_EL0:
+		instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
+		/* can only execute instruction at EL2 */
+		dpmv8_modeswitch(dpm, ARMV8_64_EL2T);
+		break;
+	case SYSTEM_CUREL_EL1:
+		instr = ARMV8_SYS(SYSTEM_ATS12E1R, 0);
+		/* can only execute instruction at EL2 */
+		dpmv8_modeswitch(dpm, ARMV8_64_EL2T);
+		break;
+	case SYSTEM_CUREL_EL2:
+		instr = ARMV8_SYS(SYSTEM_ATS1E2R, 0);
+		break;
+	case SYSTEM_CUREL_EL3:
+		instr = ARMV8_SYS(SYSTEM_ATS1E3R, 0);
+		break;
+
+	default:
+		break;
+	};
+
+	/* write VA to R0 and execute translation instruction */
+	retval = dpm->instr_write_data_r0_64(dpm, instr, (uint64_t)va);
+	/* read result from PAR_EL1 */
+	if (retval == ERROR_OK)
+		retval = dpm->instr_read_data_r0_64(dpm, ARMV8_MRS(SYSTEM_PAR_EL1, 0), &par);
+
+	dpm->finish(dpm);
+
+	/* switch back to saved PE mode */
+	dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (par & 1) {
+		LOG_ERROR("Address translation failed at stage %i, FST=%x, PTW=%i",
+				((int)(par >> 9) & 1)+1, (int)(par >> 1) & 0x3f, (int)(par >> 8) & 1);
+
+		*val = 0;
+		retval = ERROR_FAIL;
+	} else {
+		*val = (par & 0xFFFFFFFFF000UL) | (va & 0xFFF);
+		if (meminfo) {
+			int SH = (par >> 7) & 3;
+			int NS = (par >> 9) & 1;
+			int ATTR = (par >> 56) & 0xFF;
+
+			char *memtype = (ATTR & 0xF0) == 0 ? "Device Memory" : "Normal Memory";
+
+			LOG_USER("%sshareable, %s",
+					shared_name[SH], secure_name[NS]);
+			LOG_USER("%s", memtype);
+		}
+	}
+
+	return retval;
 }
 
 int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 07e3b41..497e482 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -270,6 +270,10 @@ int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 
 void armv8_set_cpsr(struct arm *arm, uint32_t cpsr);
 
+static inline int armv8_curel_from_core_mode(struct arm *arm)
+{
+	return (arm->core_mode >> 6) & 3;
+}
 extern const struct command_registration armv8_command_handlers[];
 
 #endif
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 51dc15a..41abe04 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -96,6 +96,13 @@
 #define SYSTEM_TTBR0_EL3		0b1111000100000000
 #define SYSTEM_TTBR1_EL1		0b1100000100000001
 
+/* ARMv8 address translation */
+#define SYSTEM_PAR_EL1			0b1100001110100000
+#define SYSTEM_ATS12E0R			0b0110001111000110
+#define SYSTEM_ATS12E1R			0b0110001111000100
+#define SYSTEM_ATS1E2R			0b0110001111000000
+#define SYSTEM_ATS1E3R			0b0111001111000000
+
 #define ARMV8_MRS_DSPSR(Rt)	(0xd53b4500 | (Rt))
 #define ARMV8_MSR_DSPSR(Rt)	(0xd51b4500 | (Rt))
 #define ARMV8_MRS_DLR(Rt)	(0xd53b4520 | (Rt))
-- 
cgit v1.1


From 675b0170f2752c285de359f235e50a208f7f138a Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Oct 2016 16:11:19 +0200
Subject: aarch64: simplify armv8_set_cpsr()

Translate from cpsr value to "enum arm_mode" by shifting up 4 bits and
filling the lowest nibble with 0xF.

Change-Id: Ic32186104b0c29578c4f6f99e04840ab88a0017b
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 32 +++-----------------------------
 1 file changed, 3 insertions(+), 29 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 3f3127e..a572e23 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -282,36 +282,10 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 		}
 	}
 	arm->core_state = state;
-	if (arm->core_state == ARM_STATE_AARCH64) {
-		switch (mode) {
-			case SYSTEM_AAR64_MODE_EL0t:
-				arm->core_mode = ARMV8_64_EL0T;
-			break;
-			case SYSTEM_AAR64_MODE_EL1t:
-				arm->core_mode = ARMV8_64_EL0T;
-			break;
-			case SYSTEM_AAR64_MODE_EL1h:
-				arm->core_mode = ARMV8_64_EL1H;
-			break;
-			case SYSTEM_AAR64_MODE_EL2t:
-				arm->core_mode = ARMV8_64_EL2T;
-			break;
-			case SYSTEM_AAR64_MODE_EL2h:
-				arm->core_mode = ARMV8_64_EL2H;
-			break;
-			case SYSTEM_AAR64_MODE_EL3t:
-				arm->core_mode = ARMV8_64_EL3T;
-			break;
-			case SYSTEM_AAR64_MODE_EL3h:
-				arm->core_mode = ARMV8_64_EL3H;
-			break;
-			default:
-				LOG_DEBUG("unknow mode 0x%x", (unsigned) (mode));
-			break;
-		}
-	} else {
+	if (arm->core_state == ARM_STATE_AARCH64)
+		arm->core_mode = (mode << 4) | 0xf;
+	else
 		arm->core_mode = mode;
-	}
 
 	LOG_DEBUG("set CPSR %#8.8x: %s mode, %s state", (unsigned) cpsr,
 		armv8_mode_name(arm->core_mode),
-- 
cgit v1.1


From a76e88daa645d7137c8415f48793525f2ba98545 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Oct 2016 16:19:20 +0200
Subject: aarch64: allow reading system control register when halted in EL0

There's no access to system control register in EL0. Circumvent by
moving the PE to EL1 before reading, and switch back to original mode
afterwards.

Change-Id: I309f4eea5597ffc88fc892e9bbb826982e8a44ec
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 3510db2..41bea2e 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -698,6 +698,8 @@ static int aarch64_post_debug_entry(struct target *target)
 
 	switch (armv8->arm.core_mode) {
 		case ARMV8_64_EL0T:
+			dpmv8_modeswitch(&armv8->dpm, ARMV8_64_EL1T);
+			/* fall through */
 		case ARMV8_64_EL1T:
 		case ARMV8_64_EL1H:
 			retval = armv8->arm.mrs(target, 3, /*op 0*/
@@ -725,13 +727,20 @@ static int aarch64_post_debug_entry(struct target *target)
 			if (retval != ERROR_OK)
 				return retval;
 		break;
-		default:
+
+		case ARM_MODE_SVC:
 			retval = armv8->arm.mrc(target, 15, 0, 0, 1, 0, &aarch64->system_control_reg);
 			if (retval != ERROR_OK)
 				return retval;
 			break;
+
+		default:
+			LOG_INFO("cannot read system control register in this mode");
+			break;
 	}
 
+	dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
-- 
cgit v1.1


From 2539a323081f046b14ad613b4a163baaf2679a9f Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Oct 2016 16:36:29 +0200
Subject: aarch64: simplify armv8_read_ttbcr

Read registers based on current EL instead of PE mode.

Change-Id: I05d3219ac1bf8585e9f4f024a7e8599fea0913b6
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 97 ++++++++++++++++++++++++++----------------------------
 1 file changed, 47 insertions(+), 50 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index a572e23..9b160a9 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -431,56 +431,53 @@ static __unused int armv8_read_ttbcr(struct target *target)
 	memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
 	memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
 
-	switch (arm->core_mode) {
-		case ARMV8_64_EL3H:
-		case ARMV8_64_EL3T:
-			retval = dpm->instr_read_data_r0(dpm,
-					ARMV8_MRS(SYSTEM_TCR_EL3, 0),
-					&ttbcr);
-			retval += dpm->instr_read_data_r0_64(dpm,
-					ARMV8_MRS(SYSTEM_TTBR0_EL3, 0),
-					&armv8->ttbr_base);
-			if (retval != ERROR_OK)
-				goto done;
-			armv8->va_size = 64 - (ttbcr & 0x3F);
-			armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
-			armv8->page_size = (ttbcr >> 14) & 3;
-			break;
-		case ARMV8_64_EL2T:
-		case ARMV8_64_EL2H:
-			retval = dpm->instr_read_data_r0(dpm,
-					ARMV8_MRS(SYSTEM_TCR_EL2, 0),
-					&ttbcr);
-			retval += dpm->instr_read_data_r0_64(dpm,
-					ARMV8_MRS(SYSTEM_TTBR0_EL2, 0),
-					&armv8->ttbr_base);
-			if (retval != ERROR_OK)
-				goto done;
-			armv8->va_size = 64 - (ttbcr & 0x3F);
-			armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
-			armv8->page_size = (ttbcr >> 14) & 3;
-			break;
-		case ARMV8_64_EL0T:
-		case ARMV8_64_EL1T:
-		case ARMV8_64_EL1H:
-			retval = dpm->instr_read_data_r0_64(dpm,
-					ARMV8_MRS(SYSTEM_TCR_EL1, 0),
-					&ttbcr_64);
-			armv8->va_size = 64 - (ttbcr_64 & 0x3F);
-			armv8->pa_size = armv8_pa_size((ttbcr_64 >> 32) & 7);
-			armv8->page_size = (ttbcr_64 >> 14) & 3;
-			armv8->armv8_mmu.ttbr1_used = (((ttbcr_64 >> 16) & 0x3F) != 0) ? 1 : 0;
-			armv8->armv8_mmu.ttbr0_mask  = 0x0000FFFFFFFFFFFF;
-			retval += dpm->instr_read_data_r0_64(dpm,
-					ARMV8_MRS(SYSTEM_TTBR0_EL1 | (armv8->armv8_mmu.ttbr1_used), 0),
-					&armv8->ttbr_base);
-			if (retval != ERROR_OK)
-				goto done;
-			break;
-		default:
-			LOG_ERROR("unknow core state");
-			retval = ERROR_FAIL;
-			break;
+	switch (armv8_curel_from_core_mode(arm)) {
+	case SYSTEM_CUREL_EL3:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_TCR_EL3, 0),
+				&ttbcr);
+		retval += dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TTBR0_EL3, 0),
+				&armv8->ttbr_base);
+		if (retval != ERROR_OK)
+			goto done;
+		armv8->va_size = 64 - (ttbcr & 0x3F);
+		armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+		armv8->page_size = (ttbcr >> 14) & 3;
+		break;
+	case SYSTEM_CUREL_EL2:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_TCR_EL2, 0),
+				&ttbcr);
+		retval += dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TTBR0_EL2, 0),
+				&armv8->ttbr_base);
+		if (retval != ERROR_OK)
+			goto done;
+		armv8->va_size = 64 - (ttbcr & 0x3F);
+		armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+		armv8->page_size = (ttbcr >> 14) & 3;
+		break;
+	case SYSTEM_CUREL_EL0:
+	case SYSTEM_CUREL_EL1:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TCR_EL1, 0),
+				&ttbcr_64);
+		armv8->va_size = 64 - (ttbcr_64 & 0x3F);
+		armv8->pa_size = armv8_pa_size((ttbcr_64 >> 32) & 7);
+		armv8->page_size = (ttbcr_64 >> 14) & 3;
+		armv8->armv8_mmu.ttbr1_used = (((ttbcr_64 >> 16) & 0x3F) != 0) ? 1 : 0;
+		armv8->armv8_mmu.ttbr0_mask  = 0x0000FFFFFFFFFFFF;
+		retval += dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_TTBR0_EL1 | (armv8->armv8_mmu.ttbr1_used), 0),
+				&armv8->ttbr_base);
+		if (retval != ERROR_OK)
+			goto done;
+		break;
+	default:
+		LOG_ERROR("unknow core state");
+		retval = ERROR_FAIL;
+		break;
 	}
 	if (retval != ERROR_OK)
 		goto done;
-- 
cgit v1.1


From 79c4c22e1570cf0d73bacb4d292951e614d0ab2f Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Oct 2016 16:10:38 +0200
Subject: aarch64: register access rewrite

All register access is now performed through common read/write
functions, which delegate the actual register access to the
armv8_common object. armv8_common contains function pointers
to direct read and write requests to the respective low-level
functions for each PE state.

The respective read/write functions are selected on debug state
entry.

At the same time, T32 opcodes are now formatted for ITR in
dpmv8_exec_opcode() and the T32_FMTITR macro is removed from global
visibility.

Change-Id: I9eaef017c7cc9e0c531e693c534901bfdbdb842c
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c       |  28 +--
 src/target/arm_dpm.h       |   5 +-
 src/target/armv8.c         | 456 +++++++++++++++++++++++++++++++++++++--------
 src/target/armv8.h         |  46 ++++-
 src/target/armv8_dpm.c     | 373 ++++++++++++------------------------
 src/target/armv8_dpm.h     |   1 +
 src/target/armv8_opcodes.c |  20 +-
 src/target/armv8_opcodes.h |  17 +-
 8 files changed, 580 insertions(+), 366 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 41bea2e..02f17d8 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -637,13 +637,15 @@ static int aarch64_debug_entry(struct target *target)
 	int retval = ERROR_OK;
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	enum arm_state core_state;
 
-	LOG_DEBUG("dscr = 0x%08" PRIx32, aarch64->cpudbg_dscr);
+	LOG_DEBUG("%s dscr = 0x%08" PRIx32, target_name(target), aarch64->cpudbg_dscr);
 
-	/* REVISIT see A8 TRM 12.11.4 steps 2..3 -- make sure that any
-	 * imprecise data aborts get discarded by issuing a Data
-	 * Synchronization Barrier:  ARMV4_5_MCR(15, 0, 0, 7, 10, 4).
-	 */
+	dpm->dscr = aarch64->cpudbg_dscr;
+	core_state = armv8_dpm_get_core_state(dpm);
+	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
+	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
 
 	/* make sure to clear all sticky errors */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -677,11 +679,8 @@ static int aarch64_debug_entry(struct target *target)
 
 	retval = armv8_dpm_read_current_registers(&armv8->dpm);
 
-	if (armv8->post_debug_entry) {
+	if (retval == ERROR_OK && armv8->post_debug_entry)
 		retval = armv8->post_debug_entry(target);
-		if (retval != ERROR_OK)
-			return retval;
-	}
 
 	return retval;
 }
@@ -1457,7 +1456,7 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 		/* 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,
-				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL);
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
 
 	}
 	/* Step 1.d   - Change DCC to memory mode */
@@ -1486,6 +1485,8 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		goto error_free_buff_w;
+
+	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);
@@ -1586,9 +1587,9 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		/* 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,
-				T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)), address & ~0x3ULL);
+				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, T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)));
+		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,
@@ -1648,6 +1649,9 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		goto error_free_buff_r;
+
+	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);
diff --git a/src/target/arm_dpm.h b/src/target/arm_dpm.h
index 63467d5..f8d1248 100644
--- a/src/target/arm_dpm.h
+++ b/src/target/arm_dpm.h
@@ -143,6 +143,9 @@ struct arm_dpm {
 	/** Recent value of DSCR. */
 	uint32_t dscr;
 
+	/** Recent exception level on armv8 */
+	unsigned int last_el;
+
 	/* FIXME -- read/write DCSR methods and symbols */
 };
 
@@ -150,10 +153,8 @@ int arm_dpm_setup(struct arm_dpm *dpm);
 int arm_dpm_initialize(struct arm_dpm *dpm);
 
 int arm_dpm_read_current_registers(struct arm_dpm *);
-int arm_dpm_read_current_registers_64(struct arm_dpm *);
 int dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
 
-
 int arm_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);
 
 void arm_dpm_report_wfar(struct arm_dpm *, uint32_t wfar);
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 9b160a9..32fe048 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -161,55 +161,333 @@ int armv8_mode_to_number(enum arm_mode mode)
 	}
 }
 
-
-static int armv8_read_core_reg(struct target *target, struct reg *r,
-	int num, enum arm_mode mode)
+static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regval)
 {
-	uint64_t reg_value;
+	struct arm_dpm *dpm = &armv8->dpm;
 	int retval;
-	struct arm_reg *armv8_core_reg;
-	struct armv8_common *armv8 = target_to_armv8(target);
+	uint32_t value;
+	uint64_t value_64;
+
+	switch (regnum) {
+	case 0 ... 30:
+		retval = dpm->instr_read_data_dcc_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum), &value_64);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MOVFSP_64(0), &value_64);
+		break;
+	case ARMV8_PC:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS_DLR(0), &value_64);
+		break;
+	case ARMV8_xPSR:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_DSPSR(0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_ELR_EL1:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_ELR_EL1, 0), &value_64);
+		break;
+	case ARMV8_ELR_EL2:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_ELR_EL2, 0), &value_64);
+		break;
+	case ARMV8_ELR_EL3:
+		retval = dpm->instr_read_data_r0_64(dpm,
+				ARMV8_MRS(SYSTEM_ELR_EL3, 0), &value_64);
+		break;
+	case ARMV8_ESR_EL1:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_ESR_EL1, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_ESR_EL2:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_ESR_EL2, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_ESR_EL3:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_ESR_EL3, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_SPSR_EL1:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_SPSR_EL1, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_SPSR_EL2:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_SPSR_EL2, 0), &value);
+		value_64 = value;
+		break;
+	case ARMV8_SPSR_EL3:
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS(SYSTEM_SPSR_EL3, 0), &value);
+		value_64 = value;
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
 
-	assert(num < (int)armv8->arm.core_cache->num_regs);
+	if (retval == ERROR_OK && regval != NULL)
+		*regval = value_64;
 
-	armv8_core_reg = armv8->arm.core_cache->reg_list[num].arch_info;
-	retval = armv8->load_core_reg_u64(target,
-			armv8_core_reg->num, &reg_value);
+	return retval;
+}
 
-	buf_set_u64(armv8->arm.core_cache->reg_list[num].value, 0, 64, reg_value);
-	armv8->arm.core_cache->reg_list[num].valid = 1;
-	armv8->arm.core_cache->reg_list[num].dirty = 0;
+static int armv8_write_reg(struct armv8_common *armv8, int regnum, uint64_t value_64)
+{
+	struct arm_dpm *dpm = &armv8->dpm;
+	int retval;
+	uint32_t value;
+
+	switch (regnum) {
+	case 0 ... 30:
+		retval = dpm->instr_write_data_dcc_64(dpm,
+			ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
+			value_64);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_write_data_r0_64(dpm,
+			ARMV8_MOVTSP_64(0),
+			value_64);
+		break;
+	case ARMV8_PC:
+		retval = dpm->instr_write_data_r0_64(dpm,
+			ARMV8_MSR_DLR(0),
+			value_64);
+		break;
+	case ARMV8_xPSR:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+			ARMV8_MSR_DSPSR(0),
+			value);
+		break;
+	/* registers clobbered by taking exception in debug state */
+	case ARMV8_ELR_EL1:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_ELR_EL1, 0), value_64);
+		break;
+	case ARMV8_ELR_EL2:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_ELR_EL2, 0), value_64);
+		break;
+	case ARMV8_ELR_EL3:
+		retval = dpm->instr_write_data_r0_64(dpm,
+				ARMV8_MSR_GP(SYSTEM_ELR_EL3, 0), value_64);
+		break;
+	case ARMV8_ESR_EL1:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_ESR_EL1, 0), value);
+		break;
+	case ARMV8_ESR_EL2:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_ESR_EL2, 0), value);
+		break;
+	case ARMV8_ESR_EL3:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_ESR_EL3, 0), value);
+		break;
+	case ARMV8_SPSR_EL1:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_SPSR_EL1, 0), value);
+		break;
+	case ARMV8_SPSR_EL2:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_SPSR_EL2, 0), value);
+		break;
+	case ARMV8_SPSR_EL3:
+		value = value_64;
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP(SYSTEM_SPSR_EL3, 0), value);
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
 
 	return retval;
 }
 
-#if 0
-static int armv8_write_core_reg(struct target *target, struct reg *r,
-	int num, enum arm_mode mode, target_addr_t value)
+static int armv8_read_reg32(struct armv8_common *armv8, int regnum, uint64_t *regval)
 {
+	struct arm_dpm *dpm = &armv8->dpm;
+	uint32_t value = 0;
 	int retval;
-	struct arm_reg *armv8_core_reg;
-	struct armv8_common *armv8 = target_to_armv8(target);
 
-	assert(num < (int)armv8->arm.core_cache->num_regs);
+	switch (regnum) {
+	case ARMV8_R0 ... ARMV8_R14:
+		/* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
+		retval = dpm->instr_read_data_dcc(dpm,
+			ARMV4_5_MCR(14, 0, regnum, 0, 5, 0),
+			&value);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_read_data_dcc(dpm,
+			ARMV4_5_MCR(14, 0, 13, 0, 5, 0),
+			&value);
+		break;
+	case ARMV8_PC:
+		retval = dpm->instr_read_data_r0(dpm,
+			ARMV8_MRC_DLR(0),
+			&value);
+		break;
+	case ARMV8_xPSR:
+		retval = dpm->instr_read_data_r0(dpm,
+			ARMV8_MRC_DSPSR(0),
+			&value);
+		break;
+	case ARMV8_ELR_EL1: /* mapped to LR_svc */
+		retval = dpm->instr_read_data_dcc(dpm,
+				ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
+				&value);
+		break;
+	case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_T1(0, 14, 0, 1),
+				&value);
+		break;
+	case ARMV8_ELR_EL3: /* mapped to LR_mon */
+		retval = dpm->instr_read_data_dcc(dpm,
+				ARMV4_5_MCR(14, 0, 14, 0, 5, 0),
+				&value);
+		break;
+	case ARMV8_ESR_EL1: /* mapped to DFSR */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
+				&value);
+		break;
+	case ARMV8_ESR_EL2: /* mapped to HSR */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV4_5_MRC(15, 4, 0, 5, 2, 0),
+				&value);
+		break;
+	case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
+		retval = ERROR_FAIL;
+		break;
+	case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_xPSR_T1(1, 0),
+				&value);
+		break;
+	case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_xPSR_T1(1, 0),
+				&value);
+		break;
+	case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
+		retval = dpm->instr_read_data_r0(dpm,
+				ARMV8_MRS_xPSR_T1(1, 0),
+				&value);
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
+	}
+
+	if (retval == ERROR_OK && regval != NULL)
+		*regval = value;
 
-	armv8_core_reg = armv8->arm.core_cache->reg_list[num].arch_info;
-	retval = armv8->store_core_reg_u64(target,
-					    armv8_core_reg->num,
-					    value);
-	if (retval != ERROR_OK) {
-		LOG_ERROR("JTAG failure");
-		armv8->arm.core_cache->reg_list[num].dirty = armv8->arm.core_cache->reg_list[num].valid;
-		return ERROR_JTAG_DEVICE_ERROR;
+	return retval;
+}
+
+static int armv8_write_reg32(struct armv8_common *armv8, int regnum, uint64_t value)
+{
+	struct arm_dpm *dpm = &armv8->dpm;
+	int retval;
+
+	switch (regnum) {
+	case ARMV8_R0 ... ARMV8_R14:
+		/* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, regnum, 0, 5, 0), value);
+		break;
+	case ARMV8_SP:
+		retval = dpm->instr_write_data_dcc(dpm,
+			ARMV4_5_MRC(14, 0, 13, 0, 5, 0),
+			value);
+			break;
+	case ARMV8_PC:/* PC
+		 * read r0 from DCC; then "MOV pc, r0" */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MCR_DLR(0), value);
+		break;
+	case ARMV8_xPSR: /* CPSR */
+		/* read r0 from DCC, then "MCR r0, DSPSR" */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MCR_DSPSR(0), value);
+		break;
+	case ARMV8_ELR_EL1: /* mapped to LR_svc */
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
+				value);
+		break;
+	case ARMV8_ELR_EL2: /* mapped to ELR_hyp */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_T1(0, 14, 0, 1),
+				value);
+		break;
+	case ARMV8_ELR_EL3: /* mapped to LR_mon */
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 14, 0, 5, 0),
+				value);
+		break;
+	case ARMV8_ESR_EL1: /* mapped to DFSR */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV4_5_MCR(15, 0, 0, 5, 0, 0),
+				value);
+		break;
+	case ARMV8_ESR_EL2: /* mapped to HSR */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV4_5_MCR(15, 4, 0, 5, 2, 0),
+				value);
+		break;
+	case ARMV8_ESR_EL3: /* FIXME: no equivalent in aarch32? */
+		retval = ERROR_FAIL;
+		break;
+	case ARMV8_SPSR_EL1: /* mapped to SPSR_svc */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
+				value);
+		break;
+	case ARMV8_SPSR_EL2: /* mapped to SPSR_hyp */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
+				value);
+		break;
+	case ARMV8_SPSR_EL3: /* mapped to SPSR_mon */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV8_MSR_GP_xPSR_T1(1, 0, 15),
+				value);
+		break;
+	default:
+		retval = ERROR_FAIL;
+		break;
 	}
 
-	LOG_DEBUG("write core reg %i value 0x%" PRIx64 "", num, value);
-	armv8->arm.core_cache->reg_list[num].valid = 1;
-	armv8->arm.core_cache->reg_list[num].dirty = 0;
+	return retval;
 
-	return ERROR_OK;
 }
-#endif
+
+void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64)
+{
+	if (is_aarch64) {
+		armv8->read_reg_u64 = armv8_read_reg;
+		armv8->write_reg_u64 = armv8_write_reg;
+	} else {
+		armv8->read_reg_u64 = armv8_read_reg32;
+		armv8->write_reg_u64 = armv8_write_reg32;
+	}
+}
 
 /*  retrieve core id cluster id  */
 int armv8_read_mpidr(struct armv8_common *armv8)
@@ -306,26 +584,26 @@ static void armv8_show_fault_registers32(struct armv8_common *armv8)
 
 	/* c5/c0 - {data, instruction} fault status registers */
 	retval = dpm->instr_read_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 0)),
+			ARMV4_5_MRC(15, 0, 0, 5, 0, 0),
 			&dfsr);
 	if (retval != ERROR_OK)
 		goto done;
 
 	retval = dpm->instr_read_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 5, 0, 1)),
+			ARMV4_5_MRC(15, 0, 0, 5, 0, 1),
 			&ifsr);
 	if (retval != ERROR_OK)
 		goto done;
 
 	/* c6/c0 - {data, instruction} fault address registers */
 	retval = dpm->instr_read_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 0)),
+			ARMV4_5_MRC(15, 0, 0, 6, 0, 0),
 			&dfar);
 	if (retval != ERROR_OK)
 		goto done;
 
 	retval = dpm->instr_read_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 6, 0, 2)),
+			ARMV4_5_MRC(15, 0, 0, 6, 0, 2),
 			&ifar);
 	if (retval != ERROR_OK)
 		goto done;
@@ -386,7 +664,7 @@ static __unused int armv8_read_ttbcr32(struct target *target)
 		goto done;
 	/*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
 	retval = dpm->instr_read_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 2, 0, 2)),
+			ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
 			&ttbcr);
 	if (retval != ERROR_OK)
 		goto done;
@@ -431,7 +709,7 @@ static __unused int armv8_read_ttbcr(struct target *target)
 	memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
 	memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
 
-	switch (armv8_curel_from_core_mode(arm)) {
+	switch (armv8_curel_from_core_mode(arm->core_mode)) {
 	case SYSTEM_CUREL_EL3:
 		retval = dpm->instr_read_data_r0(dpm,
 				ARMV8_MRS(SYSTEM_TCR_EL3, 0),
@@ -519,7 +797,7 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	if (retval != ERROR_OK)
 		return retval;
 
-	switch (armv8_curel_from_core_mode(arm)) {
+	switch (armv8_curel_from_core_mode(arm->core_mode)) {
 	case SYSTEM_CUREL_EL0:
 		instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
 		/* can only execute instruction at EL2 */
@@ -602,11 +880,6 @@ int armv8_init_arch_info(struct target *target, struct armv8_common *armv8)
 	armv8->arm.common_magic = ARM_COMMON_MAGIC;
 	armv8->common_magic = ARMV8_COMMON_MAGIC;
 
-	arm->read_core_reg = armv8_read_core_reg;
-#if 0
-	arm->write_core_reg = armv8_write_core_reg;
-#endif
-
 	armv8->armv8_mmu.armv8_cache.l2_cache = NULL;
 	armv8->armv8_mmu.armv8_cache.info = -1;
 	armv8->armv8_mmu.armv8_cache.flush_all_data_cache = NULL;
@@ -673,46 +946,59 @@ static const struct {
 	unsigned id;
 	const char *name;
 	unsigned bits;
+	enum arm_mode mode;
 	enum reg_type type;
 	const char *group;
 	const char *feature;
 } armv8_regs[] = {
-	{ ARMV8_R0,  "x0",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R1,  "x1",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R2,  "x2",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R3,  "x3",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R4,  "x4",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R5,  "x5",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R6,  "x6",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R7,  "x7",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R8,  "x8",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R9,  "x9",  64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R10, "x10", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R11, "x11", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R12, "x12", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R13, "x13", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R14, "x14", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R15, "x15", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R16, "x16", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R17, "x17", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R18, "x18", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R19, "x19", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R20, "x20", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R21, "x21", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R22, "x22", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R23, "x23", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R24, "x24", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R25, "x25", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R26, "x26", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R27, "x27", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R28, "x28", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R29, "x29", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_R30, "x30", 64, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
-
-	{ ARMV8_R31, "sp", 64, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
-	{ ARMV8_PC,  "pc", 64, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
-
-	{ ARMV8_xPSR, "CPSR", 32, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R0,  "x0",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R1,  "x1",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R2,  "x2",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R3,  "x3",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R4,  "x4",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R5,  "x5",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R6,  "x6",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R7,  "x7",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R8,  "x8",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R9,  "x9",  64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R10, "x10", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R11, "x11", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R12, "x12", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R13, "x13", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R14, "x14", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R15, "x15", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R16, "x16", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R17, "x17", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R18, "x18", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R19, "x19", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R20, "x20", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R21, "x21", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R22, "x22", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R23, "x23", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R24, "x24", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R25, "x25", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R26, "x26", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R27, "x27", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R28, "x28", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R29, "x29", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_R30, "x30", 64, ARM_MODE_ANY, REG_TYPE_UINT64, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_SP, "sp", 64, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.aarch64.core" },
+	{ ARMV8_PC,  "pc", 64, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_xPSR, "CPSR", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.aarch64.core" },
+
+	{ ARMV8_ELR_EL1, "ELR_EL1", 64, ARMV8_64_EL1H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_ESR_EL1, "ESR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_SPSR_EL1, "SPSR_EL1", 32, ARMV8_64_EL1H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+
+	{ ARMV8_ELR_EL2, "ELR_EL2", 64, ARMV8_64_EL2H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_ESR_EL2, "ESR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_SPSR_EL2, "SPSR_EL2", 32, ARMV8_64_EL2H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+
+	{ ARMV8_ELR_EL3, "ELR_EL3", 64, ARMV8_64_EL3H, REG_TYPE_CODE_PTR, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_ESR_EL3, "ESR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
+	{ ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
 };
 
 #define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
@@ -782,6 +1068,7 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 
 	for (i = 0; i < num_regs; i++) {
 		arch_info[i].num = armv8_regs[i].id;
+		arch_info[i].mode = armv8_regs[i].mode;
 		arch_info[i].target = target;
 		arch_info[i].arm = arm;
 
@@ -847,11 +1134,18 @@ int armv8_get_gdb_reg_list(struct target *target,
 
 	switch (reg_class) {
 	case REG_CLASS_GENERAL:
+		*reg_list_size = ARMV8_ELR_EL1;
+		*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+		for (i = 0; i < *reg_list_size; i++)
+				(*reg_list)[i] = armv8_reg_current(arm, i);
+
+		return ERROR_OK;
 	case REG_CLASS_ALL:
 		*reg_list_size = ARMV8_LAST_REG;
 		*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 
-		for (i = 0; i < ARMV8_LAST_REG; i++)
+		for (i = 0; i < *reg_list_size; i++)
 				(*reg_list)[i] = armv8_reg_current(arm, i);
 
 		return ERROR_OK;
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 497e482..85845e6 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -26,7 +26,7 @@
 #include "armv8_dpm.h"
 
 enum {
-	ARMV8_R0,
+	ARMV8_R0 = 0,
 	ARMV8_R1,
 	ARMV8_R2,
 	ARMV8_R3,
@@ -57,11 +57,23 @@ enum {
 	ARMV8_R28,
 	ARMV8_R29,
 	ARMV8_R30,
-	ARMV8_R31,
 
+	ARMV8_SP = 31,
 	ARMV8_PC = 32,
 	ARMV8_xPSR = 33,
 
+	ARMV8_ELR_EL1 = 34,
+	ARMV8_ESR_EL1 = 35,
+	ARMV8_SPSR_EL1 = 36,
+
+	ARMV8_ELR_EL2 = 37,
+	ARMV8_ESR_EL2 = 38,
+	ARMV8_SPSR_EL2 = 39,
+
+	ARMV8_ELR_EL3 = 40,
+	ARMV8_ESR_EL3 = 41,
+	ARMV8_SPSR_EL3 = 42,
+
 	ARMV8_LAST_REG,
 };
 
@@ -162,8 +174,8 @@ struct armv8_common {
 	struct armv8_mmu_common armv8_mmu;
 
 	/* Direct processor core register read and writes */
-	int (*load_core_reg_u64)(struct target *target, uint32_t num, uint64_t *value);
-	int (*store_core_reg_u64)(struct target *target, uint32_t num, uint64_t value);
+	int (*read_reg_u64)(struct armv8_common *armv8, int num, uint64_t *value);
+	int (*write_reg_u64)(struct armv8_common *armv8, int num, uint64_t value);
 
 	int (*examine_debug_reason)(struct target *target);
 	int (*post_debug_entry)(struct target *target);
@@ -270,10 +282,32 @@ int armv8_handle_cache_info_command(struct command_context *cmd_ctx,
 
 void armv8_set_cpsr(struct arm *arm, uint32_t cpsr);
 
-static inline int armv8_curel_from_core_mode(struct arm *arm)
+static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
 {
-	return (arm->core_mode >> 6) & 3;
+	switch (core_mode) {
+	/* Aarch32 modes */
+	case ARM_MODE_USR:
+		return 0;
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT: /* FIXME: EL3? */
+	case ARM_MODE_IRQ: /* FIXME: EL3? */
+	case ARM_MODE_FIQ: /* FIXME: EL3? */
+	case ARM_MODE_UND: /* FIXME: EL3? */
+	case ARM_MODE_SYS: /* FIXME: EL3? */
+		return 1;
+	/* case ARM_MODE_HYP:
+	 *     return 2;
+	 */
+	case ARM_MODE_MON:
+		return 3;
+	/* all Aarch64 modes */
+	default:
+		return (core_mode >> 6) & 3;
+	}
 }
+
+void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
+
 extern const struct command_registration armv8_command_handlers[];
 
 #endif
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 4bb2075..a92f92c 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -28,6 +28,8 @@
 
 #include "helper/time_support.h"
 
+/* T32 ITR format */
+#define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
 
 /**
  * @file
@@ -42,6 +44,29 @@
  * to minimize needless differences in debug behavior between cores.
  */
 
+/**
+ * Get core state from EDSCR, without necessity to retrieve CPSR
+ */
+enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
+{
+	int el = (dpm->dscr >> 8) & 0x3;
+	int rw = (dpm->dscr >> 10) & 0xF;
+	int pos;
+
+	dpm->last_el = el;
+
+	/* find the first '0' in DSCR.RW */
+	for (pos = 3; pos >= 0; pos--) {
+		if ((rw & (1 << pos)) == 0)
+			break;
+	}
+
+	if (el > pos)
+		return ARM_STATE_AARCH64;
+
+	return ARM_STATE_ARM;
+}
+
 /*----------------------------------------------------------------------*/
 
 static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
@@ -166,6 +191,9 @@ static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
 		}
 	}
 
+	/* update the stored copy of dscr */
+	dpm->dscr = dscr;
+
 	/* this "should never happen" ... */
 	if (dscr & DSCR_DTR_RX_FULL) {
 		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
@@ -218,6 +246,9 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 		}
 	}
 
+	if (armv8_dpm_get_core_state(dpm) != ARM_STATE_AARCH64)
+		opcode = T32_FMTITR(opcode);
+
 	retval = mem_ap_write_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_ITR, opcode);
 	if (retval != ERROR_OK)
@@ -237,6 +268,20 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 		}
 	} while ((dscr & DSCR_ITE) == 0);	/* Wait for InstrCompl bit to be set */
 
+	/* update dscr and el after each command execution */
+	dpm->dscr = dscr;
+	if (dpm->last_el != ((dscr >> 8) & 3))
+		LOG_DEBUG("EL %i -> %i", dpm->last_el, (dscr >> 8) & 3);
+	dpm->last_el = (dscr >> 8) & 3;
+
+	if (dscr & DSCR_ERR) {
+		LOG_ERROR("Opcode 0x%08"PRIx32", DSCR.ERR=1, DSCR.EL=%i", opcode, dpm->last_el);
+		/* clear the sticky error condition */
+		mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+		retval = ERROR_FAIL;
+	}
+
 	if (p_dscr)
 		*p_dscr = dscr;
 
@@ -471,7 +516,7 @@ static int dpmv8_mrc(struct target *target, int cpnum,
 
 	/* read coprocessor register into R0; return via DCC */
 	retval = dpm->instr_read_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2)),
+			ARMV4_5_MRC(cpnum, op1, 0, CRn, CRm, op2),
 			value);
 
 	/* (void) */ dpm->finish(dpm);
@@ -496,7 +541,7 @@ static int dpmv8_mcr(struct target *target, int cpnum,
 
 	/* read DCC into r0; then write coprocessor register from R0 */
 	retval = dpm->instr_write_data_r0(dpm,
-			T32_FMTITR(ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2)),
+			ARMV4_5_MCR(cpnum, op1, 0, CRn, CRm, op2),
 			value);
 
 	/* (void) */ dpm->finish(dpm);
@@ -616,210 +661,50 @@ int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	return retval;
 }
 
-static int dpmv8_read_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
-{
-	uint32_t value;
-	int retval = ERROR_FAIL;
-	bool valid = true;
-
-	switch (regnum) {
-	case 0 ... 14:
-		/* return via DCC:  "MCR p14, 0, Rnum, c0, c5, 0" */
-		retval = dpm->instr_read_data_dcc(dpm,
-			T32_FMTITR(ARMV4_5_MCR(14, 0, regnum, 0, 5, 0)),
-			&value);
-		break;
-		case ARMV8_R31:
-			retval = dpm->instr_read_data_dcc(dpm,
-				T32_FMTITR(ARMV4_5_MCR(14, 0, 13, 0, 5, 0)),
-				&value);
-				break;
-		case ARMV8_PC:
-			retval = dpm->instr_read_data_r0(dpm,
-				T32_FMTITR(ARMV8_MRC_DLR(0)),
-				&value);
-			break;
-		case ARMV8_xPSR:
-			retval = dpm->instr_read_data_r0(dpm,
-				T32_FMTITR(ARMV8_MRC_DSPSR(0)),
-				&value);
-			break;
-		default:
-			LOG_DEBUG("READ: %s ignored", r->name);
-			retval = ERROR_OK;
-			value = 0xFFFFFFFF;
-			valid = false;
-			break;
-	}
-
-	if (retval == ERROR_OK) {
-		r->valid = valid;
-		r->dirty = false;
-		buf_set_u64(r->value, 0, 32, value);
-		LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
-	}
-	return retval;
-}
-
-static int dpmv8_write_reg32(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
-{
-	int retval;
-	uint64_t value = buf_get_u64(r->value, 0, 32);
-
-	switch (regnum) {
-		case 0 ... 14:
-			/* load register from DCC:  "MRC p14, 0, Rnum, c0, c5, 0" */
-			retval = dpm->instr_write_data_dcc(dpm,
-					T32_FMTITR(ARMV4_5_MRC(14, 0, regnum, 0, 5, 0)), value);
-			break;
-		case ARMV8_PC:/* PC
-			 * read r0 from DCC; then "MOV pc, r0" */
-			retval = dpm->instr_write_data_r0(dpm,
-					T32_FMTITR(ARMV8_MCR_DLR(0)), value);
-			break;
-		case ARMV8_xPSR: /* CPSR */
-			/* read r0 from DCC, then "MCR r0, DSPSR" */
-			retval = dpm->instr_write_data_r0(dpm,
-					T32_FMTITR(ARMV8_MCR_DSPSR(0)), value);
-			break;
-		default:
-			retval = ERROR_OK;
-			LOG_DEBUG("WRITE: %s ignored", r->name);
-			break;
-	}
-
-	if (retval == ERROR_OK) {
-		r->dirty = false;
-		LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value);
-	}
-
-	return retval;
-}
-
-/* just read the register -- rely on the core mode being right */
+/*
+ * Common register read, relies on armv8_select_reg_access() having been called.
+ */
 static int dpmv8_read_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
-	uint32_t value;
+	struct armv8_common *armv8 = dpm->arm->arch_info;
 	uint64_t value_64;
-	int retval = ERROR_FAIL;
+	int retval;
 
-	switch (regnum) {
-		case 0 ... 30:
-			retval = dpm->instr_read_data_dcc_64(dpm,
-				ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, regnum),
-				&value_64);
-			break;
-		case ARMV8_R31:
-			retval = dpm->instr_read_data_r0_64(dpm,
-				ARMV8_MOVFSP_64(0),
-				&value_64);
-			break;
-		case ARMV8_PC:
-			retval = dpm->instr_read_data_r0_64(dpm,
-				ARMV8_MRS_DLR(0),
-				&value_64);
-			break;
-		case ARMV8_xPSR:
-			retval = dpm->instr_read_data_r0(dpm,
-				ARMV8_MRS_DSPSR(0),
-				&value);
-			break;
-		default:
-			LOG_DEBUG("READ: %s fail", r->name);
-			break;
-	}
+	retval = armv8->read_reg_u64(armv8, regnum, &value_64);
 
 	if (retval == ERROR_OK) {
 		r->valid = true;
 		r->dirty = false;
-		if (r->size == 64) {
-			buf_set_u64(r->value, 0, 64, value_64);
+		buf_set_u64(r->value, 0, r->size, value_64);
+		if (r->size == 64)
 			LOG_DEBUG("READ: %s, %16.8llx", r->name, (unsigned long long) value_64);
-		} else {
-			buf_set_u32(r->value, 0, 32, value);
-			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned) value);
-		}
+		else
+			LOG_DEBUG("READ: %s, %8.8x", r->name, (unsigned int) value_64);
 	}
-	return retval;
+	return ERROR_OK;
 }
 
-/* just write the register -- rely on the core mode being right */
+/*
+ * Common register write, relies on armv8_select_reg_access() having been called.
+ */
 static int dpmv8_write_reg(struct arm_dpm *dpm, struct reg *r, unsigned regnum)
 {
+	struct armv8_common *armv8 = dpm->arm->arch_info;
 	int retval = ERROR_FAIL;
-	uint32_t value = 0xFFFFFFFF;
-	uint64_t value_64 = 0xFFFFFFFFFFFFFFFF;
-
-	switch (regnum) {
-		case 0 ... 30:
-			value_64 = buf_get_u64(r->value, 0, 64);
-			retval = dpm->instr_write_data_dcc_64(dpm,
-				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, regnum),
-				value_64);
-			break;
-		case ARMV8_R31:
-			value_64 = buf_get_u64(r->value, 0, 64);
-			retval = dpm->instr_write_data_r0_64(dpm,
-				ARMV8_MOVTSP_64(0),
-				value_64);
-			break;
-		case ARMV8_PC:
-			value_64 = buf_get_u64(r->value, 0, 64);
-			retval = dpm->instr_write_data_r0_64(dpm,
-				ARMV8_MSR_DLR(0),
-				value_64);
-			break;
-		case ARMV8_xPSR:
-			value = buf_get_u32(r->value, 0, 32);
-			retval = dpm->instr_write_data_r0(dpm,
-				ARMV8_MSR_DSPSR(0),
-				value);
-			break;
-		default:
-			LOG_DEBUG("write: %s fail", r->name);
-			break;
-	}
+	uint64_t value_64;
 
+	value_64 = buf_get_u64(r->value, 0, r->size);
 
+	retval = armv8->write_reg_u64(armv8, regnum, value_64);
 	if (retval == ERROR_OK) {
 		r->dirty = false;
 		if (r->size == 64)
-			LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long) value_64);
+			LOG_DEBUG("WRITE: %s, %16.8llx", r->name, (unsigned long long)value_64);
 		else
-			LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned) value);
+			LOG_DEBUG("WRITE: %s, %8.8x", r->name, (unsigned int)value_64);
 	}
 
-	return retval;
-}
-
-static inline enum arm_state dpm_get_core_state(uint32_t dscr)
-{
-	int el = (dscr >> 8) & 0x3;
-	int rw = (dscr >> 10) & 0xF;
-
-	LOG_DEBUG("EL:%i, RW:0x%x", el, rw);
-
-	/* DSCR.RW = 0b1111 - all EL are using AArch64 state */
-	if (rw == 0xF)
-		return ARM_STATE_AARCH64;
-
-	/* DSCR.RW = 0b1110 - all EL > 0 are using AArch64 state */
-	if (rw == 0xE && el > 0)
-		return ARM_STATE_AARCH64;
-
-	/* DSCR.RW = 0b110x - all EL > 1 are using Aarch64 state */
-	if ((rw & 0xE) == 0xC && el > 1)
-		return ARM_STATE_AARCH64;
-
-	/* DSCR.RW = 0b10xx - all EL > 2 are using Aarch64 state */
-	if ((rw & 0xC) == 0x8 && el > 2)
-		return ARM_STATE_AARCH64;
-
-	/* DSCR.RW = 0b0xxx - all EL are using AArch32 state */
-	if ((rw & 0x8) == 0)
-		return ARM_STATE_ARM;
-
-	return ARM_STATE_ARM;
+	return ERROR_OK;
 }
 
 /**
@@ -833,60 +718,59 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 {
 	struct arm *arm = dpm->arm;
 	struct armv8_common *armv8 = (struct armv8_common *)arm->arch_info;
-	enum arm_state core_state;
+	struct reg_cache *cache;
+	struct reg *r;
 	uint32_t cpsr;
-
 	int retval;
-	struct reg *r;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		return retval;
 
-	core_state = dpm_get_core_state(dpm->dscr);
-
-	armv8_select_opcodes(armv8, core_state);
+	cache = arm->core_cache;
 
 	/* read R0 first (it's used for scratch), then CPSR */
-	r = arm->core_cache->reg_list + 0;
+	r = cache->reg_list + 0;
 	if (!r->valid) {
-		retval = core_state == ARM_STATE_AARCH64 ?
-					dpmv8_read_reg(dpm, r, 0) : dpmv8_read_reg32(dpm, r, 0);
+		retval = dpmv8_read_reg(dpm, r, 0);
 		if (retval != ERROR_OK)
 			goto fail;
 	}
 	r->dirty = true;
 
 	/* read cpsr to r0 and get it back */
-	retval = dpm->instr_read_data_r0(dpm, armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
 	if (retval != ERROR_OK)
 		goto fail;
 
-	/* update core mode and state, plus shadow mapping for R8..R14 */
+	/* update core mode and state */
 	armv8_set_cpsr(arm, cpsr);
 
-	/* REVISIT we can probably avoid reading R1..R14, saving time... */
-	for (unsigned i = 1; i < arm->core_cache->num_regs ; i++) {
+	for (unsigned int i = 1; i < cache->num_regs ; i++) {
+		struct arm_reg *arm_reg;
+
 		r = armv8_reg_current(arm, i);
 		if (r->valid)
 			continue;
 
-		retval = core_state == ARM_STATE_AARCH64 ?
-					dpmv8_read_reg(dpm, r, i) : dpmv8_read_reg32(dpm, r, i);
+		/*
+		 * Only read registers that are available from the
+		 * current EL (or core mode).
+		 */
+		arm_reg = r->arch_info;
+		if (arm_reg->mode != ARM_MODE_ANY &&
+				dpm->last_el != armv8_curel_from_core_mode(arm_reg->mode))
+			continue;
 
+		retval = dpmv8_read_reg(dpm, r, i);
 		if (retval != ERROR_OK)
 			goto fail;
-	}
-
-	/* NOTE: SPSR ignored (if it's even relevant). */
 
-	/* REVISIT the debugger can trigger various exceptions.  See the
-	 * ARMv7A architecture spec, section C5.7, for more info about
-	 * what defenses are needed; v6 debug has the most issues.
-	 */
+	}
 
 fail:
-	/* (void) */ dpm->finish(dpm);
+	dpm->finish(dpm);
 	return retval;
 }
 
@@ -951,7 +835,6 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 	struct arm *arm = dpm->arm;
 	struct reg_cache *cache = arm->core_cache;
 	int retval;
-	bool is_aarch64 = arm->core_state == ARM_STATE_AARCH64;
 
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
@@ -991,59 +874,48 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 	 * be queued, and need (efficient/batched) flushing later.
 	 */
 
-	/* Scan the registers until we find one that's both dirty and
-	 * eligible for flushing.  Flush that and everything else that
-	 * shares the same core mode setting.  Typically this won't
-	 * actually find anything to do...
-	 */
+	/* Restore original core mode and state */
+	retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+	if (retval != ERROR_OK)
+		goto done;
 
 	/* check everything except our scratch register R0 */
 	for (unsigned i = 1; i < cache->num_regs; i++) {
 		struct arm_reg *r;
-		unsigned regnum;
 
-		/* also skip PC, CPSR, and non-dirty */
-		if (i == (arm->core_cache->num_regs - 2))
+		/* skip PC and CPSR */
+		if (i == ARMV8_PC || i == ARMV8_xPSR)
 			continue;
-		if (arm->cpsr == cache->reg_list + i)
+		/* skip invalid */
+		if (!cache->reg_list[i].valid)
 			continue;
+		/* skip non-dirty */
 		if (!cache->reg_list[i].dirty)
 			continue;
 
+		/* skip all registers not on the current EL */
 		r = cache->reg_list[i].arch_info;
-		regnum = r->num;
+		if (r->mode != ARM_MODE_ANY &&
+				dpm->last_el != armv8_curel_from_core_mode(r->mode))
+			continue;
 
-		retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[i], regnum)
-				: dpmv8_write_reg32(dpm, &cache->reg_list[i], regnum);
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[i], i);
 		if (retval != ERROR_OK)
-			goto done;
+			break;
 	}
 
-
-	/* Restore original CPSR ... assuming either that we changed it,
-	 * or it's dirty.  Must write PC to ensure the return address is
-	 * defined, and must not write it before CPSR.
-	 */
-	retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
-	if (retval != ERROR_OK)
-		goto done;
-	arm->cpsr->dirty = false;
-
-	retval = is_aarch64 ? dpmv8_write_reg(dpm, arm->pc, (arm->core_cache->num_regs - 2))
-			: dpmv8_write_reg32(dpm, arm->pc, (arm->core_cache->num_regs - 2));
-	if (retval != ERROR_OK)
-		goto done;
-	arm->pc->dirty = false;
-
+	/* flush CPSR and PC */
+	if (retval == ERROR_OK)
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_xPSR], ARMV8_xPSR);
+	if (retval == ERROR_OK)
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[ARMV8_PC], ARMV8_PC);
 	/* flush R0 -- it's *very* dirty by now */
-	retval = is_aarch64 ? dpmv8_write_reg(dpm, &cache->reg_list[0], 0)
-			: dpmv8_write_reg32(dpm, &cache->reg_list[0], 0);
-	if (retval != ERROR_OK)
-		goto done;
-	cache->reg_list[0].dirty = false;
-
-	/* (void) */ dpm->finish(dpm);
+	if (retval == ERROR_OK)
+		retval = dpmv8_write_reg(dpm, &cache->reg_list[0], 0);
+	if (retval == ERROR_OK)
+		dpm->instr_cpsr_sync(dpm);
 done:
+	dpm->finish(dpm);
 	return retval;
 }
 
@@ -1061,19 +933,18 @@ static int armv8_dpm_read_core_reg(struct target *target, struct reg *r,
 	int retval;
 	int max = arm->core_cache->num_regs;
 
-	if (regnum < 0 || regnum > max)
+	if (regnum < 0 || regnum >= max)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	/* REVISIT what happens if we try to read SPSR in a core mode
+	/*
+	 * REVISIT what happens if we try to read SPSR in a core mode
 	 * which has no such register?
 	 */
-
 	retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = arm->core_state == ARM_STATE_AARCH64 ?
-			dpmv8_read_reg(dpm, r, regnum) : dpmv8_read_reg32(dpm, r, regnum);
+	retval = dpmv8_read_reg(dpm, r, regnum);
 	if (retval != ERROR_OK)
 		goto fail;
 
@@ -1101,12 +972,11 @@ static int armv8_dpm_write_core_reg(struct target *target, struct reg *r,
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = arm->core_state == ARM_STATE_AARCH64 ?
-			dpmv8_write_reg(dpm, r, regnum) : dpmv8_write_reg32(dpm, r, regnum);
+	retval = dpmv8_write_reg(dpm, r, regnum);
 
 	/* always clean up, regardless of error */
+	dpm->finish(dpm);
 
-	/* (void) */ dpm->finish(dpm);
 	return retval;
 }
 
@@ -1402,6 +1272,7 @@ void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dscr)
 	struct target *target = dpm->arm->target;
 
 	dpm->dscr = dscr;
+	dpm->last_el = (dscr >> 8) & 3;
 
 	/* Examine debug reason */
 	switch (DSCR_ENTRY(dscr)) {
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
index 416f4df..6470461 100644
--- a/src/target/armv8_dpm.h
+++ b/src/target/armv8_dpm.h
@@ -111,5 +111,6 @@ void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
 #define PRSR_SDR				(1 << 11)
 
 void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
+enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm);
 
 #endif /* OPENOCD_TARGET_ARM_DPM_H */
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 78b60e0..3e18a02 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -37,16 +37,16 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
-		[READ_REG_CLIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 1)),
-		[READ_REG_CSSELR]	= T32_FMTITR(ARMV4_5_MRC(15, 2, 0, 0, 0, 0)),
-		[READ_REG_CCSIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 1, 0, 0, 0, 0)),
-		[WRITE_REG_CSSELR]	= T32_FMTITR(ARMV4_5_MCR(15, 2, 0, 0, 0, 0)),
-		[READ_REG_MPIDR]	= T32_FMTITR(ARMV4_5_MRC(15, 0, 0, 0, 0, 5)),
-		[READ_REG_DTRRX]	= T32_FMTITR(ARMV4_5_MRC(14, 0, 0, 0, 5, 0)),
-		[WRITE_REG_DTRTX]	= T32_FMTITR(ARMV4_5_MCR(14, 0, 0, 0, 5, 0)),
-		[WRITE_REG_DSPSR]	= T32_FMTITR(ARMV8_MCR_DSPSR(0)),
-		[READ_REG_DSPSR]	= T32_FMTITR(ARMV8_MRC_DSPSR(0)),
-		[ARMV8_OPC_DSB_SY]	= T32_FMTITR(ARMV8_DSB_SY_T1),
+		[READ_REG_CLIDR]	= ARMV4_5_MRC(15, 1, 0, 0, 0, 1),
+		[READ_REG_CSSELR]	= ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
+		[READ_REG_CCSIDR]	= ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
+		[WRITE_REG_CSSELR]	= ARMV4_5_MCR(15, 2, 0, 0, 0, 0),
+		[READ_REG_MPIDR]	= ARMV4_5_MRC(15, 0, 0, 0, 0, 5),
+		[READ_REG_DTRRX]	= ARMV4_5_MRC(14, 0, 0, 0, 5, 0),
+		[WRITE_REG_DTRTX]	= ARMV4_5_MCR(14, 0, 0, 0, 5, 0),
+		[WRITE_REG_DSPSR]	= ARMV8_MCR_DSPSR(0),
+		[READ_REG_DSPSR]	= ARMV8_MRC_DSPSR(0),
+		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY_T1,
 };
 
 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 41abe04..cb20c84 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -66,7 +66,6 @@
 
 #define SYSTEM_ISR_EL1			0b1100011000001000
 
-
 #define SYSTEM_DBG_DSPSR_EL0    0b1101101000101000
 #define SYSTEM_DBG_DLR_EL0		0b1101101000101001
 #define SYSTEM_DBG_DTRRX_EL0	0b1001100000101000
@@ -103,14 +102,19 @@
 #define SYSTEM_ATS1E2R			0b0110001111000000
 #define SYSTEM_ATS1E3R			0b0111001111000000
 
+/* fault status and fault address */
+#define SYSTEM_FAR_EL1			0b1100001100000000
+#define SYSTEM_FAR_EL2			0b1110001100000000
+#define SYSTEM_FAR_EL3			0b1111001100000000
+#define SYSTEM_ESR_EL1			0b1100001010010000
+#define SYSTEM_ESR_EL2			0b1110001010010000
+#define SYSTEM_ESR_EL3			0b1111001010010000
+
 #define ARMV8_MRS_DSPSR(Rt)	(0xd53b4500 | (Rt))
 #define ARMV8_MSR_DSPSR(Rt)	(0xd51b4500 | (Rt))
 #define ARMV8_MRS_DLR(Rt)	(0xd53b4520 | (Rt))
 #define ARMV8_MSR_DLR(Rt)	(0xd51b4520 | (Rt))
 
-/* T32 ITR format */
-#define T32_FMTITR(instr) (((instr & 0x0000FFFF) << 16) | ((instr & 0xFFFF0000) >> 16))
-
 /* T32 instruction to access coprocessor registers */
 #define ARMV8_MCR_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MCR(cp, opc1, Rt, CRn, CRm, opc2)
 #define ARMV8_MRC_T1(cp, CRn, opc1, CRm, opc2, Rt) ARMV4_5_MRC(cp, opc1, Rt, CRn, CRm, opc2)
@@ -136,6 +140,11 @@
 #define ARMV8_MSR_IM(Op1, CRm, Op2) \
 	(0xd500401f | ((Op1) << 16)  | ((CRm) << 8) | ((Op2) << 5))
 
+#define ARMV8_MRS_T1(R, M1, Rd, M) (0xF3E08020 | (R << 20) | (M1 << 16) | (Rd << 8) | (M << 4))
+#define ARMV8_MRS_xPSR_T1(R, Rd) (0xF3EF8000 | (R << 20) | (Rd << 8))
+#define ARMV8_MSR_GP_T1(R, M1, Rd, M) (0xF3808020 | (R << 20) | (M1 << 8) | (Rd << 16) | (M << 4))
+#define ARMV8_MSR_GP_xPSR_T1(R, Rn, mask) (0xF3808000 | (R << 20) | (Rn << 16) | (mask << 8))
+
 #define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
 #define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5))
 
-- 
cgit v1.1


From 946958cb723f0b123505234275ce9a653ddbfbd2 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 11:31:40 +0200
Subject: aarch64: fix mode switching

DCPS only allows to enter higher ELs, for lower ELs you need to
use DRPS. Also, of course the encoding differs between A64 and T32.
Both DCPS and DRPS also clobber DLR and DSPSR, which then need to be
restored on resume.

Change-Id: Ifa3dcfa94212702e57170bd59fd0bb25495fb6fd
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c       |   6 +--
 src/target/armv8.c         |  16 ++++--
 src/target/armv8_dpm.c     | 128 +++++++++++++++++++++++++++++++--------------
 src/target/armv8_dpm.h     |   2 +-
 src/target/armv8_opcodes.c |   6 ++-
 src/target/armv8_opcodes.h |  12 +++--
 6 files changed, 119 insertions(+), 51 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 02f17d8..88e10c1 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -487,7 +487,7 @@ static int aarch64_internal_restore(struct target *target, int current,
 	buf_set_u64(arm->pc->value, 0, 64, resume_pc);
 	arm->pc->dirty = 1;
 	arm->pc->valid = 1;
-	dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+	armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
 	/* called it now before restoring context because it uses cpu
 	 * register r0 for restoring system control register */
@@ -697,7 +697,7 @@ static int aarch64_post_debug_entry(struct target *target)
 
 	switch (armv8->arm.core_mode) {
 		case ARMV8_64_EL0T:
-			dpmv8_modeswitch(&armv8->dpm, ARMV8_64_EL1T);
+			armv8_dpm_modeswitch(&armv8->dpm, ARMV8_64_EL1H);
 			/* fall through */
 		case ARMV8_64_EL1T:
 		case ARMV8_64_EL1H:
@@ -738,7 +738,7 @@ static int aarch64_post_debug_entry(struct target *target)
 			break;
 	}
 
-	dpmv8_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+	armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 32fe048..b55c153 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -781,6 +781,7 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm *arm = target_to_arm(target);
 	struct arm_dpm *dpm = &armv8->dpm;
+	enum arm_mode target_mode = ARM_MODE_ANY;
 	uint32_t retval;
 	uint32_t instr = 0;
 	uint64_t par;
@@ -801,12 +802,12 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	case SYSTEM_CUREL_EL0:
 		instr = ARMV8_SYS(SYSTEM_ATS12E0R, 0);
 		/* can only execute instruction at EL2 */
-		dpmv8_modeswitch(dpm, ARMV8_64_EL2T);
+		target_mode = ARMV8_64_EL2H;
 		break;
 	case SYSTEM_CUREL_EL1:
 		instr = ARMV8_SYS(SYSTEM_ATS12E1R, 0);
 		/* can only execute instruction at EL2 */
-		dpmv8_modeswitch(dpm, ARMV8_64_EL2T);
+		target_mode = ARMV8_64_EL2H;
 		break;
 	case SYSTEM_CUREL_EL2:
 		instr = ARMV8_SYS(SYSTEM_ATS1E2R, 0);
@@ -819,16 +820,23 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 		break;
 	};
 
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(dpm, target_mode);
+
 	/* write VA to R0 and execute translation instruction */
 	retval = dpm->instr_write_data_r0_64(dpm, instr, (uint64_t)va);
 	/* read result from PAR_EL1 */
 	if (retval == ERROR_OK)
 		retval = dpm->instr_read_data_r0_64(dpm, ARMV8_MRS(SYSTEM_PAR_EL1, 0), &par);
 
+	/* switch back to saved PE mode */
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
+
 	dpm->finish(dpm);
 
-	/* switch back to saved PE mode */
-	dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (retval != ERROR_OK)
 		return retval;
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index a92f92c..d6f2b87 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -610,53 +610,103 @@ static int dpmv8_msr(struct target *target, uint32_t op0,
  * Register access utilities
  */
 
-/* Toggles between recorded core mode (USR, SVC, etc) and a temporary one.
- * Routines *must* restore the original mode before returning!!
- */
-int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
+int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 {
 	struct armv8_common *armv8 = (struct armv8_common *)dpm->arm->arch_info;
-	int retval;
+	int retval = ERROR_OK;
+	unsigned int target_el;
+	enum arm_state core_state;
 	uint32_t cpsr;
 
 	/* restore previous mode */
-	if (mode == ARM_MODE_ANY)
+	if (mode == ARM_MODE_ANY) {
 		cpsr = buf_get_u32(dpm->arm->cpsr->value, 0, 32);
 
-	/* else force to the specified mode */
-	else
-		cpsr = mode >> 4;
+		LOG_DEBUG("restoring mode, cpsr = 0x%08"PRIx32, cpsr);
 
-	switch ((cpsr & 0xC) >> 2) {
-			case SYSTEM_CUREL_EL1:
-				retval = dpm->instr_execute(dpm, ARMV8_DCPS1(11));
-				if (retval != ERROR_OK)
-					return retval;
-				break;
-			case SYSTEM_CUREL_EL2:
-				retval = dpm->instr_execute(dpm, ARMV8_DCPS2(11));
-				if (retval != ERROR_OK)
-					return retval;
-				break;
-			break;
-			case SYSTEM_CUREL_EL3:
-				retval = dpm->instr_execute(dpm, ARMV8_DCPS3(11));
-				if (retval != ERROR_OK)
-					return retval;
-				break;
-			break;
-			default:
-				LOG_DEBUG("unknow mode 0x%x", (unsigned) ((cpsr & 0xC) >> 2));
-				break;
+	} 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;
 	}
 
+	switch (cpsr & 0x1f) {
+	/* aarch32 modes */
+	case ARM_MODE_USR:
+		target_el = 0;
+		break;
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT:
+	case ARM_MODE_IRQ:
+	case ARM_MODE_FIQ:
+		target_el = 1;
+		break;
+	/*
+	 * TODO: handle ARM_MODE_HYP
+	 * case ARM_MODE_HYP:
+	 *      target_el = 2;
+	 *      break;
+	 */
+	case ARM_MODE_MON:
+		target_el = 3;
+		break;
+	/* aarch64 modes */
+	default:
+		target_el = (cpsr >> 2) & 3;
+	}
 
-	retval = dpm->instr_write_data_r0(dpm, armv8_opcode(armv8, WRITE_REG_DSPSR), cpsr);
-	if (retval != ERROR_OK)
-		return retval;
+	if (target_el > SYSTEM_CUREL_EL3) {
+		LOG_ERROR("%s: Invalid target exception level %i", __func__, target_el);
+		return ERROR_FAIL;
+	}
+
+	LOG_DEBUG("target_el = %i, last_el = %i", target_el, dpm->last_el);
+	if (target_el > dpm->last_el) {
+		retval = dpm->instr_execute(dpm,
+				armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
+	} else {
+		core_state = armv8_dpm_get_core_state(dpm);
+		if (core_state != ARM_STATE_AARCH64) {
+			/* cannot do DRPS/ERET when already in EL0 */
+			if (dpm->last_el != 0) {
+				/* load SPSR with the desired mode and execute DRPS */
+				LOG_DEBUG("SPSR = 0x%08"PRIx32, cpsr);
+				retval = dpm->instr_write_data_r0(dpm,
+						ARMV8_MSR_GP_xPSR_T1(1, 0, 15), cpsr);
+				if (retval == ERROR_OK)
+					retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
+			}
+		} else {
+			/*
+			 * need to execute multiple DRPS instructions until target_el
+			 * is reached
+			 */
+			while (retval == ERROR_OK && dpm->last_el != target_el) {
+				unsigned int cur_el = dpm->last_el;
+				retval = dpm->instr_execute(dpm, armv8_opcode(armv8, ARMV8_OPC_DRPS));
+				if (cur_el == dpm->last_el) {
+					LOG_INFO("Cannot reach EL %i, SPSR corrupted?", target_el);
+					break;
+				}
+			}
+		}
 
-	if (dpm->instr_cpsr_sync)
-		retval = dpm->instr_cpsr_sync(dpm);
+		/* On executing DRPS, DSPSR and DLR become UNKNOWN, mark them as dirty */
+		dpm->arm->cpsr->dirty = true;
+		dpm->arm->pc->dirty = true;
+
+		/*
+		 * re-evaluate the core state, we might be in Aarch32 state now
+		 * we rely on dpm->dscr being up-to-date
+		 */
+		core_state = armv8_dpm_get_core_state(dpm);
+		armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
+		armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+	}
 
 	return retval;
 }
@@ -875,7 +925,7 @@ int armv8_dpm_write_dirty_registers(struct arm_dpm *dpm, bool bpwp)
 	 */
 
 	/* Restore original core mode and state */
-	retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+	retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 	if (retval != ERROR_OK)
 		goto done;
 
@@ -1022,9 +1072,9 @@ static int armv8_dpm_full_context(struct target *target)
 				 * in FIQ mode we need to patch mode.
 				 */
 				if (mode != ARM_MODE_ANY)
-					retval = dpmv8_modeswitch(dpm, mode);
+					retval = armv8_dpm_modeswitch(dpm, mode);
 				else
-					retval = dpmv8_modeswitch(dpm, ARM_MODE_USR);
+					retval = armv8_dpm_modeswitch(dpm, ARM_MODE_USR);
 
 				if (retval != ERROR_OK)
 					goto done;
@@ -1042,7 +1092,7 @@ static int armv8_dpm_full_context(struct target *target)
 
 	} while (did_read);
 
-	retval = dpmv8_modeswitch(dpm, ARM_MODE_ANY);
+	retval = armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 	/* (void) */ dpm->finish(dpm);
 done:
 	return retval;
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
index 6470461..48e2ca1 100644
--- a/src/target/armv8_dpm.h
+++ b/src/target/armv8_dpm.h
@@ -32,7 +32,7 @@ int armv8_dpm_setup(struct arm_dpm *dpm);
 int armv8_dpm_initialize(struct arm_dpm *dpm);
 
 int armv8_dpm_read_current_registers(struct arm_dpm *);
-int dpmv8_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
+int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode);
 
 
 int armv8_dpm_write_dirty_registers(struct arm_dpm *, bool bpwp);
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 3e18a02..75ea946 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -33,7 +33,9 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[WRITE_REG_DTRTX]	= ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),
 		[WRITE_REG_DSPSR]	= ARMV8_MSR_DSPSR(0),
 		[READ_REG_DSPSR]	= ARMV8_MRS_DSPSR(0),
-		[ARMV8_OPC_DSB_SY]  = ARMV8_DSB_SY,
+		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY,
+		[ARMV8_OPC_DCPS]	= ARMV8_DCPS(0, 11),
+		[ARMV8_OPC_DRPS]	= ARMV8_DRPS,
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -47,6 +49,8 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[WRITE_REG_DSPSR]	= ARMV8_MCR_DSPSR(0),
 		[READ_REG_DSPSR]	= ARMV8_MRC_DSPSR(0),
 		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY_T1,
+		[ARMV8_OPC_DCPS]	= ARMV8_DCPS_T1(0),
+		[ARMV8_OPC_DRPS]	= ARMV8_ERET_T1,
 };
 
 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 cb20c84..e57e7e9 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -125,9 +125,13 @@
 #define ARMV8_MRC_DLR(Rt)	ARMV8_MRC_T1(15, 4, 3, 5, 1, Rt)
 #define ARMV8_MCR_DLR(Rt)	ARMV8_MCR_T1(15, 4, 3, 5, 1, Rt)
 
-#define ARMV8_DCPS1(IM)	(0xd4a00001 | (((IM) & 0xFFFF) << 5))
-#define ARMV8_DCPS2(IM)	(0xd4a00002 | (((IM) & 0xFFFF) << 5))
-#define ARMV8_DCPS3(IM)	(0xd4a00003 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS1(IM)		(0xd4a00001 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS2(IM)		(0xd4a00002 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS3(IM)		(0xd4a00003 | (((IM) & 0xFFFF) << 5))
+#define ARMV8_DCPS(EL, IM)	(0xd4a00000 | (((IM) & 0xFFFF) << 5) | EL)
+#define ARMV8_DCPS_T1(EL)	(0xf78f8000 | EL)
+#define ARMV8_DRPS		0xd6bf03e0
+#define ARMV8_ERET_T1		0xf3de8f00
 
 #define ARMV8_DSB_SY				0xd5033F9F
 #define ARMV8_DSB_SY_T1				0xf3bf8f4f
@@ -166,6 +170,8 @@ enum armv8_opcode {
 	WRITE_REG_DSPSR,
 	READ_REG_DSPSR,
 	ARMV8_OPC_DSB_SY,
+	ARMV8_OPC_DCPS,
+	ARMV8_OPC_DRPS,
 	ARMV8_OPC_NUM,
 };
 
-- 
cgit v1.1


From 62c2eb4b284e0a3deb2aad7163354200bd93e634 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 13:20:26 +0200
Subject: aarch64: cache identification for aarch32 state

Use proper T32 opcodes for cache identification when the PE is in
Aarch32 state

Change-Id: I9cd9169409889273a3fd61167f388e68d8dde86d
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8_cache.c   | 17 +++++++++++------
 src/target/armv8_opcodes.c |  2 ++
 src/target/armv8_opcodes.h |  1 +
 3 files changed, 14 insertions(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index f496c3c..31e4c79 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -266,17 +266,18 @@ static int  armv8_flush_all_data(struct target *target)
 
 static int get_cache_info(struct arm_dpm *dpm, int cl, int ct, uint32_t *cache_reg)
 {
+	struct armv8_common *armv8 = dpm->arm->arch_info;
 	int retval = ERROR_OK;
 
 	/*  select cache level */
 	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MSR_GP(SYSTEM_CSSELR, 0),
+			armv8_opcode(armv8, WRITE_REG_CSSELR),
 			(cl << 1) | (ct == 1 ? 1 : 0));
 	if (retval != ERROR_OK)
 		goto done;
 
 	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(SYSTEM_CCSIDR, 0),
+			armv8_opcode(armv8, READ_REG_CCSIDR),
 			cache_reg);
  done:
 	return retval;
@@ -319,7 +320,8 @@ int armv8_identify_cache(struct armv8_common *armv8)
 		goto done;
 
 	/* retrieve CTR */
-	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS(SYSTEM_CTR, 0), &ctr);
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CTR), &ctr);
 	if (retval != ERROR_OK)
 		goto done;
 
@@ -329,7 +331,8 @@ int armv8_identify_cache(struct armv8_common *armv8)
 		 ctr, cache->iminline, cache->dminline);
 
 	/*  retrieve CLIDR */
-	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS(SYSTEM_CLIDR, 0), &clidr);
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CLIDR), &clidr);
 	if (retval != ERROR_OK)
 		goto done;
 
@@ -338,7 +341,8 @@ int armv8_identify_cache(struct armv8_common *armv8)
 
 	/*  retrieve selected cache for later restore
 	 *  MRC p15, 2,<Rd>, c0, c0, 0; Read CSSELR */
-	retval = dpm->instr_read_data_r0(dpm, ARMV8_MRS(SYSTEM_CSSELR, 0), &csselr);
+	retval = dpm->instr_read_data_r0(dpm,
+			armv8_opcode(armv8, READ_REG_CSSELR), &csselr);
 	if (retval != ERROR_OK)
 		goto done;
 
@@ -396,7 +400,8 @@ int armv8_identify_cache(struct armv8_common *armv8)
 	}
 
 	/*  restore selected cache  */
-	dpm->instr_write_data_r0(dpm, ARMV8_MSR_GP(SYSTEM_CSSELR, 0), csselr);
+	dpm->instr_write_data_r0(dpm,
+			armv8_opcode(armv8, WRITE_REG_CSSELR), csselr);
 	if (retval != ERROR_OK)
 		goto done;
 
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 75ea946..2b42cdf 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -24,6 +24,7 @@
 #include "armv8_opcodes.h"
 
 static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CTR]		= ARMV8_MRS(SYSTEM_CTR, 0),
 		[READ_REG_CLIDR]	= ARMV8_MRS(SYSTEM_CLIDR, 0),
 		[READ_REG_CSSELR]	= ARMV8_MRS(SYSTEM_CSSELR, 0),
 		[READ_REG_CCSIDR]	= ARMV8_MRS(SYSTEM_CCSIDR, 0),
@@ -39,6 +40,7 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
+		[READ_REG_CTR]		= ARMV4_5_MRC(15, 0, 0, 0, 0, 1),
 		[READ_REG_CLIDR]	= ARMV4_5_MRC(15, 1, 0, 0, 0, 1),
 		[READ_REG_CSSELR]	= ARMV4_5_MRC(15, 2, 0, 0, 0, 0),
 		[READ_REG_CCSIDR]	= ARMV4_5_MRC(15, 1, 0, 0, 0, 0),
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index e57e7e9..b489d57 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -160,6 +160,7 @@
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
 enum armv8_opcode {
+	READ_REG_CTR,
 	READ_REG_CLIDR,
 	READ_REG_CSSELR,
 	READ_REG_CCSIDR,
-- 
cgit v1.1


From c30f8d6a0716bb8151cd787d99dca65aa9845ed2 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 13:37:11 +0200
Subject: aarch64: handle exceptions taken in debug state

When an armv8-a PE causes an exception while halted, e.g. by performing
a prohibited memory or register access, its state is affected in the
same way as if it was running. That means, a number of registers is
overwritten (notably DLR and DSPSR, but also others) and also
potentially the exception level and therefore also the PE state can
change. This state must be restored before resuming normal operation.

This is done by marking the relevant cached registers "dirty" so that
they are written back before resume.

Change-Id: I9b6967a62d7cb23a477a9f7839f8d2b7087eed09
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c   |  2 ++
 src/target/armv8_dpm.c | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++
 src/target/armv8_dpm.h |  1 +
 3 files changed, 60 insertions(+)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 88e10c1..ce4a8f6 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1492,6 +1492,7 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		mem_ap_write_atomic_u32(armv8->debug_ap,
 					armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
+		armv8_dpm_handle_exception(dpm);
 		goto error_free_buff_w;
 	}
 
@@ -1657,6 +1658,7 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		mem_ap_write_atomic_u32(armv8->debug_ap,
 					armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+		armv8_dpm_handle_exception(dpm);
 		goto error_free_buff_r;
 	}
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index d6f2b87..ee9e1f3 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -279,6 +279,7 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 		/* clear the sticky error condition */
 		mem_ap_write_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+		armv8_dpm_handle_exception(dpm);
 		retval = ERROR_FAIL;
 	}
 
@@ -668,6 +669,9 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 	if (target_el > dpm->last_el) {
 		retval = dpm->instr_execute(dpm,
 				armv8_opcode(armv8, ARMV8_OPC_DCPS) | target_el);
+
+		/* DCPS clobbers registers just like an exception taken */
+		armv8_dpm_handle_exception(dpm);
 	} else {
 		core_state = armv8_dpm_get_core_state(dpm);
 		if (core_state != ARM_STATE_AARCH64) {
@@ -1311,6 +1315,59 @@ void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
 	dpm->wp_pc = addr;
 }
 
+/*
+ * Handle exceptions taken in debug state. This happens mostly for memory
+ * accesses that violated a MMU policy. Taking an exception while in debug
+ * state clobbers certain state registers on the target exception level.
+ * Just mark those registers dirty so that they get restored on resume.
+ * This works both for Aarch32 and Aarch64 states.
+ *
+ * This function must not perform any actions that trigger another exception
+ * or a recursion will happen.
+ */
+void armv8_dpm_handle_exception(struct arm_dpm *dpm)
+{
+	struct armv8_common *armv8 = dpm->arm->arch_info;
+	struct reg_cache *cache = dpm->arm->core_cache;
+	enum arm_state core_state;
+	uint64_t dlr;
+	uint32_t dspsr;
+	unsigned int el;
+
+	static const int clobbered_regs_by_el[3][5] = {
+		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL1, ARMV8_ESR_EL1, ARMV8_SPSR_EL1 },
+		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL2, ARMV8_ESR_EL2, ARMV8_SPSR_EL2 },
+		{ ARMV8_PC, ARMV8_xPSR, ARMV8_ELR_EL3, ARMV8_ESR_EL3, ARMV8_SPSR_EL3 },
+	};
+
+	el = (dpm->dscr >> 8) & 3;
+
+	/* safety check, must not happen since EL0 cannot be a target for an exception */
+	if (el < SYSTEM_CUREL_EL1 || el > SYSTEM_CUREL_EL3) {
+		LOG_ERROR("%s: EL %i is invalid, DSCR corrupted?", __func__, el);
+		return;
+	}
+
+	armv8->read_reg_u64(armv8, ARMV8_xPSR, &dlr);
+	dspsr = dlr;
+	armv8->read_reg_u64(armv8, ARMV8_PC, &dlr);
+
+	LOG_DEBUG("Exception taken to EL %i, DLR=0x%016"PRIx64" DSPSR=0x%08"PRIx32,
+			el, dlr, dspsr);
+
+	/* mark all clobbered registers as dirty */
+	for (int i = 0; i < 5; i++)
+		cache->reg_list[clobbered_regs_by_el[el-1][i]].dirty = true;
+
+	/*
+	 * re-evaluate the core state, we might be in Aarch64 state now
+	 * we rely on dpm->dscr being up-to-date
+	 */
+	core_state = armv8_dpm_get_core_state(dpm);
+	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
+	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+}
+
 /*----------------------------------------------------------------------*/
 
 /*
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
index 48e2ca1..133b367 100644
--- a/src/target/armv8_dpm.h
+++ b/src/target/armv8_dpm.h
@@ -111,6 +111,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
 #define PRSR_SDR				(1 << 11)
 
 void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
+void armv8_dpm_handle_exception(struct arm_dpm *dpm);
 enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm);
 
 #endif /* OPENOCD_TARGET_ARM_DPM_H */
-- 
cgit v1.1


From 05bf20d05afbcbb723c16cfa0e2f3a93edb435f7 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 13:59:35 +0200
Subject: aarch64: make sure to enable HDE for all SMP PEs to be halted

When halting a group of PEs through CTI, HDE must be set in EDSCR for
all of them.

Change-Id: Iaa4bc0b0fe31e46a463c709d8274023225affd85
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 55 ++++++++++++++++++++++++++--------------------------
 1 file changed, 27 insertions(+), 28 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index ce4a8f6..947c1ec 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -279,6 +279,28 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 	return retval;
 }
 
+static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	uint32_t dscr;
+
+	/* Read DSCR */
+	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	dscr &= ~bit_mask;
+	/* put new value */
+	dscr |= value & bit_mask;
+
+	/* write new DSCR */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	return retval;
+}
+
 static struct target *get_aarch64(struct target *target, int32_t coreid)
 {
 	struct target_list *head;
@@ -305,9 +327,12 @@ static int aarch64_halt_smp(struct target *target)
 		struct armv8_common *armv8 = target_to_armv8(curr);
 
 		/* open the gate for channel 0 to let HALT requests pass to the CTM */
-		if (curr->smp)
+		if (curr->smp) {
 			retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 					armv8->cti_base + CTI_GATE, CTI_CHNL(0));
+			if (retval == ERROR_OK)
+				retval = aarch64_set_dscr_bits(curr, DSCR_HDE, DSCR_HDE);
+		}
 		if (retval != ERROR_OK)
 			break;
 
@@ -411,11 +436,7 @@ static int aarch64_halt(struct target *target)
 	/*
 	 * add HDE in halting debug mode
 	 */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	if (retval == ERROR_OK)
-		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
+	retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -758,28 +779,6 @@ static int aarch64_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
-static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
-{
-	struct armv8_common *armv8 = target_to_armv8(target);
-	uint32_t dscr;
-
-	/* Read DSCR */
-	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	if (ERROR_OK != retval)
-		return retval;
-
-	/* clear bitfield */
-	dscr &= ~bit_mask;
-	/* put new value */
-	dscr |= value & bit_mask;
-
-	/* write new DSCR */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	return retval;
-}
-
 static int aarch64_step(struct target *target, int current, target_addr_t address,
 	int handle_breakpoints)
 {
-- 
cgit v1.1


From ba822308560d691263ca26b16d5c9e7a856fb262 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 14:46:11 +0200
Subject: aarch64: allow reading TTBR register when halted in EL0

There's no access to TTBR in EL0. Circumvent by moving the PE to EL1
before reading, and switch back to original mode afterwards.

Change-Id: I22891b958d3d7e6fad1cb27183c192d975d63d89
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 3 +++
 1 file changed, 3 insertions(+)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index b55c153..089982e 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -737,6 +737,8 @@ static __unused int armv8_read_ttbcr(struct target *target)
 		armv8->page_size = (ttbcr >> 14) & 3;
 		break;
 	case SYSTEM_CUREL_EL0:
+		armv8_dpm_modeswitch(dpm, ARMV8_64_EL1H);
+		/* fall through */
 	case SYSTEM_CUREL_EL1:
 		retval = dpm->instr_read_data_r0_64(dpm,
 				ARMV8_MRS(SYSTEM_TCR_EL1, 0),
@@ -764,6 +766,7 @@ static __unused int armv8_read_ttbcr(struct target *target)
 		LOG_INFO("TTBR0 access above %" PRIx64, (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
 
 done:
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 	dpm->finish(dpm);
 	return retval;
 }
-- 
cgit v1.1


From ccb1199d490b86e0f963a1d0509c748c800f20d4 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 15:36:19 +0200
Subject: aarch64: enable aarch32 debugging with arm gdb

When a PE is in Aarch32 state and gdb asks for a target description,
provide a register view compatible with the "org.gnu.gdb.arm.core"
feature. Only current-mode registers are exported, banked registers are
not visible.

Change-Id: I99a85d94831cf597fe8cff6a0a1818ce0a33613b
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c |   1 +
 src/target/armv8.c   | 190 +++++++++++++++++++++++++++++++++++++++++++--------
 2 files changed, 164 insertions(+), 27 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 947c1ec..d7d557d 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -590,6 +590,7 @@ static int aarch64_internal_restart(struct target *target, bool slave_pe)
 
 	/* registers are now invalid */
 	register_cache_invalidate(arm->core_cache);
+	register_cache_invalidate(arm->core_cache->next);
 
 	return ERROR_OK;
 }
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 089982e..a2a099d 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1012,12 +1012,39 @@ static const struct {
 	{ ARMV8_SPSR_EL3, "SPSR_EL3", 32, ARMV8_64_EL3H, REG_TYPE_UINT32, "banked", "net.sourceforge.openocd.banked" },
 };
 
-#define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
+static const struct {
+	unsigned id;
+	const char *name;
+	unsigned bits;
+	enum arm_mode mode;
+	enum reg_type type;
+	const char *group;
+	const char *feature;
+} armv8_regs32[] = {
+	{ ARMV8_R0,  "r0",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R1,  "r1",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R2,  "r2",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R3,  "r3",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R4,  "r4",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R5,  "r5",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R6,  "r6",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R7,  "r7",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R8,  "r8",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R9,  "r9",  32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R10, "r10", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R11, "r11", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R12, "r12", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R13, "sp", 32, ARM_MODE_ANY, REG_TYPE_DATA_PTR, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_R14, "lr",  32, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_PC, "pc",   32, ARM_MODE_ANY, REG_TYPE_CODE_PTR, "general", "org.gnu.gdb.arm.core" },
+	{ ARMV8_xPSR, "cpsr", 32, ARM_MODE_ANY, REG_TYPE_UINT32, "general", "org.gnu.gdb.arm.core" },
+};
 
+#define ARMV8_NUM_REGS ARRAY_SIZE(armv8_regs)
+#define ARMV8_NUM_REGS32 ARRAY_SIZE(armv8_regs32)
 
 static int armv8_get_core_reg(struct reg *reg)
 {
-	int retval;
 	struct arm_reg *armv8_reg = reg->arch_info;
 	struct target *target = armv8_reg->target;
 	struct arm *arm = target_to_arm(target);
@@ -1025,9 +1052,7 @@ static int armv8_get_core_reg(struct reg *reg)
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	retval = arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
-
-	return retval;
+	return arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
 }
 
 static int armv8_set_core_reg(struct reg *reg, uint8_t *buf)
@@ -1057,25 +1082,83 @@ static const struct reg_arch_type armv8_reg_type = {
 	.set = armv8_set_core_reg,
 };
 
+static int armv8_get_core_reg32(struct reg *reg)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+	struct reg_cache *cache = arm->core_cache;
+	struct reg *reg64;
+	int retval;
+
+	LOG_DEBUG("reg.name:%s number:%i arm.num:%i value:0x%08" PRIx64,
+			reg->name, reg->number, armv8_reg->num, buf_get_u64(reg->value, 0, 32));
+
+	/* get the corresponding Aarch64 register */
+	reg64 = cache->reg_list + armv8_reg->num;
+	if (reg64->valid) {
+		reg->valid = true;
+		return ERROR_OK;
+	}
+
+	retval = arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
+	if (retval == ERROR_OK)
+		reg->valid = reg64->valid;
+
+	return retval;
+}
+
+static int armv8_set_core_reg32(struct reg *reg, uint8_t *buf)
+{
+	struct arm_reg *armv8_reg = reg->arch_info;
+	struct target *target = armv8_reg->target;
+	struct arm *arm = target_to_arm(target);
+	struct reg_cache *cache = arm->core_cache;
+	struct reg *reg64 = cache->reg_list + armv8_reg->num;
+	uint32_t value = buf_get_u32(buf, 0, 32);
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
+	if (reg64 == arm->cpsr) {
+		armv8_set_cpsr(arm, value);
+	} else {
+		buf_set_u32(reg->value, 0, 32, value);
+		reg->valid = 1;
+		reg64->valid = 1;
+	}
+
+	reg64->dirty = 1;
+
+	return ERROR_OK;
+}
+
+static const struct reg_arch_type armv8_reg32_type = {
+	.get = armv8_get_core_reg32,
+	.set = armv8_set_core_reg32,
+};
+
 /** Builds cache of architecturally defined registers.  */
 struct reg_cache *armv8_build_reg_cache(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm *arm = &armv8->arm;
 	int num_regs = ARMV8_NUM_REGS;
+	int num_regs32 = ARMV8_NUM_REGS32;
 	struct reg_cache **cache_p = register_get_last_cache_p(&target->reg_cache);
 	struct reg_cache *cache = malloc(sizeof(struct reg_cache));
+	struct reg_cache *cache32 = malloc(sizeof(struct reg_cache));
 	struct reg *reg_list = calloc(num_regs, sizeof(struct reg));
+	struct reg *reg_list32 = calloc(num_regs32, sizeof(struct reg));
 	struct arm_reg *arch_info = calloc(num_regs, sizeof(struct arm_reg));
 	struct reg_feature *feature;
 	int i;
 
 	/* Build the process context cache */
-	cache->name = "arm v8 registers";
-	cache->next = NULL;
+	cache->name = "Aarch64 registers";
+	cache->next = cache32;
 	cache->reg_list = reg_list;
 	cache->num_regs = num_regs;
-	(*cache_p) = cache;
 
 	for (i = 0; i < num_regs; i++) {
 		arch_info[i].num = armv8_regs[i].id;
@@ -1086,8 +1169,6 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 		reg_list[i].name = armv8_regs[i].name;
 		reg_list[i].size = armv8_regs[i].bits;
 		reg_list[i].value = calloc(1, 8);
-		reg_list[i].dirty = 0;
-		reg_list[i].valid = 0;
 		reg_list[i].type = &armv8_reg_type;
 		reg_list[i].arch_info = &arch_info[i];
 
@@ -1114,6 +1195,38 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 	arm->pc = reg_list + ARMV8_PC;
 	arm->core_cache = cache;
 
+	/* shadow cache for ARM mode registers */
+	cache32->name = "Aarch32 registers";
+	cache32->next = NULL;
+	cache32->reg_list = reg_list32;
+	cache32->num_regs = num_regs32;
+
+	for (i = 0; i < num_regs32; i++) {
+		reg_list32[i].name = armv8_regs32[i].name;
+		reg_list32[i].size = armv8_regs32[i].bits;
+		reg_list32[i].value = &arch_info[armv8_regs32[i].id].value[0];
+		reg_list32[i].type = &armv8_reg32_type;
+		reg_list32[i].arch_info = &arch_info[armv8_regs32[i].id];
+		reg_list32[i].group = armv8_regs32[i].group;
+		reg_list32[i].number = i;
+		reg_list32[i].exist = true;
+		reg_list32[i].caller_save = true;
+
+		feature = calloc(1, sizeof(struct reg_feature));
+		if (feature) {
+			feature->name = armv8_regs32[i].feature;
+			reg_list32[i].feature = feature;
+		} else
+			LOG_ERROR("unable to allocate feature list");
+
+		reg_list32[i].reg_data_type = calloc(1, sizeof(struct reg_data_type));
+		if (reg_list32[i].reg_data_type)
+			reg_list32[i].reg_data_type->type = armv8_regs32[i].type;
+		else
+			LOG_ERROR("unable to allocate reg type list");
+	}
+
+	(*cache_p) = cache;
 	return cache;
 }
 
@@ -1143,27 +1256,50 @@ int armv8_get_gdb_reg_list(struct target *target,
 	struct arm *arm = target_to_arm(target);
 	int i;
 
-	switch (reg_class) {
-	case REG_CLASS_GENERAL:
-		*reg_list_size = ARMV8_ELR_EL1;
-		*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+	if (arm->core_state == ARM_STATE_AARCH64) {
 
-		for (i = 0; i < *reg_list_size; i++)
-				(*reg_list)[i] = armv8_reg_current(arm, i);
+		LOG_DEBUG("Creating Aarch64 register list");
 
-		return ERROR_OK;
-	case REG_CLASS_ALL:
-		*reg_list_size = ARMV8_LAST_REG;
-		*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+		switch (reg_class) {
+		case REG_CLASS_GENERAL:
+			*reg_list_size = ARMV8_ELR_EL1;
+			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 
-		for (i = 0; i < *reg_list_size; i++)
-				(*reg_list)[i] = armv8_reg_current(arm, i);
+			for (i = 0; i < *reg_list_size; i++)
+					(*reg_list)[i] = armv8_reg_current(arm, i);
+			return ERROR_OK;
 
-		return ERROR_OK;
+		case REG_CLASS_ALL:
+			*reg_list_size = ARMV8_LAST_REG;
+			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
 
-	default:
-		LOG_ERROR("not a valid register class type in query.");
-		return ERROR_FAIL;
-		break;
+			for (i = 0; i < *reg_list_size; i++)
+					(*reg_list)[i] = armv8_reg_current(arm, i);
+
+			return ERROR_OK;
+
+		default:
+			LOG_ERROR("not a valid register class type in query.");
+			return ERROR_FAIL;
+		}
+	} else {
+		struct reg_cache *cache32 = arm->core_cache->next;
+
+		LOG_DEBUG("Creating Aarch32 register list");
+
+		switch (reg_class) {
+		case REG_CLASS_GENERAL:
+		case REG_CLASS_ALL:
+			*reg_list_size = cache32->num_regs;
+			*reg_list = malloc(sizeof(struct reg *) * (*reg_list_size));
+
+			for (i = 0; i < *reg_list_size; i++)
+				(*reg_list)[i] = cache32->reg_list + i;
+
+			return ERROR_OK;
+		default:
+			LOG_ERROR("not a valid register class type in query.");
+			return ERROR_FAIL;
+		}
 	}
 }
-- 
cgit v1.1


From 0a53bae0ea4007f171802b7d5e65499d4aa18ad0 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 15:39:30 +0200
Subject: aarch64: cleanup context restore

Remove register cache invalidation and target state changes that are
handled appropriately in other functions.

Change-Id: Ic903f41ddc267f4b8765ea022bd4d6da1017e21f
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 15 +++------------
 1 file changed, 3 insertions(+), 12 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d7d557d..1220004 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -504,25 +504,16 @@ static int aarch64_internal_restore(struct target *target, int current,
 			LOG_ERROR("How do I resume into Jazelle state??");
 			return ERROR_FAIL;
 	}
-	LOG_DEBUG("resume pc = 0x%16" PRIx64, resume_pc);
+	LOG_DEBUG("resume pc = 0x%016" PRIx64, resume_pc);
 	buf_set_u64(arm->pc->value, 0, 64, resume_pc);
 	arm->pc->dirty = 1;
 	arm->pc->valid = 1;
-	armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
 	/* called it now before restoring context because it uses cpu
 	 * register r0 for restoring system control register */
 	retval = aarch64_restore_system_control_reg(target);
-	if (retval != ERROR_OK)
-		return retval;
-	retval = aarch64_restore_context(target, handle_breakpoints);
-	if (retval != ERROR_OK)
-		return retval;
-	target->debug_reason = DBG_REASON_NOTHALTED;
-	target->state = TARGET_RUNNING;
-
-	/* registers are now invalid */
-	register_cache_invalidate(arm->core_cache);
+	if (retval == ERROR_OK)
+		retval = aarch64_restore_context(target, handle_breakpoints);
 
 	return retval;
 }
-- 
cgit v1.1


From 9f4b5b509f016a8c78f44d2a668784fa7fc10e3f Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 16:23:40 +0200
Subject: aarch64: discard async aborts on entering debug state

recommended for Corte-A8 cores, not sure if necessary
for ARMv8 based cores as well.

Change-Id: Ibcb36170c5fac6a6b132de17f734c70a56919f9b
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c       | 7 ++++++-
 src/target/armv8_dpm.c     | 7 ++++++-
 src/target/armv8_opcodes.c | 2 ++
 src/target/armv8_opcodes.h | 3 +++
 4 files changed, 17 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 1220004..110f8dc 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -663,11 +663,16 @@ static int aarch64_debug_entry(struct target *target)
 	/* make sure to clear all sticky errors */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+
+	/* discard async exceptions */
+	if (retval == ERROR_OK)
+		retval = dpm->instr_cpsr_sync(dpm);
+
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Examine debug reason */
-	armv8_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
+	armv8_dpm_report_dscr(dpm, aarch64->cpudbg_dscr);
 
 	/* save address of instruction that triggered the watchpoint? */
 	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index ee9e1f3..8caa8b6 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -360,9 +360,14 @@ static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
 
 static int dpmv8_instr_cpsr_sync(struct arm_dpm *dpm)
 {
+	int retval;
 	struct armv8_common *armv8 = dpm->arm->arch_info;
+
 	/* "Prefetch flush" after modifying execution status in CPSR */
-	return dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), NULL);
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_DSB_SY), &dpm->dscr);
+	if (retval == ERROR_OK)
+		dpmv8_exec_opcode(dpm, armv8_opcode(armv8, ARMV8_OPC_ISB_SY), &dpm->dscr);
+	return retval;
 }
 
 static int dpmv8_instr_read_data_dcc(struct arm_dpm *dpm,
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 2b42cdf..7790144 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -37,6 +37,7 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY,
 		[ARMV8_OPC_DCPS]	= ARMV8_DCPS(0, 11),
 		[ARMV8_OPC_DRPS]	= ARMV8_DRPS,
+		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB,
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -53,6 +54,7 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DSB_SY]	= ARMV8_DSB_SY_T1,
 		[ARMV8_OPC_DCPS]	= ARMV8_DCPS_T1(0),
 		[ARMV8_OPC_DRPS]	= ARMV8_ERET_T1,
+		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB_SY_T1,
 };
 
 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 b489d57..fe6b28a 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -135,6 +135,8 @@
 
 #define ARMV8_DSB_SY				0xd5033F9F
 #define ARMV8_DSB_SY_T1				0xf3bf8f4f
+#define ARMV8_ISB				0xd5033fdf
+#define ARMV8_ISB_SY_T1				0xf3bf8f6f
 
 #define ARMV8_MRS(System, Rt)	(0xd5300000 | ((System) << 5) | (Rt))
 /* ARM V8 Move to system register. */
@@ -173,6 +175,7 @@ enum armv8_opcode {
 	ARMV8_OPC_DSB_SY,
 	ARMV8_OPC_DCPS,
 	ARMV8_OPC_DRPS,
+	ARMV8_OPC_ISB_SY,
 	ARMV8_OPC_NUM,
 };
 
-- 
cgit v1.1


From f7dbcc3fab3d5b52710f235ae3ead3135cf9c2e6 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 6 Oct 2016 16:37:25 +0200
Subject: aarch64: consolidate sticky error handling

Move clearing of DSCR "Sticky Error" condition to the
exception handling function. Clear once on entering debug state.

Change-Id: Iec1d09d6f2d9cdd7e92953da5ea19f3e399ca12c
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c   | 25 +++----------------------
 src/target/armv8_dpm.c | 13 ++++---------
 2 files changed, 7 insertions(+), 31 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 110f8dc..b716122 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -538,7 +538,9 @@ static int aarch64_internal_restart(struct target *target, bool slave_pe)
 		return retval;
 
 	if ((dscr & DSCR_ITE) == 0)
-		LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
+		LOG_ERROR("DSCR.ITE must be set before leaving debug!");
+	if ((dscr & DSCR_ERR) != 0)
+		LOG_ERROR("DSCR.ERR must be cleared before leaving debug!");
 
 	/* make sure to acknowledge the halt event before resuming */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -709,10 +711,6 @@ static int aarch64_post_debug_entry(struct target *target)
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	int retval;
 
-	/* clear sticky errors */
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-				    armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-
 	switch (armv8->arm.core_mode) {
 		case ARMV8_64_EL0T:
 			armv8_dpm_modeswitch(&armv8->dpm, ARMV8_64_EL1H);
@@ -1389,13 +1387,6 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	reg = armv8_reg_current(arm, 0);
 	reg->dirty = true;
 
-	/*  clear any abort  */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-	if (retval != ERROR_OK)
-		return retval;
-
-
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
 	/* The algorithm only copies 32 bit words, so the buffer
@@ -1486,8 +1477,6 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
-		mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
 		armv8_dpm_handle_exception(dpm);
 		goto error_free_buff_w;
 	}
@@ -1547,12 +1536,6 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	reg = armv8_reg_current(arm, 0);
 	reg->dirty = true;
 
-	/*	clear any abort  */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-	if (retval != ERROR_OK)
-		goto error_free_buff_r;
-
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
@@ -1652,8 +1635,6 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
-		mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 		armv8_dpm_handle_exception(dpm);
 		goto error_free_buff_r;
 	}
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 8caa8b6..ef53452 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -202,12 +202,6 @@ static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
 			armv8->debug_base + CPUV8_DBG_DTRRX, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
-
-		/* Clear sticky error */
-		retval = mem_ap_write_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-		if (retval != ERROR_OK)
-			return retval;
 	}
 
 	return retval;
@@ -276,9 +270,6 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 
 	if (dscr & DSCR_ERR) {
 		LOG_ERROR("Opcode 0x%08"PRIx32", DSCR.ERR=1, DSCR.EL=%i", opcode, dpm->last_el);
-		/* clear the sticky error condition */
-		mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 		armv8_dpm_handle_exception(dpm);
 		retval = ERROR_FAIL;
 	}
@@ -1353,6 +1344,10 @@ void armv8_dpm_handle_exception(struct arm_dpm *dpm)
 		return;
 	}
 
+	/* Clear sticky error */
+	mem_ap_write_u32(armv8->debug_ap,
+		armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+
 	armv8->read_reg_u64(armv8, ARMV8_xPSR, &dlr);
 	dspsr = dlr;
 	armv8->read_reg_u64(armv8, ARMV8_PC, &dlr);
-- 
cgit v1.1


From 822ebda6747b747f5c366bc41f3cc897f344aee4 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 16:48:42 +0200
Subject: aarch64: enlarge value buffer of arm_reg to store 64 bit

struct arm_reg::value[] must be 8 byte to hold a 64bit register value.

Change-Id: If253e90731d0ee855eafd9d7b63b91f84630cc7c
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/arm.h   | 2 +-
 src/target/armv8.c | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm.h b/src/target/arm.h
index 2548038..e5e336d 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -235,7 +235,7 @@ struct arm_reg {
 	enum arm_mode mode;
 	struct target *target;
 	struct arm *arm;
-	uint8_t value[4];
+	uint8_t value[8];
 };
 
 struct reg_cache *arm_build_reg_cache(struct target *target, struct arm *arm);
diff --git a/src/target/armv8.c b/src/target/armv8.c
index a2a099d..54ec42f 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1168,7 +1168,7 @@ struct reg_cache *armv8_build_reg_cache(struct target *target)
 
 		reg_list[i].name = armv8_regs[i].name;
 		reg_list[i].size = armv8_regs[i].bits;
-		reg_list[i].value = calloc(1, 8);
+		reg_list[i].value = &arch_info[i].value[0];
 		reg_list[i].type = &armv8_reg_type;
 		reg_list[i].arch_info = &arch_info[i];
 
-- 
cgit v1.1


From 7d537dc8e974084356bcd78eb4a193b07b5b745c Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 16:59:21 +0200
Subject: aarch64: improve debug output

Make debug and error messages more informative, fix spelling and
formatting errors

Change-Id: I7245f42c5153bcc95676270814d30e91c113aaed
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c   | 11 ++++++-----
 src/target/armv8_dpm.c |  7 +++----
 2 files changed, 9 insertions(+), 9 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index b716122..f0ac32c 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -389,7 +389,7 @@ static int aarch64_poll(struct target *target)
 	if (DSCR_RUN_MODE(dscr) == 0x3) {
 		if (prev_target_state != TARGET_HALTED) {
 			/* We have a halting debug event */
-			LOG_DEBUG("Target halted");
+			LOG_DEBUG("Target %s halted", target_name(target));
 			target->state = TARGET_HALTED;
 			if ((prev_target_state == TARGET_RUNNING)
 				|| (prev_target_state == TARGET_UNKNOWN)
@@ -839,13 +839,12 @@ static int aarch64_restore_context(struct target *target, bool bpwp)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 
-	LOG_DEBUG(" ");
+	LOG_DEBUG("%s", target_name(target));
 
 	if (armv8->pre_restore_context)
 		armv8->pre_restore_context(target);
 
 	return armv8_dpm_write_dirty_registers(&armv8->dpm, bpwp);
-
 }
 
 /*
@@ -1367,8 +1366,9 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	uint32_t dscr;
 	uint8_t *tmp_buff = NULL;
 
-	LOG_DEBUG("Writing APB-AP memory address 0x%" PRIx64 " size %"  PRIu32 " count%"  PRIu32,
+	LOG_DEBUG("Writing APB-AP memory address 0x%" PRIx64 " size %"  PRIu32 " count %"  PRIu32,
 			  address, size, count);
+
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -1517,8 +1517,9 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	uint8_t *u8buf_ptr;
 	uint32_t value;
 
-	LOG_DEBUG("Reading APB-AP memory address 0x%" TARGET_PRIxADDR " size %"	PRIu32 " count%"  PRIu32,
+	LOG_DEBUG("Reading APB-AP memory address 0x%" TARGET_PRIxADDR " size %"	PRIu32 " count %"  PRIu32,
 			  address, size, count);
+
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index ef53452..0188753 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -79,8 +79,7 @@ static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
 static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
 {
 	int ret;
-	LOG_DEBUG("write DCC Low word 0x%08" PRIx32, (unsigned)data);
-	LOG_DEBUG("write DCC High word 0x%08" PRIx32, (unsigned)(data >> 32));
+	LOG_DEBUG("write DCC 0x%016" PRIx64, data);
 	ret = mem_ap_write_u32(armv8->debug_ap,
 			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
 	ret += mem_ap_write_u32(armv8->debug_ap,
@@ -143,7 +142,7 @@ static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 		if (retval != ERROR_OK)
 			return retval;
 		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for read dcc");
+			LOG_ERROR("Timeout waiting for DTR_TX_FULL, dscr = 0x%08" PRIx32, dscr);
 			return ERROR_FAIL;
 		}
 	}
@@ -1305,7 +1304,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *dpm, uint64_t addr)
 			/* ?? */
 			break;
 		default:
-			LOG_DEBUG("Unknow core_state");
+			LOG_DEBUG("Unknown core_state");
 			break;
 	}
 	dpm->wp_pc = addr;
-- 
cgit v1.1


From 2407721e1487d14ecabf83793f929307e94eb452 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 17:06:13 +0200
Subject: aarch64: remove unused struct components

remove unused register index array from armv8_mode_data[]

Change-Id: I686c20eeb3da413f5e9ef6058e31ce939741afb4
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8.c | 5 -----
 1 file changed, 5 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 54ec42f..94a0e8e 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -46,11 +46,6 @@ static const char * const armv8_state_strings[] = {
 static const struct {
 	const char *name;
 	unsigned psr;
-	/* For user and system modes, these list indices for all registers.
-	 * otherwise they're just indices for the shadow registers and SPSR.
-	 */
-	unsigned short n_indices;
-	const uint8_t *indices;
 } armv8_mode_data[] = {
 	/* These special modes are currently only supported
 	 * by ARMv6M and ARMv7M profiles */
-- 
cgit v1.1


From 8f59ee387184a20c1e736aee45b5541d73755935 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 17:13:36 +0200
Subject: aarch64: remove "mrs <Xt>, currentel" opcode

"currentel" special register is not accessible in debug state.

Change-Id: I9022b01b423cd9ae8227ed018d6166078ba44832
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8_opcodes.h | 5 -----
 1 file changed, 5 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index fe6b28a..53dcb7e 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -17,11 +17,6 @@
 
 #include "arm_opcodes.h"
 
-/* ARM V8 Move from system register to general purpose register
- * R = 1: SPSR R = 0: CPSR
- * Rn: target register
- */
-#define SYSTEM_CUREL			0b1100001000010010
 #define SYSTEM_CUREL_MASK		0xC0
 #define SYSTEM_CUREL_SHIFT		6
 #define SYSTEM_CUREL_EL0		0x0
-- 
cgit v1.1


From 91663206635022bfcb30b92e8462a773c225a77a Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 17:15:00 +0200
Subject: aarch64: remove arm command chain from aarch64 target commands

arm commands are mostly unusable anyway, remove them. to be replaced
by aarch64 specific commands later

Change-Id: Ie994771bc0e86cff1c26f68f1f51ce8ec352a509
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 3 ---
 1 file changed, 3 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index f0ac32c..4871738 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2126,9 +2126,6 @@ static const struct command_registration aarch64_exec_command_handlers[] = {
 };
 static const struct command_registration aarch64_command_handlers[] = {
 	{
-		.chain = arm_command_handlers,
-	},
-	{
 		.chain = armv8_command_handlers,
 	},
 	{
-- 
cgit v1.1


From 9d2e8aabb82ad440e14fd03d4007122c076eecb4 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Thu, 20 Oct 2016 17:22:26 +0200
Subject: aarch64: use cached value of dscr register where needed

Instead of supplying a local, preinitialized "dscr" variable, use the
cached value from arm_dpm, which is kept up-to-date anyway.

Change-Id: I06d548d4dc6db68b9d984c83ed026fa9069d7875
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/armv8_dpm.c | 46 +++++++++++++++++++++-------------------------
 1 file changed, 21 insertions(+), 25 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 0188753..b4c14c2 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -82,7 +82,8 @@ static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
 	LOG_DEBUG("write DCC 0x%016" PRIx64, data);
 	ret = mem_ap_write_u32(armv8->debug_ap,
 			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
-	ret += mem_ap_write_u32(armv8->debug_ap,
+	if (ret == ERROR_OK)
+		ret = mem_ap_write_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DTRTX, data >> 32);
 	return ret;
 }
@@ -174,7 +175,7 @@ static int dpmv8_dpm_prepare(struct arm_dpm *dpm)
 	uint32_t dscr;
 	int retval;
 
-	/* set up invariant:  INSTR_COMP is set after ever DPM operation */
+	/* set up invariant:  ITE is set after ever DPM operation */
 	long long then = timeval_ms();
 	for (;; ) {
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -216,7 +217,7 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *p_dscr)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
+	uint32_t dscr = dpm->dscr;
 	int retval;
 
 	LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
@@ -333,19 +334,18 @@ static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
+	/* transfer data from DCC to R0 */
 	retval = dpmv8_write_dcc_64(armv8, data);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
-	if (retval != ERROR_OK)
-		return retval;
+	if (retval == ERROR_OK)
+		retval = dpmv8_exec_opcode(dpm, ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
 
 	/* then the opcode, taking data from R0 */
-	return dpmv8_exec_opcode(dpm, opcode, &dscr);
+	if (retval == ERROR_OK)
+		retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
+
+	return retval;
 }
 
 static int dpmv8_instr_cpsr_sync(struct arm_dpm *dpm)
@@ -364,70 +364,66 @@ static int dpmv8_instr_read_data_dcc(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to DCC */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc(armv8, data, &dscr);
+	return dpmv8_read_dcc(armv8, data, &dpm->dscr);
 }
 
 static int dpmv8_instr_read_data_dcc_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to DCC */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc_64(armv8, data, &dscr);
+	return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
 }
 
 static int dpmv8_instr_read_data_r0(struct arm_dpm *dpm,
 	uint32_t opcode, uint32_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to R0 */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* write R0 to DCC */
-	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dscr);
+	retval = dpmv8_exec_opcode(dpm, armv8_opcode(armv8, WRITE_REG_DTRTX), &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc(armv8, data, &dscr);
+	return dpmv8_read_dcc(armv8, data, &dpm->dscr);
 }
 
 static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
 	uint32_t opcode, uint64_t *data)
 {
 	struct armv8_common *armv8 = dpm->arm->arch_info;
-	uint32_t dscr = DSCR_ITE;
 	int retval;
 
 	/* the opcode, writing data to R0 */
-	retval = dpmv8_exec_opcode(dpm, opcode, &dscr);
+	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* write R0 to DCC */
-	retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dscr);
+	retval = dpmv8_exec_opcode(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0), &dpm->dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	return dpmv8_read_dcc_64(armv8, data, &dscr);
+	return dpmv8_read_dcc_64(armv8, data, &dpm->dscr);
 }
 
 #if 0
-- 
cgit v1.1


From beece50670e86c36d0de987c581db5250604695c Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 21 Oct 2016 16:59:28 +0200
Subject: aarch64: don't segfault on reset when target is not examined

Basically port a fix that was already done for the cortex_a target.

Change-Id: I4cf4519159bda03ed611bc0b2e340a5dad2d85fe
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 4871738..cabeb9c 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1316,7 +1316,8 @@ static int aarch64_assert_reset(struct target *target)
 	}
 
 	/* registers are now invalid */
-	register_cache_invalidate(armv8->arm.core_cache);
+	if (target_was_examined(target))
+		register_cache_invalidate(armv8->arm.core_cache);
 
 	target->state = TARGET_RESET;
 
@@ -1332,6 +1333,9 @@ static int aarch64_deassert_reset(struct target *target)
 	/* be certain SRST is off */
 	jtag_add_reset(0, 0);
 
+	if (!target_was_examined(target))
+		return ERROR_OK;
+
 	retval = aarch64_poll(target);
 	if (retval != ERROR_OK)
 		return retval;
-- 
cgit v1.1


From 7c2dc138b32291e12e68ecd0391053d66efd53aa Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 21 Oct 2016 17:00:54 +0200
Subject: aarch64: don't try resuming if target is not halted

At framework level, the resume hook is not protected. Make sure to
not attempt a resume if the target is not halted.

Change-Id: I4dd1975a95d6c513bd4f4e999e496bc11182a97a
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---
 src/target/aarch64.c | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index cabeb9c..0a6b67f 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -624,6 +624,10 @@ static int aarch64_resume(struct target *target, int current,
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
 		return 0;
 	}
+
+	if (target->state != TARGET_HALTED)
+		return ERROR_TARGET_NOT_HALTED;
+
 	aarch64_internal_restore(target, current, &addr, handle_breakpoints,
 				 debug_execution);
 	if (target->smp) {
-- 
cgit v1.1


From 7c85165bc1a606883faa9bec51ebefbc652301f4 Mon Sep 17 00:00:00 2001
From: Marc Schink <openocd-dev@marcschink.de>
Date: Sun, 12 Feb 2017 10:59:49 +0100
Subject: aarch64: Fix #include guards
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Change-Id: I9445b04a210dcde5f8a7cf1560ef23eb53149178
Signed-off-by: Marc Schink <openocd-dev@marcschink.de>
Reviewed-on: http://openocd.zylin.com/3975
Tested-by: jenkins
Reviewed-by: Andreas Färber <afaerber@suse.de>
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.h | 6 +++---
 src/target/armv8.h   | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.h b/src/target/aarch64.h
index 513f06d..9491623 100644
--- a/src/target/aarch64.h
+++ b/src/target/aarch64.h
@@ -16,8 +16,8 @@
  *   Free Software Foundation, Inc.,                                       *
  ***************************************************************************/
 
-#ifndef AARCH64_H
-#define AARCH64_H
+#ifndef OPENOCD_TARGET_AARCH64_H
+#define OPENOCD_TARGET_AARCH64_H
 
 #include "armv8.h"
 
@@ -75,4 +75,4 @@ target_to_aarch64(struct target *target)
 	return container_of(target->arch_info, struct aarch64_common, armv8_common.arm);
 }
 
-#endif /* AARCH64_H */
+#endif /* OPENOCD_TARGET_AARCH64_H */
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 85845e6..3b2fc59 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -310,4 +310,4 @@ void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
 
 extern const struct command_registration armv8_command_handlers[];
 
-#endif
+#endif /* OPENOCD_TARGET_ARMV8_H */
-- 
cgit v1.1


From 5d00fd9d1dc504335dd71e474a3d61dec7696c40 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Wed, 26 Oct 2016 17:32:43 +0200
Subject: aarch64: fix software breakpoints when in aarch32 state

Use the correct opcode for Aarch32 state, both for the breakpoint
instruction itself and the cache handling functions.

Change-Id: I975fa67b1e577b54f5c672a01d516419c6a614b2
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3981
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c       |  2 +-
 src/target/armv8_cache.c   | 11 ++++++-----
 src/target/armv8_dpm.c     | 11 +++++++++++
 src/target/armv8_opcodes.c |  8 ++++++++
 src/target/armv8_opcodes.h |  5 +++++
 5 files changed, 31 insertions(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 0a6b67f..833dc7a 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -915,7 +915,7 @@ static int aarch64_set_breakpoint(struct target *target,
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
 
-		buf_set_u32(code, 0, 32, ARMV8_HLT(0x11));
+		buf_set_u32(code, 0, 32, armv8_opcode(armv8, ARMV8_OPC_HLT));
 		retval = target_read_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1,
diff --git a/src/target/armv8_cache.c b/src/target/armv8_cache.c
index 31e4c79..7f610c9 100644
--- a/src/target/armv8_cache.c
+++ b/src/target/armv8_cache.c
@@ -49,8 +49,9 @@ static int armv8_i_cache_sanity_check(struct armv8_common *armv8)
 	return ERROR_TARGET_INVALID;
 }
 
-static int armv8_cache_d_inner_flush_level(struct arm_dpm *dpm, struct armv8_cachesize *size, int cl)
+static int armv8_cache_d_inner_flush_level(struct armv8_common *armv8, struct armv8_cachesize *size, int cl)
 {
+	struct arm_dpm *dpm = armv8->arm.dpm;
 	int retval = ERROR_OK;
 	int32_t c_way, c_index = size->index;
 
@@ -65,7 +66,7 @@ static int armv8_cache_d_inner_flush_level(struct arm_dpm *dpm, struct armv8_cac
 			 * line by Set/Way.
 			 */
 			retval = dpm->instr_write_data_r0(dpm,
-					ARMV8_SYS(SYSTEM_DCCISW, 0), value);
+					armv8_opcode(armv8, ARMV8_OPC_DCCISW), value);
 			if (retval != ERROR_OK)
 				goto done;
 			c_way -= 1;
@@ -97,7 +98,7 @@ static int armv8_cache_d_inner_clean_inval_all(struct armv8_common *armv8)
 		if (cache->arch[cl].ctype < CACHE_LEVEL_HAS_D_CACHE)
 			continue;
 
-		armv8_cache_d_inner_flush_level(dpm, &cache->arch[cl].d_u_size, cl);
+		armv8_cache_d_inner_flush_level(armv8, &cache->arch[cl].d_u_size, cl);
 	}
 
 	retval = dpm->finish(dpm);
@@ -133,7 +134,7 @@ int armv8_cache_d_inner_flush_virt(struct armv8_common *armv8, target_addr_t va,
 		/* DC CIVAC */
 		/* Aarch32: DCCIMVAC: ARMV4_5_MCR(15, 0, 0, 7, 14, 1) */
 		retval = dpm->instr_write_data_r0_64(dpm,
-				ARMV8_SYS(SYSTEM_DCCIVAC, 0), va_line);
+				armv8_opcode(armv8, ARMV8_OPC_DCCIVAC), va_line);
 		if (retval != ERROR_OK)
 			goto done;
 		va_line += linelen;
@@ -171,7 +172,7 @@ int armv8_cache_i_inner_inval_virt(struct armv8_common *armv8, target_addr_t va,
 	while (va_line < va_end) {
 		/* IC IVAU - Invalidate instruction cache by VA to PoU. */
 		retval = dpm->instr_write_data_r0_64(dpm,
-				ARMV8_SYS(SYSTEM_ICIVAU, 0), va_line);
+				armv8_opcode(armv8, ARMV8_OPC_ICIVAU), va_line);
 		if (retval != ERROR_OK)
 			goto done;
 		va_line += linelen;
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index b4c14c2..b06e456 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -336,6 +336,9 @@ static int dpmv8_instr_write_data_r0_64(struct arm_dpm *dpm,
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	int retval;
 
+	if (dpm->arm->core_state != ARM_STATE_AARCH64)
+		return dpmv8_instr_write_data_r0(dpm, opcode, data);
+
 	/* transfer data from DCC to R0 */
 	retval = dpmv8_write_dcc_64(armv8, data);
 	if (retval == ERROR_OK)
@@ -413,6 +416,14 @@ static int dpmv8_instr_read_data_r0_64(struct arm_dpm *dpm,
 	struct armv8_common *armv8 = dpm->arm->arch_info;
 	int retval;
 
+	if (dpm->arm->core_state != ARM_STATE_AARCH64) {
+		uint32_t tmp;
+		retval = dpmv8_instr_read_data_r0(dpm, opcode, &tmp);
+		if (retval == ERROR_OK)
+			*data = tmp;
+		return retval;
+	}
+
 	/* the opcode, writing data to R0 */
 	retval = dpmv8_exec_opcode(dpm, opcode, &dpm->dscr);
 	if (retval != ERROR_OK)
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index 7790144..d3c0b3f 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -38,6 +38,10 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCPS]	= ARMV8_DCPS(0, 11),
 		[ARMV8_OPC_DRPS]	= ARMV8_DRPS,
 		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB,
+		[ARMV8_OPC_DCCISW]	= ARMV8_SYS(SYSTEM_DCCISW, 0),
+		[ARMV8_OPC_DCCIVAC]	= ARMV8_SYS(SYSTEM_DCCIVAC, 0),
+		[ARMV8_OPC_ICIVAU]	= ARMV8_SYS(SYSTEM_ICIVAU, 0),
+		[ARMV8_OPC_HLT]		= ARMV8_HLT(11),
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -55,6 +59,10 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCPS]	= ARMV8_DCPS_T1(0),
 		[ARMV8_OPC_DRPS]	= ARMV8_ERET_T1,
 		[ARMV8_OPC_ISB_SY]	= ARMV8_ISB_SY_T1,
+		[ARMV8_OPC_DCCISW]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 2),
+		[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),
 };
 
 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 53dcb7e..94694bc 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -148,6 +148,7 @@
 
 #define ARMV8_BKPT(Im) (0xD4200000 | ((Im & 0xffff) << 5))
 #define ARMV8_HLT(Im) (0x0D4400000 | ((Im & 0xffff) << 5))
+#define ARMV8_HLT_A1(Im) (0xE1000070 | ((Im & 0xFFF0) << 4) | (Im & 0xF))
 
 #define ARMV8_MOVFSP_64(Rt) ((1 << 31) | 0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_64(Rt) ((1 << 31) | 0x11000000 | (Rt << 5) | (0x1F))
@@ -171,6 +172,10 @@ enum armv8_opcode {
 	ARMV8_OPC_DCPS,
 	ARMV8_OPC_DRPS,
 	ARMV8_OPC_ISB_SY,
+	ARMV8_OPC_DCCISW,
+	ARMV8_OPC_DCCIVAC,
+	ARMV8_OPC_ICIVAU,
+	ARMV8_OPC_HLT,
 	ARMV8_OPC_NUM,
 };
 
-- 
cgit v1.1


From f988f5960450cf9e8c456bf8e3b4d2008e35a207 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 14 Nov 2016 12:18:43 +0100
Subject: aarch64: refactor SCTLR manipulation

Reduce SLOCs in SCTLR retrieval and modification functions and make them
less complex.

Change-Id: Ida1a99c223743247f171b52eef80dc9886802101
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3982
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c       | 229 +++++++++++++++++++--------------------------
 src/target/armv8_opcodes.h |   4 +
 2 files changed, 98 insertions(+), 135 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 833dc7a..7bd3d3c 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -49,7 +49,9 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 
 static int aarch64_restore_system_control_reg(struct target *target)
 {
+	enum arm_mode target_mode = ARM_MODE_ANY;
 	int retval = ERROR_OK;
+	uint32_t instr;
 
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = target_to_armv8(target);
@@ -59,41 +61,45 @@ static int aarch64_restore_system_control_reg(struct target *target)
 		/* LOG_INFO("cp15_control_reg: %8.8" PRIx32, cortex_v8->cp15_control_reg); */
 
 		switch (armv8->arm.core_mode) {
-			case ARMV8_64_EL0T:
-			case ARMV8_64_EL1T:
-			case ARMV8_64_EL1H:
-				retval = armv8->arm.msr(target, 3, /*op 0*/
-						0, 1,	/* op1, op2 */
-						0, 0,	/* CRn, CRm */
-						aarch64->system_control_reg);
-				if (retval != ERROR_OK)
-					return retval;
+		case ARMV8_64_EL0T:
+			target_mode = ARMV8_64_EL1H;
+			/* fall through */
+		case ARMV8_64_EL1T:
+		case ARMV8_64_EL1H:
+			instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0);
 			break;
-			case ARMV8_64_EL2T:
-			case ARMV8_64_EL2H:
-				retval = armv8->arm.msr(target, 3, /*op 0*/
-						4, 1,	/* op1, op2 */
-						0, 0,	/* CRn, CRm */
-						aarch64->system_control_reg);
-				if (retval != ERROR_OK)
-					return retval;
+		case ARMV8_64_EL2T:
+		case ARMV8_64_EL2H:
+			instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL2, 0);
 			break;
-			case ARMV8_64_EL3H:
-			case ARMV8_64_EL3T:
-				retval = armv8->arm.msr(target, 3, /*op 0*/
-						6, 1,	/* op1, op2 */
-						0, 0,	/* CRn, CRm */
-						aarch64->system_control_reg);
-				if (retval != ERROR_OK)
-					return retval;
+		case ARMV8_64_EL3H:
+		case ARMV8_64_EL3T:
+			instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
 			break;
-			default:
-				retval = armv8->arm.mcr(target, 15, 0, 0, 1, 0, aarch64->system_control_reg);
-				if (retval != ERROR_OK)
-					return retval;
-				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_INFO("cannot read system control register in this mode");
+			return ERROR_FAIL;
+		}
+
+		if (target_mode != ARM_MODE_ANY)
+			armv8_dpm_modeswitch(&armv8->dpm, target_mode);
+
+		retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr, aarch64->system_control_reg);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (target_mode != ARM_MODE_ANY)
+			armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 	}
+
 	return retval;
 }
 
@@ -112,6 +118,7 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	int retval = ERROR_OK;
+	uint32_t instr = 0;
 
 	if (enable) {
 		/*	if mmu enabled at target stop and mmu not enable */
@@ -119,86 +126,42 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 			LOG_ERROR("trying to enable mmu on target stopped with mmu disable");
 			return ERROR_FAIL;
 		}
-		if (!(aarch64->system_control_reg_curr & 0x1U)) {
+		if (!(aarch64->system_control_reg_curr & 0x1U))
 			aarch64->system_control_reg_curr |= 0x1U;
-			switch (armv8->arm.core_mode) {
-				case ARMV8_64_EL0T:
-				case ARMV8_64_EL1T:
-				case ARMV8_64_EL1H:
-					retval = armv8->arm.msr(target, 3, /*op 0*/
-							0, 0,	/* op1, op2 */
-							1, 0,	/* CRn, CRm */
-							aarch64->system_control_reg_curr);
-					if (retval != ERROR_OK)
-						return retval;
-				break;
-				case ARMV8_64_EL2T:
-				case ARMV8_64_EL2H:
-					retval = armv8->arm.msr(target, 3, /*op 0*/
-							4, 0,	/* op1, op2 */
-							1, 0,	/* CRn, CRm */
-							aarch64->system_control_reg_curr);
-					if (retval != ERROR_OK)
-						return retval;
-				break;
-				case ARMV8_64_EL3H:
-				case ARMV8_64_EL3T:
-					retval = armv8->arm.msr(target, 3, /*op 0*/
-							6, 0,	/* op1, op2 */
-							1, 0,	/* CRn, CRm */
-							aarch64->system_control_reg_curr);
-					if (retval != ERROR_OK)
-						return retval;
-				break;
-				default:
-					LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
-			}
-		}
 	} else {
 		if (aarch64->system_control_reg_curr & 0x4U) {
 			/*  data cache is active */
 			aarch64->system_control_reg_curr &= ~0x4U;
-			/* flush data cache armv7 function to be called */
+			/* flush data cache armv8 function to be called */
 			if (armv8->armv8_mmu.armv8_cache.flush_all_data_cache)
 				armv8->armv8_mmu.armv8_cache.flush_all_data_cache(target);
 		}
 		if ((aarch64->system_control_reg_curr & 0x1U)) {
 			aarch64->system_control_reg_curr &= ~0x1U;
-			switch (armv8->arm.core_mode) {
-				case ARMV8_64_EL0T:
-				case ARMV8_64_EL1T:
-				case ARMV8_64_EL1H:
-					retval = armv8->arm.msr(target, 3, /*op 0*/
-							0, 0,	/* op1, op2 */
-							1, 0,	/* CRn, CRm */
-							aarch64->system_control_reg_curr);
-					if (retval != ERROR_OK)
-						return retval;
-					break;
-				case ARMV8_64_EL2T:
-				case ARMV8_64_EL2H:
-					retval = armv8->arm.msr(target, 3, /*op 0*/
-							4, 0,	/* op1, op2 */
-							1, 0,	/* CRn, CRm */
-							aarch64->system_control_reg_curr);
-					if (retval != ERROR_OK)
-						return retval;
-					break;
-				case ARMV8_64_EL3H:
-				case ARMV8_64_EL3T:
-					retval = armv8->arm.msr(target, 3, /*op 0*/
-							6, 0,	/* op1, op2 */
-							1, 0,	/* CRn, CRm */
-							aarch64->system_control_reg_curr);
-					if (retval != ERROR_OK)
-						return retval;
-					break;
-				default:
-					LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
-					break;
-			}
 		}
 	}
+
+	switch (armv8->arm.core_mode) {
+	case ARMV8_64_EL0T:
+	case ARMV8_64_EL1T:
+	case ARMV8_64_EL1H:
+		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL1, 0);
+		break;
+	case ARMV8_64_EL2T:
+	case ARMV8_64_EL2H:
+		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL2, 0);
+		break;
+	case ARMV8_64_EL3H:
+	case ARMV8_64_EL3T:
+		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
+		break;
+	default:
+		LOG_DEBUG("unknown cpu state 0x%x" PRIx32, armv8->arm.core_state);
+		break;
+	}
+
+	retval = armv8->dpm.instr_write_data_r0(&armv8->dpm, instr,
+				aarch64->system_control_reg_curr);
 	return retval;
 }
 
@@ -714,51 +677,47 @@ static int aarch64_post_debug_entry(struct target *target)
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	int retval;
+	enum arm_mode target_mode = ARM_MODE_ANY;
+	uint32_t instr;
 
 	switch (armv8->arm.core_mode) {
-		case ARMV8_64_EL0T:
-			armv8_dpm_modeswitch(&armv8->dpm, ARMV8_64_EL1H);
-			/* fall through */
-		case ARMV8_64_EL1T:
-		case ARMV8_64_EL1H:
-			retval = armv8->arm.mrs(target, 3, /*op 0*/
-					0, 0,	/* op1, op2 */
-					1, 0,	/* CRn, CRm */
-					&aarch64->system_control_reg);
-			if (retval != ERROR_OK)
-				return retval;
+	case ARMV8_64_EL0T:
+		target_mode = ARMV8_64_EL1H;
+		/* fall through */
+	case ARMV8_64_EL1T:
+	case ARMV8_64_EL1H:
+		instr = ARMV8_MRS(SYSTEM_SCTLR_EL1, 0);
 		break;
-		case ARMV8_64_EL2T:
-		case ARMV8_64_EL2H:
-			retval = armv8->arm.mrs(target, 3, /*op 0*/
-					4, 0,	/* op1, op2 */
-					1, 0,	/* CRn, CRm */
-					&aarch64->system_control_reg);
-			if (retval != ERROR_OK)
-				return retval;
+	case ARMV8_64_EL2T:
+	case ARMV8_64_EL2H:
+		instr = ARMV8_MRS(SYSTEM_SCTLR_EL2, 0);
 		break;
-		case ARMV8_64_EL3H:
-		case ARMV8_64_EL3T:
-			retval = armv8->arm.mrs(target, 3, /*op 0*/
-					6, 0,	/* op1, op2 */
-					1, 0,	/* CRn, CRm */
-					&aarch64->system_control_reg);
-			if (retval != ERROR_OK)
-				return retval;
+	case ARMV8_64_EL3H:
+	case ARMV8_64_EL3T:
+		instr = ARMV8_MRS(SYSTEM_SCTLR_EL3, 0);
 		break;
 
-		case ARM_MODE_SVC:
-			retval = armv8->arm.mrc(target, 15, 0, 0, 1, 0, &aarch64->system_control_reg);
-			if (retval != ERROR_OK)
-				return retval;
-			break;
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT:
+	case ARM_MODE_FIQ:
+	case ARM_MODE_IRQ:
+		instr = ARMV4_5_MRC(15, 0, 0, 1, 0, 0);
+		break;
 
-		default:
-			LOG_INFO("cannot read system control register in this mode");
-			break;
+	default:
+		LOG_INFO("cannot read system control register in this mode");
+		return ERROR_FAIL;
 	}
 
-	armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(&armv8->dpm, target_mode);
+
+	retval = armv8->dpm.instr_read_data_r0(&armv8->dpm, instr, &aarch64->system_control_reg);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target_mode != ARM_MODE_ANY)
+		armv8_dpm_modeswitch(&armv8->dpm, ARM_MODE_ANY);
 
 	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
 	aarch64->system_control_reg_curr = aarch64->system_control_reg;
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 94694bc..2d8ddd8 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -42,6 +42,10 @@
 #define SYSTEM_ELR_EL2			0b1110001000000001
 #define SYSTEM_ELR_EL3			0b1111001000000001
 
+#define SYSTEM_SCTLR_EL1	0b1100000010000000
+#define SYSTEM_SCTLR_EL2	0b1110000010000000
+#define SYSTEM_SCTLR_EL3	0b1111000010000000
+
 #define SYSTEM_FPCR			0b1101101000100000
 #define SYSTEM_FPSR			0b1101101000100001
 #define SYSTEM_DAIF			0b1101101000010001
-- 
cgit v1.1


From 06ba5492df66e997eafe294fced505b11e67f2fc Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 14 Nov 2016 12:23:24 +0100
Subject: aarch64: remove mrs/msr functions from struct arm

No longer needed, no users.

Change-Id: I0cc82a0ef11e1b72101fa9145f014e5d5d76df0e
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3983
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/arm.h       | 12 -----------
 src/target/armv8_dpm.c | 58 --------------------------------------------------
 2 files changed, 70 deletions(-)

(limited to 'src/target')

diff --git a/src/target/arm.h b/src/target/arm.h
index e5e336d..d63ead2 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -189,18 +189,6 @@ struct arm {
 			uint32_t CRn, uint32_t CRm,
 			uint32_t value);
 
-	/** Read coprocessor register.  */
-	int (*mrs)(struct target *target, uint32_t op0,
-			uint32_t op1, uint32_t op2,
-			uint32_t CRn, uint32_t CRm,
-			uint32_t *value);
-
-	/** Write coprocessor register.  */
-	int (*msr)(struct target *target, uint32_t cpnum,
-			uint32_t op1, uint32_t op2,
-			uint32_t CRn, uint32_t CRm,
-			uint32_t value);
-
 	void *arch_info;
 
 	/** For targets conforming to ARM Debug Interface v5,
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index b06e456..acfd1bc 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -551,62 +551,6 @@ static int dpmv8_mcr(struct target *target, int cpnum,
 	return retval;
 }
 
-static int dpmv8_mrs(struct target *target, uint32_t op0,
-	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-	uint32_t *value)
-{
-	struct arm *arm = target_to_arm(target);
-	struct arm_dpm *dpm = arm->dpm;
-	int retval;
-	uint32_t op_code;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		return retval;
-	op_code = ((op0 & 0x3) << 19 | (op1 & 0x7) << 16 | (CRn & 0xF) << 12 |\
-				(CRm & 0xF) << 8 | (op2 & 0x7) << 5);
-	op_code >>= 5;
-	LOG_DEBUG("MRS p%d, %d, r0, c%d, c%d, %d", (int)op0,
-		(int) op1, (int) CRn,
-		(int) CRm, (int) op2);
-	/* read coprocessor register into R0; return via DCC */
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV8_MRS(op_code, 0),
-			value);
-
-	/* (void) */ dpm->finish(dpm);
-	return retval;
-}
-
-static int dpmv8_msr(struct target *target, uint32_t op0,
-	uint32_t op1, uint32_t op2, uint32_t CRn, uint32_t CRm,
-	uint32_t value)
-{
-	struct arm *arm = target_to_arm(target);
-	struct arm_dpm *dpm = arm->dpm;
-	int retval;
-	uint32_t op_code;
-
-	retval = dpm->prepare(dpm);
-	if (retval != ERROR_OK)
-		return retval;
-
-	op_code = ((op0 & 0x3) << 19 | (op1 & 0x7) << 16 | (CRn & 0xF) << 12 |\
-				(CRm & 0xF) << 8 | (op2 & 0x7) << 5);
-	op_code >>= 5;
-	LOG_DEBUG("MSR p%d, %d, r0, c%d, c%d, %d", (int)op0,
-		(int) op1, (int) CRn,
-		(int) CRm, (int) op2);
-
-	/* read DCC into r0; then write coprocessor register from R0 */
-	retval = dpm->instr_write_data_r0(dpm,
-			ARMV8_MSR_GP(op_code, 0),
-			value);
-
-	/* (void) */ dpm->finish(dpm);
-	return retval;
-}
-
 /*----------------------------------------------------------------------*/
 
 /*
@@ -1449,8 +1393,6 @@ int armv8_dpm_setup(struct arm_dpm *dpm)
 	/* coprocessor access setup */
 	arm->mrc = dpmv8_mrc;
 	arm->mcr = dpmv8_mcr;
-	arm->mrs = dpmv8_mrs;
-	arm->msr = dpmv8_msr;
 
 	dpm->prepare = dpmv8_dpm_prepare;
 	dpm->finish = dpmv8_dpm_finish;
-- 
cgit v1.1


From fa8700cdd58752671fb8d60ef65bf767e4aa1bc5 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Mon, 14 Nov 2016 21:54:26 +0100
Subject: aarch64: reduce debug output to improve legibility

Suppress some very verbose LOG_DEBUG's that are not really useful
any more.

Change-Id: I67f10ba9510a9e34a027f378f4b62b8901ddc8a4
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3984
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c   | 19 -------------------
 src/target/armv8.c     |  3 ---
 src/target/armv8_dpm.c |  6 ------
 3 files changed, 28 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 7bd3d3c..d3ed57c 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -1333,9 +1333,6 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	uint32_t dscr;
 	uint8_t *tmp_buff = NULL;
 
-	LOG_DEBUG("Writing APB-AP memory address 0x%" PRIx64 " size %"  PRIu32 " count %"  PRIu32,
-			  address, size, count);
-
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -1484,9 +1481,6 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	uint8_t *u8buf_ptr;
 	uint32_t value;
 
-	LOG_DEBUG("Reading APB-AP memory address 0x%" TARGET_PRIxADDR " size %"	PRIu32 " count %"  PRIu32,
-			  address, size, count);
-
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
@@ -1634,8 +1628,6 @@ static int aarch64_read_phys_memory(struct target *target,
 	uint32_t count, uint8_t *buffer)
 {
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
-	LOG_DEBUG("Reading memory at real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32,
-		address, size, count);
 
 	if (count && buffer) {
 		/* read memory through APB-AP */
@@ -1653,10 +1645,6 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 	int mmu_enabled = 0;
 	int retval;
 
-	/* aarch64 handles unaligned memory access */
-	LOG_DEBUG("Reading memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
-
 	/* determine if MMU was enabled on target stop */
 	retval = aarch64_mmu(target, &mmu_enabled);
 	if (retval != ERROR_OK)
@@ -1680,9 +1668,6 @@ static int aarch64_write_phys_memory(struct target *target,
 {
 	int retval = ERROR_COMMAND_SYNTAX_ERROR;
 
-	LOG_DEBUG("Writing memory to real address 0x%" TARGET_PRIxADDR "; size %" PRId32 "; count %" PRId32, address,
-		size, count);
-
 	if (count && buffer) {
 		/* write memory through APB-AP */
 		retval = aarch64_mmu_modify(target, 0);
@@ -1700,10 +1685,6 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 	int mmu_enabled = 0;
 	int retval;
 
-	/* aarch64 handles unaligned memory access */
-	LOG_DEBUG("Writing memory at address 0x%" TARGET_PRIxADDR "; size %" PRId32
-		  "; count %" PRId32, address, size, count);
-
 	/* determine if MMU was enabled on target stop */
 	retval = aarch64_mmu(target, &mmu_enabled);
 	if (retval != ERROR_OK)
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 94a0e8e..7ae8cc9 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1086,9 +1086,6 @@ static int armv8_get_core_reg32(struct reg *reg)
 	struct reg *reg64;
 	int retval;
 
-	LOG_DEBUG("reg.name:%s number:%i arm.num:%i value:0x%08" PRIx64,
-			reg->name, reg->number, armv8_reg->num, buf_get_u64(reg->value, 0, 32));
-
 	/* get the corresponding Aarch64 register */
 	reg64 = cache->reg_list + armv8_reg->num;
 	if (reg64->valid) {
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index acfd1bc..56e2eb8 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -71,7 +71,6 @@ enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
 
 static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
 {
-	LOG_DEBUG("write DCC 0x%08" PRIx32, data);
 	return mem_ap_write_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DTRRX, data);
 }
@@ -79,7 +78,6 @@ static int dpmv8_write_dcc(struct armv8_common *armv8, uint32_t data)
 static int dpmv8_write_dcc_64(struct armv8_common *armv8, uint64_t data)
 {
 	int ret;
-	LOG_DEBUG("write DCC 0x%016" PRIx64, data);
 	ret = mem_ap_write_u32(armv8->debug_ap,
 			       armv8->debug_base + CPUV8_DBG_DTRRX, data);
 	if (ret == ERROR_OK)
@@ -116,7 +114,6 @@ static int dpmv8_read_dcc(struct armv8_common *armv8, uint32_t *data,
 					    data);
 	if (retval != ERROR_OK)
 		return retval;
-	LOG_DEBUG("read DCC 0x%08" PRIx32, *data);
 
 	if (dscr_p)
 		*dscr_p = dscr;
@@ -161,7 +158,6 @@ static int dpmv8_read_dcc_64(struct armv8_common *armv8, uint64_t *data,
 		return retval;
 
 	*data = *(uint32_t *)data | (uint64_t)higher << 32;
-	LOG_DEBUG("read DCC 0x%16.16" PRIx64, *data);
 
 	if (dscr_p)
 		*dscr_p = dscr;
@@ -220,8 +216,6 @@ static int dpmv8_exec_opcode(struct arm_dpm *dpm,
 	uint32_t dscr = dpm->dscr;
 	int retval;
 
-	LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
-
 	if (p_dscr)
 		dscr = *p_dscr;
 
-- 
cgit v1.1


From 7ed0b6333ded06f1e517d7fd3e15a400c98dc357 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Tue, 15 Nov 2016 22:10:03 +0100
Subject: aarch64: optimize core state detection

Replace loop by right-shift.
Inspired by patch from Alamy Liu

Change-Id: I1285f4f54c0695a93fa42e9863ed8ffa4de00f70
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3985
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/armv8_dpm.c | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index 56e2eb8..ac53460 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -51,17 +51,11 @@ enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm)
 {
 	int el = (dpm->dscr >> 8) & 0x3;
 	int rw = (dpm->dscr >> 10) & 0xF;
-	int pos;
 
 	dpm->last_el = el;
 
-	/* find the first '0' in DSCR.RW */
-	for (pos = 3; pos >= 0; pos--) {
-		if ((rw & (1 << pos)) == 0)
-			break;
-	}
-
-	if (el > pos)
+	/* In Debug state, each bit gives the current Execution state of each EL */
+	if ((rw >> el) & 0b1)
 		return ARM_STATE_AARCH64;
 
 	return ARM_STATE_ARM;
-- 
cgit v1.1


From 9527d1e595e316a4155c808fafa3a0ea6baa72f2 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sat, 19 Nov 2016 10:02:34 +0100
Subject: target: generic ARM CTI function wrapper

Not specific to ARMv8, the Cross Trigger Interface
deserves an independent access wrapper.

Change-Id: I84f8faad15ed3515e0fff7f6cc5d1109ef91a869
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3986
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/Makefile.am |   6 +-
 src/target/arm_cti.c   | 148 +++++++++++++++++++++++++++++++++++++++++++++++++
 src/target/arm_cti.h   |  73 ++++++++++++++++++++++++
 3 files changed, 225 insertions(+), 2 deletions(-)
 create mode 100644 src/target/arm_cti.c
 create mode 100644 src/target/arm_cti.h

(limited to 'src/target')

diff --git a/src/target/Makefile.am b/src/target/Makefile.am
index e936d3f..597070c 100644
--- a/src/target/Makefile.am
+++ b/src/target/Makefile.am
@@ -96,7 +96,8 @@ ARM_DEBUG_SRC = \
 	%D%/etb.c \
 	%D%/etm.c \
 	$(OOCD_TRACE_FILES) \
-	%D%/etm_dummy.c
+	%D%/etm_dummy.c \
+	%D%/arm_cti.c
 
 AVR32_SRC = \
 	%D%/avr32_ap7k.c \
@@ -205,6 +206,7 @@ INTEL_IA32_SRC = \
 	%D%/nds32_v3m.h \
 	%D%/nds32_aice.h \
 	%D%/lakemont.h \
-	%D%/x86_32_common.h
+	%D%/x86_32_common.h \
+	%D%/arm_cti.h
 
 include %D%/openrisc/Makefile.am
diff --git a/src/target/arm_cti.c b/src/target/arm_cti.c
new file mode 100644
index 0000000..75169b2
--- /dev/null
+++ b/src/target/arm_cti.c
@@ -0,0 +1,148 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <stdlib.h>
+#include <stdint.h>
+#include "target/arm_adi_v5.h"
+#include "target/arm_cti.h"
+#include "target/target.h"
+#include "helper/time_support.h"
+
+struct arm_cti {
+	uint32_t base;
+	struct adiv5_ap *ap;
+};
+
+struct arm_cti *arm_cti_create(struct adiv5_ap *ap, uint32_t base)
+{
+	struct arm_cti *self = calloc(1, sizeof(struct arm_cti));
+	if (!self)
+		return NULL;
+
+	self->base = base;
+	self->ap = ap;
+	return self;
+}
+
+static int arm_cti_mod_reg_bits(struct arm_cti *self, unsigned int reg, uint32_t mask, uint32_t value)
+{
+	uint32_t tmp;
+
+	/* Read register */
+	int retval = mem_ap_read_atomic_u32(self->ap, self->base + reg, &tmp);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	tmp &= ~mask;
+	/* put new value */
+	tmp |= value & mask;
+
+	/* write new value */
+	return mem_ap_write_atomic_u32(self->ap, self->base + reg, tmp);
+}
+
+int arm_cti_enable(struct arm_cti *self, bool enable)
+{
+	uint32_t val = enable ? 1 : 0;
+
+	return mem_ap_write_atomic_u32(self->ap, self->base + CTI_CTR, val);
+}
+
+int arm_cti_ack_events(struct arm_cti *self, uint32_t event)
+{
+	int retval;
+	uint32_t tmp;
+
+	retval = mem_ap_write_atomic_u32(self->ap, self->base + CTI_INACK, event);
+	if (retval == ERROR_OK) {
+		int64_t then = timeval_ms();
+		for (;;) {
+			retval = mem_ap_read_atomic_u32(self->ap, self->base + CTI_TROUT_STATUS, &tmp);
+			if (retval != ERROR_OK)
+				break;
+			if ((tmp & event) == 0)
+				break;
+			if (timeval_ms() > then + 1000) {
+				LOG_ERROR("timeout waiting for target");
+				retval = ERROR_TARGET_TIMEOUT;
+				break;
+			}
+		}
+	}
+
+	return retval;
+}
+
+int arm_cti_gate_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_mod_reg_bits(self, CTI_GATE, CTI_CHNL(channel), 0);
+}
+
+int arm_cti_ungate_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_mod_reg_bits(self, CTI_GATE, CTI_CHNL(channel), 0xFFFFFFFF);
+}
+
+int arm_cti_write_reg(struct arm_cti *self, unsigned int reg, uint32_t value)
+{
+	return mem_ap_write_atomic_u32(self->ap, self->base + reg, value);
+}
+
+int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *p_value)
+{
+	if (p_value == NULL)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return mem_ap_read_atomic_u32(self->ap, self->base + reg, p_value);
+}
+
+int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_write_reg(self, CTI_APPPULSE, CTI_CHNL(channel));
+}
+
+int arm_cti_set_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_write_reg(self, CTI_APPSET, CTI_CHNL(channel));
+}
+
+int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel)
+{
+	if (channel > 31)
+		return ERROR_COMMAND_ARGUMENT_INVALID;
+
+	return arm_cti_write_reg(self, CTI_APPCLEAR, CTI_CHNL(channel));
+}
diff --git a/src/target/arm_cti.h b/src/target/arm_cti.h
new file mode 100644
index 0000000..99724c4
--- /dev/null
+++ b/src/target/arm_cti.h
@@ -0,0 +1,73 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Matthias Welwarsky                              *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program; if not, write to the                         *
+ *   Free Software Foundation, Inc.,                                       *
+ *                                                                         *
+ ***************************************************************************/
+
+#ifndef OPENOCD_TARGET_ARM_CTI_H
+#define OPENOCD_TARGET_ARM_CTI_H
+
+/*define CTI(cross trigger interface)*/
+#define CTI_CTR				0x0
+#define CTI_INACK			0x10
+#define CTI_APPSET			0x14
+#define CTI_APPCLEAR		0x18
+#define CTI_APPPULSE		0x1C
+#define CTI_INEN0			0x20
+#define CTI_INEN1			0x24
+#define CTI_INEN2			0x28
+#define CTI_INEN3			0x2C
+#define CTI_INEN4			0x30
+#define CTI_INEN5			0x34
+#define CTI_INEN6			0x38
+#define CTI_INEN7			0x3C
+#define CTI_INEN(n)			(0x20 + 4 * n)
+#define CTI_OUTEN0			0xA0
+#define CTI_OUTEN1			0xA4
+#define CTI_OUTEN2			0xA8
+#define CTI_OUTEN3			0xAC
+#define CTI_OUTEN4			0xB0
+#define CTI_OUTEN5			0xB4
+#define CTI_OUTEN6			0xB8
+#define CTI_OUTEN7			0xBC
+#define CTI_OUTEN(n)		(0xA0 + 4 * n)
+#define CTI_TRIN_STATUS		0x130
+#define CTI_TROUT_STATUS	0x134
+#define CTI_CHIN_STATUS		0x138
+#define CTI_CHOU_STATUS		0x13C
+#define CTI_GATE			0x140
+#define CTI_UNLOCK			0xFB0
+
+#define CTI_CHNL(x)			(1 << x)
+#define CTI_TRIG_HALT		0
+#define CTI_TRIG_RESUME		1
+#define CTI_TRIG(n)			(1 << CTI_TRIG_##n)
+
+/* forward-declare arm_cti struct */
+struct arm_cti;
+
+extern struct arm_cti *arm_cti_create(struct adiv5_ap *ap, uint32_t base);
+extern int arm_cti_enable(struct arm_cti *self, bool enable);
+extern int arm_cti_ack_events(struct arm_cti *self, uint32_t event);
+extern int arm_cti_gate_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_ungate_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_write_reg(struct arm_cti *self, unsigned int reg, uint32_t value);
+extern int arm_cti_read_reg(struct arm_cti *self, unsigned int reg, uint32_t *value);
+extern int arm_cti_pulse_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_set_channel(struct arm_cti *self, uint32_t channel);
+extern int arm_cti_clear_channel(struct arm_cti *self, uint32_t channel);
+
+#endif /* OPENOCD_TARGET_ARM_CTI_H */
-- 
cgit v1.1


From 8b923532c16459d83655bab418b92e57f6d3312a Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sun, 27 Nov 2016 11:28:01 +0100
Subject: aarch64: remove bogus address check before memory access

Mmu faults can not be prevented on aarch64, they need to be taken and
handled accordingly. Remove the remaining stub code.

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

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d3ed57c..5dd6d7a 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -103,13 +103,6 @@ static int aarch64_restore_system_control_reg(struct target *target)
 	return retval;
 }
 
-/*  check address before aarch64_apb read write access with mmu on
- *  remove apb predictible data abort */
-static int aarch64_check_address(struct target *target, uint32_t address)
-{
-	/* TODO */
-	return ERROR_OK;
-}
 /*  modify system_control_reg in order to enable or disable mmu for :
  *  - virt2phys address conversion
  *  - read or write memory in phys or virt address */
@@ -1651,9 +1644,6 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 		return retval;
 
 	if (mmu_enabled) {
-		retval = aarch64_check_address(target, address);
-		if (retval != ERROR_OK)
-			return retval;
 		/* enable MMU as we could have disabled it for phys access */
 		retval = aarch64_mmu_modify(target, 1);
 		if (retval != ERROR_OK)
@@ -1691,9 +1681,6 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 		return retval;
 
 	if (mmu_enabled) {
-		retval = aarch64_check_address(target, address);
-		if (retval != ERROR_OK)
-			return retval;
 		/* enable MMU as we could have disabled it for phys access */
 		retval = aarch64_mmu_modify(target, 1);
 		if (retval != ERROR_OK)
-- 
cgit v1.1


From 095ff3d2103f9e8089b5b1fb0816d43874014e08 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Sun, 27 Nov 2016 11:39:47 +0100
Subject: armv8: load aarch32 register through aarch64 equivalent

The aarch32 register cache is only a separate view of the aarch64
registers. Load aarch32 registers through their aarch64 equivalents.

Change-Id: I3e932dfb782f03d73d30d942b24db340a5749e47
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3988
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/armv8.c | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 7ae8cc9..00ab6ed 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1093,7 +1093,7 @@ static int armv8_get_core_reg32(struct reg *reg)
 		return ERROR_OK;
 	}
 
-	retval = arm->read_core_reg(target, reg, armv8_reg->num, arm->core_mode);
+	retval = arm->read_core_reg(target, reg64, armv8_reg->num, arm->core_mode);
 	if (retval == ERROR_OK)
 		reg->valid = reg64->valid;
 
@@ -1109,9 +1109,6 @@ static int armv8_set_core_reg32(struct reg *reg, uint8_t *buf)
 	struct reg *reg64 = cache->reg_list + armv8_reg->num;
 	uint32_t value = buf_get_u32(buf, 0, 32);
 
-	if (target->state != TARGET_HALTED)
-		return ERROR_TARGET_NOT_HALTED;
-
 	if (reg64 == arm->cpsr) {
 		armv8_set_cpsr(arm, value);
 	} else {
@@ -1250,7 +1247,7 @@ int armv8_get_gdb_reg_list(struct target *target,
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 
-		LOG_DEBUG("Creating Aarch64 register list");
+		LOG_DEBUG("Creating Aarch64 register list for target %s", target_name(target));
 
 		switch (reg_class) {
 		case REG_CLASS_GENERAL:
@@ -1277,7 +1274,7 @@ int armv8_get_gdb_reg_list(struct target *target,
 	} else {
 		struct reg_cache *cache32 = arm->core_cache->next;
 
-		LOG_DEBUG("Creating Aarch32 register list");
+		LOG_DEBUG("Creating Aarch32 register list for target %s", target_name(target));
 
 		switch (reg_class) {
 		case REG_CLASS_GENERAL:
-- 
cgit v1.1


From 6fb9f2e3ee05d8ff6241e6d61f7de0e71afeb45c Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Wed, 15 Feb 2017 14:57:21 +0100
Subject: armv8: factor out generic bit set/clr for debug registers

introduce armv8_set_dbgreg_bits() function to make register
bit-field modifications easier to read.


Change-Id: I6b06f66262587fd301d848c9e0645e8327653de7
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3989
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c | 18 +-----------------
 src/target/armv8.c   | 21 +++++++++++++++++++++
 src/target/armv8.h   |  1 +
 3 files changed, 23 insertions(+), 17 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 5dd6d7a..65a5278 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -238,23 +238,7 @@ static int aarch64_dpm_setup(struct aarch64_common *a8, uint64_t debug)
 static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask, unsigned long value)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
-	uint32_t dscr;
-
-	/* Read DSCR */
-	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	if (ERROR_OK != retval)
-		return retval;
-
-	/* clear bitfield */
-	dscr &= ~bit_mask;
-	/* put new value */
-	dscr |= value & bit_mask;
-
-	/* write new DSCR */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	return retval;
+	return armv8_set_dbgreg_bits(armv8, CPUV8_DBG_DSCR, bit_mask, value);
 }
 
 static struct target *get_aarch64(struct target *target, int32_t coreid)
diff --git a/src/target/armv8.c b/src/target/armv8.c
index 00ab6ed..2f1d5c1 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -1292,3 +1292,24 @@ int armv8_get_gdb_reg_list(struct target *target,
 		}
 	}
 }
+
+int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value)
+{
+	uint32_t tmp;
+
+	/* Read register */
+	int retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + reg, &tmp);
+	if (ERROR_OK != retval)
+		return retval;
+
+	/* clear bitfield */
+	tmp &= ~mask;
+	/* put new value */
+	tmp |= value & mask;
+
+	/* write new value */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + reg, tmp);
+	return retval;
+}
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 3b2fc59..1cb3a3b 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -307,6 +307,7 @@ static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
 }
 
 void armv8_select_reg_access(struct armv8_common *armv8, bool is_aarch64);
+int armv8_set_dbgreg_bits(struct armv8_common *armv8, unsigned int reg, unsigned long mask, unsigned long value);
 
 extern const struct command_registration armv8_command_handlers[];
 
-- 
cgit v1.1


From d6535e0ce517bfb2c4c8acaef59eb73dd0c6d07c Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Wed, 15 Feb 2017 15:30:21 +0100
Subject: aarch64: reset fixes

Make sure all core register caches are invalidated on reset
assert, make sure to re-init debug registers on deassert.

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

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 65a5278..d14b54d 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -169,6 +169,13 @@ static int aarch64_init_debug_access(struct target *target)
 
 	LOG_DEBUG(" ");
 
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("Examine %s failed", "oslock");
+		return retval;
+	}
+
 	/* Clear Sticky Power Down status Bit in PRSR to enable access to
 	   the registers in the Core Power Domain */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
@@ -1256,8 +1263,10 @@ static int aarch64_assert_reset(struct target *target)
 	}
 
 	/* registers are now invalid */
-	if (target_was_examined(target))
+	if (target_was_examined(target)) {
 		register_cache_invalidate(armv8->arm.core_cache);
+		register_cache_invalidate(armv8->arm.core_cache->next);
+	}
 
 	target->state = TARGET_RESET;
 
@@ -1290,7 +1299,7 @@ static int aarch64_deassert_reset(struct target *target)
 		}
 	}
 
-	return ERROR_OK;
+	return aarch64_init_debug_access(target);
 }
 
 static int aarch64_write_apb_ap_memory(struct target *target,
-- 
cgit v1.1


From 257c434d3ff9a9f7a5d9cec1ed7a2bd1ff82eaf5 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 17 Feb 2017 13:42:50 +0100
Subject: aarch64: clean up target specific commands

- rename "cortex_a" command group to "aarch64"
- remove default blank check, checksum and algorithm hooks
  since they're not going to work in aarch64 mode anyway.



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

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index d14b54d..da7d2a9 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -2057,9 +2057,9 @@ static const struct command_registration aarch64_command_handlers[] = {
 		.chain = armv8_command_handlers,
 	},
 	{
-		.name = "cortex_a",
+		.name = "aarch64",
 		.mode = COMMAND_ANY,
-		.help = "Cortex-A command group",
+		.help = "Aarch64 command group",
 		.usage = "",
 		.chain = aarch64_exec_command_handlers,
 	},
@@ -2085,11 +2085,6 @@ struct target_type aarch64_target = {
 	.read_memory = aarch64_read_memory,
 	.write_memory = aarch64_write_memory,
 
-	.checksum_memory = arm_checksum_memory,
-	.blank_check_memory = arm_blank_check_memory,
-
-	.run_algorithm = armv4_5_run_algorithm,
-
 	.add_breakpoint = aarch64_add_breakpoint,
 	.add_context_breakpoint = aarch64_add_context_breakpoint,
 	.add_hybrid_breakpoint = aarch64_add_hybrid_breakpoint,
-- 
cgit v1.1


From e513fe949bdd0d334a373a2e0adb6d9b8c036c69 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 17 Feb 2017 13:57:08 +0100
Subject: aarch64: clean up struct aarch64_common

remove some rarely or completely unused components.

Change-Id: Id285bb7075901016297fa173a874db7f11a840d7
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3992
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c | 26 ++++++++++++++------------
 src/target/aarch64.h |  9 ---------
 2 files changed, 14 insertions(+), 21 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index da7d2a9..29cbb07 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -331,7 +331,6 @@ static int aarch64_poll(struct target *target)
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
-	aarch64->cpudbg_dscr = dscr;
 
 	if (DSCR_RUN_MODE(dscr) == 0x3) {
 		if (prev_target_state != TARGET_HALTED) {
@@ -601,22 +600,28 @@ static int aarch64_resume(struct target *target, int current,
 static int aarch64_debug_entry(struct target *target)
 {
 	int retval = ERROR_OK;
-	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = &armv8->dpm;
 	enum arm_state core_state;
+	uint32_t dscr;
+
+	/* make sure to clear all sticky errors */
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
+	if (retval == ERROR_OK)
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 
-	LOG_DEBUG("%s dscr = 0x%08" PRIx32, target_name(target), aarch64->cpudbg_dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	LOG_DEBUG("%s dscr = 0x%08" PRIx32, target_name(target), dscr);
 
-	dpm->dscr = aarch64->cpudbg_dscr;
+	dpm->dscr = dscr;
 	core_state = armv8_dpm_get_core_state(dpm);
 	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
 	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
 
-	/* make sure to clear all sticky errors */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
-
 	/* discard async exceptions */
 	if (retval == ERROR_OK)
 		retval = dpm->instr_cpsr_sync(dpm);
@@ -625,7 +630,7 @@ static int aarch64_debug_entry(struct target *target)
 		return retval;
 
 	/* Examine debug reason */
-	armv8_dpm_report_dscr(dpm, aarch64->cpudbg_dscr);
+	armv8_dpm_report_dscr(dpm, dscr);
 
 	/* save address of instruction that triggered the watchpoint? */
 	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
@@ -717,7 +722,6 @@ static int aarch64_post_debug_entry(struct target *target)
 		(aarch64->system_control_reg & 0x4U) ? 1 : 0;
 	armv8->armv8_mmu.armv8_cache.i_cache_enabled =
 		(aarch64->system_control_reg & 0x1000U) ? 1 : 0;
-	aarch64->curr_mode = armv8->arm.core_mode;
 	return ERROR_OK;
 }
 
@@ -1893,8 +1897,6 @@ static int aarch64_init_arch_info(struct target *target,
 
 	armv8->arm.dap = tap->dap;
 
-	aarch64->fast_reg_read = 0;
-
 	/* register arch-specific functions */
 	armv8->examine_debug_reason = NULL;
 
diff --git a/src/target/aarch64.h b/src/target/aarch64.h
index 9491623..c9ec02d 100644
--- a/src/target/aarch64.h
+++ b/src/target/aarch64.h
@@ -48,25 +48,16 @@ struct aarch64_common {
 	int common_magic;
 
 	/* Context information */
-	uint32_t cpudbg_dscr;
-
 	uint32_t system_control_reg;
 	uint32_t system_control_reg_curr;
 
-	enum arm_mode curr_mode;
-
-
 	/* Breakpoint register pairs */
 	int brp_num_context;
 	int brp_num;
 	int brp_num_available;
 	struct aarch64_brp *brp_list;
 
-	/* Use aarch64_read_regs_through_mem for fast register reads */
-	int fast_reg_read;
-
 	struct armv8_common armv8_common;
-
 };
 
 static inline struct aarch64_common *
-- 
cgit v1.1


From bf1efe05bb71b7a6fe4ec1809a6aaa9b6073aede Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 17 Feb 2017 14:24:53 +0100
Subject: aarch64: run control rework

This patch contains a major overhaul of the target run control,
mainly for the sake of satisfying gdbs ideas of how a target
should respond to various control requests for the debugger.

The changes allow gdb a slightly better control on how cores
are stepped: a core can be single-stepped while
other cores remain halted or continue normal execution
until the single-stepped core halts again.

Also, on any halting event (user command or breakpoint) the
system is brought into a stable state with all cores halted
before the halt is signaled to the debugger.

This patch also transitions the target code to make use of the
new CTI abstraction instead of accessing CTI registers directly.

Change-Id: I8ddc9abb119e04580d671b57ee12240c3f5070a0
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3993
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/aarch64.c   | 864 ++++++++++++++++++++++++++++++++++---------------
 src/target/armv8.h     |  38 +--
 src/target/armv8_dpm.h |   7 +-
 3 files changed, 614 insertions(+), 295 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 29cbb07..9977c36 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -30,6 +30,18 @@
 #include "armv8_cache.h"
 #include <helper/time_support.h>
 
+#define __unused __attribute((unused))
+
+enum restart_mode {
+	RESTART_LAZY,
+	RESTART_SYNC,
+};
+
+enum halt_mode {
+	HALT_LAZY,
+	HALT_SYNC,
+};
+
 static int aarch64_poll(struct target *target);
 static int aarch64_debug_entry(struct target *target);
 static int aarch64_restore_context(struct target *target, bool bpwp);
@@ -47,6 +59,9 @@ static int aarch64_virt2phys(struct target *target,
 static int aarch64_read_apb_ap_memory(struct target *target,
 	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 
+#define foreach_smp_target(pos, head) \
+	for (pos = head; (pos != NULL); pos = pos->next)
+
 static int aarch64_restore_system_control_reg(struct target *target)
 {
 	enum arm_mode target_mode = ARM_MODE_ANY;
@@ -191,27 +206,22 @@ static int aarch64_init_debug_access(struct target *target)
 	 */
 
 	/* Enable CTI */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_CTR, 1);
-	/* By default, gate all channel triggers to and from the CTM */
+	retval = arm_cti_enable(armv8->cti, true);
+	/* By default, gate all channel events to and from the CTM */
 	if (retval == ERROR_OK)
-		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->cti_base + CTI_GATE, 0);
-	/* output halt requests to PE on channel 0 trigger */
+		retval = arm_cti_write_reg(armv8->cti, CTI_GATE, 0);
+	/* output halt requests to PE on channel 0 event */
 	if (retval == ERROR_OK)
-		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->cti_base + CTI_OUTEN0, CTI_CHNL(0));
-	/* output restart requests to PE on channel 1 trigger */
+		retval = arm_cti_write_reg(armv8->cti, CTI_OUTEN0, CTI_CHNL(0));
+	/* output restart requests to PE on channel 1 event */
 	if (retval == ERROR_OK)
-		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->cti_base + CTI_OUTEN1, CTI_CHNL(1));
+		retval = arm_cti_write_reg(armv8->cti, CTI_OUTEN1, CTI_CHNL(1));
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Resync breakpoint registers */
 
-	/* Since this is likely called from init or reset, update target state information*/
-	return aarch64_poll(target);
+	return ERROR_OK;
 }
 
 /* Write to memory mapped registers directly with no cache or mmu handling */
@@ -248,123 +258,270 @@ static int aarch64_set_dscr_bits(struct target *target, unsigned long bit_mask,
 	return armv8_set_dbgreg_bits(armv8, CPUV8_DBG_DSCR, bit_mask, value);
 }
 
-static struct target *get_aarch64(struct target *target, int32_t coreid)
+static int aarch64_check_state_one(struct target *target,
+		uint32_t mask, uint32_t val, int *p_result, uint32_t *p_prsr)
 {
-	struct target_list *head;
-	struct target *curr;
+	struct armv8_common *armv8 = target_to_armv8(target);
+	uint32_t prsr;
+	int retval;
 
-	head = target->head;
-	while (head != (struct target_list *)NULL) {
-		curr = head->target;
-		if ((curr->coreid == coreid) && (curr->state == TARGET_HALTED))
-			return curr;
-		head = head->next;
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_PRSR, &prsr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (p_prsr)
+		*p_prsr = prsr;
+
+	if (p_result)
+		*p_result = (prsr & mask) == (val & mask);
+
+	return ERROR_OK;
+}
+
+static int aarch64_wait_halt_one(struct target *target)
+{
+	int retval = ERROR_OK;
+	uint32_t prsr;
+
+	int64_t then = timeval_ms();
+	for (;;) {
+		int halted;
+
+		retval = aarch64_check_state_one(target, PRSR_HALT, PRSR_HALT, &halted, &prsr);
+		if (retval != ERROR_OK || halted)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			retval = ERROR_TARGET_TIMEOUT;
+			LOG_DEBUG("target %s timeout, prsr=0x%08"PRIx32, target_name(target), prsr);
+			break;
+		}
 	}
-	return target;
+	return retval;
 }
-static int aarch64_halt(struct target *target);
 
-static int aarch64_halt_smp(struct target *target)
+static int aarch64_prepare_halt_smp(struct target *target, bool exc_target, struct target **p_first)
 {
 	int retval = ERROR_OK;
 	struct target_list *head = target->head;
+	struct target *first = NULL;
+
+	LOG_DEBUG("target %s exc %i", target_name(target), exc_target);
 
-	while (head != (struct target_list *)NULL) {
+	while (head != NULL) {
 		struct target *curr = head->target;
 		struct armv8_common *armv8 = target_to_armv8(curr);
+		head = head->next;
+
+		if (exc_target && curr == target)
+			continue;
+		if (!target_was_examined(curr))
+			continue;
+		if (curr->state != TARGET_RUNNING)
+			continue;
+
+		/* HACK: mark this target as prepared for halting */
+		curr->debug_reason = DBG_REASON_DBGRQ;
 
 		/* open the gate for channel 0 to let HALT requests pass to the CTM */
-		if (curr->smp) {
-			retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->cti_base + CTI_GATE, CTI_CHNL(0));
-			if (retval == ERROR_OK)
-				retval = aarch64_set_dscr_bits(curr, DSCR_HDE, DSCR_HDE);
-		}
+		retval = arm_cti_ungate_channel(armv8->cti, 0);
+		if (retval == ERROR_OK)
+			retval = aarch64_set_dscr_bits(curr, DSCR_HDE, DSCR_HDE);
 		if (retval != ERROR_OK)
 			break;
 
-		head = head->next;
+		LOG_DEBUG("target %s prepared", target_name(curr));
+
+		if (first == NULL)
+			first = curr;
 	}
 
+	if (p_first) {
+		if (exc_target && first)
+			*p_first = first;
+		else
+			*p_first = target;
+	}
+
+	return retval;
+}
+
+static int aarch64_halt_one(struct target *target, enum halt_mode mode)
+{
+	int retval = ERROR_OK;
+	struct armv8_common *armv8 = target_to_armv8(target);
+
+	LOG_DEBUG("%s", target_name(target));
+
+	/* allow Halting Debug Mode */
+	retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* trigger an event on channel 0, this outputs a halt request to the PE */
+	retval = arm_cti_pulse_channel(armv8->cti, 0);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (mode == HALT_SYNC) {
+		retval = aarch64_wait_halt_one(target);
+		if (retval != ERROR_OK) {
+			if (retval == ERROR_TARGET_TIMEOUT)
+				LOG_ERROR("Timeout waiting for target %s halt", target_name(target));
+			return retval;
+		}
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_halt_smp(struct target *target, bool exc_target)
+{
+	struct target *next = target;
+	int retval;
+
+	/* prepare halt on all PEs of the group */
+	retval = aarch64_prepare_halt_smp(target, exc_target, &next);
+
+	if (exc_target && next == target)
+		return retval;
+
 	/* halt the target PE */
 	if (retval == ERROR_OK)
-		retval = aarch64_halt(target);
+		retval = aarch64_halt_one(next, HALT_LAZY);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* wait for all PEs to halt */
+	int64_t then = timeval_ms();
+	for (;;) {
+		bool all_halted = true;
+		struct target_list *head;
+		struct target *curr;
+
+		foreach_smp_target(head, target->head) {
+			int halted;
+
+			curr = head->target;
+
+			if (!target_was_examined(curr))
+				continue;
+
+			retval = aarch64_check_state_one(curr, PRSR_HALT, PRSR_HALT, &halted, NULL);
+			if (retval != ERROR_OK || !halted) {
+				all_halted = false;
+				break;
+			}
+		}
+
+		if (all_halted)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
+		}
+
+		/*
+		 * HACK: on Hi6220 there are 8 cores organized in 2 clusters
+		 * and it looks like the CTI's are not connected by a common
+		 * trigger matrix. It seems that we need to halt one core in each
+		 * cluster explicitly. So if we find that a core has not halted
+		 * yet, we trigger an explicit halt for the second cluster.
+		 */
+		retval = aarch64_halt_one(curr, HALT_LAZY);
+		if (retval != ERROR_OK)
+			break;
+	}
 
 	return retval;
 }
 
-static int update_halt_gdb(struct target *target)
+static int update_halt_gdb(struct target *target, enum target_debug_reason debug_reason)
 {
-	int retval = 0;
-	if (target->gdb_service && target->gdb_service->core[0] == -1) {
-		target->gdb_service->target = target;
-		target->gdb_service->core[0] = target->coreid;
-		retval += aarch64_halt_smp(target);
+	struct target *gdb_target = NULL;
+	struct target_list *head;
+	struct target *curr;
+
+	if (debug_reason == DBG_REASON_NOTHALTED) {
+		LOG_INFO("Halting remaining targets in SMP group");
+		aarch64_halt_smp(target, true);
 	}
-	return retval;
+
+	/* poll all targets in the group, but skip the target that serves GDB */
+	foreach_smp_target(head, target->head) {
+		curr = head->target;
+		/* skip calling context */
+		if (curr == target)
+			continue;
+		if (!target_was_examined(curr))
+			continue;
+		/* skip targets that were already halted */
+		if (curr->state == TARGET_HALTED)
+			continue;
+		/* remember the gdb_service->target */
+		if (curr->gdb_service != NULL)
+			gdb_target = curr->gdb_service->target;
+		/* skip it */
+		if (curr == gdb_target)
+			continue;
+
+		/* avoid recursion in aarch64_poll() */
+		curr->smp = 0;
+		aarch64_poll(curr);
+		curr->smp = 1;
+	}
+
+	/* after all targets were updated, poll the gdb serving target */
+	if (gdb_target != NULL && gdb_target != target)
+		aarch64_poll(gdb_target);
+
+	return ERROR_OK;
 }
 
 /*
- * Cortex-A8 Run control
+ * Aarch64 Run control
  */
 
 static int aarch64_poll(struct target *target)
 {
+	enum target_state prev_target_state;
 	int retval = ERROR_OK;
-	uint32_t dscr;
-	struct aarch64_common *aarch64 = target_to_aarch64(target);
-	struct armv8_common *armv8 = &aarch64->armv8_common;
-	enum target_state prev_target_state = target->state;
-	/*  toggle to another core is done by gdb as follow */
-	/*  maint packet J core_id */
-	/*  continue */
-	/*  the next polling trigger an halt event sent to gdb */
-	if ((target->state == TARGET_HALTED) && (target->smp) &&
-		(target->gdb_service) &&
-		(target->gdb_service->target == NULL)) {
-		target->gdb_service->target =
-			get_aarch64(target, target->gdb_service->core[1]);
-		target_call_event_callbacks(target, TARGET_EVENT_HALTED);
-		return retval;
-	}
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	int halted;
+
+	retval = aarch64_check_state_one(target,
+				PRSR_HALT, PRSR_HALT, &halted, NULL);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (DSCR_RUN_MODE(dscr) == 0x3) {
+	if (halted) {
+		prev_target_state = target->state;
 		if (prev_target_state != TARGET_HALTED) {
+			enum target_debug_reason debug_reason = target->debug_reason;
+
 			/* We have a halting debug event */
-			LOG_DEBUG("Target %s halted", target_name(target));
 			target->state = TARGET_HALTED;
-			if ((prev_target_state == TARGET_RUNNING)
-				|| (prev_target_state == TARGET_UNKNOWN)
-				|| (prev_target_state == TARGET_RESET)) {
-				retval = aarch64_debug_entry(target);
-				if (retval != ERROR_OK)
-					return retval;
-				if (target->smp) {
-					retval = update_halt_gdb(target);
-					if (retval != ERROR_OK)
-						return retval;
-				}
-				target_call_event_callbacks(target,
-					TARGET_EVENT_HALTED);
-			}
-			if (prev_target_state == TARGET_DEBUG_RUNNING) {
-				LOG_DEBUG(" ");
-
-				retval = aarch64_debug_entry(target);
-				if (retval != ERROR_OK)
-					return retval;
-				if (target->smp) {
-					retval = update_halt_gdb(target);
-					if (retval != ERROR_OK)
-						return retval;
-				}
+			LOG_DEBUG("Target %s halted", target_name(target));
+			retval = aarch64_debug_entry(target);
+			if (retval != ERROR_OK)
+				return retval;
 
-				target_call_event_callbacks(target,
-					TARGET_EVENT_DEBUG_HALTED);
+			if (target->smp)
+				update_halt_gdb(target, debug_reason);
+
+			switch (prev_target_state) {
+			case TARGET_RUNNING:
+			case TARGET_UNKNOWN:
+			case TARGET_RESET:
+				target_call_event_callbacks(target, TARGET_EVENT_HALTED);
+				break;
+			case TARGET_DEBUG_RUNNING:
+				target_call_event_callbacks(target, TARGET_EVENT_DEBUG_HALTED);
+				break;
+			default:
+				break;
 			}
 		}
 	} else
@@ -375,43 +532,13 @@ static int aarch64_poll(struct target *target)
 
 static int aarch64_halt(struct target *target)
 {
-	int retval = ERROR_OK;
-	uint32_t dscr;
-	struct armv8_common *armv8 = target_to_armv8(target);
-
-	/*
-	 * add HDE in halting debug mode
-	 */
-	retval = aarch64_set_dscr_bits(target, DSCR_HDE, DSCR_HDE);
-	if (retval != ERROR_OK)
-		return retval;
-
-	/* trigger an event on channel 0, this outputs a halt request to the PE */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_APPPULSE, CTI_CHNL(0));
-	if (retval != ERROR_OK)
-		return retval;
-
-	long long then = timeval_ms();
-	for (;; ) {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-		if (retval != ERROR_OK)
-			return retval;
-		if ((dscr & DSCRV8_HALT_MASK) != 0)
-			break;
-		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("Timeout waiting for halt");
-			return ERROR_FAIL;
-		}
-	}
-
-	target->debug_reason = DBG_REASON_DBGRQ;
+	if (target->smp)
+		return aarch64_halt_smp(target, false);
 
-	return ERROR_OK;
+	return aarch64_halt_one(target, HALT_SYNC);
 }
 
-static int aarch64_internal_restore(struct target *target, int current,
+static int aarch64_restore_one(struct target *target, int current,
 	uint64_t *address, int handle_breakpoints, int debug_execution)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
@@ -419,6 +546,8 @@ static int aarch64_internal_restore(struct target *target, int current,
 	int retval;
 	uint64_t resume_pc;
 
+	LOG_DEBUG("%s", target_name(target));
+
 	if (!debug_execution)
 		target_free_all_working_areas(target);
 
@@ -464,19 +593,19 @@ static int aarch64_internal_restore(struct target *target, int current,
 	return retval;
 }
 
-static int aarch64_internal_restart(struct target *target, bool slave_pe)
+/**
+ * prepare single target for restart
+ *
+ *
+ */
+static int aarch64_prepare_restart_one(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
-	struct arm *arm = &armv8->arm;
 	int retval;
 	uint32_t dscr;
-	/*
-	 * * Restart core and wait for it to be started.  Clear ITRen and sticky
-	 * * exception flags: see ARMv7 ARM, C5.9.
-	 *
-	 * REVISIT: for single stepping, we probably want to
-	 * disable IRQs by default, with optional override...
-	 */
+	uint32_t tmp;
+
+	LOG_DEBUG("%s", target_name(target));
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
@@ -488,70 +617,195 @@ static int aarch64_internal_restart(struct target *target, bool slave_pe)
 	if ((dscr & DSCR_ERR) != 0)
 		LOG_ERROR("DSCR.ERR must be cleared before leaving debug!");
 
-	/* make sure to acknowledge the halt event before resuming */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->cti_base + CTI_INACK, CTI_TRIG(HALT));
-
+	/* acknowledge a pending CTI halt event */
+	retval = arm_cti_ack_events(armv8->cti, CTI_TRIG(HALT));
 	/*
 	 * open the CTI gate for channel 1 so that the restart events
-	 * get passed along to all PEs
+	 * get passed along to all PEs. Also close gate for channel 0
+	 * to isolate the PE from halt events.
 	 */
 	if (retval == ERROR_OK)
+		retval = arm_cti_ungate_channel(armv8->cti, 1);
+	if (retval == ERROR_OK)
+		retval = arm_cti_gate_channel(armv8->cti, 0);
+
+	/* make sure that DSCR.HDE is set */
+	if (retval == ERROR_OK) {
+		dscr |= DSCR_HDE;
 		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->cti_base + CTI_GATE, CTI_CHNL(1));
+				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);
+
+	return retval;
+}
+
+static int aarch64_do_restart_one(struct target *target, enum restart_mode mode)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	int retval;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	/* trigger an event on channel 1, generates a restart request to the PE */
+	retval = arm_cti_pulse_channel(armv8->cti, 1);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (!slave_pe) {
-		/* trigger an event on channel 1, generates a restart request to the PE */
-		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->cti_base + CTI_APPPULSE, CTI_CHNL(1));
-		if (retval != ERROR_OK)
-			return retval;
-
-		long long then = timeval_ms();
-		for (;; ) {
-			retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-			if (retval != ERROR_OK)
-				return retval;
-			if ((dscr & DSCR_HDE) != 0)
+	if (mode == RESTART_SYNC) {
+		int64_t then = timeval_ms();
+		for (;;) {
+			int resumed;
+			/*
+			 * if PRSR.SDR is set now, the target did restart, even
+			 * if it's now already halted again (e.g. due to breakpoint)
+			 */
+			retval = aarch64_check_state_one(target,
+						PRSR_SDR, PRSR_SDR, &resumed, NULL);
+			if (retval != ERROR_OK || resumed)
 				break;
+
 			if (timeval_ms() > then + 1000) {
-				LOG_ERROR("Timeout waiting for resume");
-				return ERROR_FAIL;
+				LOG_ERROR("%s: Timeout waiting for resume"PRIx32, target_name(target));
+				retval = ERROR_TARGET_TIMEOUT;
+				break;
 			}
 		}
 	}
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	target->debug_reason = DBG_REASON_NOTHALTED;
 	target->state = TARGET_RUNNING;
 
-	/* registers are now invalid */
-	register_cache_invalidate(arm->core_cache);
-	register_cache_invalidate(arm->core_cache->next);
-
 	return ERROR_OK;
 }
 
-static int aarch64_restore_smp(struct target *target, int handle_breakpoints)
+static int aarch64_restart_one(struct target *target, enum restart_mode mode)
 {
-	int retval = 0;
+	int retval;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	retval = aarch64_prepare_restart_one(target);
+	if (retval == ERROR_OK)
+		retval = aarch64_do_restart_one(target, mode);
+
+	return retval;
+}
+
+/*
+ * prepare all but the current target for restart
+ */
+static int aarch64_prep_restart_smp(struct target *target, int handle_breakpoints, struct target **p_first)
+{
+	int retval = ERROR_OK;
 	struct target_list *head;
-	struct target *curr;
+	struct target *first = NULL;
 	uint64_t address;
-	head = target->head;
-	while (head != (struct target_list *)NULL) {
-		curr = head->target;
-		if ((curr != target) && (curr->state != TARGET_RUNNING)) {
-			/*  resume current address , not in step mode */
-			retval += aarch64_internal_restore(curr, 1, &address,
-					handle_breakpoints, 0);
-			retval += aarch64_internal_restart(curr, true);
+
+	foreach_smp_target(head, target->head) {
+		struct target *curr = head->target;
+
+		/* skip calling target */
+		if (curr == target)
+			continue;
+		if (!target_was_examined(curr))
+			continue;
+		if (curr->state != TARGET_HALTED)
+			continue;
+
+		/*  resume at current address, not in step mode */
+		retval = aarch64_restore_one(curr, 1, &address, handle_breakpoints, 0);
+		if (retval == ERROR_OK)
+			retval = aarch64_prepare_restart_one(curr);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("failed to restore target %s", target_name(curr));
+			break;
 		}
-		head = head->next;
+		/* remember the first valid target in the group */
+		if (first == NULL)
+			first = curr;
+	}
+
+	if (p_first)
+		*p_first = first;
+
+	return retval;
+}
+
+
+static int aarch64_step_restart_smp(struct target *target)
+{
+	int retval = ERROR_OK;
+	struct target_list *head;
+	struct target *first = NULL;
+
+	LOG_DEBUG("%s", target_name(target));
+
+	retval = aarch64_prep_restart_smp(target, 0, &first);
+	if (retval != ERROR_OK)
+		return retval;
 
+	if (first != NULL)
+		retval = aarch64_do_restart_one(first, RESTART_LAZY);
+	if (retval != ERROR_OK) {
+		LOG_DEBUG("error restarting target %s", target_name(first));
+		return retval;
 	}
+
+	int64_t then = timeval_ms();
+	for (;;) {
+		struct target *curr = target;
+		bool all_resumed = true;
+
+		foreach_smp_target(head, target->head) {
+			uint32_t prsr;
+			int resumed;
+
+			curr = head->target;
+
+			if (curr == target)
+				continue;
+
+			retval = aarch64_check_state_one(curr,
+					PRSR_SDR, PRSR_SDR, &resumed, &prsr);
+			if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {
+				all_resumed = false;
+				break;
+			}
+
+			if (curr->state != TARGET_RUNNING) {
+				curr->state = TARGET_RUNNING;
+				curr->debug_reason = DBG_REASON_NOTHALTED;
+				target_call_event_callbacks(curr, TARGET_EVENT_RESUMED);
+			}
+		}
+
+		if (all_resumed)
+			break;
+
+		if (timeval_ms() > then + 1000) {
+			LOG_ERROR("%s: timeout waiting for target resume", __func__);
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
+		}
+		/*
+		 * HACK: on Hi6220 there are 8 cores organized in 2 clusters
+		 * and it looks like the CTI's are not connected by a common
+		 * trigger matrix. It seems that we need to halt one core in each
+		 * cluster explicitly. So if we find that a core has not halted
+		 * yet, we trigger an explicit resume for the second cluster.
+		 */
+		retval = aarch64_do_restart_one(curr, RESTART_LAZY);
+		if (retval != ERROR_OK)
+			break;
+}
+
 	return retval;
 }
 
@@ -561,28 +815,86 @@ static int aarch64_resume(struct target *target, int current,
 	int retval = 0;
 	uint64_t addr = address;
 
-	/* dummy resume for smp toggle in order to reduce gdb impact  */
-	if ((target->smp) && (target->gdb_service->core[1] != -1)) {
-		/*   simulate a start and halt of target */
-		target->gdb_service->target = NULL;
-		target->gdb_service->core[0] = target->gdb_service->core[1];
-		/*  fake resume at next poll we play the  target core[1], see poll*/
-		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-		return 0;
-	}
-
 	if (target->state != TARGET_HALTED)
 		return ERROR_TARGET_NOT_HALTED;
 
-	aarch64_internal_restore(target, current, &addr, handle_breakpoints,
-				 debug_execution);
+	/*
+	 * If this target is part of a SMP group, prepare the others
+	 * targets for resuming. This involves restoring the complete
+	 * target register context and setting up CTI gates to accept
+	 * resume events from the trigger matrix.
+	 */
 	if (target->smp) {
-		target->gdb_service->core[0] = -1;
-		retval = aarch64_restore_smp(target, handle_breakpoints);
+		retval = aarch64_prep_restart_smp(target, handle_breakpoints, NULL);
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	aarch64_internal_restart(target, false);
+
+	/* all targets prepared, restore and restart the current target */
+	retval = aarch64_restore_one(target, current, &addr, handle_breakpoints,
+				 debug_execution);
+	if (retval == ERROR_OK)
+		retval = aarch64_restart_one(target, RESTART_SYNC);
+	if (retval != ERROR_OK)
+		return retval;
+
+	if (target->smp) {
+		int64_t then = timeval_ms();
+		for (;;) {
+			struct target *curr = target;
+			struct target_list *head;
+			bool all_resumed = true;
+
+			foreach_smp_target(head, target->head) {
+				uint32_t prsr;
+				int resumed;
+
+				curr = head->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))) {
+					all_resumed = false;
+					break;
+				}
+
+				if (curr->state != TARGET_RUNNING) {
+					curr->state = TARGET_RUNNING;
+					curr->debug_reason = DBG_REASON_NOTHALTED;
+					target_call_event_callbacks(curr, TARGET_EVENT_RESUMED);
+				}
+			}
+
+			if (all_resumed)
+				break;
+
+			if (timeval_ms() > then + 1000) {
+				LOG_ERROR("%s: timeout waiting for target %s to resume", __func__, target_name(curr));
+				retval = ERROR_TARGET_TIMEOUT;
+				break;
+			}
+
+			/*
+			 * HACK: on Hi6220 there are 8 cores organized in 2 clusters
+			 * and it looks like the CTI's are not connected by a common
+			 * trigger matrix. It seems that we need to halt one core in each
+			 * cluster explicitly. So if we find that a core has not halted
+			 * yet, we trigger an explicit resume for the second cluster.
+			 */
+			retval = aarch64_do_restart_one(curr, RESTART_LAZY);
+			if (retval != ERROR_OK)
+				break;
+		}
+	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	target->debug_reason = DBG_REASON_NOTHALTED;
 
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
@@ -622,10 +934,12 @@ static int aarch64_debug_entry(struct target *target)
 	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
 	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
 
+	/* close the CTI gate for all events */
+	if (retval == ERROR_OK)
+		retval = arm_cti_write_reg(armv8->cti, CTI_GATE, 0);
 	/* discard async exceptions */
 	if (retval == ERROR_OK)
 		retval = dpm->instr_cpsr_sync(dpm);
-
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -725,10 +1039,14 @@ static int aarch64_post_debug_entry(struct target *target)
 	return ERROR_OK;
 }
 
+/*
+ * single-step a target
+ */
 static int aarch64_step(struct target *target, int current, target_addr_t address,
 	int handle_breakpoints)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
+	int saved_retval = ERROR_OK;
 	int retval;
 	uint32_t edecr;
 
@@ -739,39 +1057,70 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_EDECR, &edecr);
-	if (retval != ERROR_OK)
-		return retval;
-
 	/* make sure EDECR.SS is not set when restoring the register */
-	edecr &= ~0x4;
 
-	/* set EDECR.SS to enter hardware step mode */
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
+	if (retval == ERROR_OK) {
+		edecr &= ~0x4;
+		/* set EDECR.SS to enter hardware step mode */
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
+	}
+	/* disable interrupts while stepping */
+	if (retval == ERROR_OK)
+		retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
+	/* bail out if stepping setup has failed */
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* disable interrupts while stepping */
-	retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
-	if (retval != ERROR_OK)
-		return ERROR_OK;
+	if (target->smp && !handle_breakpoints) {
+		/*
+		 * isolate current target so that it doesn't get resumed
+		 * together with the others
+		 */
+		retval = arm_cti_gate_channel(armv8->cti, 1);
+		/* resume all other targets in the group */
+		if (retval == ERROR_OK)
+			retval = aarch64_step_restart_smp(target);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Failed to restart non-stepping targets in SMP group");
+			return retval;
+		}
+		LOG_DEBUG("Restarted all non-stepping targets in SMP group");
+	}
+
+	/* all other targets running, restore and restart the current target */
+	retval = aarch64_restore_one(target, current, &address, 0, 0);
+	if (retval == ERROR_OK)
+		retval = aarch64_restart_one(target, RESTART_LAZY);
 
-	/* resume the target */
-	retval = aarch64_resume(target, current, address, 0, 0);
 	if (retval != ERROR_OK)
 		return retval;
 
-	long long then = timeval_ms();
-	while (target->state != TARGET_HALTED) {
-		retval = aarch64_poll(target);
-		if (retval != ERROR_OK)
-			return retval;
+	LOG_DEBUG("target step-resumed at 0x%" PRIx64, address);
+	if (!handle_breakpoints)
+		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
+
+	int64_t then = timeval_ms();
+	for (;;) {
+		int stepped;
+		uint32_t prsr;
+
+		retval = aarch64_check_state_one(target,
+					PRSR_SDR|PRSR_HALT, PRSR_SDR|PRSR_HALT, &stepped, &prsr);
+		if (retval != ERROR_OK || stepped)
+			break;
+
 		if (timeval_ms() > then + 1000) {
-			LOG_ERROR("timeout waiting for target halt");
-			return ERROR_FAIL;
+			LOG_ERROR("timeout waiting for target %s halt after step",
+					target_name(target));
+			retval = ERROR_TARGET_TIMEOUT;
+			break;
 		}
 	}
 
+	if (retval == ERROR_TARGET_TIMEOUT)
+		saved_retval = retval;
+
 	/* restore EDECR */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_EDECR, edecr);
@@ -783,19 +1132,32 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	if (retval != ERROR_OK)
 		return ERROR_OK;
 
-	return ERROR_OK;
+	if (saved_retval != ERROR_OK)
+		return saved_retval;
+
+	return aarch64_poll(target);
 }
 
 static int aarch64_restore_context(struct target *target, bool bpwp)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+
+	int retval;
 
 	LOG_DEBUG("%s", target_name(target));
 
 	if (armv8->pre_restore_context)
 		armv8->pre_restore_context(target);
 
-	return armv8_dpm_write_dirty_registers(&armv8->dpm, bpwp);
+	retval = armv8_dpm_write_dirty_registers(&armv8->dpm, bpwp);
+	if (retval == ERROR_OK) {
+		/* registers are now invalid */
+		register_cache_invalidate(arm->core_cache);
+		register_cache_invalidate(arm->core_cache->next);
+	}
+
+	return retval;
 }
 
 /*
@@ -1723,6 +2085,7 @@ static int aarch64_examine_first(struct target *target)
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
 	struct adiv5_dap *swjdp = armv8->arm.dap;
+	uint32_t cti_base;
 	int i;
 	int retval = ERROR_OK;
 	uint64_t debug, ttypr;
@@ -1730,9 +2093,6 @@ static int aarch64_examine_first(struct target *target)
 	uint32_t tmp0, tmp1;
 	debug = ttypr = cpuid = 0;
 
-	/* We do one extra read to ensure DAP is configured,
-	 * we call ahbap_debugport_init(swjdp) instead
-	 */
 	retval = dap_dp_init(swjdp);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1750,7 +2110,7 @@ static int aarch64_examine_first(struct target *target)
 		return retval;
 	}
 
-	armv8->debug_ap->memaccess_tck = 80;
+	armv8->debug_ap->memaccess_tck = 10;
 
 	if (!target->dbgbase_set) {
 		uint32_t dbgbase;
@@ -1770,10 +2130,29 @@ static int aarch64_examine_first(struct target *target)
 	} else
 		armv8->debug_base = target->dbgbase;
 
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
+	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_DEBUG("LOCK debug access fail");
+		LOG_ERROR("target %s: failed to set power state of the core.", target_name(target));
 		return retval;
 	}
 
@@ -1819,12 +2198,15 @@ static int aarch64_examine_first(struct target *target)
 
 	if (target->ctibase == 0) {
 		/* assume a v8 rom table layout */
-		armv8->cti_base = target->ctibase = armv8->debug_base + 0x10000;
-		LOG_INFO("Target ctibase is not set, assuming 0x%0" PRIx32, target->ctibase);
+		cti_base = armv8->debug_base + 0x10000;
+		LOG_INFO("Target ctibase is not set, assuming 0x%0" PRIx32, cti_base);
 	} else
-		armv8->cti_base = target->ctibase;
+		cti_base = target->ctibase;
+
+	armv8->cti = arm_cti_create(armv8->debug_ap, cti_base);
+	if (armv8->cti == NULL)
+		return ERROR_FAIL;
 
-	armv8->arm.core_type = ARM_MODE_MON;
 	retval = aarch64_dpm_setup(aarch64, debug);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1847,6 +2229,9 @@ static int aarch64_examine_first(struct target *target)
 
 	LOG_DEBUG("Configured %i hw breakpoints", aarch64->brp_num);
 
+	target->state = TARGET_RUNNING;
+	target->debug_reason = DBG_REASON_NOTHALTED;
+
 	target_set_examined(target);
 	return ERROR_OK;
 }
@@ -1881,32 +2266,22 @@ static int aarch64_init_arch_info(struct target *target,
 	struct aarch64_common *aarch64, struct jtag_tap *tap)
 {
 	struct armv8_common *armv8 = &aarch64->armv8_common;
-	struct adiv5_dap *dap = armv8->arm.dap;
-
-	armv8->arm.dap = dap;
 
 	/* Setup struct aarch64_common */
 	aarch64->common_magic = AARCH64_COMMON_MAGIC;
 	/*  tap has no dap initialized */
 	if (!tap->dap) {
 		tap->dap = dap_init();
-
-		/* Leave (only) generic DAP stuff for debugport_init() */
 		tap->dap->tap = tap;
 	}
-
 	armv8->arm.dap = tap->dap;
 
 	/* register arch-specific functions */
 	armv8->examine_debug_reason = NULL;
-
 	armv8->post_debug_entry = aarch64_post_debug_entry;
-
 	armv8->pre_restore_context = NULL;
-
 	armv8->armv8_mmu.read_physical_memory = aarch64_read_phys_memory;
 
-	/* REVISIT v7a setup should be in a v7a-specific routine */
 	armv8_init_arch_info(target, armv8);
 	target_register_timer_callback(aarch64_handle_target_request, 1, 1, target);
 
@@ -1923,7 +2298,7 @@ static int aarch64_target_create(struct target *target, Jim_Interp *interp)
 static int aarch64_mmu(struct target *target, int *enabled)
 {
 	if (target->state != TARGET_HALTED) {
-		LOG_ERROR("%s: target not halted", __func__);
+		LOG_ERROR("%s: target %s not halted", __func__, target_name(target));
 		return ERROR_TARGET_INVALID;
 	}
 
@@ -1994,27 +2369,6 @@ COMMAND_HANDLER(aarch64_handle_smp_on_command)
 	return ERROR_OK;
 }
 
-COMMAND_HANDLER(aarch64_handle_smp_gdb_command)
-{
-	struct target *target = get_current_target(CMD_CTX);
-	int retval = ERROR_OK;
-	struct target_list *head;
-	head = target->head;
-	if (head != (struct target_list *)NULL) {
-		if (CMD_ARGC == 1) {
-			int coreid = 0;
-			COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], coreid);
-			if (ERROR_OK != retval)
-				return retval;
-			target->gdb_service->core[1] = coreid;
-
-		}
-		command_print(CMD_CTX, "gdb coreid  %" PRId32 " -> %" PRId32, target->gdb_service->core[0]
-			, target->gdb_service->core[1]);
-	}
-	return ERROR_OK;
-}
-
 static const struct command_registration aarch64_exec_command_handlers[] = {
 	{
 		.name = "cache_info",
@@ -2043,14 +2397,6 @@ static const struct command_registration aarch64_exec_command_handlers[] = {
 		.help = "Restart smp handling",
 		.usage = "",
 	},
-	{
-		.name = "smp_gdb",
-		.handler = aarch64_handle_smp_gdb_command,
-		.mode = COMMAND_EXEC,
-		.help = "display/fix current core played to gdb",
-		.usage = "",
-	},
-
 
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 1cb3a3b..02663ca 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -24,6 +24,7 @@
 #include "armv4_5_mmu.h"
 #include "armv4_5_cache.h"
 #include "armv8_dpm.h"
+#include "arm_cti.h"
 
 enum {
 	ARMV8_R0 = 0,
@@ -155,7 +156,6 @@ struct armv8_common {
 	/* Core Debug Unit */
 	struct arm_dpm dpm;
 	uint32_t debug_base;
-	uint32_t cti_base;
 	struct adiv5_ap *debug_ap;
 
 	const uint32_t *opcodes;
@@ -173,6 +173,8 @@ struct armv8_common {
 
 	struct armv8_mmu_common armv8_mmu;
 
+	struct arm_cti *cti;
+
 	/* Direct processor core register read and writes */
 	int (*read_reg_u64)(struct armv8_common *armv8, int num, uint64_t *value);
 	int (*write_reg_u64)(struct armv8_common *armv8, int num, uint64_t value);
@@ -222,40 +224,6 @@ target_to_armv8(struct target *target)
 
 #define CPUV8_DBG_AUTHSTATUS	0xFB8
 
-/*define CTI(cross trigger interface)*/
-#define CTI_CTR				0x0
-#define CTI_INACK			0x10
-#define CTI_APPSET			0x14
-#define CTI_APPCLEAR		0x18
-#define CTI_APPPULSE		0x1C
-#define CTI_INEN0			0x20
-#define CTI_INEN1			0x24
-#define CTI_INEN2			0x28
-#define CTI_INEN3			0x2C
-#define CTI_INEN4			0x30
-#define CTI_INEN5			0x34
-#define CTI_INEN6			0x38
-#define CTI_INEN7			0x3C
-#define CTI_OUTEN0			0xA0
-#define CTI_OUTEN1			0xA4
-#define CTI_OUTEN2			0xA8
-#define CTI_OUTEN3			0xAC
-#define CTI_OUTEN4			0xB0
-#define CTI_OUTEN5			0xB4
-#define CTI_OUTEN6			0xB8
-#define CTI_OUTEN7			0xBC
-#define CTI_TRIN_STATUS		0x130
-#define CTI_TROUT_STATUS	0x134
-#define CTI_CHIN_STATUS		0x138
-#define CTI_CHOU_STATUS		0x13C
-#define CTI_GATE			0x140
-#define CTI_UNLOCK			0xFB0
-
-#define CTI_CHNL(x)			(1 << x)
-#define CTI_TRIG_HALT		0
-#define CTI_TRIG_RESUME		1
-#define CTI_TRIG(n)			(1 << CTI_TRIG_##n)
-
 #define PAGE_SIZE_4KB				0x1000
 #define PAGE_SIZE_4KB_LEVEL0_BITS	39
 #define PAGE_SIZE_4KB_LEVEL1_BITS	30
diff --git a/src/target/armv8_dpm.h b/src/target/armv8_dpm.h
index 133b367..c039359 100644
--- a/src/target/armv8_dpm.h
+++ b/src/target/armv8_dpm.h
@@ -96,7 +96,7 @@ void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
 #define DRCR_RESTART			(1 << 1)
 #define DRCR_CLEAR_EXCEPTIONS	(1 << 2)
 
-/* PRCR (processor debug status register) bits */
+/* PRSR (processor debug status register) bits */
 #define PRSR_PU					(1 << 0)
 #define PRSR_SPD				(1 << 1)
 #define PRSR_RESET				(1 << 2)
@@ -110,6 +110,11 @@ void armv8_dpm_report_wfar(struct arm_dpm *, uint64_t wfar);
 #define PRSR_SPMAD				(1 << 10)
 #define PRSR_SDR				(1 << 11)
 
+/* PRCR (processor debug control register) bits */
+#define PRCR_CORENPDRQ			(1 << 0)
+#define PRCR_CWRR				(1 << 2)
+#define PRCR_COREPURQ			(1 << 3)
+
 void armv8_dpm_report_dscr(struct arm_dpm *dpm, uint32_t dcsr);
 void armv8_dpm_handle_exception(struct arm_dpm *dpm);
 enum arm_state armv8_dpm_get_core_state(struct arm_dpm *dpm);
-- 
cgit v1.1


From 2861ed533bfadac2b9259540e18c34c505ff2060 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 17 Feb 2017 15:05:15 +0100
Subject: armv8: spelling and formatting updates

small changes to correct code formatting and spelling of some
log messages.

Change-Id: I645e675f8f9f4731b0271ddc55f64e8cf56ec1db
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3994
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/armv8.c | 13 ++++---------
 1 file changed, 4 insertions(+), 9 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 2f1d5c1..db7a871 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -408,8 +408,7 @@ static int armv8_write_reg32(struct armv8_common *armv8, int regnum, uint64_t va
 		break;
 	case ARMV8_SP:
 		retval = dpm->instr_write_data_dcc(dpm,
-			ARMV4_5_MRC(14, 0, 13, 0, 5, 0),
-			value);
+				ARMV4_5_MRC(14, 0, 13, 0, 5, 0), value);
 			break;
 	case ARMV8_PC:/* PC
 		 * read r0 from DCC; then "MOV pc, r0" */
@@ -505,10 +504,9 @@ int armv8_read_mpidr(struct armv8_common *armv8)
 		LOG_INFO("%s cluster %x core %x %s", target_name(armv8->arm.target),
 			armv8->cluster_id,
 			armv8->cpu_id,
-			armv8->multi_processor_system == 0 ? "multi core" : "mono core");
-
+			armv8->multi_processor_system == 0 ? "multi core" : "single core");
 	} else
-		LOG_ERROR("mpdir not in multiprocessor format");
+		LOG_ERROR("mpidr not in multiprocessor format");
 
 done:
 	dpm->finish(dpm);
@@ -612,7 +610,7 @@ done:
 	/* (void) */ dpm->finish(dpm);
 }
 
-static void armv8_show_fault_registers(struct target *target)
+static __unused void armv8_show_fault_registers(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 
@@ -836,9 +834,6 @@ int armv8_mmu_translate_va_pa(struct target *target, target_addr_t va,
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (retval != ERROR_OK)
-		return retval;
-
 	if (par & 1) {
 		LOG_ERROR("Address translation failed at stage %i, FST=%x, PTW=%i",
 				((int)(par >> 9) & 1)+1, (int)(par >> 1) & 0x3f, (int)(par >> 8) & 1);
-- 
cgit v1.1


From b73628141a932881953bb816ec7bfa47d9e40680 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 17 Feb 2017 16:21:41 +0100
Subject: armv8_dpm: retrieve only necessary registers on halt

to speed up debugging, don't load the complete register context
on a halt event, load only those registers that might be
clobbered during debugging.

Change-Id: I0b58e97aad6f28aefce4a52e870af61e1ef1a44f
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3995
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/armv8_dpm.c | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

(limited to 'src/target')

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index ac53460..e5d10df 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -718,14 +718,22 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	cache = arm->core_cache;
 
 	/* read R0 first (it's used for scratch), then CPSR */
-	r = cache->reg_list + 0;
+	r = cache->reg_list + ARMV8_R0;
 	if (!r->valid) {
-		retval = dpmv8_read_reg(dpm, r, 0);
+		retval = dpmv8_read_reg(dpm, r, ARMV8_R0);
 		if (retval != ERROR_OK)
 			goto fail;
 	}
 	r->dirty = true;
 
+	/* read R1, too, it will be clobbered during memory access */
+	r = cache->reg_list + ARMV8_R1;
+	if (!r->valid) {
+		retval = dpmv8_read_reg(dpm, r, ARMV8_R1);
+		if (retval != ERROR_OK)
+			goto fail;
+	}
+
 	/* read cpsr to r0 and get it back */
 	retval = dpm->instr_read_data_r0(dpm,
 			armv8_opcode(armv8, READ_REG_DSPSR), &cpsr);
@@ -735,7 +743,7 @@ int armv8_dpm_read_current_registers(struct arm_dpm *dpm)
 	/* update core mode and state */
 	armv8_set_cpsr(arm, cpsr);
 
-	for (unsigned int i = 1; i < cache->num_regs ; i++) {
+	for (unsigned int i = ARMV8_PC; i < cache->num_regs ; i++) {
 		struct arm_reg *arm_reg;
 
 		r = armv8_reg_current(arm, i);
-- 
cgit v1.1


From 9981093ce076a388700b2f307a9dc79e14a6e0f1 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 17 Feb 2017 16:22:52 +0100
Subject: armv8_dpm: fix exception handling

after handling of an exception in debug state, immediately
restore the original core state.

Change-Id: Ie53b63c9f19815f717f4df4390fbc13f0a204cc2
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/3996
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/armv8_dpm.c | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src/target')

diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index e5d10df..f4e7a07 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -1312,6 +1312,8 @@ void armv8_dpm_handle_exception(struct arm_dpm *dpm)
 	core_state = armv8_dpm_get_core_state(dpm);
 	armv8_select_opcodes(armv8, core_state == ARM_STATE_AARCH64);
 	armv8_select_reg_access(armv8, core_state == ARM_STATE_AARCH64);
+
+	armv8_dpm_modeswitch(dpm, ARM_MODE_ANY);
 }
 
 /*----------------------------------------------------------------------*/
-- 
cgit v1.1


From 6b2acc0243f6dd54823c336ded8c20d16cdc50a3 Mon Sep 17 00:00:00 2001
From: Girts Folkmanis <girtsf@users.noreply.github.com>
Date: Fri, 3 Mar 2017 09:49:58 -0800
Subject: arm_dpm: fix dpm setup

When ARM64 support was being merged, a comparison ended up being
inverted. This causes NULL pointer access when target attempts to
use core cache.

Change-Id: Ic8873ddd13dbdd8100856a71b4717f44cd336e23
Signed-off-by: Girts Folkmanis <opensource@girts.me>
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4042
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/arm_dpm.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/target')

diff --git a/src/target/arm_dpm.c b/src/target/arm_dpm.c
index f15bff7..3e8180c 100644
--- a/src/target/arm_dpm.c
+++ b/src/target/arm_dpm.c
@@ -973,7 +973,7 @@ int arm_dpm_setup(struct arm_dpm *dpm)
 	arm->read_core_reg = arm_dpm_read_core_reg;
 	arm->write_core_reg = arm_dpm_write_core_reg;
 
-	if (arm->core_cache != NULL) {
+	if (arm->core_cache == NULL) {
 		cache = arm_build_reg_cache(target, arm);
 		if (!cache)
 			return ERROR_FAIL;
-- 
cgit v1.1


From 332d66c75a89516750726f1a9d2b65a624037770 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Wed, 1 Mar 2017 16:15:33 +0100
Subject: aarch64: clear CTI halt event early at debug entry

The halt event was left pending in the CTI, better to clear it immediately
after debug entry.

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

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 9977c36..e647bba 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -923,6 +923,8 @@ static int aarch64_debug_entry(struct target *target)
 	if (retval == ERROR_OK)
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval == ERROR_OK)
+		retval = arm_cti_ack_events(armv8->cti, CTI_TRIG(HALT));
 
 	if (retval != ERROR_OK)
 		return retval;
-- 
cgit v1.1


From 518fcd38834ef2f7b19707ccaa52914c2bea9f91 Mon Sep 17 00:00:00 2001
From: Marc Schink <openocd-dev@marcschink.de>
Date: Sun, 8 Jan 2017 20:19:29 +0100
Subject: target: Fix memory leak
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Change-Id: Ib23dfd653d8edacb890a46179e9d437c027d58e8
Signed-off-by: Marc Schink <openocd-dev@marcschink.de>
Reviewed-on: http://openocd.zylin.com/4048
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>
Reviewed-by: Chengyu Zheng <chengyu.zheng@polimi.it>
Reviewed-by: Andreas Färber <afaerber@suse.de>
---
 src/target/target.c | 23 +++++++++++++++++++----
 src/target/target.h |  2 +-
 2 files changed, 20 insertions(+), 5 deletions(-)

(limited to 'src/target')

diff --git a/src/target/target.c b/src/target/target.c
index ee302ee..e04ecc4 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1870,6 +1870,17 @@ int target_free_working_area(struct target *target, struct working_area *area)
 	return target_free_working_area_restore(target, area, 1);
 }
 
+static void target_destroy(struct target *target)
+{
+	if (target->type->deinit_target)
+		target->type->deinit_target(target);
+
+	free(target->type);
+	free(target->trace_info);
+	free(target->cmd_name);
+	free(target);
+}
+
 void target_quit(void)
 {
 	struct target_event_callback *pe = target_event_callbacks;
@@ -1888,11 +1899,15 @@ void target_quit(void)
 	}
 	target_timer_callbacks = NULL;
 
-	for (struct target *target = all_targets;
-	     target; target = target->next) {
-		if (target->type->deinit_target)
-			target->type->deinit_target(target);
+	for (struct target *target = all_targets; target;) {
+		struct target *tmp;
+
+		tmp = target->next;
+		target_destroy(target);
+		target = tmp;
 	}
+
+	all_targets = NULL;
 }
 
 /* free resources and restore memory, if restoring memory fails,
diff --git a/src/target/target.h b/src/target/target.h
index 76630b9..53f9e26 100644
--- a/src/target/target.h
+++ b/src/target/target.h
@@ -125,7 +125,7 @@ enum target_register_class {
 /* target_type.h contains the full definition of struct target_type */
 struct target {
 	struct target_type *type;			/* target type definition (name, access functions) */
-	const char *cmd_name;				/* tcl Name of target */
+	char *cmd_name;				/* tcl Name of target */
 	int target_number;					/* DO NOT USE!  field to be removed in 2010 */
 	struct jtag_tap *tap;				/* where on the jtag chain is this */
 	int32_t coreid;						/* which device on the TAP? */
-- 
cgit v1.1


From 832f5974f2b408a051b0f673ca64453deb960185 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Mon, 20 Feb 2017 22:19:15 +0100
Subject: mips32, pic32 use uint8_t in 8 bit scan function

Makes code shorter.

Change-Id: I6cc01adffbea063ccb071ddf3a3e3d81727b29ce
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4004
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips_ejtag.c | 21 +++++++--------------
 src/target/mips_ejtag.h |  2 +-
 2 files changed, 8 insertions(+), 15 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 594711f..650af3e 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -169,31 +169,24 @@ void mips_ejtag_drscan_32_out(struct mips_ejtag *ejtag_info, uint32_t data)
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 }
 
-int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint32_t *data)
+int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint8_t *data)
 {
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field field;
-	uint8_t t[4] = {0, 0, 0, 0}, r[4];
-	int retval;
-
 	field.num_bits = 8;
-	field.out_value = t;
-	buf_set_u32(t, 0, field.num_bits, *data);
-	field.in_value = r;
+
+	field.out_value = data;
+	field.in_value = data;
 
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 
-	retval = jtag_execute_queue();
+	int retval = jtag_execute_queue();
 	if (retval != ERROR_OK) {
 		LOG_ERROR("register read failed");
 		return retval;
 	}
-
-	*data = buf_get_u32(field.in_value, 0, 32);
-
 	return ERROR_OK;
 }
 
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 6ef0867..2178afc 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -220,7 +220,7 @@ void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info,
 void mips_ejtag_drscan_32_out(struct mips_ejtag *ejtag_info, uint32_t data);
 int mips_ejtag_drscan_32(struct mips_ejtag *ejtag_info, uint32_t *data);
 void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data);
-int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint32_t *data);
+int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint8_t *data);
 int mips_ejtag_fastdata_scan(struct mips_ejtag *ejtag_info, int write_t, uint32_t *data);
 
 int mips_ejtag_init(struct mips_ejtag *ejtag_info);
-- 
cgit v1.1


From d81fc78d9bf6891251cb44eff9e70bd8781a2def Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Mon, 20 Feb 2017 23:05:38 +0100
Subject: mips32, add generic scan 32 function

Will be used later, allow queuing all needed scans in a pracc
access. This makes faster execution with ftdi based adapters
working in sync with pracc.
Added now because the overall code is shorter.

Change-Id: Ib32b89307b75785f88870db8d7c9255dc5bbd426
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4005
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips_ejtag.c | 48 +++++++++++++++++++-----------------------------
 1 file changed, 19 insertions(+), 29 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 650af3e..ebf0d73 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -121,52 +121,42 @@ void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info, uint32_t ctrl, uint32
 	keep_alive();
 }
 
-int mips_ejtag_drscan_32(struct mips_ejtag *ejtag_info, uint32_t *data)
+void mips_ejtag_drscan_32_queued(struct mips_ejtag *ejtag_info, uint32_t data_out, uint8_t *data_in)
 {
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field field;
-	uint8_t t[4], r[4];
-	int retval;
-
 	field.num_bits = 32;
-	field.out_value = t;
-	buf_set_u32(t, 0, field.num_bits, *data);
-	field.in_value = r;
 
+	uint8_t scan_out[4];
+	field.out_value = scan_out;
+	buf_set_u32(scan_out, 0, field.num_bits, data_out);
+
+	field.in_value = data_in;
 	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 
-	retval = jtag_execute_queue();
+	keep_alive();
+}
+
+int mips_ejtag_drscan_32(struct mips_ejtag *ejtag_info, uint32_t *data)
+{
+	uint8_t scan_in[4];
+	mips_ejtag_drscan_32_queued(ejtag_info, *data, scan_in);
+
+	int retval = jtag_execute_queue();
 	if (retval != ERROR_OK) {
 		LOG_ERROR("register read failed");
 		return retval;
 	}
 
-	*data = buf_get_u32(field.in_value, 0, 32);
-
-	keep_alive();
-
+	*data = buf_get_u32(scan_in, 0, 32);
 	return ERROR_OK;
 }
 
 void mips_ejtag_drscan_32_out(struct mips_ejtag *ejtag_info, uint32_t data)
 {
-	uint8_t t[4];
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
-
-	struct scan_field field;
-
-	field.num_bits = 32;
-	field.out_value = t;
-	buf_set_u32(t, 0, field.num_bits, data);
-
-	field.in_value = NULL;
-
-	jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
+	mips_ejtag_drscan_32_queued(ejtag_info, data, NULL);
 }
 
 int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint8_t *data)
-- 
cgit v1.1


From 7ccd53bdde3f3a2fbd957f8b75f38125468c199f Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Mon, 20 Feb 2017 23:48:40 +0100
Subject: mips32, homogenize code in the scan functions

Change-Id: I32fed3332857737048dd12da94fcaba140acb726
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4006
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips_ejtag.c | 33 ++++++++++++++++-----------------
 src/target/mips_ejtag.h |  3 +--
 2 files changed, 17 insertions(+), 19 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index ebf0d73..1fbdf3c 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -28,20 +28,20 @@
 #include "mips_ejtag.h"
 #include "mips32_dmaacc.h"
 
-void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, int new_instr)
+void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, uint32_t new_instr)
 {
-	struct jtag_tap *tap;
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
-	tap = ejtag_info->tap;
-	assert(tap != NULL);
+	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr) {
 
-	if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != (uint32_t)new_instr) {
 		struct scan_field field;
-		uint8_t t[4];
-
 		field.num_bits = tap->ir_length;
+
+		uint8_t t[4];
 		field.out_value = t;
 		buf_set_u32(t, 0, field.num_bits, new_instr);
+
 		field.in_value = NULL;
 
 		jtag_add_ir_scan(tap, &field, TAP_IDLE);
@@ -182,13 +182,12 @@ int mips_ejtag_drscan_8(struct mips_ejtag *ejtag_info, uint8_t *data)
 
 void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data)
 {
-	struct jtag_tap *tap;
-	tap  = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field field;
-
 	field.num_bits = 8;
+
 	field.out_value = &data;
 	field.in_value = NULL;
 
@@ -427,22 +426,22 @@ int mips_ejtag_init(struct mips_ejtag *ejtag_info)
 
 int mips_ejtag_fastdata_scan(struct mips_ejtag *ejtag_info, int write_t, uint32_t *data)
 {
-	struct jtag_tap *tap;
-
-	tap = ejtag_info->tap;
-	assert(tap != NULL);
+	assert(ejtag_info->tap != NULL);
+	struct jtag_tap *tap = ejtag_info->tap;
 
 	struct scan_field fields[2];
-	uint8_t spracc = 0;
-	uint8_t t[4] = {0, 0, 0, 0};
 
 	/* fastdata 1-bit register */
 	fields[0].num_bits = 1;
+
+	uint8_t spracc = 0;
 	fields[0].out_value = &spracc;
 	fields[0].in_value = NULL;
 
 	/* processor access data register 32 bit */
 	fields[1].num_bits = 32;
+
+	uint8_t t[4] = {0, 0, 0, 0};
 	fields[1].out_value = t;
 
 	if (write_t) {
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 2178afc..3043df9 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -210,8 +210,7 @@ struct mips_ejtag {
 	uint32_t ejtag_dba_step_size;	/* size of step till next *DBAn register. */
 };
 
-void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info,
-		int new_instr);
+void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, uint32_t new_instr);
 int mips_ejtag_enter_debug(struct mips_ejtag *ejtag_info);
 int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info);
 int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info, uint32_t *idcode);
-- 
cgit v1.1


From 09ebc1afadcc710e3d0f37bdaba3d76a1487d306 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Wed, 22 Feb 2017 23:14:36 +0100
Subject: mips32, drop unnecessary code in mips32_pracc.c

Struct mips32_pracc_context no more in use.
In current code cp0 reg/sel do not requires special handling.
In sync mode ctx.store_count not used, drop check.
In fasdata transfer function use mips32_pracc_read_ctrl_addr()
to reduce code.

Change-Id: Ibd4cfa5a44ebc106ed0db042f4e54a2e0b3d43cb
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4007
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 72 ++++++++---------------------------------------
 1 file changed, 12 insertions(+), 60 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 5456e8c..69d9a00 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -73,16 +73,6 @@
 #include "mips32.h"
 #include "mips32_pracc.h"
 
-struct mips32_pracc_context {
-	uint32_t *local_oparam;
-	int num_oparam;
-	const uint32_t *code;
-	int code_len;
-	uint32_t stack[32];
-	int stack_offset;
-	struct mips_ejtag *ejtag_info;
-};
-
 static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
 {
 	uint32_t ejtag_ctrl;
@@ -564,10 +554,10 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_r
 		goto exit;
 
 	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, 0, 0) | (cp0_reg << 11) | cp0_sel);	/* move COP0 [cp0_reg select] to $8 */
+	pracc_add(&ctx, 0, MIPS32_MFC0(8, cp0_reg, cp0_sel));			/* move cp0 reg / sel to $8 */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave */
 	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits  of $8 */
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 bits of $8 */
@@ -576,23 +566,6 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_r
 exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
-
-	/**
-	 * Note that our input parametes cp0_reg and cp0_sel
-	 * are numbers (not gprs) which make part of mfc0 instruction opcode.
-	 *
-	 * These are not fix, but can be different for each mips32_cp0_read() function call,
-	 * and that is why we must insert them directly into opcode,
-	 * i.e. we can not pass it on EJTAG microprogram stack (via param_in),
-	 * and put them into the gprs later from MIPS32_PRACC_STACK
-	 * because mfc0 do not use gpr as a parameter for the cp0_reg and select part,
-	 * but plain (immediate) number.
-	 *
-	 * MIPS32_MTC0 is implemented via MIPS32_R_INST macro.
-	 * In order to insert our parameters, we must change rd and funct fields.
-	 *
-	 * code[2] |= (cp0_reg << 11) | cp0_sel;   change rd and funct of MIPS32_R_INST macro
-	 **/
 }
 
 int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel)
@@ -602,24 +575,18 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 	if (ctx.retval != ERROR_OK)
 		goto exit;
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, UPPER16(val)));				/* Load val to $15 */
+	pracc_add(&ctx, 0, MIPS32_LUI(15, UPPER16(val)));			/* Load val to $15 */
 	pracc_add(&ctx, 0, MIPS32_ORI(15, 15, LOWER16(val)));
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, 0, 0) | (cp0_reg << 11) | cp0_sel);	/* write cp0 reg / sel */
+	pracc_add(&ctx, 0, MIPS32_MTC0(15, cp0_reg, cp0_sel));			/* write $15 to cp0 reg / sel */
 
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
 exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
-
-	/**
-	 * Note that MIPS32_MTC0 macro is implemented via MIPS32_R_INST macro.
-	 * In order to insert our parameters, we must change rd and funct fields.
-	 * code[3] |= (cp0_reg << 11) | cp0_sel;   change rd and funct fields of MIPS32_R_INST macro
-	 **/
 }
 
 /**
@@ -956,9 +923,6 @@ int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));					/* load $15 in DeSave */
 
-	if (ejtag_info->mode == 0)
-		ctx.store_count++;	/* Needed by legacy code, due to offset from reg0 */
-
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, regs);
 
 	ejtag_info->reg8 = regs[8];	/* reg8 is saved but not restored, next called function should restore it */
@@ -1015,7 +979,7 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	};
 
 	int retval, i;
-	uint32_t val, ejtag_ctrl, address;
+	uint32_t val, ejtag_ctrl;
 
 	if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
@@ -1054,19 +1018,13 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		mips_ejtag_drscan_32_out(ejtag_info, ejtag_ctrl);
 	}
 
-	/* wait PrAcc pending bit for FASTDATA write */
-	retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
+	/* wait PrAcc pending bit for FASTDATA write, read address */
+	retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* next fetch to dmseg should be in FASTDATA_AREA, check */
-	address = 0;
-	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
-	retval = mips_ejtag_drscan_32(ejtag_info, &address);
-	if (retval != ERROR_OK)
-		return retval;
-
-	if (address != MIPS32_PRACC_FASTDATA_AREA)
+	if (ejtag_info->pa_addr != MIPS32_PRACC_FASTDATA_AREA)
 		return ERROR_FAIL;
 
 	/* Send the load start address */
@@ -1100,17 +1058,11 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		return retval;
 	}
 
-	retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
-	if (retval != ERROR_OK)
-		return retval;
-
-	address = 0;
-	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
-	retval = mips_ejtag_drscan_32(ejtag_info, &address);
+	retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
-	if (address != MIPS32_PRACC_TEXT)
+	if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT)
 		LOG_ERROR("mini program did not return to start");
 
 	return retval;
-- 
cgit v1.1


From 5fdcbbdb25d19a36e3fed27edb21a805eb34399f Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Thu, 23 Feb 2017 15:14:46 +0100
Subject: mips32, mips32_pracc_finish() queued only

In most of the cases there is no need to request execution,
the check for a new pracc access already does it.
Requesting execution if not needed makes execution slower and
code larger due the additional checks.
Reduce code in fasdata transfer function.
Call for execution when exiting debug.

Change-Id: I3b45f6d1f62da5fad3e3db84f82a9299b16e1bd9
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4010
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 27 ++++++++-------------------
 1 file changed, 8 insertions(+), 19 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 69d9a00..caa566b 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -116,24 +116,21 @@ static int mips32_pracc_read_ctrl_addr(struct mips_ejtag *ejtag_info)
 }
 
 /* Finish processor access */
-static int mips32_pracc_finish(struct mips_ejtag *ejtag_info)
+static void mips32_pracc_finish(struct mips_ejtag *ejtag_info)
 {
 	uint32_t ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 	mips_ejtag_drscan_32_out(ejtag_info, ctrl);
-
-	return jtag_execute_queue();
 }
 
 int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 {
 	uint32_t jt_code = MIPS32_J((0x0FFFFFFF & MIPS32_PRACC_TEXT) >> 2);
-	int retval;
 
 	/* do 3 0/nops to clean pipeline before a jump to pracc text, NOP in delay slot */
 	for (int i = 0; i != 5; i++) {
 		/* Wait for pracc */
-		retval = wait_for_pracc_rw(ejtag_info, &ejtag_info->pa_ctrl);
+		int retval = wait_for_pracc_rw(ejtag_info, &ejtag_info->pa_ctrl);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -143,25 +140,21 @@ int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 		mips_ejtag_drscan_32_out(ejtag_info, data);
 
 		/* finish pa */
-		retval = mips32_pracc_finish(ejtag_info);
-		if (retval != ERROR_OK)
-			return retval;
+		mips32_pracc_finish(ejtag_info);
 	}
 
 	if (ejtag_info->mode != 0)	/* async mode support only for MIPS ... */
 		return ERROR_OK;
 
 	for (int i = 0; i != 2; i++) {
-		retval = mips32_pracc_read_ctrl_addr(ejtag_info);
+		int retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 		if (retval != ERROR_OK)
 			return retval;
 
 		if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT) {		/* LEXRA/BMIPS ?, shift out another NOP, max 2 */
 			mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
 			mips_ejtag_drscan_32_out(ejtag_info, MIPS32_NOP);
-			retval = mips32_pracc_finish(ejtag_info);
-			if (retval != ERROR_OK)
-				return retval;
+			mips32_pracc_finish(ejtag_info);
 		} else
 			break;
 	}
@@ -298,12 +291,10 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 			mips_ejtag_drscan_32_out(ejtag_info, instr);
 		}
 		/* finish processor access, let the processor eat! */
-		retval = mips32_pracc_finish(ejtag_info);
-		if (retval != ERROR_OK)
-			return retval;
+		mips32_pracc_finish(ejtag_info);
 
 		if (instr == MIPS32_DRET)	/* after leaving debug mode nothing to do */
-			return ERROR_OK;
+			return jtag_execute_queue();
 
 		if (store_pending == 0 && pass) {	/* store access done, but after passing pracc text */
 			LOG_DEBUG("warning: store access pass pracc text");
@@ -1013,9 +1004,7 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		mips_ejtag_drscan_32_out(ejtag_info, jmp_code[i]);
 
 		/* Clear the access pending bit (let the processor eat!) */
-		ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
-		mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
-		mips_ejtag_drscan_32_out(ejtag_info, ejtag_ctrl);
+		mips32_pracc_finish(ejtag_info);
 	}
 
 	/* wait PrAcc pending bit for FASTDATA write, read address */
-- 
cgit v1.1


From 9bdc3bf0a817e27879c6d2758d70ee274fe7dcb7 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Thu, 23 Feb 2017 22:55:34 +0100
Subject: mips32, in wait_for_pracc_rw() use ejtag_info->pa_ctrl

Makes code shorter
In fasdata transfer fuction declare variables locally.
Avoid cast.

Change-Id: I0367b66339560fc20521a0598488e7ff9076808e
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4011
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 34 ++++++++++++++--------------------
 1 file changed, 14 insertions(+), 20 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index caa566b..4ca92ae 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -73,21 +73,20 @@
 #include "mips32.h"
 #include "mips32_pracc.h"
 
-static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
+static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info)
 {
-	uint32_t ejtag_ctrl;
 	int64_t then = timeval_ms();
 
 	/* wait for the PrAcc to become "1" */
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
 
 	while (1) {
-		ejtag_ctrl = ejtag_info->ejtag_ctrl;
-		int retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
+		ejtag_info->pa_ctrl = ejtag_info->ejtag_ctrl;
+		int retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_info->pa_ctrl);
 		if (retval != ERROR_OK)
 			return retval;
 
-		if (ejtag_ctrl & EJTAG_CTRL_PRACC)
+		if (ejtag_info->pa_ctrl & EJTAG_CTRL_PRACC)
 			break;
 
 		int64_t timeout = timeval_ms() - then;
@@ -97,22 +96,20 @@ static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
 		}
 	}
 
-	*ctrl = ejtag_ctrl;
 	return ERROR_OK;
 }
 
 /* Shift in control and address for a new processor access, save them in ejtag_info */
 static int mips32_pracc_read_ctrl_addr(struct mips_ejtag *ejtag_info)
 {
-	int retval = wait_for_pracc_rw(ejtag_info, &ejtag_info->pa_ctrl);
+	int retval = wait_for_pracc_rw(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
-	ejtag_info->pa_addr = 0;
-	retval = mips_ejtag_drscan_32(ejtag_info, &ejtag_info->pa_addr);
 
-	return retval;
+	ejtag_info->pa_addr = 0;
+	return  mips_ejtag_drscan_32(ejtag_info, &ejtag_info->pa_addr);
 }
 
 /* Finish processor access */
@@ -130,7 +127,7 @@ int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 	/* do 3 0/nops to clean pipeline before a jump to pracc text, NOP in delay slot */
 	for (int i = 0; i != 5; i++) {
 		/* Wait for pracc */
-		int retval = wait_for_pracc_rw(ejtag_info, &ejtag_info->pa_ctrl);
+		int retval = wait_for_pracc_rw(ejtag_info);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -969,9 +966,6 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		MIPS32_NOP,
 	};
 
-	int retval, i;
-	uint32_t val, ejtag_ctrl;
-
 	if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
@@ -995,8 +989,8 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	jmp_code[0] |= UPPER16(source->address);
 	jmp_code[1] |= LOWER16(source->address);
 
-	for (i = 0; i < (int) ARRAY_SIZE(jmp_code); i++) {
-		retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
+	for (unsigned i = 0; i < ARRAY_SIZE(jmp_code); i++) {
+		int retval = wait_for_pracc_rw(ejtag_info);
 		if (retval != ERROR_OK)
 			return retval;
 
@@ -1008,7 +1002,7 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	}
 
 	/* wait PrAcc pending bit for FASTDATA write, read address */
-	retval = mips32_pracc_read_ctrl_addr(ejtag_info);
+	int retval = mips32_pracc_read_ctrl_addr(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1017,11 +1011,11 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		return ERROR_FAIL;
 
 	/* Send the load start address */
-	val = addr;
+	uint32_t val = addr;
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
 	mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
 
-	retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl);
+	retval = wait_for_pracc_rw(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -1034,7 +1028,7 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	if (ejtag_info->mode != 0)
 		num_clocks = ((uint64_t)(ejtag_info->scan_delay) * jtag_get_speed_khz() + 500000) / 1000000;
 
-	for (i = 0; i < count; i++) {
+	for (int i = 0; i < count; i++) {
 		jtag_add_clocks(num_clocks);
 		retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++);
 		if (retval != ERROR_OK)
-- 
cgit v1.1


From bff6205bab5531c92a269c1dab88ca535cdb4660 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Fri, 24 Feb 2017 16:44:12 +0100
Subject: mips32, implement assembler li instruction

Implement it as a function, the code was already in. Added optimize
option.

Change-Id: Ib9ad3f00d6c4f0b91c4e4960a50ec8d102f4e333
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4017
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 64 ++++++++++++++++++++---------------------------
 1 file changed, 27 insertions(+), 37 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 4ca92ae..6da1843 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -321,6 +321,18 @@ inline void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t inst
 		ctx->store_count++;
 }
 
+inline void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize)
+{
+	if (LOWER16(data) == 0 && optimize)
+		pracc_add(ctx, 0, MIPS32_LUI(reg_num, UPPER16(data)));	/* load only upper value */
+	else if (UPPER16(data) == 0 && optimize)
+		pracc_add(ctx, 0, MIPS32_ORI(reg_num, 0, LOWER16(data)));	/* load only lower */
+	else {
+		pracc_add(ctx, 0, MIPS32_LUI(reg_num, UPPER16(data)));		/* load upper and lower */
+		pracc_add(ctx, 0, MIPS32_ORI(reg_num, reg_num, LOWER16(data)));
+	}
+}
+
 inline void pracc_queue_free(struct pracc_queue_info *ctx)
 {
 	if (ctx->code_count > ctx->max_code)	/* Only for internal check, will be erased */
@@ -438,8 +450,7 @@ int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t
 	pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 8));				/* lw $8, LOWER16(addr)($8) */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* sw $8,PRACC_OUT_OFFSET($15) */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 of $8 */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 of $8 */
+	pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
 
@@ -499,10 +510,8 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 					  MIPS32_SW(8, PRACC_OUT_OFFSET + i * 4, 15));		/* store $8 at param out */
 			addr += size;
 		}
-		pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits of reg 8 */
-		pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of reg 8 */
-		pracc_add(&ctx, 0, MIPS32_LUI(9, UPPER16(ejtag_info->reg9)));		/* restore upper 16 bits of reg 9 */
-		pracc_add(&ctx, 0, MIPS32_ORI(9, 9, LOWER16(ejtag_info->reg9)));	/* restore lower 16 bits of reg 9 */
+		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
+		pracc_add_li32(&ctx, 9, ejtag_info->reg9, 0);				/* restore $9 */
 
 		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
 		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave */
@@ -563,11 +572,9 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 	if (ctx.retval != ERROR_OK)
 		goto exit;
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, UPPER16(val)));			/* Load val to $15 */
-	pracc_add(&ctx, 0, MIPS32_ORI(15, 15, LOWER16(val)));
+	pracc_add_li32(&ctx, 15, val, 0);				/* Load val to $15 */
 
 	pracc_add(&ctx, 0, MIPS32_MTC0(15, cp0_reg, cp0_sel));			/* write $15 to cp0 reg / sel */
-
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
@@ -621,8 +628,8 @@ static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
 
-		pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));			/* restore upper 16 bits  of $8 */
-		pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 bits of $8 */
+		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
+
 		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
 
@@ -732,15 +739,8 @@ static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 				last_upper_base_addr = upper_base_addr;
 			}
 
-			if (size == 4) {			/* for word writes check if one half word is 0 and load it accordingly */
-				if (LOWER16(*buf32) == 0)
-					pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(*buf32)));		/* load only upper value */
-				else if (UPPER16(*buf32) == 0)
-						pracc_add(&ctx, 0, MIPS32_ORI(8, 0, LOWER16(*buf32)));	/* load only lower */
-				else {
-					pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(*buf32)));		/* load upper and lower */
-					pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(*buf32)));
-				}
+			if (size == 4) {
+				pracc_add_li32(&ctx, 8, *buf32, 1);			/* load with li32, optimize */
 				pracc_add(&ctx, 0, MIPS32_SW(8, LOWER16(addr), 15));		/* store word to memory */
 				buf32++;
 
@@ -757,8 +757,7 @@ static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 			addr += size;
 		}
 
-		pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits of reg 8 */
-		pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of reg 8 */
+		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 
 		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
 		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
@@ -844,26 +843,17 @@ int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 	if (ctx.retval != ERROR_OK)
 		goto exit;
 
-	/* load registers 2 to 31 with lui and ori instructions, check if some instructions can be saved */
-	for (int i = 2; i < 32; i++) {
-		if (LOWER16((regs[i])) == 0)					/* if lower half word is 0, lui instruction only */
-			pracc_add(&ctx, 0, MIPS32_LUI(i, UPPER16((regs[i]))));
-		else if (UPPER16((regs[i])) == 0)					/* if upper half word is 0, ori with $0 only*/
-			pracc_add(&ctx, 0, MIPS32_ORI(i, 0, LOWER16((regs[i]))));
-		else {									/* default, load with lui and ori instructions */
-			pracc_add(&ctx, 0, MIPS32_LUI(i, UPPER16((regs[i]))));
-			pracc_add(&ctx, 0, MIPS32_ORI(i, i, LOWER16((regs[i]))));
-		}
-	}
+	/* load registers 2 to 31 with li32, optimize */
+	for (int i = 2; i < 32; i++)
+		pracc_add_li32(&ctx, i, regs[i], 1);
 
 	for (int i = 0; i != 6; i++) {
-		pracc_add(&ctx, 0, MIPS32_LUI(1, UPPER16((regs[i + 32]))));		/* load CPO value in $1, with lui and ori */
-		pracc_add(&ctx, 0, MIPS32_ORI(1, 1, LOWER16((regs[i + 32]))));
-		pracc_add(&ctx, 0, cp0_write_code[i]);					/* write value from $1 to CPO register */
+		pracc_add_li32(&ctx, 1, regs[i + 32], 0);	/* load CPO value in $1 */
+		pracc_add(&ctx, 0, cp0_write_code[i]);			/* write value from $1 to CPO register */
 	}
 	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));				/* load $15 in DeSave */
 	pracc_add(&ctx, 0, MIPS32_LUI(1, UPPER16((regs[1]))));			/* load upper half word in $1 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_ORI(1, 1, LOWER16((regs[1]))));		/* load lower half word in $1 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
-- 
cgit v1.1


From cb317eabf2d162365467aeb89b564828e5e6d6f3 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Fri, 24 Feb 2017 23:39:39 +0100
Subject: mips32, write handler code in a more compact way

Less code and probably cleaner.
Don't check if it is ever ERROR_OK.

Change-Id: I1045b58fd4542ec24430332f49679364ae97b1dc
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4018
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 46 +++++++++++++++++-----------------------------
 1 file changed, 17 insertions(+), 29 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 6da1843..45d2242 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -921,7 +921,6 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		int write_t, uint32_t addr, int count, uint32_t *buf)
 {
 	uint32_t handler_code[] = {
-		/* caution when editing, table is modified below */
 		/* r15 points to the start of this code */
 		MIPS32_SW(8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
 		MIPS32_SW(9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
@@ -930,13 +929,14 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		/* start of fastdata area in t0 */
 		MIPS32_LUI(8, UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
 		MIPS32_ORI(8, 8, LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
-		MIPS32_LW(9, 0, 8),								/* start addr in t1 */
-		MIPS32_LW(10, 0, 8),							/* end addr to t2 */
-														/* loop: */
-		/* 8 */ MIPS32_LW(11, 0, 0),					/* lw t3,[t8 | r9] */
-		/* 9 */ MIPS32_SW(11, 0, 0),					/* sw t3,[r9 | r8] */
-		MIPS32_BNE(10, 9, NEG16(3)),					/* bne $t2,t1,loop */
-		MIPS32_ADDI(9, 9, 4),							/* addi t1,t1,4 */
+		MIPS32_LW(9, 0, 8),						/* start addr in t1 */
+		MIPS32_LW(10, 0, 8),						/* end addr to t2 */
+					/* loop: */
+		write_t ? MIPS32_LW(11, 0, 8) : MIPS32_LW(11, 0, 9),	/* from xfer area : from memory */
+		write_t ? MIPS32_SW(11, 0, 9) : MIPS32_SW(11, 0, 8),	/* to memory      : to xfer area */
+
+		MIPS32_BNE(10, 9, NEG16(3)),			/* bne $t2,t1,loop */
+		MIPS32_ADDI(9, 9, 4),					/* addi t1,t1,4 */
 
 		MIPS32_LW(8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
 		MIPS32_LW(9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
@@ -949,25 +949,10 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 		MIPS32_MFC0(15, 31, 0),						/* move COP0 DeSave to $15 */
 	};
 
-	uint32_t jmp_code[] = {
-		/* 0 */ MIPS32_LUI(15, 0),		/* addr of working area added below */
-		/* 1 */ MIPS32_ORI(15, 15, 0),	/* addr of working area added below */
-		MIPS32_JR(15),					/* jump to ram program */
-		MIPS32_NOP,
-	};
-
 	if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
-	if (write_t) {
-		handler_code[8] = MIPS32_LW(11, 0, 8);	/* load data from probe at fastdata area */
-		handler_code[9] = MIPS32_SW(11, 0, 9);	/* store data to RAM @ r9 */
-	} else {
-		handler_code[8] = MIPS32_LW(11, 0, 9);	/* load data from RAM @ r9 */
-		handler_code[9] = MIPS32_SW(11, 0, 8);	/* store data to probe at fastdata area */
-	}
-
-	/* write program into RAM */
+		/* write program into RAM */
 	if (write_t != ejtag_info->fast_access_save) {
 		mips32_pracc_write_mem(ejtag_info, source->address, 4, ARRAY_SIZE(handler_code), handler_code);
 		/* save previous operation to speed to any consecutive read/writes */
@@ -976,9 +961,14 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 
 	LOG_DEBUG("%s using 0x%.8" TARGET_PRIxADDR " for write handler", __func__, source->address);
 
-	jmp_code[0] |= UPPER16(source->address);
-	jmp_code[1] |= LOWER16(source->address);
+	uint32_t jmp_code[] = {
+		MIPS32_LUI(15, UPPER16(source->address)),		/* load addr of jump in $15 */
+		MIPS32_ORI(15, 15, LOWER16(source->address)),
+		MIPS32_JR(15),					/* jump to ram program */
+		MIPS32_NOP,
+	};
 
+	/* execute jump code, with no address check */
 	for (unsigned i = 0; i < ARRAY_SIZE(jmp_code); i++) {
 		int retval = wait_for_pracc_rw(ejtag_info);
 		if (retval != ERROR_OK)
@@ -1020,9 +1010,7 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 
 	for (int i = 0; i < count; i++) {
 		jtag_add_clocks(num_clocks);
-		retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++);
-		if (retval != ERROR_OK)
-			return retval;
+		mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++);
 	}
 
 	retval = jtag_execute_queue();
-- 
cgit v1.1


From 3414daed26c87a41ba66f0fef6e6292564bb5111 Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Tue, 2 May 2017 16:47:21 +0200
Subject: Fix compile failure on MacOSX

MacOSX tool chain defines __unused in "sys/cdefs.h", causing a collision.
Remove the local define to avoid polluting the compilers internal
symbol namespace.

Change-Id: I16370c4518e6aeec482dd689e7db80628f846ee3
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Reviewed-on: http://openocd.zylin.com/4118
Reviewed-by: Steven Stallion <sstallion@gmail.com>
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
Reviewed-by: Andreas Fritiofson <andreas.fritiofson@gmail.com>
Tested-by: jenkins
---
 src/target/aarch64.c | 2 --
 src/target/armv8.c   | 8 +++-----
 2 files changed, 3 insertions(+), 7 deletions(-)

(limited to 'src/target')

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index e647bba..5e5d3fc 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -30,8 +30,6 @@
 #include "armv8_cache.h"
 #include <helper/time_support.h>
 
-#define __unused __attribute((unused))
-
 enum restart_mode {
 	RESTART_LAZY,
 	RESTART_SYNC,
diff --git a/src/target/armv8.c b/src/target/armv8.c
index db7a871..df5e251 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -37,8 +37,6 @@
 #include "target.h"
 #include "target_type.h"
 
-#define __unused __attribute__((unused))
-
 static const char * const armv8_state_strings[] = {
 	"AArch32", "Thumb", "Jazelle", "ThumbEE", "AArch64",
 };
@@ -610,7 +608,7 @@ done:
 	/* (void) */ dpm->finish(dpm);
 }
 
-static __unused void armv8_show_fault_registers(struct target *target)
+static __attribute__((unused)) void armv8_show_fault_registers(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 
@@ -647,7 +645,7 @@ static uint8_t armv8_pa_size(uint32_t ps)
 	return ret;
 }
 
-static __unused int armv8_read_ttbcr32(struct target *target)
+static __attribute__((unused)) int armv8_read_ttbcr32(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
@@ -686,7 +684,7 @@ done:
 	return retval;
 }
 
-static __unused int armv8_read_ttbcr(struct target *target)
+static __attribute__((unused)) int armv8_read_ttbcr(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
-- 
cgit v1.1


From c8b31aaa155be4361c090b369bc73f0f87751154 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Sun, 26 Feb 2017 09:11:02 +0100
Subject: mips32, change in pracc_list for dynamic allocation

pracc_list points to an array with code in the lower half
and addr in the upper half. Change it to a struct with
an instruction field and an address field.
Requiered to make reallocation easier.
As a side effect the code is less quirky.

Change-Id: Ibf904a33a2f35a7f69284d2a2114f4b4ae79219f
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4019
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 78 ++++++++++++++++++++++-------------------------
 src/target/mips32_pracc.h |  7 ++++-
 src/target/mips_ejtag.c   |  4 +--
 3 files changed, 45 insertions(+), 44 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 45d2242..c55688b 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -235,18 +235,17 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 						restart = 1;
 						continue;
 					}
-
 					return ERROR_JTAG_DEVICE_ERROR;
 				}
 				/* check for store instruction at dmseg */
-				uint32_t store_addr = ctx->pracc_list[ctx->max_code + code_count];
+				uint32_t store_addr = ctx->pracc_list[code_count].addr;
 				if (store_addr != 0) {
 					if (store_addr > max_store_addr)
 						max_store_addr = store_addr;
 					store_pending++;
 				}
 
-				instr = ctx->pracc_list[code_count++];
+				instr = ctx->pracc_list[code_count++].instr;
 				if (code_count == ctx->code_count)	/* last instruction, start final check */
 					final_check = 1;
 
@@ -306,7 +305,7 @@ inline void pracc_queue_init(struct pracc_queue_info *ctx)
 	ctx->code_count = 0;
 	ctx->store_count = 0;
 
-	ctx->pracc_list = malloc(2 * ctx->max_code * sizeof(uint32_t));
+	ctx->pracc_list = malloc(ctx->max_code * sizeof(pa_list));
 	if (ctx->pracc_list == NULL) {
 		LOG_ERROR("Out of memory");
 		ctx->retval = ERROR_FAIL;
@@ -315,8 +314,8 @@ inline void pracc_queue_init(struct pracc_queue_info *ctx)
 
 inline void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
 {
-	ctx->pracc_list[ctx->max_code + ctx->code_count] = addr;
-	ctx->pracc_list[ctx->code_count++] = instr;
+	ctx->pracc_list[ctx->code_count].instr = instr;
+	ctx->pracc_list[ctx->code_count++].addr = addr;
 	if (addr)
 		ctx->store_count++;
 }
@@ -367,16 +366,16 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ALL);
 
 	int scan_count = 0;
-	for (int i = 0; i != 2 * ctx->code_count; i++) {
-		uint32_t data = 0;
-		if (i & 1u) {			/* Check store address from previous instruction, if not the first */
-			if (i < 2 || 0 == ctx->pracc_list[ctx->max_code + (i / 2) - 1])
-				continue;
-		} else
-			data = ctx->pracc_list[i / 2];
-
+	for (int i = 0; i != ctx->code_count; i++) {
 		jtag_add_clocks(num_clocks);
-		mips_ejtag_add_scan_96(ejtag_info, ejtag_ctrl, data, scan_in[scan_count++].scan_96);
+		mips_ejtag_add_scan_96(ejtag_info, ejtag_ctrl, ctx->pracc_list[i].instr,
+				       scan_in[scan_count++].scan_96);
+
+		/* Check store address from previous instruction, if not the first */
+		if (i > 0 && ctx->pracc_list[i - 1].addr) {
+			jtag_add_clocks(num_clocks);
+			mips_ejtag_add_scan_96(ejtag_info, ejtag_ctrl, 0, scan_in[scan_count++].scan_96);
+		}
 	}
 
 	int retval = jtag_execute_queue();		/* execute queued scans */
@@ -385,24 +384,35 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 
 	uint32_t fetch_addr = MIPS32_PRACC_TEXT;		/* start address */
 	scan_count = 0;
-	for (int i = 0; i != 2 * ctx->code_count; i++) {				/* verify every pracc access */
-		uint32_t store_addr = 0;
-		if (i & 1u) {			/* Read store addres from previous instruction, if not the first */
-			store_addr = ctx->pracc_list[ctx->max_code + (i / 2) - 1];
-			if (i < 2 || 0 == store_addr)
-				continue;
-		}
-
+	for (int i = 0; i != ctx->code_count; i++) {				/* verify every pracc access */
+		/* check pracc bit */
 		ejtag_ctrl = buf_get_u32(scan_in[scan_count].scan_32.ctrl, 0, 32);
+		uint32_t addr = buf_get_u32(scan_in[scan_count].scan_32.addr, 0, 32);
 		if (!(ejtag_ctrl & EJTAG_CTRL_PRACC)) {
 			LOG_ERROR("Error: access not pending  count: %d", scan_count);
 			retval = ERROR_FAIL;
 			goto exit;
 		}
+		if (ejtag_ctrl & EJTAG_CTRL_PRNW) {
+			LOG_ERROR("Not a fetch/read access, count: %d", scan_count);
+			retval = ERROR_FAIL;
+			goto exit;
+		}
+		if (addr != fetch_addr) {
+			LOG_ERROR("Fetch addr mismatch, read: %" PRIx32 " expected: %" PRIx32 " count: %d",
+					  addr, fetch_addr, scan_count);
+			retval = ERROR_FAIL;
+			goto exit;
+		}
+		fetch_addr += 4;
+		scan_count++;
 
-		uint32_t addr = buf_get_u32(scan_in[scan_count].scan_32.addr, 0, 32);
+		/* check if previous intrucction is a store instruction at dmesg */
+		if (i > 0 && ctx->pracc_list[i - 1].addr) {
+			uint32_t store_addr = ctx->pracc_list[i - 1].addr;
+			ejtag_ctrl = buf_get_u32(scan_in[scan_count].scan_32.ctrl, 0, 32);
+			addr = buf_get_u32(scan_in[scan_count].scan_32.addr, 0, 32);
 
-		if (store_addr != 0) {
 			if (!(ejtag_ctrl & EJTAG_CTRL_PRNW)) {
 				LOG_ERROR("Not a store/write access, count: %d", scan_count);
 				retval = ERROR_FAIL;
@@ -410,28 +420,14 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 			}
 			if (addr != store_addr) {
 				LOG_ERROR("Store address mismatch, read: %" PRIx32 " expected: %" PRIx32 " count: %d",
-						addr, store_addr, scan_count);
+							      addr, store_addr, scan_count);
 				retval = ERROR_FAIL;
 				goto exit;
 			}
 			int buf_index = (addr - MIPS32_PRACC_PARAM_OUT) / 4;
 			buf[buf_index] = buf_get_u32(scan_in[scan_count].scan_32.data, 0, 32);
-
-		} else {
-			if (ejtag_ctrl & EJTAG_CTRL_PRNW) {
-				LOG_ERROR("Not a fetch/read access, count: %d", scan_count);
-				retval = ERROR_FAIL;
-				goto exit;
-			}
-			if (addr != fetch_addr) {
-				LOG_ERROR("Fetch addr mismatch, read: %" PRIx32 " expected: %" PRIx32 " count: %d",
-					  addr, fetch_addr, scan_count);
-				retval = ERROR_FAIL;
-				goto exit;
-			}
-			fetch_addr += 4;
+			scan_count++;
 		}
-		scan_count++;
 	}
 exit:
 	free(scan_in);
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index 2ede5b2..166cbb4 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -42,12 +42,17 @@
 #define NEG16(v)						(((~(v)) + 1) & 0xFFFF)
 /*#define NEG18(v) (((~(v)) + 1) & 0x3FFFF)*/
 
+typedef struct {
+	uint32_t instr;
+	uint32_t addr;
+} pa_list;
+
 struct pracc_queue_info {
 	int retval;
 	const int max_code;
 	int code_count;
 	int store_count;
-	uint32_t *pracc_list;	/* Code and store addresses */
+	pa_list *pracc_list;	/* Code and store addresses at dmseg */
 };
 void pracc_queue_init(struct pracc_queue_info *ctx);
 void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr);
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 1fbdf3c..943a868 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -272,8 +272,8 @@ error:
 
 int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info)
 {
-	uint32_t pracc_list[] = {MIPS32_DRET, 0};
-	struct pracc_queue_info ctx = {.max_code = 1, .pracc_list = pracc_list, .code_count = 1, .store_count = 0};
+	pa_list pracc_list = {.instr = MIPS32_DRET, .addr = 0};
+	struct pracc_queue_info ctx = {.max_code = 1, .pracc_list = &pracc_list, .code_count = 1, .store_count = 0};
 
 	/* execute our dret instruction */
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
-- 
cgit v1.1


From 1392c27cf9ff1a7f5f5477d23880f2e5c59c898e Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Sun, 26 Feb 2017 20:35:01 +0100
Subject: mips32, add realloc code

If max_code is reached realloc memory. If fails to realloc
the error is propagated and every call to pracc_add() returns
immediately. The exec function logs the error.

Change-Id: Idd4ed9d9b8b19b7d6842d0bc5ebb05f943726705
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4020
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 69 +++++++++++++++++++++--------------------------
 src/target/mips32_pracc.h |  5 +++-
 src/target/mips_ejtag.c   |  5 +---
 3 files changed, 35 insertions(+), 44 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index c55688b..da1cee1 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -304,23 +304,31 @@ inline void pracc_queue_init(struct pracc_queue_info *ctx)
 	ctx->retval = ERROR_OK;
 	ctx->code_count = 0;
 	ctx->store_count = 0;
-
-	ctx->pracc_list = malloc(ctx->max_code * sizeof(pa_list));
-	if (ctx->pracc_list == NULL) {
-		LOG_ERROR("Out of memory");
-		ctx->retval = ERROR_FAIL;
-	}
+	ctx->max_code = 0;
+	ctx->pracc_list = NULL;
 }
 
-inline void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
+void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
 {
+	if (ctx->retval != ERROR_OK)	/* On previous out of memory, return */
+		return;
+	if (ctx->code_count == ctx->max_code) {
+		void *p = realloc(ctx->pracc_list, sizeof(pa_list) * (ctx->max_code + PRACC_BLOCK));
+		if (p) {
+			ctx->max_code += PRACC_BLOCK;
+			ctx->pracc_list = p;
+		} else {
+			ctx->retval = ERROR_FAIL;	/* Out of memory */
+			return;
+		}
+	}
 	ctx->pracc_list[ctx->code_count].instr = instr;
 	ctx->pracc_list[ctx->code_count++].addr = addr;
 	if (addr)
 		ctx->store_count++;
 }
 
-inline void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize)
+void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize)
 {
 	if (LOWER16(data) == 0 && optimize)
 		pracc_add(ctx, 0, MIPS32_LUI(reg_num, UPPER16(data)));	/* load only upper value */
@@ -334,14 +342,17 @@ inline void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint3
 
 inline void pracc_queue_free(struct pracc_queue_info *ctx)
 {
-	if (ctx->code_count > ctx->max_code)	/* Only for internal check, will be erased */
-		LOG_ERROR("Internal error, code count: %d > max code: %d", ctx->code_count, ctx->max_code);
 	if (ctx->pracc_list != NULL)
 		free(ctx->pracc_list);
 }
 
 int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *buf)
 {
+	if (ctx->retval != ERROR_OK) {
+		LOG_ERROR("Out of memory");
+		return ERROR_FAIL;
+	}
+
 	if (ejtag_info->mode == 0)
 		return mips32_pracc_exec(ejtag_info, ctx, buf);
 
@@ -436,10 +447,8 @@ exit:
 
 int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 {
-	struct pracc_queue_info ctx = {.max_code = 8};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
 	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16((addr + 0x8000))));		/* load  $8 with modified upper address */
@@ -451,7 +460,6 @@ int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf);
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -462,10 +470,8 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 		return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t *)buf);
 
 	uint32_t *data = NULL;
-	struct pracc_queue_info ctx = {.max_code = 256 * 3 + 8 + 1};	/* alloc memory for the worst case */
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	if (size != 4) {
 		data = malloc(256 * sizeof(uint32_t));
@@ -541,10 +547,8 @@ exit:
 
 int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_reg, uint32_t cp0_sel)
 {
-	struct pracc_queue_info ctx = {.max_code = 7};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
 	pracc_add(&ctx, 0, MIPS32_MFC0(8, cp0_reg, cp0_sel));			/* move cp0 reg / sel to $8 */
@@ -556,17 +560,14 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_r
 	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 bits of $8 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val);
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
 
 int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel)
 {
-	struct pracc_queue_info ctx = {.max_code = 6};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	pracc_add_li32(&ctx, 15, val, 0);				/* Load val to $15 */
 
@@ -575,7 +576,6 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -610,10 +610,9 @@ exit:
 static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 					 uint32_t start_addr, uint32_t end_addr, int cached, int rel)
 {
-	struct pracc_queue_info ctx = {.max_code = 256 * 2 + 5};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
+
 	/** Find cache line size in bytes */
 	uint32_t clsiz;
 	if (rel) {	/* Release 2 (rel = 1) */
@@ -711,10 +710,8 @@ exit:
 static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 		uint32_t addr, int size, int count, const void *buf)
 {
-	struct pracc_queue_info ctx = {.max_code = 128 * 3 + 5 + 1};	/* alloc memory for the worst case */
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	const uint32_t *buf32 = buf;
 	const uint16_t *buf16 = buf;
@@ -834,10 +831,8 @@ int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 		MIPS32_MTC0(1, 24, 0),							/* move $1 to depc (pc) */
 	};
 
-	struct pracc_queue_info ctx = {.max_code = 37 * 2 + 7 + 1};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	/* load registers 2 to 31 with li32, optimize */
 	for (int i = 2; i < 32; i++)
@@ -856,7 +851,6 @@ int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 
 	ejtag_info->reg8 = regs[8];
 	ejtag_info->reg9 = regs[9];
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -872,10 +866,8 @@ int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 		MIPS32_MFC0(8, 24, 0),							/* move depc (pc) to $8 */
 	};
 
-	struct pracc_queue_info ctx = {.max_code = 49};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	pracc_add(&ctx, 0, MIPS32_MTC0(1, 31, 0));						/* move $1 to COP0 DeSave */
 	pracc_add(&ctx, 0, MIPS32_LUI(1, PRACC_UPPER_BASE_ADDR));				/* $1 = MIP32_PRACC_BASE_ADDR */
@@ -901,7 +893,6 @@ int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 
 	ejtag_info->reg8 = regs[8];	/* reg8 is saved but not restored, next called function should restore it */
 	ejtag_info->reg9 = regs[9];
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index 166cbb4..525d6e6 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -42,6 +42,8 @@
 #define NEG16(v)						(((~(v)) + 1) & 0xFFFF)
 /*#define NEG18(v) (((~(v)) + 1) & 0x3FFFF)*/
 
+#define PRACC_BLOCK	128	/* 1 Kbyte */
+
 typedef struct {
 	uint32_t instr;
 	uint32_t addr;
@@ -49,13 +51,14 @@ typedef struct {
 
 struct pracc_queue_info {
 	int retval;
-	const int max_code;
 	int code_count;
 	int store_count;
+	int max_code;		/* max intstructions with currently allocated memory */
 	pa_list *pracc_list;	/* Code and store addresses at dmseg */
 };
 void pracc_queue_init(struct pracc_queue_info *ctx);
 void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr);
+void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize);
 void pracc_queue_free(struct pracc_queue_info *ctx);
 int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info,
 			    struct pracc_queue_info *ctx, uint32_t *buf);
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 943a868..1a8a843 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -197,10 +197,8 @@ void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data)
 /* Set (to enable) or clear (to disable stepping) the SSt bit (bit 8) in Cp0 Debug reg (reg 23, sel 0) */
 int mips_ejtag_config_step(struct mips_ejtag *ejtag_info, int enable_step)
 {
-	struct pracc_queue_info ctx = {.max_code = 7};
+	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
-	if (ctx.retval != ERROR_OK)
-		goto exit;
 
 	pracc_add(&ctx, 0, MIPS32_MFC0(8, 23, 0));			/* move COP0 Debug to $8 */
 	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, 0x0100));			/* set SSt bit in debug reg */
@@ -213,7 +211,6 @@ int mips_ejtag_config_step(struct mips_ejtag *ejtag_info, int enable_step)
 	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of $8 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
-exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
-- 
cgit v1.1


From e32058811794388038fcb7da930f3154c0cb7ea2 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Mon, 27 Feb 2017 12:25:49 +0100
Subject: mips32, add option to avoid check in last instruction

This option is needed, for example, when exiting
debug mode in bmips targets. The last instruction
is a NOP, not a DERET. When working in async mode
this check is not done, mips32_pracc_queue_exec() pass
the parameter to mips32_pracc_exec() and never use it.

Change-Id: I4c7ed4feb1588b62e2645b955b501b6671113b36
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4021
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32_pracc.c | 60 ++++++++++++++++++++++++-----------------------
 src/target/mips32_pracc.h |  5 ++--
 src/target/mips_ejtag.c   |  4 ++--
 3 files changed, 36 insertions(+), 33 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index da1cee1..7324246 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -159,7 +159,8 @@ int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 	return ERROR_OK;
 }
 
-int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *param_out)
+int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx,
+					uint32_t *param_out, bool check_last)
 {
 	int code_count = 0;
 	int store_pending = 0;		/* increases with every store instruction at dmseg, decreases with every store pa */
@@ -289,7 +290,7 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 		/* finish processor access, let the processor eat! */
 		mips32_pracc_finish(ejtag_info);
 
-		if (instr == MIPS32_DRET)	/* after leaving debug mode nothing to do */
+		if (final_check && !check_last)			/* last instr, don't check, execute and exit */
 			return jtag_execute_queue();
 
 		if (store_pending == 0 && pass) {	/* store access done, but after passing pracc text */
@@ -346,7 +347,8 @@ inline void pracc_queue_free(struct pracc_queue_info *ctx)
 		free(ctx->pracc_list);
 }
 
-int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *buf)
+int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx,
+					uint32_t *buf, bool check_last)
 {
 	if (ctx->retval != ERROR_OK) {
 		LOG_ERROR("Out of memory");
@@ -354,7 +356,7 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 	}
 
 	if (ejtag_info->mode == 0)
-		return mips32_pracc_exec(ejtag_info, ctx, buf);
+		return mips32_pracc_exec(ejtag_info, ctx, buf, check_last);
 
 	union scan_in {
 		uint8_t scan_96[12];
@@ -459,7 +461,7 @@ int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -491,40 +493,40 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 		int this_round_count = (count > 256) ? 256 : count;
 		uint32_t last_upper_base_addr = UPPER16((addr + 0x8000));
 
-		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
-		pracc_add(&ctx, 0, MIPS32_LUI(9, last_upper_base_addr));		/* load the upper memory address in $9 */
+		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+		pracc_add(&ctx, 0, MIPS32_LUI(9, last_upper_base_addr));	/* load the upper memory address in $9 */
 
 		for (int i = 0; i != this_round_count; i++) {			/* Main code loop */
 			uint32_t upper_base_addr = UPPER16((addr + 0x8000));
-			if (last_upper_base_addr != upper_base_addr) {			/* if needed, change upper address in $9 */
+			if (last_upper_base_addr != upper_base_addr) {		/* if needed, change upper address in $9 */
 				pracc_add(&ctx, 0, MIPS32_LUI(9, upper_base_addr));
 				last_upper_base_addr = upper_base_addr;
 			}
 
 			if (size == 4)
-				pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 9));		/* load from memory to $8 */
+				pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 9));	/* load from memory to $8 */
 			else if (size == 2)
 				pracc_add(&ctx, 0, MIPS32_LHU(8, LOWER16(addr), 9));
 			else
 				pracc_add(&ctx, 0, MIPS32_LBU(8, LOWER16(addr), 9));
 
 			pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + i * 4,
-					  MIPS32_SW(8, PRACC_OUT_OFFSET + i * 4, 15));		/* store $8 at param out */
+					  MIPS32_SW(8, PRACC_OUT_OFFSET + i * 4, 15));	/* store $8 at param out */
 			addr += size;
 		}
 		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 		pracc_add_li32(&ctx, 9, ejtag_info->reg9, 0);				/* restore $9 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave */
+		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
 		if (size == 4) {
-			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf32);
+			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf32, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 			buf32 += this_round_count;
 		} else {
-			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, data);
+			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, data, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 
@@ -550,16 +552,16 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_r
 	struct pracc_queue_info ctx;
 	pracc_queue_init(&ctx);
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
+	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));		/* $15 = MIPS32_PRACC_BASE_ADDR */
 	pracc_add(&ctx, 0, MIPS32_MFC0(8, cp0_reg, cp0_sel));			/* move cp0 reg / sel to $8 */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave */
 	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits  of $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));		/* restore lower 16 bits of $8 */
+	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of $8 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -575,7 +577,7 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -616,19 +618,19 @@ static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 	/** Find cache line size in bytes */
 	uint32_t clsiz;
 	if (rel) {	/* Release 2 (rel = 1) */
-		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
+		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
 
-		pracc_add(&ctx, 0, MIPS32_RDHWR(8, MIPS32_SYNCI_STEP));			/* load synci_step value to $8 */
+		pracc_add(&ctx, 0, MIPS32_RDHWR(8, MIPS32_SYNCI_STEP));		/* load synci_step value to $8 */
 
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
 				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
 
 		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
-		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, &clsiz);
+		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, &clsiz, 1);
 		if (ctx.retval != ERROR_OK)
 			goto exit;
 
@@ -689,7 +691,7 @@ static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 			pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
 			pracc_add(&ctx, 0, MIPS32_NOP);						/* nop in delay slot */
 
-			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 
@@ -701,7 +703,7 @@ static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave*/
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 exit:
 	pracc_queue_free(&ctx);
 	return ctx.retval;
@@ -755,7 +757,7 @@ static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
 		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
 
-		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 		if (ctx.retval != ERROR_OK)
 			goto exit;
 		count -= this_round_count;
@@ -847,7 +849,7 @@ int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_ORI(1, 1, LOWER16((regs[1]))));		/* load lower half word in $1 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 
 	ejtag_info->reg8 = regs[8];
 	ejtag_info->reg9 = regs[9];
@@ -889,7 +891,7 @@ int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));					/* load $15 in DeSave */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, regs);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, regs, 1);
 
 	ejtag_info->reg8 = regs[8];	/* reg8 is saved but not restored, next called function should restore it */
 	ejtag_info->reg9 = regs[9];
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index 525d6e6..e990f8d 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -61,7 +61,7 @@ void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr);
 void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize);
 void pracc_queue_free(struct pracc_queue_info *ctx);
 int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info,
-			    struct pracc_queue_info *ctx, uint32_t *buf);
+			    struct pracc_queue_info *ctx, uint32_t *buf, bool check_last);
 
 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info,
 		uint32_t addr, int size, int count, void *buf);
@@ -73,7 +73,8 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs);
 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs);
 
-int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx, uint32_t *param_out);
+int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ctx,
+				uint32_t *param_out, bool check_last);
 
 /**
  * \b mips32_cp0_read
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index 1a8a843..e35758d 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -210,7 +210,7 @@ int mips_ejtag_config_step(struct mips_ejtag *ejtag_info, int enable_step)
 	pracc_add(&ctx, 0, MIPS32_B(NEG16((ctx.code_count + 1))));			/* jump to start */
 	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of $8 */
 
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 	pracc_queue_free(&ctx);
 	return ctx.retval;
 }
@@ -273,7 +273,7 @@ int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info)
 	struct pracc_queue_info ctx = {.max_code = 1, .pracc_list = &pracc_list, .code_count = 1, .store_count = 0};
 
 	/* execute our dret instruction */
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 0);
 
 	/* pic32mx workaround, false pending at low core clock */
 	jtag_add_sleep(1000);
-- 
cgit v1.1


From 6012a87d4464cdbf65ba46cb5c98d6113b5d7aea Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Mon, 27 Feb 2017 13:06:46 +0100
Subject: mips32, add microMips instruction subset

Only reencoded MIPS32 instructions. Added some instructions
for crc code. Micromips isa in debug mode is only needed for
pic32mm cores. Pic32mz seems that only works with MIPS32
isa when in debug mode.

Change-Id: I07059e153a7000ea9204f20b6b37edf6a7623455
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4022
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32.h | 95 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 95 insertions(+)

(limited to 'src/target')

diff --git a/src/target/mips32.h b/src/target/mips32.h
index 2e7067f..968c3f7 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -217,6 +217,101 @@ struct mips32_algorithm {
 #define MIPS32_SDBBP				0x7000003F	/* MIPS32_J_INST(MIPS32_OP_SPECIAL2, MIPS32_OP_SDBBP) */
 #define MIPS16_SDBBP				0xE801
 
+/*MICRO MIPS INSTRUCTIONS, see doc MD00582 */
+#define POOL32A					0X00u
+#define POOL32AXf				0x3Cu
+#define POOL32B					0x08u
+#define POOL32I					0x10u
+#define MMIPS32_OP_ADDI			0x04u
+#define MMIPS32_OP_ADDIU		0x0Cu
+#define MMIPS32_OP_ADDU			0x150u
+#define MMIPS32_OP_AND			0x250u
+#define MMIPS32_OP_ANDI			0x34u
+#define MMIPS32_OP_BEQ			0x25u
+#define MMIPS32_OP_BGTZ			0x06u
+#define MMIPS32_OP_BNE			0x2Du
+#define MMIPS32_OP_CACHE		0x06u
+#define MMIPS32_OP_J			0x35u
+#define MMIPS32_OP_JALR			0x03Cu
+#define MMIPS32_OP_LB			0x07u
+#define MMIPS32_OP_LBU			0x05u
+#define MMIPS32_OP_LHU			0x0Du
+#define MMIPS32_OP_LUI			0x0Du
+#define MMIPS32_OP_LW			0x3Fu
+#define MMIPS32_OP_MFC0			0x03u
+#define MMIPS32_OP_MTC0			0x0Bu
+#define MMIPS32_OP_MFLO			0x075u
+#define MMIPS32_OP_MFHI			0x035u
+#define MMIPS32_OP_MTLO			0x0F5u
+#define MMIPS32_OP_MTHI			0x0B5u
+#define MMIPS32_OP_MOVN			0x018u
+#define MMIPS32_OP_ORI			0x14u
+#define MMIPS32_OP_RDHWR		0x1ACu
+#define MMIPS32_OP_SB			0x06u
+#define MMIPS32_OP_SH			0x0Eu
+#define MMIPS32_OP_SW			0x3Eu
+#define MMIPS32_OP_SLTU			0x390u
+#define MMIPS32_OP_SLL			0x000u
+#define MMIPS32_OP_SLTI			0x24u
+#define MMIPS32_OP_SRL			0x040u
+#define MMIPS32_OP_SYNCI		0x10u
+#define MMIPS32_OP_XOR			0x310u
+#define MMIPS32_OP_XORI			0x1Cu
+
+#define MMIPS32_ADDI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ADDI, tar, src, val)
+#define MMIPS32_ADDIU(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ADDIU, tar, src, val)
+#define MMIPS32_ADDU(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_ADDU)
+#define MMIPS32_AND(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_AND)
+#define MMIPS32_ANDI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ANDI, tar, src, val)
+
+#define MMIPS32_B(off)				MMIPS32_BEQ(0, 0, off)
+#define MMIPS32_BEQ(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BEQ, tar, src, off)
+#define MMIPS32_BGTZ(reg, off)			MIPS32_I_INST(POOL32I, MMIPS32_OP_BGTZ, reg, off)
+#define MMIPS32_BNE(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BNE, tar, src, off)
+#define MMIPS32_CACHE(op, off, base)		MIPS32_R_INST(POOL32B, op, base, MMIPS32_OP_CACHE << 1, 0, off)
+
+#define MMIPS32_J(tar)				MIPS32_J_INST(MMIPS32_OP_J, (0x07FFFFFFu & (tar)) >> 1)
+#define MMIPS32_JR(reg)				MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_JALR, POOL32AXf)
+#define MMIPS32_LB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LB, reg, base, off)
+#define MMIPS32_LBU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LBU, reg, base, off)
+#define MMIPS32_LHU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LHU, reg, base, off)
+#define MMIPS32_LUI(reg, val)			MIPS32_I_INST(POOL32I, MMIPS32_OP_LUI, reg, val)
+#define MMIPS32_LW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LW, reg, base, off)
+
+#define MMIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(POOL32A, gpr, cpr, sel, MMIPS32_OP_MFC0, POOL32AXf)
+#define MMIPS32_MFLO(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MFLO, POOL32AXf)
+#define MMIPS32_MFHI(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MFHI, POOL32AXf)
+#define MMIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(POOL32A, gpr, cpr, sel, MMIPS32_OP_MTC0, POOL32AXf)
+#define MMIPS32_MTLO(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MTLO, POOL32AXf)
+#define MMIPS32_MTHI(reg)			MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_MTHI, POOL32AXf)
+
+#define MMIPS32_MOVN(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_MOVN)
+#define MMIPS32_NOP				0
+#define MMIPS32_ORI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_ORI, tar, src, val)
+#define MMIPS32_RDHWR(tar, dst)			MIPS32_R_INST(POOL32A, dst, tar, 0, MMIPS32_OP_RDHWR, POOL32AXf)
+#define MMIPS32_SB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SB, reg, base, off)
+#define MMIPS32_SH(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SH, reg, base, off)
+#define MMIPS32_SW(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_SW, reg, base, off)
+
+#define MMIPS32_SRL(reg, src, off)		MIPS32_R_INST(POOL32A, reg, src, off, 0, MMIPS32_OP_SRL)
+#define MMIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(POOL32A, tar, src, dst, 0, MMIPS32_OP_SLTU)
+#define MMIPS32_SYNCI(off, base)		MIPS32_I_INST(POOL32I, MMIPS32_OP_SYNCI, base, off)
+#define MMIPS32_SLL(dst, src, sa)		MIPS32_R_INST(POOL32A, dst, src, sa, 0, MMIPS32_OP_SLL)
+#define MMIPS32_SLTI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_SLTI, tar, src, val)
+#define MMIPS32_SYNC				0x00001A7Cu /* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1ADu, POOL32AXf) */
+
+#define MMIPS32_XOR(reg, val1, val2)		MIPS32_R_INST(POOL32A, val1, val2, reg, 0, MMIPS32_OP_XOR)
+#define MMIPS32_XORI(tar, src, val)		MIPS32_I_INST(MMIPS32_OP_XORI, tar, src, val)
+
+#define MMIPS32_SYNCI_STEP	0x1u	/* reg num od address step size to be used with synci instruction */
+
+
+/* ejtag specific instructions */
+#define MMIPS32_DRET			0x0000E37Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x38D, POOL32AXf) */
+#define MMIPS32_SDBBP			0x0000DB7Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1BD, POOL32AXf) */
+#define MMIPS16_SDBBP			0x46C0u		/* POOL16C instr */
+
+
 extern const struct command_registration mips32_command_handlers[];
 
 int mips32_arch_state(struct target *target);
-- 
cgit v1.1


From 2279c23cdeaea05839a28ff3addf12b9b0f5357e Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Sun, 7 May 2017 11:58:25 +0200
Subject: mips32, add support for micromips in debug mode

Micromips is 16bit oriented, branch and jumps are
16 bit based. The upper half 16bits of a 32bit instruction
with the major opcode, must go first in the instruction
stream, hence the SWAP16 macro and swap16 array function,
needed if the code is written as 32 bit word in little endian
cores. Endianess info added to ejtag_iinfo. Pointer to
ejtag_info and isa field added to pracc context.
MIPS32 code are renamed to MIPS32_ISA_...
To select the isa, the new code has an additional isa parameter
(1 for micromips, 0 for mips32).
In JR instruction the isa bit must be set to execute
micromips code.
The suffix u is added to the OP codes to avoid signed/unsigned
comparison errors and to make sure the right shift is
performed logically.
The isa in debug mode is updated in the poll function.
Code for miniprograms, in kernel mode, need to be converted.
CFI code only for mips32.

Change-Id: I79a8b637d49b0e2d92b6dd5eb5aa8aa0520bf938
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4032
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32.c       |   6 +-
 src/target/mips32.h       | 224 ++++++++++++++++++++++------------
 src/target/mips32_pracc.c | 304 ++++++++++++++++++++++++----------------------
 src/target/mips32_pracc.h |  11 ++
 src/target/mips_ejtag.c   |  20 +--
 src/target/mips_ejtag.h   |   3 +
 src/target/mips_m4k.c     |  14 ++-
 7 files changed, 343 insertions(+), 239 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32.c b/src/target/mips32.c
index cec4549..5c85fea 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -383,9 +383,11 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 	mips32->ejtag_info.tap = tap;
 	mips32->read_core_reg = mips32_read_core_reg;
 	mips32->write_core_reg = mips32_write_core_reg;
-
-	mips32->ejtag_info.scan_delay = MIPS32_SCAN_DELAY_LEGACY_MODE;	/* Initial default value */
+	/* if unknown endianness defaults to little endian, 1 */
+	mips32->ejtag_info.endianness = target->endianness == TARGET_BIG_ENDIAN ? 0 : 1;
+	mips32->ejtag_info.scan_delay = MIPS32_SCAN_DELAY_LEGACY_MODE;
 	mips32->ejtag_info.mode = 0;			/* Initial default value */
+	mips32->ejtag_info.isa = 0;	/* isa on debug mips32, updated by poll function */
 
 	return ERROR_OK;
 }
diff --git a/src/target/mips32.h b/src/target/mips32.h
index 968c3f7..d79451f 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -122,44 +122,49 @@ struct mips32_algorithm {
 	enum mips32_isa_mode isa_mode;
 };
 
-#define MIPS32_OP_ADDIU 0x21
-#define MIPS32_OP_ANDI	0x0C
-#define MIPS32_OP_BEQ	0x04
-#define MIPS32_OP_BGTZ	0x07
-#define MIPS32_OP_BNE	0x05
-#define MIPS32_OP_ADDI	0x08
-#define MIPS32_OP_AND	0x24
-#define MIPS32_OP_CACHE	0x2F
-#define MIPS32_OP_COP0	0x10
-#define MIPS32_OP_J	0x02
-#define MIPS32_OP_JR	0x08
-#define MIPS32_OP_LUI	0x0F
-#define MIPS32_OP_LW	0x23
-#define MIPS32_OP_LBU	0x24
-#define MIPS32_OP_LHU	0x25
-#define MIPS32_OP_MFHI	0x10
-#define MIPS32_OP_MTHI	0x11
-#define MIPS32_OP_MFLO	0x12
-#define MIPS32_OP_MTLO	0x13
-#define MIPS32_OP_RDHWR 0x3B
-#define MIPS32_OP_SB	0x28
-#define MIPS32_OP_SH	0x29
-#define MIPS32_OP_SW	0x2B
-#define MIPS32_OP_ORI	0x0D
-#define MIPS32_OP_XORI	0x0E
-#define MIPS32_OP_XOR	0x26
-#define MIPS32_OP_SLTU  0x2B
-#define MIPS32_OP_SRL	0x03
-#define MIPS32_OP_SYNCI	0x1F
-
-#define MIPS32_OP_REGIMM	0x01
-#define MIPS32_OP_SDBBP		0x3F
-#define MIPS32_OP_SPECIAL	0x00
-#define MIPS32_OP_SPECIAL2	0x07
-#define MIPS32_OP_SPECIAL3	0x1F
-
-#define MIPS32_COP0_MF	0x00
-#define MIPS32_COP0_MT	0x04
+#define MIPS32_OP_ADDU	0x21u
+#define MIPS32_OP_ADDIU	0x09u
+#define MIPS32_OP_ANDI	0x0Cu
+#define MIPS32_OP_BEQ	0x04u
+#define MIPS32_OP_BGTZ	0x07u
+#define MIPS32_OP_BNE	0x05u
+#define MIPS32_OP_ADDI	0x08u
+#define MIPS32_OP_AND	0x24u
+#define MIPS32_OP_CACHE	0x2Fu
+#define MIPS32_OP_COP0	0x10u
+#define MIPS32_OP_J	0x02u
+#define MIPS32_OP_JR	0x08u
+#define MIPS32_OP_LUI	0x0Fu
+#define MIPS32_OP_LW	0x23u
+#define MIPS32_OP_LB	0x20u
+#define MIPS32_OP_LBU	0x24u
+#define MIPS32_OP_LHU	0x25u
+#define MIPS32_OP_MFHI	0x10u
+#define MIPS32_OP_MTHI	0x11u
+#define MIPS32_OP_MFLO	0x12u
+#define MIPS32_OP_MTLO	0x13u
+#define MIPS32_OP_RDHWR	0x3Bu
+#define MIPS32_OP_SB	0x28u
+#define MIPS32_OP_SH	0x29u
+#define MIPS32_OP_SW	0x2Bu
+#define MIPS32_OP_ORI	0x0Du
+#define MIPS32_OP_XORI	0x0Eu
+#define MIPS32_OP_XOR	0x26u
+#define MIPS32_OP_SLTU	0x2Bu
+#define MIPS32_OP_SRL	0x03u
+#define MIPS32_OP_SYNCI	0x1Fu
+#define MIPS32_OP_SLL	0x00u
+#define MIPS32_OP_SLTI	0x0Au
+#define MIPS32_OP_MOVN	0x0Bu
+
+#define MIPS32_OP_REGIMM	0x01u
+#define MIPS32_OP_SDBBP	0x3Fu
+#define MIPS32_OP_SPECIAL	0x00u
+#define MIPS32_OP_SPECIAL2	0x07u
+#define MIPS32_OP_SPECIAL3	0x1Fu
+
+#define MIPS32_COP0_MF	0x00u
+#define MIPS32_COP0_MT	0x04u
 
 #define MIPS32_R_INST(opcode, rs, rt, rd, shamt, funct) \
 	(((opcode) << 26) | ((rs) << 21) | ((rt) << 16) | ((rd) << 11) | ((shamt) << 6) | (funct))
@@ -167,41 +172,52 @@ struct mips32_algorithm {
 	(((opcode) << 26) | ((rs) << 21) | ((rt) << 16) | (immd))
 #define MIPS32_J_INST(opcode, addr)	(((opcode) << 26) | (addr))
 
-#define MIPS32_NOP						0
-#define MIPS32_ADDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDI, src, tar, val)
-#define MIPS32_ADDU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_ADDIU)
-#define MIPS32_AND(reg, off, val)		MIPS32_R_INST(0, off, val, reg, 0, MIPS32_OP_AND)
-#define MIPS32_ANDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ANDI, src, tar, val)
-#define MIPS32_B(off)					MIPS32_BEQ(0, 0, off)
-#define MIPS32_BEQ(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BEQ, src, tar, off)
-#define MIPS32_BGTZ(reg, off)			MIPS32_I_INST(MIPS32_OP_BGTZ, reg, 0, off)
-#define MIPS32_BNE(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BNE, src, tar, off)
-#define MIPS32_CACHE(op, off, base)		MIPS32_I_INST(MIPS32_OP_CACHE, base, op, off)
-#define MIPS32_J(tar)				MIPS32_J_INST(MIPS32_OP_J, tar)
-#define MIPS32_JR(reg)					MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_JR)
-#define MIPS32_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MF, gpr, cpr, 0, sel)
-#define MIPS32_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MT, gpr, cpr, 0, sel)
-#define MIPS32_LBU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LBU, base, reg, off)
-#define MIPS32_LHU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LHU, base, reg, off)
-#define MIPS32_LUI(reg, val)			MIPS32_I_INST(MIPS32_OP_LUI, 0, reg, val)
-#define MIPS32_LW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LW, base, reg, off)
-#define MIPS32_MFLO(reg)				MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFLO)
-#define MIPS32_MFHI(reg)				MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFHI)
-#define MIPS32_MTLO(reg)				MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTLO)
-#define MIPS32_MTHI(reg)				MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTHI)
-#define MIPS32_ORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ORI, src, tar, val)
-#define MIPS32_XORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_XORI, src, tar, val)
-#define MIPS32_RDHWR(tar, dst)			MIPS32_R_INST(MIPS32_OP_SPECIAL3, 0, tar, dst, 0, MIPS32_OP_RDHWR)
-#define MIPS32_SB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SB, base, reg, off)
-#define MIPS32_SH(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SH, base, reg, off)
-#define MIPS32_SW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SW, base, reg, off)
-#define MIPS32_XOR(reg, val1, val2)		MIPS32_R_INST(0, val1, val2, reg, 0, MIPS32_OP_XOR)
-#define MIPS32_SRL(reg, src, off)		MIPS32_R_INST(0, 0, src, reg, off, MIPS32_OP_SRL)
-#define MIPS32_SLTU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_SLTU)
-#define MIPS32_SYNCI(off, base)			MIPS32_I_INST(MIPS32_OP_REGIMM, base, MIPS32_OP_SYNCI, off)
-
-#define MIPS32_SYNC			0xF
-#define MIPS32_SYNCI_STEP	0x1	/* reg num od address step size to be used with synci instruction */
+#define MIPS32_ISA_NOP				0
+#define MIPS32_ISA_ADDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDI, src, tar, val)
+#define MIPS32_ISA_ADDIU(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ADDIU, src, tar, val)
+#define MIPS32_ISA_ADDU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_ADDU)
+#define MIPS32_ISA_AND(dst, src, tar)		MIPS32_R_INST(0, src, tar, dst, 0, MIPS32_OP_AND)
+#define MIPS32_ISA_ANDI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ANDI, src, tar, val)
+
+#define MIPS32_ISA_B(off)			MIPS32_ISA_BEQ(0, 0, off)
+#define MIPS32_ISA_BEQ(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BEQ, src, tar, off)
+#define MIPS32_ISA_BGTZ(reg, off)		MIPS32_I_INST(MIPS32_OP_BGTZ, reg, 0, off)
+#define MIPS32_ISA_BNE(src, tar, off)		MIPS32_I_INST(MIPS32_OP_BNE, src, tar, off)
+#define MIPS32_ISA_CACHE(op, off, base)		MIPS32_I_INST(MIPS32_OP_CACHE, base, op, off)
+#define MIPS32_ISA_J(tar)			MIPS32_J_INST(MIPS32_OP_J, (0x0FFFFFFFu & (tar)) >> 2)
+#define MIPS32_ISA_JR(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_JR)
+
+#define MIPS32_ISA_LB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LB, base, reg, off)
+#define MIPS32_ISA_LBU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LBU, base, reg, off)
+#define MIPS32_ISA_LHU(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LHU, base, reg, off)
+#define MIPS32_ISA_LUI(reg, val)		MIPS32_I_INST(MIPS32_OP_LUI, 0, reg, val)
+#define MIPS32_ISA_LW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_LW, base, reg, off)
+
+#define MIPS32_ISA_MFC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MF, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MTC0(gpr, cpr, sel)		MIPS32_R_INST(MIPS32_OP_COP0, MIPS32_COP0_MT, gpr, cpr, 0, sel)
+#define MIPS32_ISA_MFLO(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFLO)
+#define MIPS32_ISA_MFHI(reg)			MIPS32_R_INST(0, 0, 0, reg, 0, MIPS32_OP_MFHI)
+#define MIPS32_ISA_MTLO(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTLO)
+#define MIPS32_ISA_MTHI(reg)			MIPS32_R_INST(0, reg, 0, 0, 0, MIPS32_OP_MTHI)
+
+#define MIPS32_ISA_MOVN(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_MOVN)
+#define MIPS32_ISA_ORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_ORI, src, tar, val)
+#define MIPS32_ISA_RDHWR(tar, dst)		MIPS32_R_INST(MIPS32_OP_SPECIAL3, 0, tar, dst, 0, MIPS32_OP_RDHWR)
+#define MIPS32_ISA_SB(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SB, base, reg, off)
+#define MIPS32_ISA_SH(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SH, base, reg, off)
+#define MIPS32_ISA_SW(reg, off, base)		MIPS32_I_INST(MIPS32_OP_SW, base, reg, off)
+
+#define MIPS32_ISA_SLL(dst, src, sa)		MIPS32_R_INST(MIPS32_OP_SPECIAL, 0, src, dst, sa, MIPS32_OP_SLL)
+#define MIPS32_ISA_SLTI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_SLTI, src, tar, val)
+#define MIPS32_ISA_SLTU(dst, src, tar)		MIPS32_R_INST(MIPS32_OP_SPECIAL, src, tar, dst, 0, MIPS32_OP_SLTU)
+#define MIPS32_ISA_SRL(reg, src, off)		MIPS32_R_INST(0, 0, src, reg, off, MIPS32_OP_SRL)
+#define MIPS32_ISA_SYNC				0xFu
+#define MIPS32_ISA_SYNCI(off, base)		MIPS32_I_INST(MIPS32_OP_REGIMM, base, MIPS32_OP_SYNCI, off)
+
+#define MIPS32_ISA_XOR(reg, val1, val2)		MIPS32_R_INST(0, val1, val2, reg, 0, MIPS32_OP_XOR)
+#define MIPS32_ISA_XORI(tar, src, val)		MIPS32_I_INST(MIPS32_OP_XORI, src, tar, val)
+
+#define MIPS32_ISA_SYNCI_STEP		0x1	/* reg num od address step size to be used with synci instruction */
 
 /**
  * Cache operations definitions
@@ -213,9 +229,10 @@ struct mips32_algorithm {
 #define MIPS32_CACHE_I_HIT_INVALIDATE ((0x0 << 0) | (0x4 << 2))
 
 /* ejtag specific instructions */
-#define MIPS32_DRET					0x4200001F
-#define MIPS32_SDBBP				0x7000003F	/* MIPS32_J_INST(MIPS32_OP_SPECIAL2, MIPS32_OP_SDBBP) */
-#define MIPS16_SDBBP				0xE801
+#define MIPS32_ISA_DRET				0x4200001Fu
+/* MIPS32_ISA_J_INST(MIPS32_ISA_OP_SPECIAL2, MIPS32_ISA_OP_SDBBP) */
+#define MIPS32_ISA_SDBBP			0x7000003Fu
+#define MIPS16_ISA_SDBBP			0xE801u
 
 /*MICRO MIPS INSTRUCTIONS, see doc MD00582 */
 #define POOL32A					0X00u
@@ -270,7 +287,7 @@ struct mips32_algorithm {
 #define MMIPS32_BNE(src, tar, off)		MIPS32_I_INST(MMIPS32_OP_BNE, tar, src, off)
 #define MMIPS32_CACHE(op, off, base)		MIPS32_R_INST(POOL32B, op, base, MMIPS32_OP_CACHE << 1, 0, off)
 
-#define MMIPS32_J(tar)				MIPS32_J_INST(MMIPS32_OP_J, (0x07FFFFFFu & (tar)) >> 1)
+#define MMIPS32_J(tar)				MIPS32_J_INST(MMIPS32_OP_J, ((0x07FFFFFFu & ((tar) >> 1))))
 #define MMIPS32_JR(reg)				MIPS32_R_INST(POOL32A, 0, reg, 0, MMIPS32_OP_JALR, POOL32AXf)
 #define MMIPS32_LB(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LB, reg, base, off)
 #define MMIPS32_LBU(reg, off, base)		MIPS32_I_INST(MMIPS32_OP_LBU, reg, base, off)
@@ -311,6 +328,59 @@ struct mips32_algorithm {
 #define MMIPS32_SDBBP			0x0000DB7Cu	/* MIPS32_R_INST(POOL32A, 0, 0, 0, 0x1BD, POOL32AXf) */
 #define MMIPS16_SDBBP			0x46C0u		/* POOL16C instr */
 
+/* instruction code with isa selection */
+#define MIPS32_NOP				0	/* same for both isa's */
+#define MIPS32_ADDI(isa, tar, src, val)		(isa ? MMIPS32_ADDI(tar, src, val) : MIPS32_ISA_ADDI(tar, src, val))
+#define MIPS32_ADDIU(isa, tar, src, val)	(isa ? MMIPS32_ADDIU(tar, src, val) : MIPS32_ISA_ADDIU(tar, src, val))
+#define MIPS32_ADDU(isa, dst, src, tar)		(isa ? MMIPS32_ADDU(dst, src, tar) : MIPS32_ISA_ADDU(dst, src, tar))
+#define MIPS32_AND(isa, dst, src, tar)		(isa ? MMIPS32_AND(dst, src, tar) : MIPS32_ISA_AND(dst, src, tar))
+#define MIPS32_ANDI(isa, tar, src, val)		(isa ? MMIPS32_ANDI(tar, src, val) : MIPS32_ISA_ANDI(tar, src, val))
+
+#define MIPS32_B(isa, off)			(isa ? MMIPS32_B(off) : MIPS32_ISA_B(off))
+#define MIPS32_BEQ(isa, src, tar, off)		(isa ? MMIPS32_BEQ(src, tar, off) : MIPS32_ISA_BEQ(src, tar, off))
+#define MIPS32_BGTZ(isa, reg, off)		(isa ? MMIPS32_BGTZ(reg, off) : MIPS32_ISA_BGTZ(reg, off))
+#define MIPS32_BNE(isa, src, tar, off)		(isa ? MMIPS32_BNE(src, tar, off) : MIPS32_ISA_BNE(src, tar, off))
+#define MIPS32_CACHE(isa, op, off, base)	(isa ? MMIPS32_CACHE(op, off, base) : MIPS32_ISA_CACHE(op, off, base))
+
+#define MIPS32_J(isa, tar)			(isa ? MMIPS32_J(tar) : MIPS32_ISA_J(tar))
+#define MIPS32_JR(isa, reg)			(isa ? MMIPS32_JR(reg) : MIPS32_ISA_JR(reg))
+#define MIPS32_LB(isa, reg, off, base)		(isa ? MMIPS32_LB(reg, off, base) : MIPS32_ISA_LB(reg, off, base))
+#define MIPS32_LBU(isa, reg, off, base)		(isa ? MMIPS32_LBU(reg, off, base) : MIPS32_ISA_LBU(reg, off, base))
+#define MIPS32_LHU(isa, reg, off, base)		(isa ? MMIPS32_LHU(reg, off, base) : MIPS32_ISA_LHU(reg, off, base))
+#define MIPS32_LW(isa, reg, off, base)		(isa ? MMIPS32_LW(reg, off, base) : MIPS32_ISA_LW(reg, off, base))
+#define MIPS32_LUI(isa, reg, val)		(isa ? MMIPS32_LUI(reg, val) : MIPS32_ISA_LUI(reg, val))
+
+#define MIPS32_MFC0(isa, gpr, cpr, sel)		(isa ? MMIPS32_MFC0(gpr, cpr, sel) : MIPS32_ISA_MFC0(gpr, cpr, sel))
+#define MIPS32_MTC0(isa, gpr, cpr, sel)		(isa ? MMIPS32_MTC0(gpr, cpr, sel) : MIPS32_ISA_MTC0(gpr, cpr, sel))
+#define MIPS32_MFLO(isa, reg)			(isa ? MMIPS32_MFLO(reg) : MIPS32_ISA_MFLO(reg))
+#define MIPS32_MFHI(isa, reg)			(isa ? MMIPS32_MFHI(reg) : MIPS32_ISA_MFHI(reg))
+#define MIPS32_MTLO(isa, reg)			(isa ? MMIPS32_MTLO(reg) : MIPS32_ISA_MTLO(reg))
+#define MIPS32_MTHI(isa, reg)			(isa ? MMIPS32_MTHI(reg) : MIPS32_ISA_MTHI(reg))
+
+#define MIPS32_MOVN(isa, dst, src, tar)		(isa ? MMIPS32_MOVN(dst, src, tar) : MIPS32_ISA_MOVN(dst, src, tar))
+#define MIPS32_ORI(isa, tar, src, val)		(isa ? MMIPS32_ORI(tar, src, val) : MIPS32_ISA_ORI(tar, src, val))
+#define MIPS32_RDHWR(isa, tar, dst)		(isa ? MMIPS32_RDHWR(tar, dst) : MIPS32_ISA_RDHWR(tar, dst))
+#define MIPS32_SB(isa, reg, off, base)		(isa ? MMIPS32_SB(reg, off, base) : MIPS32_ISA_SB(reg, off, base))
+#define MIPS32_SH(isa, reg, off, base)		(isa ? MMIPS32_SH(reg, off, base) : MIPS32_ISA_SH(reg, off, base))
+#define MIPS32_SW(isa, reg, off, base)		(isa ? MMIPS32_SW(reg, off, base) : MIPS32_ISA_SW(reg, off, base))
+
+#define MIPS32_SLL(isa, dst, src, sa)		(isa ? MMIPS32_SLL(dst, src, sa) : MIPS32_ISA_SLL(dst, src, sa))
+#define MIPS32_SLTI(isa, tar, src, val)		(isa ? MMIPS32_SLTI(tar, src, val) : MIPS32_ISA_SLTI(tar, src, val))
+#define MIPS32_SLTU(isa, dst, src, tar)		(isa ? MMIPS32_SLTU(dst, src, tar) : MIPS32_ISA_SLTU(dst, src, tar))
+#define MIPS32_SRL(isa, reg, src, off)		(isa ? MMIPS32_SRL(reg, src, off) : MIPS32_ISA_SRL(reg, src, off))
+
+#define MIPS32_SYNCI(isa, off, base)		(isa ? MMIPS32_SYNCI(off, base) : MIPS32_ISA_SYNCI(off, base))
+#define MIPS32_SYNC(isa)			(isa ? MMIPS32_SYNC : MIPS32_ISA_SYNC)
+#define MIPS32_XOR(isa, reg, val1, val2)	(isa ? MMIPS32_XOR(reg, val1, val2) : MIPS32_ISA_XOR(reg, val1, val2))
+#define MIPS32_XORI(isa, tar, src, val)		(isa ? MMIPS32_XORI(tar, src, val) : MIPS32_ISA_XORI(tar, src, val))
+
+#define MIPS32_SYNCI_STEP			0x1
+
+/* ejtag specific instructions */
+#define MIPS32_DRET(isa)			(isa ? MMIPS32_DRET : MIPS32_ISA_DRET)
+#define MIPS32_SDBBP(isa)			(isa ? MMIPS32_SDBBP : MIPS32_ISA_SDBBP)
+
+#define MIPS16_SDBBP(isa)			(isa ? MMIPS16_SDBBP : MIPS16_ISA_SDBBP)
 
 extern const struct command_registration mips32_command_handlers[];
 
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c
index 7324246..790c8dc 100644
--- a/src/target/mips32_pracc.c
+++ b/src/target/mips32_pracc.c
@@ -122,8 +122,8 @@ static void mips32_pracc_finish(struct mips_ejtag *ejtag_info)
 
 int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 {
-	uint32_t jt_code = MIPS32_J((0x0FFFFFFF & MIPS32_PRACC_TEXT) >> 2);
-
+	uint32_t jt_code = MIPS32_J(ejtag_info->isa, MIPS32_PRACC_TEXT);
+	pracc_swap16_array(ejtag_info, &jt_code, 1);
 	/* do 3 0/nops to clean pipeline before a jump to pracc text, NOP in delay slot */
 	for (int i = 0; i != 5; i++) {
 		/* Wait for pracc */
@@ -148,7 +148,7 @@ int mips32_pracc_clean_text_jump(struct mips_ejtag *ejtag_info)
 		if (retval != ERROR_OK)
 			return retval;
 
-		if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT) {		/* LEXRA/BMIPS ?, shift out another NOP, max 2 */
+		if (ejtag_info->pa_addr != MIPS32_PRACC_TEXT) {	/* LEXRA/BMIPS ?, shift out another NOP, max 2 */
 			mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
 			mips_ejtag_drscan_32_out(ejtag_info, MIPS32_NOP);
 			mips32_pracc_finish(ejtag_info);
@@ -163,7 +163,7 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 					uint32_t *param_out, bool check_last)
 {
 	int code_count = 0;
-	int store_pending = 0;		/* increases with every store instruction at dmseg, decreases with every store pa */
+	int store_pending = 0;		/* increases with every store instr at dmseg, decreases with every store pa */
 	uint32_t max_store_addr = 0;	/* for store pa address testing */
 	bool restart = 0;		/* restarting control */
 	int restart_count = 0;
@@ -186,12 +186,12 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 			LOG_DEBUG("restarting code");
 		}
 
-		retval = mips32_pracc_read_ctrl_addr(ejtag_info);		/* update current pa info: control and address */
+		retval = mips32_pracc_read_ctrl_addr(ejtag_info); /* update current pa info: control and address */
 		if (retval != ERROR_OK)
 			return retval;
 
 		/* Check for read or write access */
-		if (ejtag_info->pa_ctrl & EJTAG_CTRL_PRNW) {						/* write/store access */
+		if (ejtag_info->pa_ctrl & EJTAG_CTRL_PRNW) {				/* write/store access */
 			/* Check for pending store from a previous store instruction at dmseg */
 			if (store_pending == 0) {
 				LOG_DEBUG("unexpected write at address %" PRIx32, ejtag_info->pa_addr);
@@ -202,8 +202,8 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 					return ERROR_JTAG_DEVICE_ERROR;
 			} else {
 				/* check address */
-				if (ejtag_info->pa_addr < MIPS32_PRACC_PARAM_OUT || ejtag_info->pa_addr > max_store_addr) {
-
+				if (ejtag_info->pa_addr < MIPS32_PRACC_PARAM_OUT ||
+						ejtag_info->pa_addr > max_store_addr) {
 					LOG_DEBUG("writing at unexpected address %" PRIx32, ejtag_info->pa_addr);
 					return ERROR_JTAG_DEVICE_ERROR;
 				}
@@ -227,7 +227,8 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 							ejtag_info->pa_addr, MIPS32_PRACC_TEXT + code_count * 4);
 
 					/* restart code execution only in some cases */
-					if (code_count == 1 && ejtag_info->pa_addr == MIPS32_PRACC_TEXT && restart_count == 0) {
+					if (code_count == 1 && ejtag_info->pa_addr == MIPS32_PRACC_TEXT &&
+										restart_count == 0) {
 						LOG_DEBUG("restarting, without clean jump");
 						restart_count++;
 						code_count = 0;
@@ -264,13 +265,14 @@ int mips32_pracc_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_info *ct
 					}
 				} else {
 					if (ejtag_info->pa_addr != (MIPS32_PRACC_TEXT + code_count * 4)) {
-						LOG_DEBUG("unexpected read address in final check: %" PRIx32 ", expected: %x",
-							  ejtag_info->pa_addr, MIPS32_PRACC_TEXT + code_count * 4);
+						LOG_DEBUG("unexpected read address in final check: %"
+							PRIx32 ", expected: %x", ejtag_info->pa_addr,
+							MIPS32_PRACC_TEXT + code_count * 4);
 						return ERROR_JTAG_DEVICE_ERROR;
 					}
 				}
 				if (!pass) {
-					if ((code_count - ctx->code_count) > 1) {	 /* allow max 2 instruction delay slot */
+					if ((code_count - ctx->code_count) > 1) { /* allow max 2 instr delay slot */
 						LOG_DEBUG("failed to jump back to pracc text");
 						return ERROR_JTAG_DEVICE_ERROR;
 					}
@@ -307,6 +309,7 @@ inline void pracc_queue_init(struct pracc_queue_info *ctx)
 	ctx->store_count = 0;
 	ctx->max_code = 0;
 	ctx->pracc_list = NULL;
+	ctx->isa = ctx->ejtag_info->isa ? 1 : 0;
 }
 
 void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
@@ -332,12 +335,12 @@ void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr)
 void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize)
 {
 	if (LOWER16(data) == 0 && optimize)
-		pracc_add(ctx, 0, MIPS32_LUI(reg_num, UPPER16(data)));	/* load only upper value */
+		pracc_add(ctx, 0, MIPS32_LUI(ctx->isa, reg_num, UPPER16(data)));	/* load only upper value */
 	else if (UPPER16(data) == 0 && optimize)
-		pracc_add(ctx, 0, MIPS32_ORI(reg_num, 0, LOWER16(data)));	/* load only lower */
+		pracc_add(ctx, 0, MIPS32_ORI(ctx->isa, reg_num, 0, LOWER16(data)));	/* load only lower */
 	else {
-		pracc_add(ctx, 0, MIPS32_LUI(reg_num, UPPER16(data)));		/* load upper and lower */
-		pracc_add(ctx, 0, MIPS32_ORI(reg_num, reg_num, LOWER16(data)));
+		pracc_add(ctx, 0, MIPS32_LUI(ctx->isa, reg_num, UPPER16(data)));	/* load upper and lower */
+		pracc_add(ctx, 0, MIPS32_ORI(ctx->isa, reg_num, reg_num, LOWER16(data)));
 	}
 }
 
@@ -355,6 +358,10 @@ int mips32_pracc_queue_exec(struct mips_ejtag *ejtag_info, struct pracc_queue_in
 		return ERROR_FAIL;
 	}
 
+	if (ejtag_info->isa && ejtag_info->endianness)
+		for (int i = 0; i != ctx->code_count; i++)
+			ctx->pracc_list[i].instr = SWAP16(ctx->pracc_list[i].instr);
+
 	if (ejtag_info->mode == 0)
 		return mips32_pracc_exec(ejtag_info, ctx, buf, check_last);
 
@@ -449,17 +456,17 @@ exit:
 
 int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
 {
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));			/* $15 = MIPS32_PRACC_BASE_ADDR */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16((addr + 0x8000))));		/* load  $8 with modified upper address */
-	pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 8));				/* lw $8, LOWER16(addr)($8) */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16((addr + 0x8000)))); /* load  $8 with modified upper addr */
+	pracc_add(&ctx, 0, MIPS32_LW(ctx.isa, 8, LOWER16(addr), 8));			/* lw $8, LOWER16(addr)($8) */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
-				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* sw $8,PRACC_OUT_OFFSET($15) */
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* sw $8,PRACC_OUT_OFFSET($15) */
 	pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* move COP0 DeSave to $15 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* move COP0 DeSave to $15 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf, 1);
 	pracc_queue_free(&ctx);
@@ -471,10 +478,10 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 	if (count == 1 && size == 4)
 		return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t *)buf);
 
-	uint32_t *data = NULL;
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
+	uint32_t *data = NULL;
 	if (size != 4) {
 		data = malloc(256 * sizeof(uint32_t));
 		if (data == NULL) {
@@ -490,35 +497,36 @@ int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size
 	while (count) {
 		ctx.code_count = 0;
 		ctx.store_count = 0;
+
 		int this_round_count = (count > 256) ? 256 : count;
 		uint32_t last_upper_base_addr = UPPER16((addr + 0x8000));
 
-		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
-		pracc_add(&ctx, 0, MIPS32_LUI(9, last_upper_base_addr));	/* load the upper memory address in $9 */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR)); /* $15 = MIPS32_PRACC_BASE_ADDR */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 9, last_upper_base_addr));	/* upper memory addr to $9 */
 
 		for (int i = 0; i != this_round_count; i++) {			/* Main code loop */
 			uint32_t upper_base_addr = UPPER16((addr + 0x8000));
-			if (last_upper_base_addr != upper_base_addr) {		/* if needed, change upper address in $9 */
-				pracc_add(&ctx, 0, MIPS32_LUI(9, upper_base_addr));
+			if (last_upper_base_addr != upper_base_addr) {	/* if needed, change upper addr in $9 */
+				pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 9, upper_base_addr));
 				last_upper_base_addr = upper_base_addr;
 			}
 
-			if (size == 4)
-				pracc_add(&ctx, 0, MIPS32_LW(8, LOWER16(addr), 9));	/* load from memory to $8 */
+			if (size == 4)				/* load from memory to $8 */
+				pracc_add(&ctx, 0, MIPS32_LW(ctx.isa, 8, LOWER16(addr), 9));
 			else if (size == 2)
-				pracc_add(&ctx, 0, MIPS32_LHU(8, LOWER16(addr), 9));
+				pracc_add(&ctx, 0, MIPS32_LHU(ctx.isa, 8, LOWER16(addr), 9));
 			else
-				pracc_add(&ctx, 0, MIPS32_LBU(8, LOWER16(addr), 9));
+				pracc_add(&ctx, 0, MIPS32_LBU(ctx.isa, 8, LOWER16(addr), 9));
 
-			pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + i * 4,
-					  MIPS32_SW(8, PRACC_OUT_OFFSET + i * 4, 15));	/* store $8 at param out */
+			pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + i * 4,			/* store $8 at param out */
+					  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + i * 4, 15));
 			addr += size;
 		}
 		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 		pracc_add_li32(&ctx, 9, ejtag_info->reg9, 0);				/* restore $9 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
 		if (size == 4) {
 			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, buf32, 1);
@@ -549,17 +557,17 @@ exit:
 
 int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_reg, uint32_t cp0_sel)
 {
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));		/* $15 = MIPS32_PRACC_BASE_ADDR */
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, cp0_reg, cp0_sel));			/* move cp0 reg / sel to $8 */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, cp0_reg, cp0_sel));		/* move cp0 reg / sel to $8 */
 	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
-				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits  of $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of $8 */
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));	/* store $8 to pracc_out */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* restore $15 from DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16(ejtag_info->reg8)));	/* restore upper 16 bits  of $8 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, LOWER16(ejtag_info->reg8))); /* restore lower 16 bits of $8 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val, 1);
 	pracc_queue_free(&ctx);
@@ -568,14 +576,14 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp0_r
 
 int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel)
 {
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
 	pracc_add_li32(&ctx, 15, val, 0);				/* Load val to $15 */
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, cp0_reg, cp0_sel));			/* write $15 to cp0 reg / sel */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, cp0_reg, cp0_sel));		/* write $15 to cp0 reg / sel */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 	pracc_queue_free(&ctx);
@@ -612,23 +620,23 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r
 static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 					 uint32_t start_addr, uint32_t end_addr, int cached, int rel)
 {
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
 	/** Find cache line size in bytes */
 	uint32_t clsiz;
 	if (rel) {	/* Release 2 (rel = 1) */
-		pracc_add(&ctx, 0, MIPS32_LUI(15, PRACC_UPPER_BASE_ADDR));	/* $15 = MIPS32_PRACC_BASE_ADDR */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR)); /* $15 = MIPS32_PRACC_BASE_ADDR */
 
-		pracc_add(&ctx, 0, MIPS32_RDHWR(8, MIPS32_SYNCI_STEP));		/* load synci_step value to $8 */
+		pracc_add(&ctx, 0, MIPS32_RDHWR(ctx.isa, 8, MIPS32_SYNCI_STEP)); /* load synci_step value to $8 */
 
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT,
-				MIPS32_SW(8, PRACC_OUT_OFFSET, 15));			/* store $8 to pracc_out */
+				MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15));		/* store $8 to pracc_out */
 
 		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));	/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
 		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, &clsiz, 1);
 		if (ctx.retval != ERROR_OK)
@@ -663,45 +671,48 @@ static int mips32_pracc_synchronize_cache(struct mips_ejtag *ejtag_info,
 	end_addr |= clsiz - 1;
 
 	ctx.code_count = 0;
+	ctx.store_count = 0;
+
 	int count = 0;
 	uint32_t last_upper_base_addr = UPPER16((start_addr + 0x8000));
 
-	pracc_add(&ctx, 0, MIPS32_LUI(15, last_upper_base_addr));		/* load upper memory base address to $15 */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, last_upper_base_addr)); /* load upper memory base addr to $15 */
 
 	while (start_addr <= end_addr) {						/* main loop */
 		uint32_t upper_base_addr = UPPER16((start_addr + 0x8000));
-		if (last_upper_base_addr != upper_base_addr) {				/* if needed, change upper address in $15 */
-			pracc_add(&ctx, 0, MIPS32_LUI(15, upper_base_addr));
+		if (last_upper_base_addr != upper_base_addr) {		/* if needed, change upper addr in $15 */
+			pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, upper_base_addr));
 			last_upper_base_addr = upper_base_addr;
 		}
-		if (rel)
-			pracc_add(&ctx, 0, MIPS32_SYNCI(LOWER16(start_addr), 15));		/* synci instruction, offset($15) */
+		if (rel)			/* synci instruction, offset($15) */
+			pracc_add(&ctx, 0, MIPS32_SYNCI(ctx.isa, LOWER16(start_addr), 15));
 
 		else {
-			if (cached == 3)
-				pracc_add(&ctx, 0, MIPS32_CACHE(MIPS32_CACHE_D_HIT_WRITEBACK,
-							LOWER16(start_addr), 15));		/* cache Hit_Writeback_D, offset($15) */
-
-			pracc_add(&ctx, 0, MIPS32_CACHE(MIPS32_CACHE_I_HIT_INVALIDATE,
-							LOWER16(start_addr), 15));		/* cache Hit_Invalidate_I, offset($15) */
+			if (cached == 3)	/* cache Hit_Writeback_D, offset($15) */
+				pracc_add(&ctx, 0, MIPS32_CACHE(ctx.isa, MIPS32_CACHE_D_HIT_WRITEBACK,
+							LOWER16(start_addr), 15));
+			/* cache Hit_Invalidate_I, offset($15) */
+			pracc_add(&ctx, 0, MIPS32_CACHE(ctx.isa, MIPS32_CACHE_I_HIT_INVALIDATE,
+							LOWER16(start_addr), 15));
 		}
 		start_addr += clsiz;
 		count++;
-		if (count == 256 && start_addr <= end_addr) {				/* more ?, then execute code list */
-			pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));		/* jump to start */
-			pracc_add(&ctx, 0, MIPS32_NOP);						/* nop in delay slot */
+		if (count == 256 && start_addr <= end_addr) {			/* more ?, then execute code list */
+			pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* to start */
+			pracc_add(&ctx, 0, MIPS32_NOP);					/* nop in delay slot */
 
 			ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 			if (ctx.retval != ERROR_OK)
 				goto exit;
 
-			ctx.code_count = 0;
+			ctx.code_count = 0;	/* reset counters for another loop */
+			ctx.store_count = 0;
 			count = 0;
 		}
 	}
-	pracc_add(&ctx, 0, MIPS32_SYNC);
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));					/* restore $15 from DeSave*/
+	pracc_add(&ctx, 0, MIPS32_SYNC(ctx.isa));
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));				/* restore $15 from DeSave*/
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 exit:
@@ -712,7 +723,7 @@ exit:
 static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 		uint32_t addr, int size, int count, const void *buf)
 {
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
 	const uint32_t *buf32 = buf;
@@ -722,31 +733,32 @@ static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 	while (count) {
 		ctx.code_count = 0;
 		ctx.store_count = 0;
+
 		int this_round_count = (count > 128) ? 128 : count;
 		uint32_t last_upper_base_addr = UPPER16((addr + 0x8000));
-
-		pracc_add(&ctx, 0, MIPS32_LUI(15, last_upper_base_addr));		/* load $15 with memory base address */
+			      /* load $15 with memory base address */
+		pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, last_upper_base_addr));
 
 		for (int i = 0; i != this_round_count; i++) {
 			uint32_t upper_base_addr = UPPER16((addr + 0x8000));
-			if (last_upper_base_addr != upper_base_addr) {
-				pracc_add(&ctx, 0, MIPS32_LUI(15, upper_base_addr));	/* if needed, change upper address in $15*/
+			if (last_upper_base_addr != upper_base_addr) {	/* if needed, change upper address in $15*/
+				pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, upper_base_addr));
 				last_upper_base_addr = upper_base_addr;
 			}
 
 			if (size == 4) {
-				pracc_add_li32(&ctx, 8, *buf32, 1);			/* load with li32, optimize */
-				pracc_add(&ctx, 0, MIPS32_SW(8, LOWER16(addr), 15));		/* store word to memory */
+				pracc_add_li32(&ctx, 8, *buf32, 1);		/* load with li32, optimize */
+				pracc_add(&ctx, 0, MIPS32_SW(ctx.isa, 8, LOWER16(addr), 15)); /* store word to mem */
 				buf32++;
 
 			} else if (size == 2) {
-				pracc_add(&ctx, 0, MIPS32_ORI(8, 0, *buf16));		/* load lower value */
-				pracc_add(&ctx, 0, MIPS32_SH(8, LOWER16(addr), 15));	/* store half word to memory */
+				pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 0, *buf16));		/* load lower value */
+				pracc_add(&ctx, 0, MIPS32_SH(ctx.isa, 8, LOWER16(addr), 15)); /* store half word */
 				buf16++;
 
 			} else {
-				pracc_add(&ctx, 0, MIPS32_ORI(8, 0, *buf8));		/* load lower value */
-				pracc_add(&ctx, 0, MIPS32_SB(8, LOWER16(addr), 15));	/* store byte to memory */
+				pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 0, *buf8));		/* load lower value */
+				pracc_add(&ctx, 0, MIPS32_SB(ctx.isa, 8, LOWER16(addr), 15));	/* store byte */
 				buf8++;
 			}
 			addr += size;
@@ -754,8 +766,8 @@ static int mips32_pracc_write_mem_generic(struct mips_ejtag *ejtag_info,
 
 		pracc_add_li32(&ctx, 8, ejtag_info->reg8, 0);				/* restore $8 */
 
-		pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
-		pracc_add(&ctx, 0, MIPS32_MFC0(15, 31, 0));				/* restore $15 from DeSave */
+		pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));	/* jump to start */
+		pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0));			/* restore $15 from DeSave */
 
 		ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 		if (ctx.retval != ERROR_OK)
@@ -824,18 +836,18 @@ int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int siz
 
 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 {
-	static const uint32_t cp0_write_code[] = {
-		MIPS32_MTC0(1, 12, 0),							/* move $1 to status */
-		MIPS32_MTLO(1),									/* move $1 to lo */
-		MIPS32_MTHI(1),									/* move $1 to hi */
-		MIPS32_MTC0(1, 8, 0),							/* move $1 to badvaddr */
-		MIPS32_MTC0(1, 13, 0),							/* move $1 to cause*/
-		MIPS32_MTC0(1, 24, 0),							/* move $1 to depc (pc) */
-	};
-
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
+	uint32_t cp0_write_code[] = {
+		MIPS32_MTC0(ctx.isa, 1, 12, 0),					/* move $1 to status */
+		MIPS32_MTLO(ctx.isa, 1),						/* move $1 to lo */
+		MIPS32_MTHI(ctx.isa, 1),						/* move $1 to hi */
+		MIPS32_MTC0(ctx.isa, 1, 8, 0),					/* move $1 to badvaddr */
+		MIPS32_MTC0(ctx.isa, 1, 13, 0),					/* move $1 to cause*/
+		MIPS32_MTC0(ctx.isa, 1, 24, 0),					/* move $1 to depc (pc) */
+	};
+
 	/* load registers 2 to 31 with li32, optimize */
 	for (int i = 2; i < 32; i++)
 		pracc_add_li32(&ctx, i, regs[i], 1);
@@ -844,10 +856,10 @@ int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 		pracc_add_li32(&ctx, 1, regs[i + 32], 0);	/* load CPO value in $1 */
 		pracc_add(&ctx, 0, cp0_write_code[i]);			/* write value from $1 to CPO register */
 	}
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));				/* load $15 in DeSave */
-	pracc_add(&ctx, 0, MIPS32_LUI(1, UPPER16((regs[1]))));			/* load upper half word in $1 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));				/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(1, 1, LOWER16((regs[1]))));		/* load lower half word in $1 */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, 31, 0));				/* load $15 in DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 1, UPPER16((regs[1]))));		/* load upper half word in $1 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 1, 1, LOWER16((regs[1]))));	/* load lower half word in $1 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 
@@ -859,37 +871,37 @@ int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 
 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 {
-	static int cp0_read_code[] = {
-		MIPS32_MFC0(8, 12, 0),							/* move status to $8 */
-		MIPS32_MFLO(8),									/* move lo to $8 */
-		MIPS32_MFHI(8),									/* move hi to $8 */
-		MIPS32_MFC0(8, 8, 0),							/* move badvaddr to $8 */
-		MIPS32_MFC0(8, 13, 0),							/* move cause to $8 */
-		MIPS32_MFC0(8, 24, 0),							/* move depc (pc) to $8 */
-	};
-
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(1, 31, 0));						/* move $1 to COP0 DeSave */
-	pracc_add(&ctx, 0, MIPS32_LUI(1, PRACC_UPPER_BASE_ADDR));				/* $1 = MIP32_PRACC_BASE_ADDR */
+	uint32_t cp0_read_code[] = {
+		MIPS32_MFC0(ctx.isa, 8, 12, 0),					/* move status to $8 */
+		MIPS32_MFLO(ctx.isa, 8),						/* move lo to $8 */
+		MIPS32_MFHI(ctx.isa, 8),						/* move hi to $8 */
+		MIPS32_MFC0(ctx.isa, 8, 8, 0),					/* move badvaddr to $8 */
+		MIPS32_MFC0(ctx.isa, 8, 13, 0),					/* move cause to $8 */
+		MIPS32_MFC0(ctx.isa, 8, 24, 0),					/* move depc (pc) to $8 */
+	};
+
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 1, 31, 0));				/* move $1 to COP0 DeSave */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 1, PRACC_UPPER_BASE_ADDR));	/* $1 = MIP32_PRACC_BASE_ADDR */
 
 	for (int i = 2; i != 32; i++)					/* store GPR's 2 to 31 */
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + (i * 4),
-				  MIPS32_SW(i, PRACC_OUT_OFFSET + (i * 4), 1));
+				  MIPS32_SW(ctx.isa, i, PRACC_OUT_OFFSET + (i * 4), 1));
 
 	for (int i = 0; i != 6; i++) {
 		pracc_add(&ctx, 0, cp0_read_code[i]);				/* load COP0 needed registers to $8 */
 		pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + (i + 32) * 4,			/* store $8 at PARAM OUT */
-				  MIPS32_SW(8, PRACC_OUT_OFFSET + (i + 32) * 4, 1));
+				  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + (i + 32) * 4, 1));
 	}
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, 31, 0));					/* move DeSave to $8, reg1 value */
-	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + 4,					/* store reg1 value from $8 to param out */
-			  MIPS32_SW(8, PRACC_OUT_OFFSET + 4, 1));
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, 31, 0));			/* move DeSave to $8, reg1 value */
+	pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + 4,			/* store reg1 value from $8 to param out */
+			  MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + 4, 1));
 
-	pracc_add(&ctx, 0, MIPS32_MFC0(1, 31, 0));					/* move COP0 DeSave to $1, restore reg1 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16(ctx.code_count + 1)));					/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_MTC0(15, 31, 0));					/* load $15 in DeSave */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 1, 31, 0));		/* move COP0 DeSave to $1, restore reg1 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, 31, 0));				/* load $15 in DeSave */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, regs, 1);
 
@@ -909,38 +921,40 @@ int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
 int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
 		int write_t, uint32_t addr, int count, uint32_t *buf)
 {
+	uint32_t isa = ejtag_info->isa ? 1 : 0;
 	uint32_t handler_code[] = {
 		/* r15 points to the start of this code */
-		MIPS32_SW(8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
-		MIPS32_SW(9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
-		MIPS32_SW(10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
-		MIPS32_SW(11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
+		MIPS32_SW(isa, 8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
+		MIPS32_SW(isa, 9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
+		MIPS32_SW(isa, 10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
+		MIPS32_SW(isa, 11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
 		/* start of fastdata area in t0 */
-		MIPS32_LUI(8, UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
-		MIPS32_ORI(8, 8, LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
-		MIPS32_LW(9, 0, 8),						/* start addr in t1 */
-		MIPS32_LW(10, 0, 8),						/* end addr to t2 */
+		MIPS32_LUI(isa, 8, UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
+		MIPS32_ORI(isa, 8, 8, LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
+		MIPS32_LW(isa, 9, 0, 8),						/* start addr in t1 */
+		MIPS32_LW(isa, 10, 0, 8),						/* end addr to t2 */
 					/* loop: */
-		write_t ? MIPS32_LW(11, 0, 8) : MIPS32_LW(11, 0, 9),	/* from xfer area : from memory */
-		write_t ? MIPS32_SW(11, 0, 9) : MIPS32_SW(11, 0, 8),	/* to memory      : to xfer area */
+		write_t ? MIPS32_LW(isa, 11, 0, 8) : MIPS32_LW(isa, 11, 0, 9),	/* from xfer area : from memory */
+		write_t ? MIPS32_SW(isa, 11, 0, 9) : MIPS32_SW(isa, 11, 0, 8),	/* to memory      : to xfer area */
 
-		MIPS32_BNE(10, 9, NEG16(3)),			/* bne $t2,t1,loop */
-		MIPS32_ADDI(9, 9, 4),					/* addi t1,t1,4 */
+		MIPS32_BNE(isa, 10, 9, NEG16(3 << isa)),			/* bne $t2,t1,loop */
+		MIPS32_ADDI(isa, 9, 9, 4),					/* addi t1,t1,4 */
 
-		MIPS32_LW(8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
-		MIPS32_LW(9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
-		MIPS32_LW(10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
-		MIPS32_LW(11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
+		MIPS32_LW(isa, 8, MIPS32_FASTDATA_HANDLER_SIZE - 4, 15),
+		MIPS32_LW(isa, 9, MIPS32_FASTDATA_HANDLER_SIZE - 8, 15),
+		MIPS32_LW(isa, 10, MIPS32_FASTDATA_HANDLER_SIZE - 12, 15),
+		MIPS32_LW(isa, 11, MIPS32_FASTDATA_HANDLER_SIZE - 16, 15),
 
-		MIPS32_LUI(15, UPPER16(MIPS32_PRACC_TEXT)),
-		MIPS32_ORI(15, 15, LOWER16(MIPS32_PRACC_TEXT)),
-		MIPS32_JR(15),								/* jr start */
-		MIPS32_MFC0(15, 31, 0),						/* move COP0 DeSave to $15 */
+		MIPS32_LUI(isa, 15, UPPER16(MIPS32_PRACC_TEXT)),
+		MIPS32_ORI(isa, 15, 15, LOWER16(MIPS32_PRACC_TEXT) | isa),	/* isa bit for JR instr */
+		MIPS32_JR(isa, 15),								/* jr start */
+		MIPS32_MFC0(isa, 15, 31, 0),					/* move COP0 DeSave to $15 */
 	};
 
 	if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
+	pracc_swap16_array(ejtag_info, handler_code, ARRAY_SIZE(handler_code));
 		/* write program into RAM */
 	if (write_t != ejtag_info->fast_access_save) {
 		mips32_pracc_write_mem(ejtag_info, source->address, 4, ARRAY_SIZE(handler_code), handler_code);
@@ -951,12 +965,14 @@ int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_are
 	LOG_DEBUG("%s using 0x%.8" TARGET_PRIxADDR " for write handler", __func__, source->address);
 
 	uint32_t jmp_code[] = {
-		MIPS32_LUI(15, UPPER16(source->address)),		/* load addr of jump in $15 */
-		MIPS32_ORI(15, 15, LOWER16(source->address)),
-		MIPS32_JR(15),					/* jump to ram program */
-		MIPS32_NOP,
+		MIPS32_LUI(isa, 15, UPPER16(source->address)),			/* load addr of jump in $15 */
+		MIPS32_ORI(isa, 15, 15, LOWER16(source->address) | isa),	/* isa bit for JR instr */
+		MIPS32_JR(isa, 15),						/* jump to ram program */
+		isa ? MIPS32_XORI(isa, 15, 15, 1) : MIPS32_NOP,	/* drop isa bit, needed for LW/SW instructions */
 	};
 
+	pracc_swap16_array(ejtag_info, jmp_code, ARRAY_SIZE(jmp_code));
+
 	/* execute jump code, with no address check */
 	for (unsigned i = 0; i < ARRAY_SIZE(jmp_code); i++) {
 		int retval = wait_for_pracc_rw(ejtag_info);
diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index e990f8d..fe9f814 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -40,6 +40,7 @@
 #define UPPER16(uint32_t)				(uint32_t >> 16)
 #define LOWER16(uint32_t)				(uint32_t & 0xFFFF)
 #define NEG16(v)						(((~(v)) + 1) & 0xFFFF)
+#define SWAP16(v)				((LOWER16(v) << 16) | (UPPER16(v)))
 /*#define NEG18(v) (((~(v)) + 1) & 0x3FFFF)*/
 
 #define PRACC_BLOCK	128	/* 1 Kbyte */
@@ -50,12 +51,15 @@ typedef struct {
 } pa_list;
 
 struct pracc_queue_info {
+	struct mips_ejtag *ejtag_info;
+	unsigned isa;
 	int retval;
 	int code_count;
 	int store_count;
 	int max_code;		/* max intstructions with currently allocated memory */
 	pa_list *pracc_list;	/* Code and store addresses at dmseg */
 };
+
 void pracc_queue_init(struct pracc_queue_info *ctx);
 void pracc_add(struct pracc_queue_info *ctx, uint32_t addr, uint32_t instr);
 void pracc_add_li32(struct pracc_queue_info *ctx, uint32_t reg_num, uint32_t data, bool optimize);
@@ -108,4 +112,11 @@ int mips32_cp0_read(struct mips_ejtag *ejtag_info,
 int mips32_cp0_write(struct mips_ejtag *ejtag_info,
 		uint32_t val, uint32_t cp0_reg, uint32_t cp0_sel);
 
+inline void pracc_swap16_array(struct mips_ejtag *ejtag_info, uint32_t *buf, int count)
+{
+	if (ejtag_info->isa && ejtag_info->endianness)
+		for (int i = 0; i != count; i++)
+			buf[i] = SWAP16(buf[i]);
+}
+
 #endif /* OPENOCD_TARGET_MIPS32_PRACC_H */
diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index e35758d..d335c1b 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -197,18 +197,18 @@ void mips_ejtag_drscan_8_out(struct mips_ejtag *ejtag_info, uint8_t data)
 /* Set (to enable) or clear (to disable stepping) the SSt bit (bit 8) in Cp0 Debug reg (reg 23, sel 0) */
 int mips_ejtag_config_step(struct mips_ejtag *ejtag_info, int enable_step)
 {
-	struct pracc_queue_info ctx;
+	struct pracc_queue_info ctx = {.ejtag_info = ejtag_info};
 	pracc_queue_init(&ctx);
 
-	pracc_add(&ctx, 0, MIPS32_MFC0(8, 23, 0));			/* move COP0 Debug to $8 */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, 0x0100));			/* set SSt bit in debug reg */
+	pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 8, 23, 0));			/* move COP0 Debug to $8 */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, 0x0100));			/* set SSt bit in debug reg */
 	if (!enable_step)
-		pracc_add(&ctx, 0, MIPS32_XORI(8, 8, 0x0100));		/* clear SSt bit in debug reg */
+		pracc_add(&ctx, 0, MIPS32_XORI(ctx.isa, 8, 8, 0x0100));		/* clear SSt bit in debug reg */
 
-	pracc_add(&ctx, 0, MIPS32_MTC0(8, 23, 0));			/* move $8 to COP0 Debug */
-	pracc_add(&ctx, 0, MIPS32_LUI(8, UPPER16(ejtag_info->reg8)));		/* restore upper 16 bits  of $8 */
-	pracc_add(&ctx, 0, MIPS32_B(NEG16((ctx.code_count + 1))));			/* jump to start */
-	pracc_add(&ctx, 0, MIPS32_ORI(8, 8, LOWER16(ejtag_info->reg8)));	/* restore lower 16 bits of $8 */
+	pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 8, 23, 0));			/* move $8 to COP0 Debug */
+	pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16(ejtag_info->reg8)));	/* restore upper 16 bits  of $8 */
+	pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa)));		/* jump to start */
+	pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, LOWER16(ejtag_info->reg8))); /* restore lower 16 bits of $8 */
 
 	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 1);
 	pracc_queue_free(&ctx);
@@ -269,11 +269,11 @@ error:
 
 int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info)
 {
-	pa_list pracc_list = {.instr = MIPS32_DRET, .addr = 0};
+	pa_list pracc_list = {.instr = MIPS32_DRET(ejtag_info->isa), .addr = 0};
 	struct pracc_queue_info ctx = {.max_code = 1, .pracc_list = &pracc_list, .code_count = 1, .store_count = 0};
 
 	/* execute our dret instruction */
-	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 0);
+	ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, NULL, 0); /* shift out instr, omit last check */
 
 	/* pic32mx workaround, false pending at low core clock */
 	jtag_add_sleep(1000);
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 3043df9..ade1b4c 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -58,6 +58,7 @@
 #define EJTAG_CTRL_DERR			(1 << 10)
 #define EJTAG_CTRL_DSTRT		(1 << 11)
 #define EJTAG_CTRL_JTAGBRK		(1 << 12)
+#define EJTAG_CTRL_DBGISA		(1 << 13)
 #define EJTAG_CTRL_SETDEV		(1 << 14)
 #define EJTAG_CTRL_PROBEN		(1 << 15)
 #define EJTAG_CTRL_PRRST		(1 << 16)
@@ -189,6 +190,8 @@ struct mips_ejtag {
 	uint32_t pa_ctrl;
 	uint32_t pa_addr;
 	unsigned int ejtag_version;
+	uint32_t isa;
+	uint32_t endianness;
 
 	/* Memory-Mapped Registers. This addresses are not same on different
 	 * EJTAG versions. */
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index bd3c602..8b32d11 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -195,6 +195,8 @@ static int mips_m4k_poll(struct target *target)
 	if (retval != ERROR_OK)
 		return retval;
 
+	ejtag_info->isa = (ejtag_ctrl & EJTAG_CTRL_DBGISA) ? 1 : 0;
+
 	/* clear this bit before handling polling
 	 * as after reset registers will read zero */
 	if (ejtag_ctrl & EJTAG_CTRL_ROCC) {
@@ -648,14 +650,14 @@ static int mips_m4k_set_breakpoint(struct target *target,
 					breakpoint->orig_instr);
 			if (retval != ERROR_OK)
 				return retval;
-			retval = target_write_u32(target, breakpoint->address, MIPS32_SDBBP);
+			retval = target_write_u32(target, breakpoint->address, MIPS32_SDBBP(ejtag_info->isa));
 			if (retval != ERROR_OK)
 				return retval;
 
 			retval = target_read_u32(target, breakpoint->address, &verify);
 			if (retval != ERROR_OK)
 				return retval;
-			if (verify != MIPS32_SDBBP) {
+			if (verify != MIPS32_SDBBP(ejtag_info->isa)) {
 				LOG_ERROR("Unable to set 32-bit breakpoint at address " TARGET_ADDR_FMT
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
@@ -667,14 +669,14 @@ static int mips_m4k_set_breakpoint(struct target *target,
 					breakpoint->orig_instr);
 			if (retval != ERROR_OK)
 				return retval;
-			retval = target_write_u16(target, breakpoint->address, MIPS16_SDBBP);
+			retval = target_write_u16(target, breakpoint->address, MIPS16_SDBBP(ejtag_info->isa));
 			if (retval != ERROR_OK)
 				return retval;
 
 			retval = target_read_u16(target, breakpoint->address, &verify);
 			if (retval != ERROR_OK)
 				return retval;
-			if (verify != MIPS16_SDBBP) {
+			if (verify != MIPS16_SDBBP(ejtag_info->isa)) {
 				LOG_ERROR("Unable to set 16-bit breakpoint at address " TARGET_ADDR_FMT
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
@@ -735,7 +737,7 @@ static int mips_m4k_unset_breakpoint(struct target *target,
 			 */
 			current_instr = target_buffer_get_u32(target, (uint8_t *)&current_instr);
 
-			if (current_instr == MIPS32_SDBBP) {
+			if (current_instr == MIPS32_SDBBP(ejtag_info->isa)) {
 				retval = target_write_memory(target, breakpoint->address, 4, 1,
 						breakpoint->orig_instr);
 				if (retval != ERROR_OK)
@@ -750,7 +752,7 @@ static int mips_m4k_unset_breakpoint(struct target *target,
 			if (retval != ERROR_OK)
 				return retval;
 			current_instr = target_buffer_get_u16(target, (uint8_t *)&current_instr);
-			if (current_instr == MIPS16_SDBBP) {
+			if (current_instr == MIPS16_SDBBP(ejtag_info->isa)) {
 				retval = target_write_memory(target, breakpoint->address, 2, 1,
 						breakpoint->orig_instr);
 				if (retval != ERROR_OK)
-- 
cgit v1.1


From f5151b6d46cb0b939e3beb0fc61cb8718908f4b8 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Sun, 7 May 2017 13:36:12 +0200
Subject: mips32, convert miniprograms with code definition

Needed to run in micromips mode. Seems that if an isa
is supported in debug mode it also supported in kernel
mode. The contrary is not true.

Change-Id: I1feb8e2c376f4db97089f05c20bc0cd177208fb3
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4033
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32.c | 107 +++++++++++++++++++++++++++++++---------------------
 1 file changed, 63 insertions(+), 44 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32.c b/src/target/mips32.c
index 5c85fea..c5b7fbc 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -705,47 +705,55 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,
 	struct reg_param reg_params[2];
 	struct mips32_algorithm mips32_info;
 
-	/* see contrib/loaders/checksum/mips32.s for src */
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
-	static const uint32_t mips_crc_code[] = {
-		0x248C0000,		/* addiu	$t4, $a0, 0 */
-		0x24AA0000,		/* addiu	$t2, $a1, 0 */
-		0x2404FFFF,		/* addiu	$a0, $zero, 0xffffffff */
-		0x10000010,		/* beq		$zero, $zero, ncomp */
-		0x240B0000,		/* addiu	$t3, $zero, 0 */
+	/* see contrib/loaders/checksum/mips32.s for src */
+	uint32_t isa = ejtag_info->isa ? 1 : 0;
+
+	uint32_t mips_crc_code[] = {
+		MIPS32_ADDIU(isa, 12, 4, 0),			/* addiu	$t4, $a0, 0 */
+		MIPS32_ADDIU(isa, 10, 5, 0),			/* addiu	$t2, $a1, 0 */
+		MIPS32_ADDIU(isa, 4, 0, 0xFFFF),		/* addiu	$a0, $zero, 0xffff */
+		MIPS32_BEQ(isa, 0, 0, 0x10 << isa),		/* beq		$zero, $zero, ncomp */
+		MIPS32_ADDIU(isa, 11, 0, 0),			/* addiu	$t3, $zero, 0 */
 						/* nbyte: */
-		0x81850000,		/* lb		$a1, ($t4) */
-		0x218C0001,		/* addi		$t4, $t4, 1 */
-		0x00052E00,		/* sll		$a1, $a1, 24 */
-		0x3C0204C1,		/* lui		$v0, 0x04c1 */
-		0x00852026,		/* xor		$a0, $a0, $a1 */
-		0x34471DB7,		/* ori		$a3, $v0, 0x1db7 */
-		0x00003021,		/* addu		$a2, $zero, $zero */
-						/* loop: */
-		0x00044040,		/* sll		$t0, $a0, 1 */
-		0x24C60001,		/* addiu	$a2, $a2, 1 */
-		0x28840000,		/* slti		$a0, $a0, 0 */
-		0x01074826,		/* xor		$t1, $t0, $a3 */
-		0x0124400B,		/* movn		$t0, $t1, $a0 */
-		0x28C30008,		/* slti		$v1, $a2, 8 */
-		0x1460FFF9,		/* bne		$v1, $zero, loop */
-		0x01002021,		/* addu		$a0, $t0, $zero */
-						/* ncomp: */
-		0x154BFFF0,		/* bne		$t2, $t3, nbyte */
-		0x256B0001,		/* addiu	$t3, $t3, 1 */
-		0x7000003F,		/* sdbbp */
+		MIPS32_LB(isa, 5, 0, 12),			/* lb		$a1, ($t4) */
+		MIPS32_ADDI(isa, 12, 12, 1),			/* addi		$t4, $t4, 1 */
+		MIPS32_SLL(isa, 5, 5, 24),			/* sll		$a1, $a1, 24 */
+		MIPS32_LUI(isa, 2, 0x04c1),			/* lui		$v0, 0x04c1 */
+		MIPS32_XOR(isa, 4, 4, 5),			/* xor		$a0, $a0, $a1 */
+		MIPS32_ORI(isa, 7, 2, 0x1db7),			/* ori		$a3, $v0, 0x1db7 */
+		MIPS32_ADDU(isa, 6, 0, 0),			/* addu		$a2, $zero, $zero */
+						/* loop */
+		MIPS32_SLL(isa, 8, 4, 1),			/* sll		$t0, $a0, 1 */
+		MIPS32_ADDIU(isa, 6, 6, 1),			/* addiu	$a2, $a2, 1 */
+		MIPS32_SLTI(isa, 4, 4, 0),			/* slti		$a0, $a0, 0 */
+		MIPS32_XOR(isa, 9, 8, 7),			/* xor		$t1, $t0, $a3 */
+		MIPS32_MOVN(isa, 8, 9, 4),			/* movn		$t0, $t1, $a0 */
+		MIPS32_SLTI(isa, 3, 6, 8),			/* slti		$v1, $a2, 8 */
+		MIPS32_BNE(isa, 3, 0, NEG16(7 << isa)),		/* bne		$v1, $zero, loop */
+		MIPS32_ADDU(isa, 4, 8, 0),			/* addu		$a0, $t0, $zero */
+						/* ncomp */
+		MIPS32_BNE(isa, 10, 11, NEG16(16 << isa)),	/* bne		$t2, $t3, nbyte */
+		MIPS32_ADDIU(isa, 11, 11, 1),			/* addiu	$t3, $t3, 1 */
+		MIPS32_SDBBP(isa),
 	};
 
 	/* make sure we have a working area */
 	if (target_alloc_working_area(target, sizeof(mips_crc_code), &crc_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
+	pracc_swap16_array(ejtag_info, mips_crc_code, ARRAY_SIZE(mips_crc_code));
+
 	/* convert mips crc code into a buffer in target endianness */
 	uint8_t mips_crc_code_8[sizeof(mips_crc_code)];
 	target_buffer_set_u32_array(target, mips_crc_code_8,
 					ARRAY_SIZE(mips_crc_code), mips_crc_code);
 
-	target_write_buffer(target, crc_algorithm->address, sizeof(mips_crc_code), mips_crc_code_8);
+	int retval = target_write_buffer(target, crc_algorithm->address, sizeof(mips_crc_code), mips_crc_code_8);
+	if (retval != ERROR_OK)
+		return retval;
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
 	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
@@ -758,9 +766,11 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,
 
 	int timeout = 20000 * (1 + (count / (1024 * 1024)));
 
-	int retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
-			crc_algorithm->address, crc_algorithm->address + (sizeof(mips_crc_code) - 4), timeout,
-			&mips32_info);
+	/* same isa as in debug mode */
+	retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
+			crc_algorithm->address | isa,
+			(crc_algorithm->address + (sizeof(mips_crc_code) - 4)) | isa,
+			timeout, &mips32_info);
 
 	if (retval == ERROR_OK)
 		*checksum = buf_get_u32(reg_params[0].value, 0, 32);
@@ -781,32 +791,40 @@ int mips32_blank_check_memory(struct target *target,
 	struct reg_param reg_params[3];
 	struct mips32_algorithm mips32_info;
 
-	static const uint32_t erase_check_code[] = {
-						/* nbyte: */
-		0x80880000,		/* lb		$t0, ($a0) */
-		0x00C83024,		/* and		$a2, $a2, $t0 */
-		0x24A5FFFF,		/* addiu	$a1, $a1, -1 */
-		0x14A0FFFC,		/* bne		$a1, $zero, nbyte */
-		0x24840001,		/* addiu	$a0, $a0, 1 */
-		0x7000003F		/* sdbbp */
-	};
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
 
 	if (erased_value != 0xff) {
 		LOG_ERROR("Erase value 0x%02" PRIx8 " not yet supported for MIPS32",
 			erased_value);
 		return ERROR_FAIL;
 	}
+	uint32_t isa = ejtag_info->isa ? 1 : 0;
+	uint32_t erase_check_code[] = {
+						/* nbyte: */
+		MIPS32_LB(isa, 8, 0, 4),			/* lb		$t0, ($a0) */
+		MIPS32_AND(isa, 6, 6, 8),			/* and		$a2, $a2, $t0 */
+		MIPS32_ADDIU(isa, 5, 5, NEG16(1)),		/* addiu	$a1, $a1, -1 */
+		MIPS32_BNE(isa, 5, 0, NEG16(4 << isa)),		/* bne		$a1, $zero, nbyte */
+		MIPS32_ADDIU(isa, 4, 4, 1),			/* addiu	$a0, $a0, 1 */
+		MIPS32_SDBBP(isa)				/* sdbbp */
+	};
 
 	/* make sure we have a working area */
 	if (target_alloc_working_area(target, sizeof(erase_check_code), &erase_check_algorithm) != ERROR_OK)
 		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 
+	pracc_swap16_array(ejtag_info, erase_check_code, ARRAY_SIZE(erase_check_code));
+
 	/* convert erase check code into a buffer in target endianness */
 	uint8_t erase_check_code_8[sizeof(erase_check_code)];
 	target_buffer_set_u32_array(target, erase_check_code_8,
 					ARRAY_SIZE(erase_check_code), erase_check_code);
 
-	target_write_buffer(target, erase_check_algorithm->address, sizeof(erase_check_code), erase_check_code_8);
+	int retval = target_write_buffer(target, erase_check_algorithm->address,
+						sizeof(erase_check_code), erase_check_code_8);
+	if (retval != ERROR_OK)
+		return retval;
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
 	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
@@ -820,9 +838,10 @@ int mips32_blank_check_memory(struct target *target,
 	init_reg_param(&reg_params[2], "r6", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
-	int retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-			erase_check_algorithm->address,
-			erase_check_algorithm->address + (sizeof(erase_check_code) - 4),
+	/* same isa as in debug mode */
+	retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
+			erase_check_algorithm->address | isa,
+			(erase_check_algorithm->address + (sizeof(erase_check_code) - 4)) | isa,
 			10000, &mips32_info);
 
 	if (retval == ERROR_OK)
-- 
cgit v1.1


From 4831ce4433adf4e2fbf5729bebf8c0f7c2fce1c1 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Sun, 7 May 2017 18:39:17 +0200
Subject: mips32: add micromips isa handling

Read and save configuration registers, up to 4.
Config3 holds the micromips implementation info.
Added isa implementation info to mips32_common.
Added isa filter to avoid common mistakes, but only
if one isa mode is implemented.
When resuming the isa requested is set if more than
one isa mode is implemented.

Change-Id: I1d6526c5525bffac8d75e031b842b2edc6310e28
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4123
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips32.c     | 67 ++++++++++++++++++++++++++++++++++++++-----------
 src/target/mips32.h     | 14 +++++++++++
 src/target/mips_ejtag.h |  3 +++
 src/target/mips_m4k.c   | 15 +++++++----
 src/target/mips_m4k.h   | 11 ++++++++
 5 files changed, 91 insertions(+), 19 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips32.c b/src/target/mips32.c
index c5b7fbc..93fb4e6 100644
--- a/src/target/mips32.c
+++ b/src/target/mips32.c
@@ -34,7 +34,7 @@
 #include "register.h"
 
 static const char *mips_isa_strings[] = {
-	"MIPS32", "MIPS16"
+	"MIPS32", "MIPS16", "", "MICRO MIPS32",
 };
 
 #define MIPS32_GDB_DUMMY_FP_REG 1
@@ -375,6 +375,7 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 	target->arch_info = mips32;
 	mips32->common_magic = MIPS32_COMMON_MAGIC;
 	mips32->fast_data_area = NULL;
+	mips32->isa_imp = MIPS32_ONLY;	/* default */
 
 	/* has breakpoint/watchpoint unit been scanned */
 	mips32->bp_scanned = 0;
@@ -388,7 +389,7 @@ int mips32_init_arch_info(struct target *target, struct mips32_common *mips32, s
 	mips32->ejtag_info.scan_delay = MIPS32_SCAN_DELAY_LEGACY_MODE;
 	mips32->ejtag_info.mode = 0;			/* Initial default value */
 	mips32->ejtag_info.isa = 0;	/* isa on debug mips32, updated by poll function */
-
+	mips32->ejtag_info.config_regs = 0;	/* no config register read */
 	return ERROR_OK;
 }
 
@@ -698,6 +699,50 @@ int mips32_enable_interrupts(struct target *target, int enable)
 	return ERROR_OK;
 }
 
+/* read config to config3 cp0 registers and log isa implementation */
+int mips32_read_config_regs(struct target *target)
+{
+	struct mips32_common *mips32 = target_to_mips32(target);
+	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
+
+	if (ejtag_info->config_regs == 0)
+		for (int i = 0; i != 4; i++) {
+			int retval = mips32_cp0_read(ejtag_info, &ejtag_info->config[i], 16, i);
+			if (retval != ERROR_OK) {
+				LOG_ERROR("isa info not available, failed to read cp0 config register: %" PRId32, i);
+				ejtag_info->config_regs = 0;
+				return retval;
+			}
+			ejtag_info->config_regs = i + 1;
+			if ((ejtag_info->config[i] & (1 << 31)) == 0)
+				break;	/* no more config registers implemented */
+		}
+	else
+		return ERROR_OK;	/* already succesfully read */
+
+	LOG_DEBUG("read  %"PRId32" config registers", ejtag_info->config_regs);
+
+	if (ejtag_info->impcode & EJTAG_IMP_MIPS16) {
+		mips32->isa_imp = MIPS32_MIPS16;
+		LOG_USER("MIPS32 with MIPS16 support implemented");
+
+	} else if (ejtag_info->config_regs >= 4) {	/* config3 implemented */
+		unsigned isa_imp = (ejtag_info->config[3] & MIPS32_CONFIG3_ISA_MASK) >> MIPS32_CONFIG3_ISA_SHIFT;
+		if (isa_imp == 1) {
+			mips32->isa_imp = MMIPS32_ONLY;
+			LOG_USER("MICRO MIPS32 only implemented");
+
+		} else if (isa_imp != 0) {
+			mips32->isa_imp = MIPS32_MMIPS32;
+			LOG_USER("MIPS32 and MICRO MIPS32 implemented");
+		}
+	}
+
+	if (mips32->isa_imp == MIPS32_ONLY)	/* initial default value */
+		LOG_USER("MIPS32 only implemented");
+
+	return ERROR_OK;
+}
 int mips32_checksum_memory(struct target *target, target_addr_t address,
 		uint32_t count, uint32_t *checksum)
 {
@@ -756,7 +801,7 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,
 		return retval;
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
-	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
+	mips32_info.isa_mode = isa ? MIPS32_ISA_MMIPS32 : MIPS32_ISA_MIPS32;	/* run isa as in debug mode */
 
 	init_reg_param(&reg_params[0], "r4", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[0].value, 0, 32, address);
@@ -766,11 +811,8 @@ int mips32_checksum_memory(struct target *target, target_addr_t address,
 
 	int timeout = 20000 * (1 + (count / (1024 * 1024)));
 
-	/* same isa as in debug mode */
-	retval = target_run_algorithm(target, 0, NULL, 2, reg_params,
-			crc_algorithm->address | isa,
-			(crc_algorithm->address + (sizeof(mips_crc_code) - 4)) | isa,
-			timeout, &mips32_info);
+	retval = target_run_algorithm(target, 0, NULL, 2, reg_params, crc_algorithm->address,
+				      crc_algorithm->address + (sizeof(mips_crc_code) - 4), timeout, &mips32_info);
 
 	if (retval == ERROR_OK)
 		*checksum = buf_get_u32(reg_params[0].value, 0, 32);
@@ -827,7 +869,7 @@ int mips32_blank_check_memory(struct target *target,
 		return retval;
 
 	mips32_info.common_magic = MIPS32_COMMON_MAGIC;
-	mips32_info.isa_mode = MIPS32_ISA_MIPS32;
+	mips32_info.isa_mode = isa ? MIPS32_ISA_MMIPS32 : MIPS32_ISA_MIPS32;
 
 	init_reg_param(&reg_params[0], "r4", 32, PARAM_OUT);
 	buf_set_u32(reg_params[0].value, 0, 32, address);
@@ -838,11 +880,8 @@ int mips32_blank_check_memory(struct target *target,
 	init_reg_param(&reg_params[2], "r6", 32, PARAM_IN_OUT);
 	buf_set_u32(reg_params[2].value, 0, 32, erased_value);
 
-	/* same isa as in debug mode */
-	retval = target_run_algorithm(target, 0, NULL, 3, reg_params,
-			erase_check_algorithm->address | isa,
-			(erase_check_algorithm->address + (sizeof(erase_check_code) - 4)) | isa,
-			10000, &mips32_info);
+	retval = target_run_algorithm(target, 0, NULL, 3, reg_params, erase_check_algorithm->address,
+			erase_check_algorithm->address + (sizeof(erase_check_code) - 4), 10000, &mips32_info);
 
 	if (retval == ERROR_OK)
 		*blank = buf_get_u32(reg_params[2].value, 0, 32);
diff --git a/src/target/mips32.h b/src/target/mips32.h
index d79451f..928598f 100644
--- a/src/target/mips32.h
+++ b/src/target/mips32.h
@@ -58,6 +58,9 @@
 #define MIPS32_CONFIG1_DL_SHIFT 10
 #define MIPS32_CONFIG1_DL_MASK (0x7 << MIPS32_CONFIG1_DL_SHIFT)
 
+#define MIPS32_CONFIG3_ISA_SHIFT	14
+#define MIPS32_CONFIG3_ISA_MASK		(3 << MIPS32_CONFIG3_ISA_SHIFT)
+
 #define MIPS32_ARCH_REL1 0x0
 #define MIPS32_ARCH_REL2 0x1
 
@@ -73,6 +76,14 @@ enum {
 enum mips32_isa_mode {
 	MIPS32_ISA_MIPS32 = 0,
 	MIPS32_ISA_MIPS16E = 1,
+	MIPS32_ISA_MMIPS32 = 3,
+};
+
+enum mips32_isa_imp {
+	MIPS32_ONLY = 0,
+	MMIPS32_ONLY = 1,
+	MIPS32_MIPS16 = 2,
+	MIPS32_MMIPS32 = 3,
 };
 
 struct mips32_comparator {
@@ -88,6 +99,7 @@ struct mips32_common {
 	struct mips_ejtag ejtag_info;
 	uint32_t core_regs[MIPS32NUMCOREREGS];
 	enum mips32_isa_mode isa_mode;
+	enum mips32_isa_imp isa_imp;
 
 	/* working area for fastdata access */
 	struct working_area *fast_data_area;
@@ -406,6 +418,8 @@ int mips32_enable_interrupts(struct target *target, int enable);
 
 int mips32_examine(struct target *target);
 
+int mips32_read_config_regs(struct target *target);
+
 int mips32_register_commands(struct command_context *cmd_ctx);
 
 int mips32_get_gdb_reg_list(struct target *target,
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index ade1b4c..226863f 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -183,6 +183,9 @@ struct mips_ejtag {
 	uint32_t idcode;
 	uint32_t ejtag_ctrl;
 	int fast_access_save;
+	uint32_t config_regs;	/* number of config registers read */
+	uint32_t config[4];	/* cp0 config to config3 */
+
 	uint32_t reg8;
 	uint32_t reg9;
 	unsigned scan_delay;
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index 8b32d11..7ab30fe 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -41,7 +41,7 @@ static int mips_m4k_set_breakpoint(struct target *target,
 static int mips_m4k_unset_breakpoint(struct target *target,
 		struct breakpoint *breakpoint);
 static int mips_m4k_internal_restore(struct target *target, int current,
-		uint32_t address, int handle_breakpoints,
+		target_addr_t address, int handle_breakpoints,
 		int debug_execution);
 static int mips_m4k_halt(struct target *target);
 static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
@@ -108,11 +108,14 @@ static int mips_m4k_debug_entry(struct target *target)
 	/* attempt to find halt reason */
 	mips_m4k_examine_debug_reason(target);
 
+	mips32_read_config_regs(target);
+
 	/* default to mips32 isa, it will be changed below if required */
 	mips32->isa_mode = MIPS32_ISA_MIPS32;
 
-	if (ejtag_info->impcode & EJTAG_IMP_MIPS16)
-		mips32->isa_mode = buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1);
+	/* other than mips32 only and isa bit set ? */
+	if (mips32->isa_imp && buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1))
+		mips32->isa_mode = mips32->isa_imp == 2 ? MIPS32_ISA_MIPS16E : MIPS32_ISA_MMIPS32;
 
 	LOG_DEBUG("entered debug state at PC 0x%" PRIx32 ", target->state: %s",
 			buf_get_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32),
@@ -431,7 +434,7 @@ static int mips_m4k_restore_smp(struct target *target, uint32_t address, int han
 }
 
 static int mips_m4k_internal_restore(struct target *target, int current,
-		uint32_t address, int handle_breakpoints, int debug_execution)
+		target_addr_t address, int handle_breakpoints, int debug_execution)
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 	struct mips_ejtag *ejtag_info = &mips32->ejtag_info;
@@ -451,12 +454,13 @@ static int mips_m4k_internal_restore(struct target *target, int current,
 
 	/* current = 1: continue on current pc, otherwise continue at <address> */
 	if (!current) {
+		mips_m4k_isa_filter(mips32->isa_imp, &address);
 		buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32, address);
 		mips32->core_cache->reg_list[MIPS32_PC].dirty = 1;
 		mips32->core_cache->reg_list[MIPS32_PC].valid = 1;
 	}
 
-	if (ejtag_info->impcode & EJTAG_IMP_MIPS16)
+	if ((mips32->isa_imp > 1) &&  debug_execution)	/* if more than one isa supported */
 		buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 1, mips32->isa_mode);
 
 	if (!current)
@@ -544,6 +548,7 @@ static int mips_m4k_step(struct target *target, int current,
 
 	/* current = 1: continue on current pc, otherwise continue at <address> */
 	if (!current) {
+		mips_m4k_isa_filter(mips32->isa_imp, &address);
 		buf_set_u32(mips32->core_cache->reg_list[MIPS32_PC].value, 0, 32, address);
 		mips32->core_cache->reg_list[MIPS32_PC].dirty = 1;
 		mips32->core_cache->reg_list[MIPS32_PC].valid = 1;
diff --git a/src/target/mips_m4k.h b/src/target/mips_m4k.h
index cf82661..ea09ae5 100644
--- a/src/target/mips_m4k.h
+++ b/src/target/mips_m4k.h
@@ -41,6 +41,17 @@ target_to_m4k(struct target *target)
 			struct mips_m4k_common, mips32);
 }
 
+static inline void mips_m4k_isa_filter(enum mips32_isa_imp isa_imp, target_addr_t  *addr)
+{
+	if (isa_imp <= 1) {	/* if only one isa implemented */
+		target_addr_t address = (*addr & ~1) | isa_imp;
+
+		if (address != *addr) {
+			LOG_USER("Warning: isa bit changed due to isa not implemented");
+			*addr = address;
+		}
+	}
+}
 extern const struct command_registration mips_m4k_command_handlers[];
 
 #endif /* OPENOCD_TARGET_MIPS_M4K_H */
-- 
cgit v1.1


From b702b16dc2d9bb0bcc4b9b4c909e8c1067aba238 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <sarroyofdez@yahoo.es>
Date: Mon, 8 May 2017 09:44:50 +0200
Subject: mips32: add micromips breakpoints support

Breakpoint setting based on length (kind) only.
Added 16bit aligned 32bit software breakpoints
support and same filtering before setting
breakpoint.
Set the required isa bit in hardware breakpoints.
Drop the isa bit in software breakpoints.

Change-Id: I7020f27be16015194b76f385d9b8e5af496d0dfc
Signed-off-by: Salvador Arroyo <sarroyofdez@yahoo.es>
Reviewed-on: http://openocd.zylin.com/4124
Tested-by: jenkins
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips_m4k.c | 151 +++++++++++++++++++++++++++++++++++---------------
 1 file changed, 105 insertions(+), 46 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index 7ab30fe..e2007d9 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -631,6 +631,11 @@ static int mips_m4k_set_breakpoint(struct target *target,
 		comparator_list[bp_num].used = 1;
 		comparator_list[bp_num].bp_value = breakpoint->address;
 
+		if (breakpoint->length != 4)			/* make sure isa bit set */
+			comparator_list[bp_num].bp_value |= 1;
+		else						/* make sure isa bit cleared */
+			comparator_list[bp_num].bp_value &= ~1;
+
 		/* EJTAG 2.0 uses 30bit IBA. First 2 bits are reserved.
 		 * Warning: there is no IB ASID registers in 2.0.
 		 * Do not set it! :) */
@@ -648,41 +653,77 @@ static int mips_m4k_set_breakpoint(struct target *target,
 				  bp_num, comparator_list[bp_num].bp_value);
 	} else if (breakpoint->type == BKPT_SOFT) {
 		LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
-		if (breakpoint->length == 4) {
+
+		uint32_t isa_req = breakpoint->length & 1;	/* micro mips request bit */
+		uint32_t bplength = breakpoint->length & ~1;	/* drop micro mips request bit for length */
+		uint32_t bpaddr = breakpoint->address & ~1;	/* drop isa bit from address, if set */
+
+		if (bplength == 4) {
 			uint32_t verify = 0xffffffff;
+			uint32_t sdbbp32_instr = MIPS32_SDBBP(isa_req);
+			if (ejtag_info->endianness && isa_req)
+				sdbbp32_instr = SWAP16(sdbbp32_instr);
 
-			retval = target_read_memory(target, breakpoint->address, breakpoint->length, 1,
-					breakpoint->orig_instr);
-			if (retval != ERROR_OK)
-				return retval;
-			retval = target_write_u32(target, breakpoint->address, MIPS32_SDBBP(ejtag_info->isa));
-			if (retval != ERROR_OK)
-				return retval;
+			if ((breakpoint->address & 3) == 0) {	/* word alligned */
 
-			retval = target_read_u32(target, breakpoint->address, &verify);
-			if (retval != ERROR_OK)
-				return retval;
-			if (verify != MIPS32_SDBBP(ejtag_info->isa)) {
-				LOG_ERROR("Unable to set 32-bit breakpoint at address " TARGET_ADDR_FMT
-						" - check that memory is read/writable", breakpoint->address);
+				retval = target_read_memory(target, bpaddr, bplength, 1, breakpoint->orig_instr);
+				if (retval != ERROR_OK)
+					return retval;
+
+				retval = target_write_u32(target, bpaddr, sdbbp32_instr);
+				if (retval != ERROR_OK)
+					return retval;
+
+				retval = target_read_u32(target, bpaddr, &verify);
+				if (retval != ERROR_OK)
+					return retval;
+
+				if (verify != sdbbp32_instr)
+					verify = 0;
+
+			} else {	/* 16 bit aligned */
+				retval = target_read_memory(target, bpaddr, 2, 2, breakpoint->orig_instr);
+				if (retval != ERROR_OK)
+					return retval;
+
+				uint8_t sdbbp_buf[4];
+				target_buffer_set_u32(target, sdbbp_buf, sdbbp32_instr);
+
+				retval = target_write_memory(target, bpaddr, 2, 2, sdbbp_buf);
+				if (retval != ERROR_OK)
+					return retval;
+
+				retval = target_read_memory(target, bpaddr, 2, 2, sdbbp_buf);
+				if (retval != ERROR_OK)
+					return retval;
+
+				if (target_buffer_get_u32(target, sdbbp_buf) != sdbbp32_instr)
+					verify = 0;
+			}
+
+			if (verify == 0) {
+				LOG_ERROR("Unable to set 32bit breakpoint at address %08" PRIx64
+					" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
+
 		} else {
 			uint16_t verify = 0xffff;
 
-			retval = target_read_memory(target, breakpoint->address, breakpoint->length, 1,
-					breakpoint->orig_instr);
+			retval = target_read_memory(target, bpaddr, bplength, 1, breakpoint->orig_instr);
 			if (retval != ERROR_OK)
 				return retval;
-			retval = target_write_u16(target, breakpoint->address, MIPS16_SDBBP(ejtag_info->isa));
+
+			retval = target_write_u16(target, bpaddr, MIPS16_SDBBP(isa_req));
 			if (retval != ERROR_OK)
 				return retval;
 
-			retval = target_read_u16(target, breakpoint->address, &verify);
+			retval = target_read_u16(target, bpaddr, &verify);
 			if (retval != ERROR_OK)
 				return retval;
-			if (verify != MIPS16_SDBBP(ejtag_info->isa)) {
-				LOG_ERROR("Unable to set 16-bit breakpoint at address " TARGET_ADDR_FMT
+
+			if (verify != MIPS16_SDBBP(isa_req)) {
+				LOG_ERROR("Unable to set 16bit breakpoint at address %08" PRIx64
 						" - check that memory is read/writable", breakpoint->address);
 				return ERROR_OK;
 			}
@@ -725,46 +766,58 @@ static int mips_m4k_unset_breakpoint(struct target *target,
 
 	} else {
 		/* restore original instruction (kept in target endianness) */
+		uint32_t isa_req = breakpoint->length & 1;
+		uint32_t bplength = breakpoint->length & ~1;
+		uint8_t current_instr[4];
 		LOG_DEBUG("bpid: %" PRIu32, breakpoint->unique_id);
-		if (breakpoint->length == 4) {
-			uint32_t current_instr;
-
-			/* check that user program has not modified breakpoint instruction */
-			retval = target_read_memory(target, breakpoint->address, 4, 1,
-					(uint8_t *)&current_instr);
-			if (retval != ERROR_OK)
-				return retval;
-
-			/**
-			 * target_read_memory() gets us data in _target_ endianess.
-			 * If we want to use this data on the host for comparisons with some macros
-			 * we must first transform it to _host_ endianess using target_buffer_get_u32().
-			 */
-			current_instr = target_buffer_get_u32(target, (uint8_t *)&current_instr);
-
-			if (current_instr == MIPS32_SDBBP(ejtag_info->isa)) {
-				retval = target_write_memory(target, breakpoint->address, 4, 1,
-						breakpoint->orig_instr);
+		if (bplength == 4) {
+			uint32_t sdbbp32_instr =  MIPS32_SDBBP(isa_req);
+			if (ejtag_info->endianness && isa_req)
+				sdbbp32_instr = SWAP16(sdbbp32_instr);
+
+			if ((breakpoint->address & 3) == 0) {		/* 32bit aligned */
+				/* check that user program has not modified breakpoint instruction */
+				retval = target_read_memory(target, breakpoint->address, 4, 1, current_instr);
+				if (retval != ERROR_OK)
+					return retval;
+				/**
+				* target_read_memory() gets us data in _target_ endianess.
+				* If we want to use this data on the host for comparisons with some macros
+				* we must first transform it to _host_ endianess using target_buffer_get_u16().
+				*/
+				if (sdbbp32_instr == target_buffer_get_u32(target, current_instr)) {
+					retval = target_write_memory(target, breakpoint->address, 4, 1,
+										breakpoint->orig_instr);
+					if (retval != ERROR_OK)
+						return retval;
+				}
+			} else {	/* 16bit alligned */
+				retval = target_read_memory(target, breakpoint->address, 2, 2, current_instr);
 				if (retval != ERROR_OK)
 					return retval;
+
+				if (sdbbp32_instr == target_buffer_get_u32(target, current_instr)) {
+					retval = target_write_memory(target, breakpoint->address, 2, 2,
+										breakpoint->orig_instr);
+					if (retval != ERROR_OK)
+						return retval;
+				}
 			}
 		} else {
-			uint16_t current_instr;
-
 			/* check that user program has not modified breakpoint instruction */
-			retval = target_read_memory(target, breakpoint->address, 2, 1,
-					(uint8_t *)&current_instr);
+			retval = target_read_memory(target, breakpoint->address, 2, 1, current_instr);
 			if (retval != ERROR_OK)
 				return retval;
-			current_instr = target_buffer_get_u16(target, (uint8_t *)&current_instr);
-			if (current_instr == MIPS16_SDBBP(ejtag_info->isa)) {
+
+			if (target_buffer_get_u16(target, current_instr) == MIPS16_SDBBP(isa_req)) {
 				retval = target_write_memory(target, breakpoint->address, 2, 1,
-						breakpoint->orig_instr);
+									breakpoint->orig_instr);
 				if (retval != ERROR_OK)
 					return retval;
 			}
 		}
 	}
+
 	breakpoint->set = 0;
 
 	return ERROR_OK;
@@ -774,6 +827,12 @@ static int mips_m4k_add_breakpoint(struct target *target, struct breakpoint *bre
 {
 	struct mips32_common *mips32 = target_to_mips32(target);
 
+	if ((breakpoint->length > 5 || breakpoint->length < 2) ||		/* out of range */
+		(breakpoint->length == 4 && (breakpoint->address & 2)) ||	/* mips32 unaligned */
+		(mips32->isa_imp == MIPS32_ONLY && breakpoint->length != 4) ||	/* misp32 specific */
+		((mips32->isa_imp & 1) != (breakpoint->length & 1)))		/* isa not implemented */
+		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+
 	if (breakpoint->type == BKPT_HARD) {
 		if (mips32->num_inst_bpoints_avail < 1) {
 			LOG_INFO("no hardware breakpoint available");
-- 
cgit v1.1


From f22b89d9fbc0d44b23f56e71429d0df909430036 Mon Sep 17 00:00:00 2001
From: Salvador Arroyo <salvador@telecable.es>
Date: Mon, 20 Feb 2017 20:45:37 +0100
Subject: mips32, use scan32 function for reading impcode/idcode.

There is no need to implement scan code in functions
mips_ejtag_get_idcode/impcode(), use mips_ejtag_drscan_32().
Impcode/idcode saved in ejtag.info.
Reorder the code in the callers of this functions.

Change-Id: Ia829c783a0b24c6a65cade736113fa6f67b0a170
Signed-off-by: Salvador Arroyo <salvador@telecable.es>
Reviewed-on: http://openocd.zylin.com/4003
Tested-by: jenkins
Reviewed-by: Peter Mamonov <pmamonov@gmail.com>
Reviewed-by: Freddie Chopin <freddie.chopin@gmail.com>
---
 src/target/mips_ejtag.c | 55 +++++++++----------------------------------------
 src/target/mips_ejtag.h |  2 +-
 src/target/mips_m4k.c   | 22 +++++++-------------
 3 files changed, 19 insertions(+), 60 deletions(-)

(limited to 'src/target')

diff --git a/src/target/mips_ejtag.c b/src/target/mips_ejtag.c
index d335c1b..03a0952 100644
--- a/src/target/mips_ejtag.c
+++ b/src/target/mips_ejtag.c
@@ -48,54 +48,20 @@ void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, uint32_t new_instr)
 	}
 }
 
-int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info, uint32_t *idcode)
+int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info)
 {
-	struct scan_field field;
-	uint8_t r[4];
-
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IDCODE);
 
-	field.num_bits = 32;
-	field.out_value = NULL;
-	field.in_value = r;
-
-	jtag_add_dr_scan(ejtag_info->tap, 1, &field, TAP_IDLE);
-
-	int retval;
-	retval = jtag_execute_queue();
-	if (retval != ERROR_OK) {
-		LOG_ERROR("register read failed");
-		return retval;
-	}
-
-	*idcode = buf_get_u32(field.in_value, 0, 32);
-
-	return ERROR_OK;
+	ejtag_info->idcode = 0;
+	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->idcode);
 }
 
-static int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info, uint32_t *impcode)
+int mips_ejtag_get_impcode(struct mips_ejtag *ejtag_info)
 {
-	struct scan_field field;
-	uint8_t r[4];
-
 	mips_ejtag_set_instr(ejtag_info, EJTAG_INST_IMPCODE);
 
-	field.num_bits = 32;
-	field.out_value = NULL;
-	field.in_value = r;
-
-	jtag_add_dr_scan(ejtag_info->tap, 1, &field, TAP_IDLE);
-
-	int retval;
-	retval = jtag_execute_queue();
-	if (retval != ERROR_OK) {
-		LOG_ERROR("register read failed");
-		return retval;
-	}
-
-	*impcode = buf_get_u32(field.in_value, 0, 32);
-
-	return ERROR_OK;
+	ejtag_info->impcode = 0;
+	return mips_ejtag_drscan_32(ejtag_info, &ejtag_info->impcode);
 }
 
 void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info, uint32_t ctrl, uint32_t data, uint8_t *in_scan_buf)
@@ -368,12 +334,11 @@ static void ejtag_main_print_imp(struct mips_ejtag *ejtag_info)
 
 int mips_ejtag_init(struct mips_ejtag *ejtag_info)
 {
-	int retval;
-
-	retval = mips_ejtag_get_impcode(ejtag_info, &ejtag_info->impcode);
-	if (retval != ERROR_OK)
+	int retval = mips_ejtag_get_impcode(ejtag_info);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("impcode read failed");
 		return retval;
-	LOG_DEBUG("impcode: 0x%8.8" PRIx32 "", ejtag_info->impcode);
+	}
 
 	/* get ejtag version */
 	ejtag_info->ejtag_version = ((ejtag_info->impcode >> 29) & 0x07);
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 226863f..71f5c1b 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -219,7 +219,7 @@ struct mips_ejtag {
 void mips_ejtag_set_instr(struct mips_ejtag *ejtag_info, uint32_t new_instr);
 int mips_ejtag_enter_debug(struct mips_ejtag *ejtag_info);
 int mips_ejtag_exit_debug(struct mips_ejtag *ejtag_info);
-int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info, uint32_t *idcode);
+int mips_ejtag_get_idcode(struct mips_ejtag *ejtag_info);
 void mips_ejtag_add_scan_96(struct mips_ejtag *ejtag_info,
 			    uint32_t ctrl, uint32_t data, uint8_t *in_scan_buf);
 void mips_ejtag_drscan_32_out(struct mips_ejtag *ejtag_info, uint32_t data);
diff --git a/src/target/mips_m4k.c b/src/target/mips_m4k.c
index e2007d9..7d1c06c 100644
--- a/src/target/mips_m4k.c
+++ b/src/target/mips_m4k.c
@@ -1174,36 +1174,30 @@ static int mips_m4k_target_create(struct target *target, Jim_Interp *interp)
 
 static int mips_m4k_examine(struct target *target)
 {
-	int retval;
 	struct mips_m4k_common *mips_m4k = target_to_m4k(target);
 	struct mips_ejtag *ejtag_info = &mips_m4k->mips32.ejtag_info;
-	uint32_t idcode = 0;
 
 	if (!target_was_examined(target)) {
-		retval = mips_ejtag_get_idcode(ejtag_info, &idcode);
-		if (retval != ERROR_OK)
+		int retval = mips_ejtag_get_idcode(ejtag_info);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("idcode read failed");
 			return retval;
-		ejtag_info->idcode = idcode;
-
-		if (((idcode >> 1) & 0x7FF) == 0x29) {
+		}
+		if (((ejtag_info->idcode >> 1) & 0x7FF) == 0x29) {
 			/* we are using a pic32mx so select ejtag port
 			 * as it is not selected by default */
 			mips_ejtag_set_instr(ejtag_info, MTAP_SW_ETAP);
-			LOG_DEBUG("PIC32MX Detected - using EJTAG Interface");
+			LOG_DEBUG("PIC32 Detected - using EJTAG Interface");
 			mips_m4k->is_pic32mx = true;
 		}
 	}
 
 	/* init rest of ejtag interface */
-	retval = mips_ejtag_init(ejtag_info);
+	int retval = mips_ejtag_init(ejtag_info);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = mips32_examine(target);
-	if (retval != ERROR_OK)
-		return retval;
-
-	return ERROR_OK;
+	return mips32_examine(target);
 }
 
 static int mips_m4k_bulk_write_memory(struct target *target, target_addr_t address,
-- 
cgit v1.1


From 6b9d19d3675a82ccc501fd7cba036c5b04d04590 Mon Sep 17 00:00:00 2001
From: Tobias Diedrich <ranma+coreboot@tdiedrich.de>
Date: Mon, 1 Aug 2016 15:57:20 +0200
Subject: flash: Add support for Atheros (ath79) SPI interface

Supported SoCs: AR71xx, AR724x, AR91xx, AR93xx, QCA9558

Extended and revised version of my original patch submitted by Dmytro
here: http://openocd.zylin.com/#/c/3390

This driver is using pure SPI mode, so the flash base address is not
used except some flash commands (e.g. "flash program") need it to
distinguish the banks.

Example config with all 3 chip selects:
flash bank flash0 ath79 0 0 0 0 $_TARGETNAME cs0
flash bank flash1 ath79 0x10000000 0 0 0 $_TARGETNAME cs1
flash bank flash2 ath79 0x20000000 0 0 0 $_TARGETNAME cs2

Example usage:
> flash probe flash0
Found flash device 'win w25q128fv' (ID 0x001840ef)
flash 'ath79' found at 0x00000000
> flash probe flash1
No SPI flash found
> flash probe flash2
No SPI flash found
> flash banks
> flash read_bank flash0 /tmp/test.bin 0x00000000 0x1000
reading 4096 bytes from flash @0x00000000
wrote 4096 bytes to file /tmp/test.bin from flash bank 0 at offset
0x00000000 in 28.688066s (0.139 KiB/s)

Change-Id: I5feb697722c07e83a9c1b361a9db7b06bc699aa8
Signed-off-by: Tobias Diedrich <ranma+openocd@tdiedrich.de>
Reviewed-on: http://openocd.zylin.com/3612
Tested-by: jenkins
Reviewed-by: Dmytro <dioptimizer@hotmail.com>
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
---
 src/target/mips32_pracc.h | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src/target')

diff --git a/src/target/mips32_pracc.h b/src/target/mips32_pracc.h
index fe9f814..b8b93c6 100644
--- a/src/target/mips32_pracc.h
+++ b/src/target/mips32_pracc.h
@@ -34,6 +34,8 @@
 #define MIPS32_PRACC_PARAM_OUT			0xFF202000
 
 #define PRACC_UPPER_BASE_ADDR			(MIPS32_PRACC_BASE_ADDR >> 16)
+#define PRACC_MAX_CODE				(MIPS32_PRACC_PARAM_OUT - MIPS32_PRACC_TEXT)
+#define PRACC_MAX_INSTRUCTIONS			(PRACC_MAX_CODE / 4)
 #define PRACC_OUT_OFFSET			(MIPS32_PRACC_PARAM_OUT - MIPS32_PRACC_BASE_ADDR)
 
 #define MIPS32_FASTDATA_HANDLER_SIZE	0x80
-- 
cgit v1.1