Check return values to avoid infinite wait in loop on error.

[fw/openocd] / src / target / cortex_a8.c

diff --git a/src/target/cortex_a8.c b/src/target/cortex_a8.c
index 55a3c45b5549dbf61b3a2943f901b18257b95560..ae97f87ed180ab1a1605375147bf59c8eac6bddf 100644 (file)
--- a/src/target/cortex_a8.c
+++ b/src/target/cortex_a8.c
@@ -153,7 +153,7 @@ mdw 0x54011080 4
 int cortex_a8_exec_opcode(target_t *target, uint32_t opcode)
 {
        uint32_t dscr;
-       int retvalue;
+       int retval;
        /* get pointers to arch-specific information */
        armv4_5_common_t *armv4_5 = target->arch_info;
        armv7a_common_t *armv7a = armv4_5->arch_info;
@@ -162,8 +162,10 @@ int cortex_a8_exec_opcode(target_t *target, uint32_t opcode)
        LOG_DEBUG("exec opcode 0x%08" PRIx32, opcode);
        do
        {
-               retvalue = mem_ap_read_atomic_u32(swjdp,
+               retval = mem_ap_read_atomic_u32(swjdp,
                                OMAP3530_DEBUG_BASE + CPUDBG_DSCR, &dscr);
+               if (retval != ERROR_OK)
+                       return retval;
        }
        while ((dscr & (1 << DSCR_INSTR_COMP)) == 0); /* Wait for InstrCompl bit to be set */
 
@@ -171,12 +173,14 @@ int cortex_a8_exec_opcode(target_t *target, uint32_t opcode)
 
        do
        {
-               retvalue = mem_ap_read_atomic_u32(swjdp,
+               retval = mem_ap_read_atomic_u32(swjdp,
                                OMAP3530_DEBUG_BASE + CPUDBG_DSCR, &dscr);
+               if (retval != ERROR_OK)
+                       return retval;
        }
        while ((dscr & (1 << DSCR_INSTR_COMP)) == 0); /* Wait for InstrCompl bit to be set */
 
-       return retvalue;
+       return retval;
 }
 
 /**************************************************************************
@@ -224,7 +228,6 @@ int cortex_a8_read_cp(target_t *target, uint32_t *value, uint8_t CP,
 
 int cortex_a8_write_cp(target_t *target, uint32_t value,
        uint8_t CP, uint8_t op1, uint8_t CRn, uint8_t CRm, uint8_t op2)
-/* TODO Fix this */
 {
        int retval;
        /* get pointers to arch-specific information */
@@ -237,7 +240,7 @@ int cortex_a8_write_cp(target_t *target, uint32_t value,
        /* Move DTRRX to r0 */
        cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0));
 
-       cortex_a8_exec_opcode(target, ARMV4_5_MCR(CP, 0, 0, 0, 5, 0));
+       cortex_a8_exec_opcode(target, ARMV4_5_MCR(CP, op1, 0, CRn, CRm, op2));
        return retval;
 }
 
@@ -1254,6 +1257,24 @@ int cortex_a8_write_memory(struct target_s *target, uint32_t address,
                        exit(-1);
        }
 
+       /* The Cache handling will NOT work with MMU active, the wrong addresses will be invalidated */
+       /* invalidate I-Cache */
+       if (armv7a->armv4_5_mmu.armv4_5_cache.i_cache_enabled)
+       {
+               /* Invalidate ICache single entry with MVA, repeat this for all cache
+                  lines in the address range, Cortex-A8 has fixed 64 byte line length */
+               /* Invalidate Cache single entry with MVA to PoU */
+               for (uint32_t cacheline=address; cacheline<address+size*count; cacheline+=64)
+                       armv7a->write_cp15(target, 0, 1, 7, 5, cacheline); /* I-Cache to PoU */
+       }
+       /* invalidate D-Cache */
+       if (armv7a->armv4_5_mmu.armv4_5_cache.d_u_cache_enabled)
+       {
+               /* Invalidate Cache single entry with MVA to PoC */
+               for (uint32_t cacheline=address; cacheline<address+size*count; cacheline+=64)
+                       armv7a->write_cp15(target, 0, 1, 7, 6, cacheline); /* U/D cache to PoC */
+       }
+
        return retval;
 }