armv7a: correctly handle invalidation of inner data caches

D-Cache invalidate is a dangerous operation. It will only work correctly
if full cache lines are invalidated. When partial cache lines are
invalidated, i.e. the target address range does not start and end
at a cache line boundary, cpu data writes outside of the target range
will be dropped. This patch adds special treatment for partial cache
lines by doing a clean & invalidate on the partial lines before
invalidating the rest of the range.

Change-Id: I64099ddb058638e990a7eb0ee911b9cc8f6f8901
Signed-off-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-on: http://openocd.zylin.com/3034
Tested-by: jenkins
Reviewed-by: Paul Fertser <fercerpav@gmail.com>

diff --git a/src/target/armv7a_cache.c b/src/target/armv7a_cache.c
index 89e85025a9429832812a052dd5c760329a3be592..a049174cbe85b65c028ba351fac34ba8c9f541bf 100644 (file)
--- a/src/target/armv7a_cache.c
+++ b/src/target/armv7a_cache.c
@@ -158,7 +158,8 @@ static int armv7a_l1_d_cache_inval_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);
@@ -169,15 +170,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;
 
-               /* DCIMVAC - Clean and invalidate data cache line by VA to PoC. */
+       /* handle unaligned start */
+       if (virt != va_line) {
+               /* 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_line);
                if (retval != ERROR_OK)
                        goto done;
+               va_line += linelen;
        }
+
+       /* 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, 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: