X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Farmv7a_cache.c;h=7af3e6d4e8a2d6ff1cf8ba9917613fcea9fe1f0b;hb=e9493cc20a0c2160696b24757274ec4ee302ac39;hp=89e85025a9429832812a052dd5c760329a3be592;hpb=f3716894c6c2ab0e16bdb073d79e4df855897c10;p=openocd.git

diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index 89e85025a9..7af3e6d4e8 100644
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -11,6 +11,9 @@
  *   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
@@ -152,13 +155,14 @@ 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);
 	struct arm_dpm *dpm = armv7a->arm.dpm;
 	struct armv7a_cache_common *armv7a_cache = &armv7a->armv7a_mmu.armv7a_cache;
-	uint32_t i, linelen = armv7a_cache->dminline;
+	uint32_t linelen = armv7a_cache->dminline;
+	uint32_t va_line, va_end;
 	int retval;
 
 	retval = armv7a_l1_d_cache_sanity_check(target);
@@ -169,15 +173,39 @@ static int armv7a_l1_d_cache_inval_virt(struct target *target, uint32_t virt,
 	if (retval != ERROR_OK)
 		goto done;
 
-	for (i = 0; i < size; i += linelen) {
-		uint32_t offs = virt + i;
+	va_line = virt & (-linelen);
+	va_end = virt + size;
+
+	/* handle unaligned start */
+	if (virt != va_line) {
+		/* DCCIMVAC */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV4_5_MCR(15, 0, 0, 7, 14, 1), va_line);
+		if (retval != ERROR_OK)
+			goto done;
+		va_line += linelen;
+	}
 
-		/* DCIMVAC - Clean and invalidate data cache line by VA to PoC. */
+	/* handle unaligned end */
+	if ((va_end & (linelen-1)) != 0) {
+		va_end &= (-linelen);
+		/* DCCIMVAC */
 		retval = dpm->instr_write_data_r0(dpm,
-				ARMV4_5_MCR(15, 0, 0, 7, 6, 1), offs);
+				ARMV4_5_MCR(15, 0, 0, 7, 14, 1), va_end);
 		if (retval != ERROR_OK)
 			goto done;
 	}
+
+	while (va_line < va_end) {
+		/* DCIMVAC - Invalidate data cache line by VA to PoC. */
+		retval = dpm->instr_write_data_r0(dpm,
+				ARMV4_5_MCR(15, 0, 0, 7, 6, 1), va_line);
+		if (retval != ERROR_OK)
+			goto done;
+		va_line += linelen;
+	}
+
+	dpm->finish(dpm);
 	return retval;
 
 done:
@@ -193,7 +221,8 @@ int armv7a_l1_d_cache_clean_virt(struct target *target, uint32_t virt,
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct arm_dpm *dpm = armv7a->arm.dpm;
 	struct armv7a_cache_common *armv7a_cache = &armv7a->armv7a_mmu.armv7a_cache;
-	uint32_t i, linelen = armv7a_cache->dminline;
+	uint32_t linelen = armv7a_cache->dminline;
+	uint32_t va_line, va_end;
 	int retval;
 
 	retval = armv7a_l1_d_cache_sanity_check(target);
@@ -204,15 +233,19 @@ int armv7a_l1_d_cache_clean_virt(struct target *target, uint32_t virt,
 	if (retval != ERROR_OK)
 		goto done;
 
-	for (i = 0; i < size; i += linelen) {
-		uint32_t offs = virt + i;
+	va_line = virt & (-linelen);
+	va_end = virt + size;
 
+	while (va_line < va_end) {
 		/* DCCMVAC - Data Cache Clean by MVA to PoC */
 		retval = dpm->instr_write_data_r0(dpm,
-				ARMV4_5_MCR(15, 0, 0, 7, 10, 1), offs);
+				ARMV4_5_MCR(15, 0, 0, 7, 10, 1), va_line);
 		if (retval != ERROR_OK)
 			goto done;
+		va_line += linelen;
 	}
+
+	dpm->finish(dpm);
 	return retval;
 
 done:
@@ -228,7 +261,8 @@ int armv7a_l1_d_cache_flush_virt(struct target *target, uint32_t virt,
 	struct armv7a_common *armv7a = target_to_armv7a(target);
 	struct arm_dpm *dpm = armv7a->arm.dpm;
 	struct armv7a_cache_common *armv7a_cache = &armv7a->armv7a_mmu.armv7a_cache;
-	uint32_t i, linelen = armv7a_cache->dminline;
+	uint32_t linelen = armv7a_cache->dminline;
+	uint32_t va_line, va_end;
 	int retval;
 
 	retval = armv7a_l1_d_cache_sanity_check(target);
@@ -239,15 +273,19 @@ int armv7a_l1_d_cache_flush_virt(struct target *target, uint32_t virt,
 	if (retval != ERROR_OK)
 		goto done;
 
-	for (i = 0; i < size; i += linelen) {
-		uint32_t offs = virt + i;
+	va_line = virt & (-linelen);
+	va_end = virt + size;
 
+	while (va_line < va_end) {
 		/* DCCIMVAC */
 		retval = dpm->instr_write_data_r0(dpm,
-				ARMV4_5_MCR(15, 0, 0, 7, 14, 1), offs);
+				ARMV4_5_MCR(15, 0, 0, 7, 14, 1), va_line);
 		if (retval != ERROR_OK)
 			goto done;
+		va_line += linelen;
 	}
+
+	dpm->finish(dpm);
 	return retval;
 
 done: