Make sure that DSCR_DTR_RX is not full before writing

git-svn-id: svn://svn.berlios.de/openocd/trunk@2794 b42882b7-edfa-0310-969c-e2dbd0fdcd60

diff --git a/src/target/cortex_a8.c b/src/target/cortex_a8.c
index 821d7194eb63d1613ee02398e80d78c85fc58081..7e9a0a9b572ce69ac49c1994b2c4a08626581c71 100644 (file)
--- a/src/target/cortex_a8.c
+++ b/src/target/cortex_a8.c
@@ -236,11 +236,25 @@ 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)
 {
        int retval;
+       uint32_t dscr;
+
        /* get pointers to arch-specific information */
        armv4_5_common_t *armv4_5 = target->arch_info;
        armv7a_common_t *armv7a = armv4_5->arch_info;
        swjdp_common_t *swjdp = &armv7a->swjdp_info;
 
+       LOG_DEBUG("CP%i, CRn %i, value 0x%08" PRIx32, CP, CRn, value);
+
+       /* Check that DCCRX is not full */
+       retval = mem_ap_read_atomic_u32(swjdp,
+                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
+       if (dscr & (1 << DSCR_DTR_RX_FULL))
+       {
+               LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
+               /* Clear DCCRX with MCR(p14, 0, Rd, c0, c5, 0), opcode  0xEE000E15 */
+               cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0));
+       }
+       
        retval = mem_ap_write_u32(swjdp,
                        armv7a->debug_base + CPUDBG_DTRRX, value);
        /* Move DTRRX to r0 */
@@ -311,12 +325,25 @@ int cortex_a8_dap_write_coreregister_u32(target_t *target, uint32_t value, int r
 {
        int retval = ERROR_OK;
        uint8_t Rd = regnum&0xFF;
+       uint32_t dscr;
 
        /* get pointers to arch-specific information */
        armv4_5_common_t *armv4_5 = target->arch_info;
        armv7a_common_t *armv7a = armv4_5->arch_info;
        swjdp_common_t *swjdp = &armv7a->swjdp_info;
        
+       LOG_DEBUG("register %i, value 0x%08" PRIx32, regnum, value);
+
+       /* Check that DCCRX is not full */
+       retval = mem_ap_read_atomic_u32(swjdp,
+                               armv7a->debug_base + CPUDBG_DSCR, &dscr);
+       if (dscr & (1 << DSCR_DTR_RX_FULL))
+       {
+               LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
+               /* Clear DCCRX with MCR(p14, 0, Rd, c0, c5, 0), opcode  0xEE000E15 */
+               cortex_a8_exec_opcode(target, ARMV4_5_MRC(14, 0, 0, 0, 5, 0));
+       }
+       
        if (Rd > 16)
                return retval;