From f24aa404ba14bb048f55752b241a6a8185f77b3f Mon Sep 17 00:00:00 2001
From: Matthias Welwarsky <matthias@welwarsky.de>
Date: Sun, 18 Oct 2015 13:50:58 +0200
Subject: [PATCH] cortex_a: Update instruction cache after setting a soft
 breakpoint

Call armv7a_l1_d_cache_flush_virt() before writing the breakpoint,
to make sure the d-cache is clean and invalid at the breakpoint
location down to PoC.

Call armv7a_l1_d_cache_inval_virt() after writing the breakpoint
again, so that d-cache will pick up the modified code.
Call armv7a_l1_i_cache_inval_virt() after writing the breakpoint
to memory to make the change visible to the CPU.

Change-Id: I24fc27058d99cb00d7f6002ccb623cab66b0d234
Signed-off-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-on: http://openocd.zylin.com/3033
Reviewed-by: Paul Fertser <fercerpav@gmail.com>
Tested-by: jenkins
---
 src/target/armv7a_cache.c |  2 +-
 src/target/armv7a_cache.h |  2 ++
 src/target/cortex_a.c     | 27 +++++++++++++++++++++++++++
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index a049174cb..acc388a27 100644
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -152,7 +152,7 @@ int armv7a_cache_auto_flush_all_data(struct target *target)
 }
 
 
-static int armv7a_l1_d_cache_inval_virt(struct target *target, uint32_t virt,
+int armv7a_l1_d_cache_inval_virt(struct target *target, uint32_t virt,
 					uint32_t size)
 {
 	struct armv7a_common *armv7a = target_to_armv7a(target);
diff --git a/src/target/armv7a_cache.h b/src/target/armv7a_cache.h
index fbd158b4d..e0ebb618b 100644
--- a/src/target/armv7a_cache.h
+++ b/src/target/armv7a_cache.h
@@ -21,6 +21,8 @@
 
 int armv7a_l1_d_cache_clean_virt(struct target *target, uint32_t virt,
 					unsigned int size);
+int armv7a_l1_d_cache_inval_virt(struct target *target, uint32_t virt,
+					unsigned int size);
 int armv7a_l1_d_cache_flush_virt(struct target *target, uint32_t virt,
 					unsigned int size);
 int armv7a_l1_i_cache_inval_all(struct target *target);
diff --git a/src/target/cortex_a.c b/src/target/cortex_a.c
index 39e59ae7b..5268cf216 100644
--- a/src/target/cortex_a.c
+++ b/src/target/cortex_a.c
@@ -1509,11 +1509,25 @@ static int cortex_a_set_breakpoint(struct target *target,
 				breakpoint->orig_instr);
 		if (retval != ERROR_OK)
 			return retval;
+
+		/* make sure data cache is cleaned & invalidated down to PoC */
+		if (!armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled) {
+			armv7a_cache_flush_virt(target, breakpoint->address,
+						breakpoint->length);
+		}
+
 		retval = target_write_memory(target,
 				breakpoint->address & 0xFFFFFFFE,
 				breakpoint->length, 1, code);
 		if (retval != ERROR_OK)
 			return retval;
+
+		/* update i-cache at breakpoint location */
+		armv7a_l1_d_cache_inval_virt(target, breakpoint->address,
+					breakpoint->length);
+		armv7a_l1_i_cache_inval_virt(target, breakpoint->address,
+						 breakpoint->length);
+
 		breakpoint->set = 0x11;	/* Any nice value but 0 */
 	}
 
@@ -1733,6 +1747,13 @@ static int cortex_a_unset_breakpoint(struct target *target, struct breakpoint *b
 			return ERROR_OK;
 		}
 	} else {
+
+		/* make sure data cache is cleaned & invalidated down to PoC */
+		if (!armv7a->armv7a_mmu.armv7a_cache.auto_cache_enabled) {
+			armv7a_cache_flush_virt(target, breakpoint->address,
+						breakpoint->length);
+		}
+
 		/* restore original instruction (kept in target endianness) */
 		if (breakpoint->length == 4) {
 			retval = target_write_memory(target,
@@ -1747,6 +1768,12 @@ static int cortex_a_unset_breakpoint(struct target *target, struct breakpoint *b
 			if (retval != ERROR_OK)
 				return retval;
 		}
+
+		/* update i-cache at breakpoint location */
+		armv7a_l1_d_cache_inval_virt(target, breakpoint->address,
+						 breakpoint->length);
+		armv7a_l1_i_cache_inval_virt(target, breakpoint->address,
+						 breakpoint->length);
 	}
 	breakpoint->set = 0;