+#define RW_MISC_CMD_ADDRESS 1
+#define RW_MISC_CMD_WRITE 2
+#define RW_MISC_CMD_READ 3
+#define RW_MISC_CMD_APNUM 5
+
+static int stlink_usb_misc_rw_segment(void *handle, const struct dap_queue *q, unsigned int len, unsigned int items)
+{
+ uint8_t buf[2 * 4 * items];
+
+ LOG_DEBUG("Queue: %u commands in %u items", len, items);
+
+ int ap_num = DP_APSEL_INVALID;
+ unsigned int cmd_index = 0;
+ unsigned int val_index = ALIGN_UP(items, 4);
+ for (unsigned int i = 0; i < len; i++) {
+ if (ap_num != q[i].mem_ap.ap->ap_num) {
+ ap_num = q[i].mem_ap.ap->ap_num;
+ buf[cmd_index++] = RW_MISC_CMD_APNUM;
+ h_u32_to_le(&buf[val_index], ap_num);
+ val_index += 4;
+ }
+
+ switch (q[i].cmd) {
+ case CMD_MEM_AP_READ32:
+ buf[cmd_index++] = RW_MISC_CMD_READ;
+ h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
+ val_index += 4;
+ break;
+ case CMD_MEM_AP_WRITE32:
+ buf[cmd_index++] = RW_MISC_CMD_ADDRESS;
+ h_u32_to_le(&buf[val_index], q[i].mem_ap.addr);
+ val_index += 4;
+ buf[cmd_index++] = RW_MISC_CMD_WRITE;
+ h_u32_to_le(&buf[val_index], q[i].mem_ap.data);
+ val_index += 4;
+ break;
+ default:
+ /* Not supposed to happen */
+ return ERROR_FAIL;
+ }
+ }
+ /* pad after last command */
+ while (!IS_ALIGNED(cmd_index, 4))
+ buf[cmd_index++] = 0;
+
+ int retval = stlink_usb_rw_misc_out(handle, items, buf);
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = stlink_usb_rw_misc_in(handle, items, buf);
+ if (retval != ERROR_OK)
+ return retval;
+
+ ap_num = DP_APSEL_INVALID;
+ val_index = 0;
+ unsigned int err_index = 4 * items;
+ for (unsigned int i = 0; i < len; i++) {
+ uint32_t errcode = le_to_h_u32(&buf[err_index]);
+ if (errcode != STLINK_DEBUG_ERR_OK) {
+ LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+ return ERROR_FAIL;
+ }
+ if (ap_num != q[i].mem_ap.ap->ap_num) {
+ ap_num = q[i].mem_ap.ap->ap_num;
+ err_index += 4;
+ val_index += 4;
+ errcode = le_to_h_u32(&buf[err_index]);
+ if (errcode != STLINK_DEBUG_ERR_OK) {
+ LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+ return ERROR_FAIL;
+ }
+ }
+
+ if (q[i].cmd == CMD_MEM_AP_READ32) {
+ *q[i].mem_ap.p_data = le_to_h_u32(&buf[val_index]);
+ } else { /* q[i]->cmd == CMD_MEM_AP_WRITE32 */
+ err_index += 4;
+ val_index += 4;
+ errcode = le_to_h_u32(&buf[err_index]);
+ if (errcode != STLINK_DEBUG_ERR_OK) {
+ LOG_ERROR("unknown/unexpected STLINK status code 0x%x", errcode);
+ return ERROR_FAIL;
+ }
+ }
+ err_index += 4;
+ val_index += 4;
+ }
+
+ return ERROR_OK;
+}
+