wip. fixed gaffe in jtag_add_shift()

[openocd] / src / target / cortex_swjdp.c

diff --git a/src/target/cortex_swjdp.c b/src/target/cortex_swjdp.c
index 85a2abc3c677a9086126557ae0e9e9d0a146466b..81ff6f993c60dc87e12c4da0e9062ef87c388967 100644 (file)
--- a/src/target/cortex_swjdp.c
+++ b/src/target/cortex_swjdp.c
@@ -22,7 +22,7 @@
  * CoreSight (Light?) SerialWireJtagDebugPort                              *
  *                                                                         *
  * CoreSight™ DAP-Lite TRM, ARM DDI 0316A                                  *
- * Cortex-M3™ TRM, ARM DDI 0337C                                            *
+ * Cortex-M3™ TRM, ARM DDI 0337C                                           *
  *                                                                         *
 ***************************************************************************/
 #ifdef HAVE_CONFIG_H
@@ -87,7 +87,7 @@ int swjdp_scan(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u8 *outvalu
        fields[1].in_check_value = NULL;
        fields[1].in_check_mask = NULL;
 
-       jtag_add_dr_scan(2, fields, -1, NULL);
+       jtag_add_dr_scan(2, fields, -1);
 
        return ERROR_OK;
 }
@@ -132,7 +132,7 @@ int swjdp_scan_u32(arm_jtag_t *jtag_info, u8 instr, u8 reg_addr, u8 RnW, u32 out
        fields[1].in_check_value = NULL;
        fields[1].in_check_mask = NULL;
 
-       jtag_add_dr_scan(2, fields, -1, NULL);
+       jtag_add_dr_scan(2, fields, -1);
 
        return ERROR_OK;
 }
@@ -613,6 +613,11 @@ int ahbap_block_read_u32(swjdp_common_t *swjdp, u32 *buffer, int count, u32 addr
 
 int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
 {
+       int retval;
+       u32 dcrdr;
+       
+       ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
+       
        swjdp->trans_mode = TRANS_MODE_COMPOSITE;
 
        /* ahbap_write_system_u32(swjdp, DCB_DCRSR, regnum); */
@@ -623,11 +628,18 @@ int ahbap_read_coreregister_u32(swjdp_common_t *swjdp, u32 *value, int regnum)
        ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRDR & 0xFFFFFFF0);
        ahbap_read_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRDR & 0xC), value );
        
-       return swjdp_transaction_endcheck(swjdp);
+       retval = swjdp_transaction_endcheck(swjdp);
+       ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
+       return retval;
 }
 
 int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
 {
+       int retval;
+       u32 dcrdr;
+       
+       ahbap_read_system_atomic_u32(swjdp, DCB_DCRDR, &dcrdr);
+       
        swjdp->trans_mode = TRANS_MODE_COMPOSITE;
        
        /* ahbap_write_system_u32(swjdp, DCB_DCRDR, core_regs[i]); */
@@ -637,8 +649,10 @@ int ahbap_write_coreregister_u32(swjdp_common_t *swjdp, u32 value, int regnum)
        /* ahbap_write_system_u32(swjdp, DCB_DCRSR, i | DCRSR_WnR       ); */
        ahbap_setup_accessport(swjdp, CSW_32BIT | CSW_ADDRINC_OFF, DCB_DCRSR & 0xFFFFFFF0);
        ahbap_write_reg_u32(swjdp, AHBAP_BD0 | (DCB_DCRSR & 0xC), regnum | DCRSR_WnR );
-
-       return swjdp_transaction_endcheck(swjdp);
+       
+       retval = swjdp_transaction_endcheck(swjdp);
+       ahbap_write_system_atomic_u32(swjdp, DCB_DCRDR, dcrdr);
+       return retval;
 }
 
 int ahbap_debugport_init(swjdp_common_t *swjdp)