+ return retval;
+}
+
+/* FIX!!! is this a potential performance bottleneck w.r.t. requiring too many
+ * roundtrips when jtag_execute_queue() has a large overhead(e.g. for USB)s?
+ *
+ * The solution is to arrange for a large out/in scan in this loop and
+ * and convert data afterwards.
+ */
+static int mem_ap_read_buf_packed_u8(struct adiv5_dap *dap,
+ uint8_t *buffer, int count, uint32_t address)
+{
+ uint32_t invalue;
+ int retval = ERROR_OK;
+ int wcount, blocksize, readcount, i;
+
+ wcount = count;
+
+ while (wcount > 0) {
+ int nbytes;
+
+ /* Adjust to read blocks within boundaries aligned to the TAR autoincremnent size*/
+ blocksize = max_tar_block_size(dap->tar_autoincr_block, address);
+
+ if (wcount < blocksize)
+ blocksize = wcount;
+
+ retval = dap_setup_accessport(dap, CSW_8BIT | CSW_ADDRINC_PACKED, address);
+ if (retval != ERROR_OK)
+ return retval;
+ readcount = blocksize;
+
+ do {
+ retval = dap_queue_ap_read(dap, AP_REG_DRW, &invalue);
+ if (retval != ERROR_OK)
+ return retval;
+ retval = dap_run(dap);
+ if (retval != ERROR_OK) {
+ LOG_WARNING("Block read error address 0x%" PRIx32 ", count 0x%x", address, count);
+ return retval;
+ }
+
+ nbytes = MIN(readcount, 4);
+
+ for (i = 0; i < nbytes; i++) {
+ *((uint8_t *)buffer) = (invalue >> 8 * (address & 0x3));
+ buffer++;
+ address++;
+ }
+
+ readcount -= nbytes;
+ } while (readcount);
+ wcount -= blocksize;
+ }
+
+ return retval;
+}
+
+/**
+ * Synchronously read a block of bytes into a buffer
+ * @param dap The DAP connected to the MEM-AP.
+ * @param buffer where the bytes will be stored.
+ * @param count How many bytes to read.
+ * @param address Memory address from which to read data; all the
+ * data must be readable by the currently selected MEM-AP.
+ */
+int mem_ap_read_buf_u8(struct adiv5_dap *dap, uint8_t *buffer,
+ int count, uint32_t address)
+{
+ uint32_t invalue;
+ int retval = ERROR_OK;
+
+ if (dap->packed_transfers && count >= 4)
+ return mem_ap_read_buf_packed_u8(dap, buffer, count, address);
+
+ while (count > 0) {
+ retval = dap_setup_accessport(dap, CSW_8BIT | CSW_ADDRINC_SINGLE, address);
+ if (retval != ERROR_OK)
+ return retval;
+ retval = dap_queue_ap_read(dap, AP_REG_DRW, &invalue);
+ if (retval != ERROR_OK)
+ return retval;
+ retval = dap_run(dap);
+ if (retval != ERROR_OK)
+ break;
+
+ *((uint8_t *)buffer) = (invalue >> 8 * (address & 0x3));
+ count--;
+ address++;
+ buffer++;
+ }
+
+ return retval;
+}
+
+/*--------------------------------------------------------------------*/
+/* Wrapping function with selection of AP */
+/*--------------------------------------------------------------------*/
+int mem_ap_sel_read_u32(struct adiv5_dap *swjdp, uint8_t ap,
+ uint32_t address, uint32_t *value)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_read_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_write_u32(struct adiv5_dap *swjdp, uint8_t ap,
+ uint32_t address, uint32_t value)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_write_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_read_atomic_u32(struct adiv5_dap *swjdp, uint8_t ap,
+ uint32_t address, uint32_t *value)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_read_atomic_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_write_atomic_u32(struct adiv5_dap *swjdp, uint8_t ap,
+ uint32_t address, uint32_t value)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_write_atomic_u32(swjdp, address, value);
+}
+
+int mem_ap_sel_read_buf_u8(struct adiv5_dap *swjdp, uint8_t ap,
+ uint8_t *buffer, int count, uint32_t address)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_read_buf_u8(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_read_buf_u16(struct adiv5_dap *swjdp, uint8_t ap,
+ uint8_t *buffer, int count, uint32_t address)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_read_buf_u16(swjdp, buffer, count, address);
+}
+
+int mem_ap_sel_read_buf_u32_noincr(struct adiv5_dap *swjdp, uint8_t ap,
+ uint8_t *buffer, int count, uint32_t address)
+{
+ dap_ap_select(swjdp, ap);
+ return mem_ap_read_buf_u32(swjdp, buffer, count, address, false);