arm_adi_v5: fix mem_ap_read_buf_u32() JTAG nastiness..

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

diff --git a/src/target/adi_v5_jtag.c b/src/target/adi_v5_jtag.c
index 9f37bd553d841080b8c416eceed2c553d2f1e524..e500416b6e10587ffcb6c0fa544a7a4e60a586b4 100644 (file)
--- a/src/target/adi_v5_jtag.c
+++ b/src/target/adi_v5_jtag.c
@@ -75,10 +75,7 @@
  * @param ack points to where the three bit JTAG_ACK_* code will be stored
  */
 
-/* FIXME don't export ... this is a temporary workaround for the
- * mem_ap_read_buf_u32() mess, until it's no longer JTAG-specific.
- */
-int adi_jtag_dp_scan(struct adiv5_dap *dap,
+static int adi_jtag_dp_scan(struct adiv5_dap *dap,
                uint8_t instr, uint8_t reg_addr, uint8_t RnW,
                uint8_t *outvalue, uint8_t *invalue, uint8_t *ack)
 {
@@ -417,6 +414,40 @@ static int jtag_ap_q_write(struct adiv5_dap *dap, unsigned reg,
        return adi_jtag_ap_write_check(dap, reg, out_value_buf);
 }
 
+static int jtag_ap_q_read_block(struct adiv5_dap *dap, unsigned reg,
+               uint32_t blocksize, uint8_t *buffer)
+{
+       uint32_t readcount;
+       int retval = ERROR_OK;
+
+       /* Scan out first read */
+       retval = adi_jtag_dp_scan(dap, JTAG_DP_APACC, reg,
+                       DPAP_READ, 0, NULL, NULL);
+       if (retval != ERROR_OK)
+               return retval;
+
+       for (readcount = 0; readcount < blocksize - 1; readcount++) {
+               /* Scan out next read; scan in posted value for the
+                * previous one.  Assumes read is acked "OK/FAULT",
+                * and CTRL_STAT says that meant "OK".
+                */
+               retval = adi_jtag_dp_scan(dap, JTAG_DP_APACC, reg,
+                               DPAP_READ, 0, buffer + 4 * readcount,
+                               &dap->ack);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
+
+       /* Scan in last posted value; RDBUFF has no other effect,
+        * assuming ack is OK/FAULT and CTRL_STAT says "OK".
+        */
+       retval = adi_jtag_dp_scan(dap, JTAG_DP_DPACC, DP_RDBUFF,
+                       DPAP_READ, 0, buffer + 4 * readcount,
+                       &dap->ack);
+
+       return retval;
+}
+
 static int jtag_ap_q_abort(struct adiv5_dap *dap, uint8_t *ack)
 {
        /* for JTAG, this is the only valid ABORT register operation */
@@ -433,13 +464,14 @@ static int jtag_dp_run(struct adiv5_dap *dap)
  * part of DAP setup
 */
 const struct dap_ops jtag_dp_ops = {
-       .queue_idcode_read =    jtag_idcode_q_read,
-       .queue_dp_read =        jtag_dp_q_read,
-       .queue_dp_write =       jtag_dp_q_write,
-       .queue_ap_read =        jtag_ap_q_read,
-       .queue_ap_write =       jtag_ap_q_write,
-       .queue_ap_abort =       jtag_ap_q_abort,
-       .run =                  jtag_dp_run,
+       .queue_idcode_read   = jtag_idcode_q_read,
+       .queue_dp_read       = jtag_dp_q_read,
+       .queue_dp_write      = jtag_dp_q_write,
+       .queue_ap_read       = jtag_ap_q_read,
+       .queue_ap_write      = jtag_ap_q_write,
+       .queue_ap_read_block = jtag_ap_q_read_block,
+       .queue_ap_abort      = jtag_ap_q_abort,
+       .run                 = jtag_dp_run,
 };