armv7a: fix-up dcache clean and flush functions inner loop pattern

[openocd.git] / src / target / armv7a_cache.c

diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index acc388a273d6b3cb0d27b8df15baf34b56150c75..9237e73fe5fffd43830d98049857abd8d32822f3 100644 (file)
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -218,7 +218,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);
@@ -229,15 +230,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:
@@ -253,7 +258,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);
@@ -264,15 +270,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: