+ rc = vdebug_run_jtag_queue(hsock, pm, le_to_h_u16(pm->waddr));
+ vdc.trans_first = vdc.trans_last; /* flush forces trans_first flag */
+
+ return rc;
+}
+
+static int vdebug_reg_write(int hsock, struct vd_shm *pm, const uint32_t reg,
+ const uint32_t data, uint8_t aspace, uint8_t f_last)
+{
+ uint32_t waddr;
+ int rc = ERROR_OK;
+
+ pm->cmd = VD_CMD_REGWRITE;
+ vdc.trans_last = f_last || (vdc.trans_batch == VD_BATCH_NO);
+ if (vdc.trans_first)
+ waddr = 0; /* reset buffer offset */
+ else
+ waddr = le_to_h_u16(pm->offseth); /* continue from the previous transaction */
+
+ if (4 * waddr + 2 * sizeof(uint64_t) + 4 > VD_BUFFER_LEN)
+ vdc.trans_last = 1; /* force flush, no room for next request */
+
+ uint64_t rhdr = ((uint64_t)reg << 32) + (1UL << 30) + (2UL << 27) + (1UL << 16) + aspace;
+ h_u64_to_le(&pm->wd8[4 * waddr], rhdr);
+ h_u32_to_le(&pm->wd8[4 * (waddr + 2)], data);
+ h_u16_to_le(pm->wid, le_to_h_u16(pm->wid) + 1);
+ h_u16_to_le(pm->wwords, waddr + 3);
+ h_u16_to_le(pm->waddr, le_to_h_u16(pm->waddr) + 1);
+ if (!vdc.trans_last) /* buffered request */
+ h_u16_to_le(pm->offseth, waddr + 3);
+ else
+ rc = vdebug_run_reg_queue(hsock, pm, le_to_h_u16(pm->waddr));
+ vdc.trans_first = vdc.trans_last; /* flush forces trans_first flag */
+
+ return rc;
+}
+
+static int vdebug_reg_read(int hsock, struct vd_shm *pm, const uint32_t reg,
+ uint32_t *data, uint8_t aspace, uint8_t f_last)
+{
+ uint32_t waddr;
+ int rc = ERROR_OK;
+
+ pm->cmd = VD_CMD_REGREAD;
+ vdc.trans_last = f_last || (vdc.trans_batch == VD_BATCH_NO);
+ if (vdc.trans_first)
+ waddr = 0; /* reset buffer offset */
+ else
+ waddr = le_to_h_u16(pm->offseth); /* continue from the previous transaction */
+
+ if (4 * waddr + 2 * sizeof(uint64_t) + 4 > VD_BUFFER_LEN)
+ vdc.trans_last = 1; /* force flush, no room for next request */
+
+ uint64_t rhdr = ((uint64_t)reg << 32) + (2UL << 30) + (2UL << 27) + ((data ? 1UL : 0UL) << 16) + aspace;
+ h_u64_to_le(&pm->wd8[4 * waddr], rhdr);
+ h_u16_to_le(pm->wid, le_to_h_u16(pm->wid) + 1);
+ if (data) {
+ struct vd_rdata *rd;
+ if (le_to_h_u16(pm->rwords) == 0) {
+ rd = &vdc.rdataq;
+ } else {
+ rd = calloc(1, sizeof(struct vd_rdata));
+ if (!rd) /* check allocation for 24B */
+ return ERROR_FAIL;
+ list_add_tail(&rd->lh, &vdc.rdataq.lh);
+ }
+ rd->rdata = (uint8_t *)data;
+ h_u16_to_le(pm->rwords, le_to_h_u16(pm->rwords) + 1);
+ }
+ h_u16_to_le(pm->wwords, waddr + 2);
+ h_u16_to_le(pm->waddr, le_to_h_u16(pm->waddr) + 1);
+ if (!vdc.trans_last) /* buffered request */
+ h_u16_to_le(pm->offseth, waddr + 2);
+ else
+ rc = vdebug_run_reg_queue(hsock, pm, le_to_h_u16(pm->waddr));