git.gag.com Git - fw/openocd/blob - src/jtag/drivers/vdebug.c

   1 /* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-Clause) */
   2 /*----------------------------------------------------------------------------
   3  * Copyright 2020-2022 Cadence Design Systems, Inc.
   4  *
   5  * Redistribution and use in source and binary forms, with or without modification,
   6  * are permitted provided that the following conditions are met:
   7  * 1. Redistributions of source code must retain the above copyright notice,
   8  * this list of conditions and the following disclaimer.
   9  * 2. Redistributions in binary form must reproduce the above copyright notice,
  10  * this list of conditions and the following disclaimer in the documentation
  11  * and/or other materials provided with the distribution.
  12  *----------------------------------------------------------------------------
  13  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  14  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  15  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  16  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  17  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  18  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  19  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  20  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  21  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  22  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  23  *----------------------------------------------------------------------------
  24 */
  25
  26 /*!
  27  * @file
  28  *
  29  * @brief the virtual debug interface provides a connection between a sw debugger
  30  * and the simulated, emulated core. The openOCD client connects via TCP sockets
  31  * with vdebug server and over DPI-based transactor with the emulation or simulation
  32  * The vdebug debug driver supports JTAG and DAP-level transports
  33  *
  34 */
  35
  36 #ifdef HAVE_CONFIG_H
  37 #include "config.h"
  38 #endif
  39
  40 #ifdef _WIN32
  41 #define WIN32_LEAN_AND_MEAN
  42 #include <windows.h>
  43 #else
  44 #ifdef HAVE_UNISTD_H
  45 #include <unistd.h>          /* close */
  46 #endif
  47 #ifdef HAVE_SYS_SOCKET_H
  48 #include <sys/socket.h>
  49 #endif
  50 #ifdef HAVE_ARPA_INET_H
  51 #include <arpa/inet.h>
  52 #endif
  53 #ifdef HAVE_NETDB_H
  54 #include <netdb.h>
  55 #endif
  56 #endif
  57 #include <stdio.h>
  58 #ifdef HAVE_STDINT_H
  59 #include <stdint.h>
  60 #endif
  61 #ifdef HAVE_STDLIB_H
  62 #include <stdlib.h>
  63 #endif
  64 #include <stdarg.h>
  65 #include <string.h>
  66 #include <errno.h>
  67
  68 #include "jtag/interface.h"
  69 #include "jtag/commands.h"
  70 #include "transport/transport.h"
  71 #include "target/arm_adi_v5.h"
  72 #include "helper/time_support.h"
  73 #include "helper/replacements.h"
  74 #include "helper/log.h"
  75 #include "helper/list.h"
  76
  77 #define VD_VERSION 44
  78 #define VD_BUFFER_LEN 4024
  79 #define VD_CHEADER_LEN 24
  80 #define VD_SHEADER_LEN 16
  81
  82 #define VD_MAX_MEMORIES 20
  83 #define VD_POLL_INTERVAL 500
  84 #define VD_SCALE_PSTOMS 1000000000
  85
  86 /**
  87  * @brief List of transactor types
  88  */
  89 enum {
  90         VD_BFM_JTDP   = 0x0001,  /* transactor DAP JTAG DP */
  91         VD_BFM_SWDP   = 0x0002,  /* transactor DAP SWD DP */
  92         VD_BFM_AHB    = 0x0003,  /* transactor AMBA AHB */
  93         VD_BFM_APB    = 0x0004,  /* transactor AMBA APB */
  94         VD_BFM_AXI    = 0x0005,  /* transactor AMBA AXI */
  95         VD_BFM_JTAG   = 0x0006,  /* transactor serial JTAG */
  96         VD_BFM_SWD    = 0x0007,  /* transactor serial SWD */
  97 };
  98
  99 /**
 100  * @brief List of signals that can be read or written by the debugger
 101  */
 102 enum {
 103         VD_SIG_TCK    = 0x0001,  /* JTAG clock; tclk */
 104         VD_SIG_TDI    = 0x0002,  /* JTAG TDI;   tdi */
 105         VD_SIG_TMS    = 0x0004,  /* JTAG TMS;   tms */
 106         VD_SIG_RESET  = 0x0008,  /* DUT reset;  rst */
 107         VD_SIG_TRST   = 0x0010,  /* JTAG Reset; trstn */
 108         VD_SIG_TDO    = 0x0020,  /* JTAG TDO;   tdo */
 109         VD_SIG_POWER  = 0x0100,  /* BFM power;  bfm_up */
 110         VD_SIG_TCKDIV = 0x0200,  /* JTAG clock divider; tclkdiv */
 111         VD_SIG_BUF    = 0x1000,  /* memory buffer; mem */
 112 };
 113
 114 /**
 115  * @brief List of errors
 116  */
 117 enum {
 118         VD_ERR_NONE       = 0x0000,  /* no error */
 119         VD_ERR_NOT_IMPL   = 0x0100,  /* feature not implemented */
 120         VD_ERR_USAGE      = 0x0101,  /* incorrect usage */
 121         VD_ERR_PARAM      = 0x0102,  /* incorrect parameter */
 122         VD_ERR_CONFIG     = 0x0107,  /* incorrect configuration */
 123         VD_ERR_NO_MEMORY  = 0x0104,  /* out of memory */
 124         VD_ERR_SHM_OPEN   = 0x010a,  /* cannot open shared memory */
 125         VD_ERR_SHM_MAP    = 0x010b,  /* cannot map shared memory */
 126         VD_ERR_SOC_OPEN   = 0x011a,  /* cannot open socket */
 127         VD_ERR_SOC_OPT    = 0x011b,  /* cannot set socket option */
 128         VD_ERR_SOC_ADDR   = 0x011c,  /* cannot resolve host address */
 129         VD_ERR_SOC_CONN   = 0x011d,  /* cannot connect to host */
 130         VD_ERR_SOC_SEND   = 0x011e,  /* error sending data on socket */
 131         VD_ERR_SOC_RECV   = 0x011f,  /* error receiving data from socket */
 132         VD_ERR_LOCKED     = 0x0202,  /* device locked */
 133         VD_ERR_NOT_RUN    = 0x0204,  /* transactor not running */
 134         VD_ERR_NOT_OPEN   = 0x0205,  /* transactor not open/connected */
 135         VD_ERR_LICENSE    = 0x0206,  /* cannot check out the license */
 136         VD_ERR_VERSION    = 0x0207,  /* transactor version mismatch */
 137         VD_ERR_TIME_OUT   = 0x0301,  /* time out, waiting */
 138         VD_ERR_NO_POWER   = 0x0302,  /* power out error */
 139         VD_ERR_BUS_ERROR  = 0x0304,  /* bus protocol error, like pslverr */
 140         VD_ERR_NO_ACCESS  = 0x0306,  /* no access to an object */
 141         VD_ERR_INV_HANDLE = 0x0307,  /* invalid object handle */
 142         VD_ERR_INV_SCOPE  = 0x0308,  /* invalid scope */
 143 };
 144
 145 enum {
 146         VD_CMD_OPEN       = 0x01,
 147         VD_CMD_CLOSE      = 0x02,
 148         VD_CMD_CONNECT    = 0x04,
 149         VD_CMD_DISCONNECT = 0x05,
 150         VD_CMD_WAIT       = 0x09,
 151         VD_CMD_SIGSET     = 0x0a,
 152         VD_CMD_SIGGET     = 0x0b,
 153         VD_CMD_JTAGCLOCK  = 0x0f,
 154         VD_CMD_REGWRITE   = 0x15,
 155         VD_CMD_REGREAD    = 0x16,
 156         VD_CMD_JTAGSHTAP  = 0x1a,
 157         VD_CMD_MEMOPEN    = 0x21,
 158         VD_CMD_MEMCLOSE   = 0x22,
 159         VD_CMD_MEMWRITE   = 0x23,
 160 };
 161
 162 enum {
 163         VD_ASPACE_AP      = 0x01,
 164         VD_ASPACE_DP      = 0x02,
 165         VD_ASPACE_ID      = 0x03,
 166         VD_ASPACE_AB      = 0x04,
 167 };
 168
 169 enum {
 170         VD_BATCH_NO       = 0,
 171         VD_BATCH_WO       = 1,
 172         VD_BATCH_WR       = 2,
 173 };
 174
 175 struct vd_shm {
 176         struct {                     /* VD_CHEADER_LEN written by client */
 177                 uint8_t cmd;             /* 000; command */
 178                 uint8_t type;            /* 001; interface type */
 179                 uint8_t waddr[2];        /* 002; write pointer */
 180                 uint8_t wbytes[2];       /* 004; data bytes */
 181                 uint8_t rbytes[2];       /* 006; data bytes to read */
 182                 uint8_t wwords[2];       /* 008; data words */
 183                 uint8_t rwords[2];       /* 00a; data words to read */
 184                 uint8_t rwdata[4];       /* 00c; read/write data */
 185                 uint8_t offset[4];       /* 010; address offset */
 186                 uint8_t offseth[2];      /* 014; address offset 47:32 */
 187                 uint8_t wid[2];          /* 016; request id*/
 188         };
 189         uint8_t wd8[VD_BUFFER_LEN];  /* 018; */
 190         struct {                     /* VD_SHEADER_LEN written by server */
 191                 uint8_t rid[2];          /* fd0: request id read */
 192                 uint8_t awords[2];       /* fd2: actual data words read back */
 193                 uint8_t status[4];       /* fd4; */
 194                 uint8_t duttime[8];      /* fd8; */
 195         };
 196         uint8_t rd8[VD_BUFFER_LEN];  /* fe0: */
 197         uint8_t state[4];            /* 1f98; connection state */
 198         uint8_t count[4];            /* 1f9c; */
 199         uint8_t dummy[96];           /* 1fa0; 48+40B+8B; */
 200 } __attribute__((packed));
 201
 202 struct vd_rdata {
 203         struct list_head lh;
 204         uint8_t *rdata;
 205 };
 206
 207 struct vd_client {
 208         uint8_t trans_batch;
 209         bool trans_first;
 210         bool trans_last;
 211         uint8_t mem_ndx;
 212         uint8_t buf_width;
 213         uint8_t addr_bits;
 214         uint8_t bfm_type;
 215         uint16_t sig_read;
 216         uint16_t sig_write;
 217         uint32_t bfm_period;
 218         uint32_t mem_base[VD_MAX_MEMORIES];
 219         uint32_t mem_size[VD_MAX_MEMORIES];
 220         uint32_t mem_width[VD_MAX_MEMORIES];
 221         uint32_t mem_depth[VD_MAX_MEMORIES];
 222         uint16_t server_port;
 223         uint32_t poll_cycles;
 224         uint32_t poll_min;
 225         uint32_t poll_max;
 226         uint32_t targ_time;
 227         int hsocket;
 228         char server_name[32];
 229         char bfm_path[128];
 230         char mem_path[VD_MAX_MEMORIES][128];
 231         struct vd_rdata rdataq;
 232 };
 233
 234 struct vd_jtag_hdr {
 235         uint64_t tlen:24;
 236         uint64_t post:3;
 237         uint64_t pre:3;
 238         uint64_t cmd:2;
 239         uint64_t wlen:16;
 240         uint64_t rlen:16;
 241 };
 242
 243 struct vd_reg_hdr {
 244         uint64_t prot:3;
 245         uint64_t nonincr:1;
 246         uint64_t haddr:12;
 247         uint64_t tlen:11;
 248         uint64_t asize:3;
 249         uint64_t cmd:2;
 250         uint64_t addr:32;
 251 };
 252
 253 static struct vd_shm *pbuf;
 254 static struct vd_client vdc;
 255
 256 static int vdebug_socket_error(void)
 257 {
 258 #ifdef _WIN32
 259         return WSAGetLastError();
 260 #else
 261         return errno;
 262 #endif
 263 }
 264
 265 static int vdebug_socket_open(char *server_addr, uint32_t port)
 266 {
 267         int hsock;
 268         int rc = 0;
 269         uint32_t buflen = sizeof(struct vd_shm); /* size of the send and rcv buffer */
 270         struct addrinfo *ainfo = NULL;
 271         struct addrinfo ahint = { 0, AF_INET, SOCK_STREAM, 0, 0, NULL, NULL, NULL };
 272
 273 #ifdef _WIN32
 274         hsock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
 275         if (hsock == INVALID_SOCKET)
 276                 rc = vdebug_socket_error();
 277 #else
 278         uint32_t rcvwat = VD_SHEADER_LEN;    /* size of the rcv header, as rcv min watermark */
 279         hsock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
 280         if (hsock < 0)
 281                 rc = errno;
 282         else if (setsockopt(hsock, SOL_SOCKET, SO_RCVLOWAT, &rcvwat, sizeof(rcvwat)) < 0)
 283                 rc = errno;
 284 #endif
 285         else if (setsockopt(hsock, SOL_SOCKET, SO_SNDBUF, (const char *)&buflen, sizeof(buflen)) < 0)
 286                 rc = vdebug_socket_error();
 287         else if (setsockopt(hsock, SOL_SOCKET, SO_RCVBUF, (const char *)&buflen, sizeof(buflen)) < 0)
 288                 rc = vdebug_socket_error();
 289
 290         if (rc) {
 291                 LOG_ERROR("socket_open: cannot set socket option, error %d", rc);
 292         } else if (getaddrinfo(server_addr, NULL, &ahint, &ainfo) != 0) {
 293                 LOG_ERROR("socket_open: cannot resolve address %s, error %d", server_addr, vdebug_socket_error());
 294                 rc = VD_ERR_SOC_ADDR;
 295         } else {
 296                 buf_set_u32((uint8_t *)ainfo->ai_addr->sa_data, 0, 16, htons(port));
 297                 if (connect(hsock, ainfo->ai_addr, sizeof(struct sockaddr)) < 0) {
 298                         LOG_ERROR("socket_open: cannot connect to %s:%d, error %d", server_addr, port, vdebug_socket_error());
 299                         rc = VD_ERR_SOC_CONN;
 300                 }
 301         }
 302
 303         if (rc) {
 304                 close_socket(hsock);
 305                 hsock = 0;
 306         }
 307
 308         if (ainfo)
 309                 freeaddrinfo(ainfo);
 310
 311         return hsock;
 312 }
 313
 314 static int vdebug_socket_receive(int hsock, struct vd_shm *pmem)
 315 {
 316         int rc;
 317         int dreceived = 0;
 318         int offset = &pmem->rid[0] - &pmem->cmd;
 319         int to_receive = VD_SHEADER_LEN + le_to_h_u16(pmem->rbytes);
 320         char *pb = (char *)pmem;
 321
 322         do {
 323                 rc = recv(hsock, pb + offset, to_receive, 0);
 324                 if (rc <= 0) {
 325                         LOG_WARNING("socket_receive: recv failed, error %d", rc < 0 ? vdebug_socket_error() : 0);
 326                         return rc;
 327                 }
 328                 to_receive -= rc;
 329                 offset += rc;
 330                 LOG_DEBUG_IO("socket_receive: received %d, to receive %d", rc, to_receive);
 331                 dreceived += rc;
 332         } while (to_receive);
 333
 334         return dreceived;
 335 }
 336
 337 static int vdebug_socket_send(int hsock, struct vd_shm *pmem)
 338 {
 339         int rc = send(hsock, (const char *)&pmem->cmd, VD_CHEADER_LEN + le_to_h_u16(pmem->wbytes), 0);
 340         if (rc <= 0)
 341                 LOG_WARNING("socket_send: send failed, error %d", vdebug_socket_error());
 342         else
 343                 LOG_DEBUG_IO("socket_send: sent %d, to send 0", rc);
 344
 345         return rc;
 346 }
 347
 348 static uint32_t vdebug_wait_server(int hsock, struct vd_shm *pmem)
 349 {
 350         if (!hsock)
 351                 return VD_ERR_SOC_OPEN;
 352
 353         int st = vdebug_socket_send(hsock, pmem);
 354         if (st <= 0)
 355                 return VD_ERR_SOC_SEND;
 356
 357         int rd = vdebug_socket_receive(hsock, pmem);
 358         if (rd  <= 0)
 359                 return VD_ERR_SOC_RECV;
 360
 361         int rc = le_to_h_u32(pmem->status);
 362         LOG_DEBUG_IO("wait_server: cmd %02" PRIx8 " done, sent %d, rcvd %d, status %d",
 363                                  pmem->cmd, st, rd, rc);
 364
 365         return rc;
 366 }
 367
 368 int vdebug_run_jtag_queue(int hsock, struct vd_shm *pm, unsigned int count)
 369 {
 370         uint8_t  num_pre, num_post, tdi, tms;
 371         unsigned int num, anum, bytes, hwords, words;
 372         unsigned int req, waddr, rwords;
 373         int64_t ts, te;
 374         uint8_t *tdo;
 375         int rc;
 376         uint64_t jhdr;
 377         struct vd_rdata *rd;
 378
 379         req = 0;                            /* beginning of request */
 380         waddr = 0;
 381         rwords = 0;
 382         h_u16_to_le(pm->wbytes, le_to_h_u16(pm->wwords) * vdc.buf_width);
 383         h_u16_to_le(pm->rbytes, le_to_h_u16(pm->rwords) * vdc.buf_width);
 384         ts = timeval_ms();
 385         rc = vdebug_wait_server(hsock, pm);
 386         while (!rc && (req < count)) {      /* loop over requests to read data and print out */
 387                 jhdr = le_to_h_u64(&pm->wd8[waddr * 4]);
 388                 words = jhdr >> 48;
 389                 hwords = (jhdr >> 32) & 0xffff;
 390                 anum = jhdr & 0xffffff;
 391                 num_pre = (jhdr >> 27) & 0x7;
 392                 num_post = (jhdr >> 24) & 0x7;
 393                 if (num_post)
 394                         num = anum - num_pre - num_post + 1;
 395                 else
 396                         num = anum - num_pre;
 397                 bytes = (num + 7) / 8;
 398                 vdc.trans_last = (req + 1) < count ? 0 : 1;
 399                 vdc.trans_first = waddr ? 0 : 1;
 400                 if (((jhdr >> 30) & 0x3) == 3) { /* cmd is read */
 401                         if (!rwords) {
 402                                 rd = &vdc.rdataq;
 403                                 tdo = rd->rdata;
 404                         } else {
 405                                 rd = list_first_entry(&vdc.rdataq.lh, struct vd_rdata, lh);
 406                                 tdo = rd->rdata;
 407                                 list_del(&rd->lh);
 408                                 free(rd);
 409                         }
 410                         for (unsigned int j = 0; j < bytes; j++) {
 411                                 tdo[j] = (pm->rd8[rwords * 8 + j] >> num_pre) | (pm->rd8[rwords * 8 + j + 1] << (8 - num_pre));
 412                                 LOG_DEBUG_IO("%04x D0[%02x]:%02x", le_to_h_u16(pm->wid) - count + req, j, tdo[j]);
 413                         }
 414                         rwords += words;           /* read data offset */
 415                 } else {
 416                         tdo = NULL;
 417                 }
 418                 waddr += sizeof(uint64_t) / 4; /* waddr past header */
 419                 tdi = (pm->wd8[waddr * 4] >> num_pre) | (pm->wd8[waddr * 4 + 1] << (8 - num_pre));
 420                 tms = (pm->wd8[waddr * 4 + 4] >> num_pre) | (pm->wd8[waddr * 4 + 4 + 1] << (8 - num_pre));
 421                 LOG_DEBUG_IO("%04x L:%02d O:%05x @%03x DI:%02x MS:%02x DO:%02x",
 422                         le_to_h_u16(pm->wid) - count + req, num, (vdc.trans_first << 14) | (vdc.trans_last << 15),
 423                         waddr - 2, tdi, tms, (tdo ? tdo[0] : 0xdd));
 424                 waddr += hwords * 2;           /* start of next request */
 425                 req += 1;
 426         }
 427
 428         if (rc) {
 429                 LOG_ERROR("0x%x executing transaction", rc);
 430                 rc = ERROR_FAIL;
 431         }
 432
 433         te = timeval_ms();
 434         vdc.targ_time += (uint32_t)(te - ts);
 435         h_u16_to_le(pm->offseth, 0);      /* reset buffer write address */
 436         h_u32_to_le(pm->offset, 0);
 437         h_u16_to_le(pm->rwords, 0);
 438         h_u16_to_le(pm->waddr, 0);
 439         assert(list_empty(&vdc.rdataq.lh));/* list should be empty after run queue */
 440
 441         return rc;
 442 }
 443
 444 int vdebug_run_reg_queue(int hsock, struct vd_shm *pm, unsigned int count)
 445 {
 446         unsigned int num, awidth, wwidth;
 447         unsigned int req, waddr, rwords;
 448         uint8_t aspace;
 449         uint32_t addr;
 450         int64_t ts, te;
 451         uint8_t *data;
 452         int rc;
 453         uint64_t rhdr;
 454         struct vd_rdata *rd;
 455
 456         req = 0;                            /* beginning of request */
 457         waddr = 0;
 458         rwords = 0;
 459         h_u16_to_le(pm->wbytes, le_to_h_u16(pm->wwords) * vdc.buf_width);
 460         h_u16_to_le(pm->rbytes, le_to_h_u16(pm->rwords) * vdc.buf_width);
 461         ts = timeval_ms();
 462         rc = vdebug_wait_server(hsock, pm);
 463         while (!rc && (req < count)) {      /* loop over requests to read data and print out */
 464                 rhdr = le_to_h_u64(&pm->wd8[waddr * 4]);
 465                 addr = rhdr >> 32;              /* reconstruct data for a single request */
 466                 num = (rhdr >> 16) & 0x7ff;
 467                 aspace = rhdr & 0x3;
 468                 awidth = (1 << ((rhdr >> 27) & 0x7));
 469                 wwidth = (awidth + vdc.buf_width - 1) / vdc.buf_width;
 470                 vdc.trans_last = (req + 1) < count ? 0 : 1;
 471                 vdc.trans_first = waddr ? 0 : 1;
 472                 if (((rhdr >> 30) & 0x3) == 2) { /* cmd is read */
 473                         if (num) {
 474                                 if (!rwords) {
 475                                         rd = &vdc.rdataq;
 476                                         data = rd->rdata;
 477                                 } else {
 478                                         rd = list_first_entry(&vdc.rdataq.lh, struct vd_rdata, lh);
 479                                         data = rd->rdata;
 480                                         list_del(&rd->lh);
 481                                         free(rd);
 482                                 }
 483                                 for (unsigned int j = 0; j < num; j++)
 484                                         memcpy(&data[j * awidth], &pm->rd8[(rwords + j) * awidth], awidth);
 485                         }
 486                         LOG_DEBUG_IO("read  %04x AS:%02x RG:%02x O:%05x @%03x D:%08x", le_to_h_u16(pm->wid) - count + req,
 487                                 aspace, addr, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr,
 488                                 (num ? le_to_h_u32(&pm->rd8[rwords * 4]) : 0xdead));
 489                         rwords += num * wwidth;
 490                         waddr += sizeof(uint64_t) / 4; /* waddr past header */
 491                 } else {
 492                         LOG_DEBUG_IO("write %04x AS:%02x RG:%02x O:%05x @%03x D:%08x", le_to_h_u16(pm->wid) - count + req,
 493                                 aspace, addr, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr,
 494                                 le_to_h_u32(&pm->wd8[(waddr + num + 1) * 4]));
 495                         waddr += sizeof(uint64_t) / 4 + (num * wwidth * awidth + 3) / 4;
 496                 }
 497                 req += 1;
 498         }
 499
 500         if (rc) {
 501                 LOG_ERROR("0x%x executing transaction", rc);
 502                 rc = ERROR_FAIL;
 503         }
 504
 505         te = timeval_ms();
 506         vdc.targ_time += (uint32_t)(te - ts);
 507         h_u16_to_le(pm->offseth, 0);      /* reset buffer write address */
 508         h_u32_to_le(pm->offset, 0);
 509         h_u16_to_le(pm->rwords, 0);
 510         h_u16_to_le(pm->waddr, 0);
 511         assert(list_empty(&vdc.rdataq.lh));/* list should be empty after run queue */
 512
 513         return rc;
 514 }
 515
 516 static int vdebug_open(int hsock, struct vd_shm *pm, const char *path,
 517                                                 uint8_t type, uint32_t period_ps, uint32_t sig_mask)
 518 {
 519         int rc = VD_ERR_NOT_OPEN;
 520
 521         pm->cmd = VD_CMD_OPEN;
 522         h_u16_to_le(pm->wid, VD_VERSION);  /* client version */
 523         h_u16_to_le(pm->wbytes, 0);
 524         h_u16_to_le(pm->rbytes, 0);
 525         h_u16_to_le(pm->wwords, 0);
 526         h_u16_to_le(pm->rwords, 0);
 527         rc = vdebug_wait_server(hsock, pm);
 528         if (rc != 0) {                     /* communication problem */
 529                 LOG_ERROR("0x%x connecting to server", rc);
 530         } else if (le_to_h_u16(pm->rid) < le_to_h_u16(pm->wid)) {
 531                 LOG_ERROR("server version %d too old for the client %d", le_to_h_u16(pm->rid), le_to_h_u16(pm->wid));
 532                 pm->cmd = VD_CMD_CLOSE;        /* let server close the connection */
 533                 vdebug_wait_server(hsock, pm);
 534                 rc = VD_ERR_VERSION;
 535         } else {
 536                 pm->cmd = VD_CMD_CONNECT;
 537                 pm->type = type;               /* BFM type to connect to, here JTAG */
 538                 h_u32_to_le(pm->rwdata, sig_mask | VD_SIG_BUF | (VD_SIG_BUF << 16));
 539                 h_u16_to_le(pm->wbytes, strlen(path) + 1);
 540                 h_u16_to_le(pm->rbytes, 12);
 541                 h_u16_to_le(pm->wid, 0);       /* reset wid for transaction ID */
 542                 h_u16_to_le(pm->wwords, 0);
 543                 h_u16_to_le(pm->rwords, 0);
 544                 memcpy(pm->wd8, path, le_to_h_u16(pm->wbytes));
 545                 rc = vdebug_wait_server(hsock, pm);
 546                 vdc.sig_read = le_to_h_u32(pm->rwdata) >> 16;  /* signal read mask */
 547                 vdc.sig_write = le_to_h_u32(pm->rwdata);     /* signal write mask */
 548                 vdc.bfm_period = period_ps;
 549                 vdc.buf_width = le_to_h_u32(&pm->rd8[0]) / 8;/* access width in bytes */
 550                 vdc.addr_bits = le_to_h_u32(&pm->rd8[2 * 4]);    /* supported address bits */
 551         }
 552
 553         if (rc) {
 554                 LOG_ERROR("0x%x connecting to BFM %s", rc, path);
 555                 return ERROR_FAIL;
 556         }
 557
 558         INIT_LIST_HEAD(&vdc.rdataq.lh);
 559         LOG_DEBUG("%s type %0x, period %dps, buffer %dx%dB signals r%04xw%04x",
 560                 path, type, vdc.bfm_period, VD_BUFFER_LEN / vdc.buf_width,
 561                 vdc.buf_width, vdc.sig_read, vdc.sig_write);
 562
 563         return ERROR_OK;
 564 }
 565
 566 static int vdebug_close(int hsock, struct vd_shm *pm, uint8_t type)
 567 {
 568         pm->cmd = VD_CMD_DISCONNECT;
 569         pm->type = type;              /* BFM type, here JTAG */
 570         h_u16_to_le(pm->wbytes, 0);
 571         h_u16_to_le(pm->rbytes, 0);
 572         h_u16_to_le(pm->wwords, 0);
 573         h_u16_to_le(pm->rwords, 0);
 574         vdebug_wait_server(hsock, pm);
 575         pm->cmd = VD_CMD_CLOSE;
 576         h_u16_to_le(pm->wid, VD_VERSION);    /* client version */
 577         h_u16_to_le(pm->wbytes, 0);
 578         h_u16_to_le(pm->rbytes, 0);
 579         h_u16_to_le(pm->wwords, 0);
 580         h_u16_to_le(pm->rwords, 0);
 581         vdebug_wait_server(hsock, pm);
 582         LOG_DEBUG("type %0x", type);
 583
 584         return ERROR_OK;
 585 }
 586
 587 static int vdebug_wait(int hsock, struct vd_shm *pm, uint32_t cycles)
 588 {
 589         if (cycles) {
 590                 pm->cmd = VD_CMD_WAIT;
 591                 h_u16_to_le(pm->wbytes, 0);
 592                 h_u16_to_le(pm->rbytes, 0);
 593                 h_u32_to_le(pm->rwdata, cycles);  /* clock sycles to wait */
 594                 int rc = vdebug_wait_server(hsock, pm);
 595                 if (rc) {
 596                         LOG_ERROR("0x%x waiting %" PRIx32 " cycles", rc, cycles);
 597                         return ERROR_FAIL;
 598                 }
 599                 LOG_DEBUG("%d cycles", cycles);
 600         }
 601
 602         return ERROR_OK;
 603 }
 604
 605 static int vdebug_sig_set(int hsock, struct vd_shm *pm, uint32_t write_mask, uint32_t value)
 606 {
 607         pm->cmd = VD_CMD_SIGSET;
 608         h_u16_to_le(pm->wbytes, 0);
 609         h_u16_to_le(pm->rbytes, 0);
 610         h_u32_to_le(pm->rwdata, (write_mask << 16) | (value & 0xffff)); /* mask and value of signals to set */
 611         int rc = vdebug_wait_server(hsock, pm);
 612         if (rc) {
 613                 LOG_ERROR("0x%x setting signals %04" PRIx32, rc, write_mask);
 614                 return ERROR_FAIL;
 615         }
 616
 617         LOG_DEBUG("setting signals %04" PRIx32 " to %04" PRIx32, write_mask, value);
 618
 619         return ERROR_OK;
 620 }
 621
 622 static int vdebug_jtag_clock(int hsock, struct vd_shm *pm, uint32_t value)
 623 {
 624         pm->cmd = VD_CMD_JTAGCLOCK;
 625         h_u16_to_le(pm->wbytes, 0);
 626         h_u16_to_le(pm->rbytes, 0);
 627         h_u32_to_le(pm->rwdata, value);  /* divider value */
 628         int rc = vdebug_wait_server(hsock, pm);
 629         if (rc) {
 630                 LOG_ERROR("0x%x setting jtag_clock", rc);
 631                 return ERROR_FAIL;
 632         }
 633
 634         LOG_DEBUG("setting jtag clock divider to %" PRIx32, value);
 635
 636         return ERROR_OK;
 637 }
 638
 639 static int vdebug_jtag_shift_tap(int hsock, struct vd_shm *pm, uint8_t num_pre,
 640                                                                  const uint8_t tms_pre, uint32_t num, const uint8_t *tdi,
 641                                                                  uint8_t num_post, const uint8_t tms_post, uint8_t *tdo,
 642                                                                  uint8_t f_last)
 643 {
 644         const uint32_t tobits = 8;
 645         uint16_t bytes, hwords, anum, words, waddr;
 646         int rc = 0;
 647
 648         pm->cmd = VD_CMD_JTAGSHTAP;
 649         vdc.trans_last = f_last || (vdc.trans_batch == VD_BATCH_NO);
 650         if (vdc.trans_first)
 651                 waddr = 0;             /* reset buffer offset */
 652         else
 653                 waddr = le_to_h_u32(pm->offseth);   /* continue from the previous transaction */
 654         if (num_post)          /* actual number of bits to shift */
 655                 anum = num + num_pre + num_post - 1;
 656         else
 657                 anum = num + num_pre;
 658         hwords = (anum + 4 * vdc.buf_width - 1) / (4 * vdc.buf_width); /* in 4B TDI/TMS words */
 659         words = (hwords + 1) / 2;    /* in 8B TDO words to read */
 660         bytes = (num + 7) / 8;       /* data only portion in bytes */
 661         /* buffer overflow check and flush */
 662         if (4 * waddr + sizeof(uint64_t) + 8 * hwords + 64 > VD_BUFFER_LEN) {
 663                 vdc.trans_last = 1;        /* force flush within 64B of buffer end */
 664         } else if (4 * waddr + sizeof(uint64_t) + 8 * hwords > VD_BUFFER_LEN) {
 665                 /* this req does not fit, discard it */
 666                 LOG_ERROR("%04x L:%02d O:%05x @%04x too many bits to shift",
 667                         le_to_h_u16(pm->wid), anum, (vdc.trans_first << 14) | (vdc.trans_last << 15), waddr);
 668                 rc = ERROR_FAIL;
 669         }
 670
 671         if (!rc && anum) {
 672                 uint16_t i, j;       /* portability requires to use bit operations for 8B JTAG header */
 673                 uint64_t jhdr = (tdo ? ((uint64_t)(words) << 48) : 0) + ((uint64_t)(hwords) << 32) +
 674                         ((tdo ? 3UL : 1UL) << 30) + (num_pre << 27) + (num_post << 24) + anum;
 675                 h_u64_to_le(&pm->wd8[4 * waddr], jhdr);
 676
 677                 h_u16_to_le(pm->wid, le_to_h_u16(pm->wid) + 1);    /* transaction ID */
 678                 waddr += 2;              /* waddr past header */
 679                 /* TDI/TMS data follows as 32 bit word pairs {TMS,TDI} */
 680                 pm->wd8[4 * waddr] = (tdi ? (tdi[0] << num_pre) : 0);
 681                 pm->wd8[4 * waddr + 4] = tms_pre;    /* init with tms_pre */
 682                 if (num + num_pre <= 8)            /* and tms_post for num <=4 */
 683                         pm->wd8[4 * waddr + 4] |= (tms_post << (num + num_pre - 1));
 684                 for (i = 1, j = 4 * waddr; i < bytes; i++) {
 685                         if (i == bytes - 1 && num + num_pre <= bytes * tobits)
 686                                 pm->wd8[j + i + 4] = tms_post << ((num + num_pre - 1) % 8);
 687                         else
 688                                 pm->wd8[j + i + 4] = 0x0;/* placing 4 bytes of TMS bits into high word */
 689                         if (!tdi)             /* placing 4 bytes of TDI bits into low word */
 690                                 pm->wd8[j + i] = 0x0;
 691                         else
 692                                 pm->wd8[j + i] = (tdi[i] << num_pre) | (tdi[i - 1] >> (8 - num_pre));
 693                         if (i % 4 == 3)
 694                                 j += 4;
 695                 }
 696
 697                 if (tdi)
 698                         if (num + num_pre > bytes * tobits) /* in case 1 additional byte needed for TDI */
 699                                 pm->wd8[j + i] = (tdi[i - 1] >> (8 - num_pre)); /* put last TDI bits there */
 700
 701                 if (num + num_pre <= bytes * tobits) { /* in case no or 1 additional byte needed */
 702                         pm->wd8[j + i + 4] = tms_post >> (8 - (num + num_pre - 1) % 8); /* may need to add higher part */
 703                 /* in case exactly 1 additional byte needed */
 704                 } else if (num + num_pre > bytes * tobits && anum <= (bytes + 1) * tobits) {
 705                         pm->wd8[j + i + 4] = tms_post << ((num + num_pre - 1) % 8); /* add whole tms_post */
 706                 } else {                           /* in case 2 additional bytes, tms_post split */
 707                         pm->wd8[j + i + 4] = tms_post << ((num + num_pre - 1) % 8);/* add lower part of tms_post */
 708                         if (i % 4 == 3)              /* next byte is in the next 32b word */
 709                                 pm->wd8[j + i + 4 + 5] = tms_post >> (8 - (num + num_pre - 1) % 8); /* and higher part */
 710                         else                         /* next byte is in the same 32b word */
 711                                 pm->wd8[j + i + 4 + 1] = tms_post >> (8 - (num + num_pre - 1) % 8); /* and higher part */
 712                 }
 713
 714                 if (tdo) {
 715                         struct vd_rdata *rd;
 716                         if (le_to_h_u16(pm->rwords) == 0) {
 717                                 rd = &vdc.rdataq;
 718                         } else {
 719                                 rd = calloc(1, sizeof(struct vd_rdata));
 720                                 if (!rd)                   /* check allocation for 24B */
 721                                         return ERROR_FAIL;
 722                                 list_add_tail(&rd->lh, &vdc.rdataq.lh);
 723                         }
 724                         rd->rdata = tdo;
 725                         h_u16_to_le(pm->rwords, le_to_h_u16(pm->rwords) + words);/* keep track of the words to read */
 726                 }
 727                 h_u16_to_le(pm->wwords, waddr / 2 + hwords); /* payload size *2 to include both TDI and TMS data */
 728                 h_u16_to_le(pm->waddr, le_to_h_u16(pm->waddr) + 1);
 729         }
 730
 731         if (!waddr)                        /* flush issued, but buffer empty */
 732                 ;
 733         else if (!vdc.trans_last)          /* buffered request */
 734                 h_u16_to_le(pm->offseth, waddr + hwords * 2);  /* offset for next transaction, must be even */
 735         else                               /* execute batch of requests */
 736                 rc = vdebug_run_jtag_queue(hsock, pm, le_to_h_u16(pm->waddr));
 737         vdc.trans_first = vdc.trans_last; /* flush forces trans_first flag */
 738
 739         return rc;
 740 }
 741
 742 static int vdebug_reg_write(int hsock, struct vd_shm *pm, const uint32_t reg,
 743                                                         const uint32_t data, uint8_t aspace, uint8_t f_last)
 744 {
 745         uint32_t waddr;
 746         int rc = ERROR_OK;
 747
 748         pm->cmd = VD_CMD_REGWRITE;
 749         vdc.trans_last = f_last || (vdc.trans_batch == VD_BATCH_NO);
 750         if (vdc.trans_first)
 751                 waddr = 0;             /* reset buffer offset */
 752         else
 753                 waddr = le_to_h_u16(pm->offseth);   /* continue from the previous transaction */
 754
 755         if (4 * waddr + 2 * sizeof(uint64_t) + 4 > VD_BUFFER_LEN)
 756                 vdc.trans_last = 1;    /* force flush, no room for next request */
 757
 758         uint64_t rhdr = ((uint64_t)reg << 32) + (1UL << 30) + (2UL << 27) + (1UL << 16) + aspace;
 759         h_u64_to_le(&pm->wd8[4 * waddr], rhdr);
 760         h_u32_to_le(&pm->wd8[4 * (waddr + 2)], data);
 761         h_u16_to_le(pm->wid, le_to_h_u16(pm->wid) + 1);
 762         h_u16_to_le(pm->wwords, waddr + 3);
 763         h_u16_to_le(pm->waddr, le_to_h_u16(pm->waddr) + 1);
 764         if (!vdc.trans_last)       /* buffered request */
 765                 h_u16_to_le(pm->offseth, waddr + 3);
 766         else
 767                 rc = vdebug_run_reg_queue(hsock, pm, le_to_h_u16(pm->waddr));
 768         vdc.trans_first = vdc.trans_last; /* flush forces trans_first flag */
 769
 770         return rc;
 771 }
 772
 773 static int vdebug_reg_read(int hsock, struct vd_shm *pm, const uint32_t reg,
 774                                                         uint32_t *data, uint8_t aspace, uint8_t f_last)
 775 {
 776         uint32_t waddr;
 777         int rc = ERROR_OK;
 778
 779         pm->cmd = VD_CMD_REGREAD;
 780         vdc.trans_last = f_last || (vdc.trans_batch == VD_BATCH_NO);
 781         if (vdc.trans_first)
 782                 waddr = 0;             /* reset buffer offset */
 783         else
 784                 waddr = le_to_h_u16(pm->offseth);   /* continue from the previous transaction */
 785
 786         if (4 * waddr + 2 * sizeof(uint64_t) + 4 > VD_BUFFER_LEN)
 787                 vdc.trans_last = 1;    /* force flush, no room for next request */
 788
 789         uint64_t rhdr = ((uint64_t)reg << 32) + (2UL << 30) + (2UL << 27) + ((data ? 1UL : 0UL) << 16) + aspace;
 790         h_u64_to_le(&pm->wd8[4 * waddr], rhdr);
 791         h_u16_to_le(pm->wid, le_to_h_u16(pm->wid) + 1);
 792         if (data) {
 793                 struct vd_rdata *rd;
 794                 if (le_to_h_u16(pm->rwords) == 0) {
 795                         rd = &vdc.rdataq;
 796                 } else {
 797                         rd = calloc(1, sizeof(struct vd_rdata));
 798                         if (!rd)                   /* check allocation for 24B */
 799                                 return ERROR_FAIL;
 800                         list_add_tail(&rd->lh, &vdc.rdataq.lh);
 801                 }
 802                 rd->rdata = (uint8_t *)data;
 803                 h_u16_to_le(pm->rwords, le_to_h_u16(pm->rwords) + 1);
 804         }
 805         h_u16_to_le(pm->wwords, waddr + 2);
 806         h_u16_to_le(pm->waddr, le_to_h_u16(pm->waddr) + 1);
 807         if (!vdc.trans_last)       /* buffered request */
 808                 h_u16_to_le(pm->offseth, waddr + 2);
 809         else
 810                 rc = vdebug_run_reg_queue(hsock, pm, le_to_h_u16(pm->waddr));
 811         vdc.trans_first = vdc.trans_last; /* flush forces trans_first flag */
 812
 813         return rc;
 814 }
 815
 816 static int vdebug_mem_open(int hsock, struct vd_shm *pm, const char *path, uint8_t ndx)
 817 {
 818         int rc;
 819
 820         if (!path)
 821                 return ERROR_OK;
 822
 823         pm->cmd = VD_CMD_MEMOPEN;
 824         h_u16_to_le(pm->wbytes, strlen(path) + 1);   /* includes terminating 0 */
 825         h_u16_to_le(pm->rbytes, 8);
 826         h_u16_to_le(pm->wwords, 0);
 827         h_u16_to_le(pm->rwords, 0);
 828         memcpy(pm->wd8, path, le_to_h_u16(pm->wbytes));
 829         rc = vdebug_wait_server(hsock, pm);
 830         if (rc) {
 831                 LOG_ERROR("0x%x opening memory %s", rc, path);
 832         } else if (ndx != pm->rd8[2]) {
 833                 LOG_WARNING("Invalid memory index %" PRIu16 " returned. Direct memory access disabled", pm->rd8[2]);
 834         } else {
 835                 vdc.mem_width[ndx] = le_to_h_u16(&pm->rd8[0]) / 8;   /* memory width in bytes */
 836                 vdc.mem_depth[ndx] = le_to_h_u32(&pm->rd8[4]);       /* memory depth in words */
 837                 LOG_DEBUG("%" PRIx8 ": %s memory %" PRIu32 "x%" PRIu32 "B, buffer %" PRIu32 "x%" PRIu32 "B", ndx, path,
 838                         vdc.mem_depth[ndx], vdc.mem_width[ndx], VD_BUFFER_LEN / vdc.mem_width[ndx], vdc.mem_width[ndx]);
 839         }
 840
 841         return ERROR_OK;
 842 }
 843
 844 static void vdebug_mem_close(int hsock, struct vd_shm *pm, uint8_t ndx)
 845 {
 846         pm->cmd = VD_CMD_MEMCLOSE;
 847         h_u32_to_le(pm->rwdata, ndx);        /* which memory */
 848         h_u16_to_le(pm->wbytes, 0);
 849         h_u16_to_le(pm->rbytes, 0);
 850         h_u16_to_le(pm->wwords, 0);
 851         h_u16_to_le(pm->rwords, 0);
 852         vdebug_wait_server(hsock, pm);
 853         LOG_DEBUG("%" PRIx8 ": %s", ndx, vdc.mem_path[ndx]);
 854 }
 855
 856
 857 static int vdebug_init(void)
 858 {
 859         vdc.hsocket = vdebug_socket_open(vdc.server_name, vdc.server_port);
 860         pbuf = calloc(1, sizeof(struct vd_shm));
 861         if (!pbuf) {
 862                 close_socket(vdc.hsocket);
 863                 vdc.hsocket = 0;
 864                 LOG_ERROR("cannot allocate %zu bytes", sizeof(struct vd_shm));
 865                 return ERROR_FAIL;
 866         }
 867         if (vdc.hsocket <= 0) {
 868                 free(pbuf);
 869                 pbuf = NULL;
 870                 LOG_ERROR("cannot connect to vdebug server %s:%" PRIu16,
 871                         vdc.server_name, vdc.server_port);
 872                 return ERROR_FAIL;
 873         }
 874         vdc.trans_first = 1;
 875         vdc.poll_cycles = vdc.poll_max;
 876         uint32_t sig_mask = VD_SIG_RESET;
 877         if (transport_is_jtag())
 878                 sig_mask |= VD_SIG_TRST | VD_SIG_TCKDIV;
 879
 880         int rc = vdebug_open(vdc.hsocket, pbuf, vdc.bfm_path, vdc.bfm_type, vdc.bfm_period, sig_mask);
 881         if (rc != 0) {
 882                 LOG_ERROR("0x%x cannot connect to %s", rc, vdc.bfm_path);
 883                 close_socket(vdc.hsocket);
 884                 vdc.hsocket = 0;
 885                 free(pbuf);
 886                 pbuf = NULL;
 887         } else {
 888                 for (uint8_t i = 0; i < vdc.mem_ndx; i++) {
 889                         rc = vdebug_mem_open(vdc.hsocket, pbuf, vdc.mem_path[i], i);
 890                         if (rc != 0)
 891                                 LOG_ERROR("0x%x cannot connect to %s", rc, vdc.mem_path[i]);
 892                 }
 893
 894                 LOG_INFO("vdebug %d connected to %s through %s:%" PRIu16,
 895                                  VD_VERSION, vdc.bfm_path, vdc.server_name, vdc.server_port);
 896         }
 897
 898         return rc;
 899 }
 900
 901 static int vdebug_quit(void)
 902 {
 903         for (uint8_t i = 0; i < vdc.mem_ndx; i++)
 904                 if (vdc.mem_width[i])
 905                         vdebug_mem_close(vdc.hsocket, pbuf, i);
 906         int rc = vdebug_close(vdc.hsocket, pbuf, vdc.bfm_type);
 907         LOG_INFO("vdebug %d disconnected from %s through %s:%" PRIu16 " rc:%d", VD_VERSION,
 908                 vdc.bfm_path, vdc.server_name, vdc.server_port, rc);
 909         if (vdc.hsocket)
 910                 close_socket(vdc.hsocket);
 911         free(pbuf);
 912         pbuf = NULL;
 913
 914         return ERROR_OK;
 915 }
 916
 917 static int vdebug_reset(int trst, int srst)
 918 {
 919         uint16_t sig_val = 0xffff;
 920         uint16_t sig_mask = 0;
 921
 922         sig_mask |= VD_SIG_RESET;
 923         if (srst)
 924                 sig_val &= ~VD_SIG_RESET;/* active low */
 925         if (transport_is_jtag()) {
 926                 sig_mask |= VD_SIG_TRST;
 927                 if (trst)
 928                         sig_val &= ~VD_SIG_TRST; /* active low */
 929         }
 930
 931         LOG_INFO("rst trst:%d srst:%d mask:%" PRIx16 " val:%" PRIx16, trst, srst, sig_mask, sig_val);
 932         int rc = vdebug_sig_set(vdc.hsocket, pbuf, sig_mask, sig_val);
 933         if (rc == 0)
 934                 rc = vdebug_wait(vdc.hsocket, pbuf, 20); /* 20 clock cycles pulse */
 935
 936         return rc;
 937 }
 938
 939 static int vdebug_jtag_tms_seq(const uint8_t *tms, int num, uint8_t f_flush)
 940 {
 941         LOG_INFO("tms  len:%d tms:%x", num, *tms);
 942
 943         return vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num, *tms, 0, NULL, 0, 0, NULL, f_flush);
 944 }
 945
 946 static int vdebug_jtag_path_move(struct pathmove_command *cmd, uint8_t f_flush)
 947 {
 948         uint8_t tms[DIV_ROUND_UP(cmd->num_states, 8)];
 949         LOG_INFO("path num states %d", cmd->num_states);
 950
 951         memset(tms, 0, DIV_ROUND_UP(cmd->num_states, 8));
 952
 953         for (uint8_t i = 0; i < cmd->num_states; i++) {
 954                 if (tap_state_transition(tap_get_state(), true) == cmd->path[i])
 955                         buf_set_u32(tms, i, 1, 1);
 956                 tap_set_state(cmd->path[i]);
 957         }
 958
 959         return vdebug_jtag_tms_seq(tms, cmd->num_states, f_flush);
 960 }
 961
 962 static int vdebug_jtag_tlr(tap_state_t state, uint8_t f_flush)
 963 {
 964         int rc = ERROR_OK;
 965
 966         uint8_t cur = tap_get_state();
 967         uint8_t tms_pre = tap_get_tms_path(cur, state);
 968         uint8_t num_pre = tap_get_tms_path_len(cur, state);
 969         LOG_INFO("tlr  from %" PRIx8 " to %" PRIx8, cur, state);
 970         if (cur != state) {
 971                 rc = vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num_pre, tms_pre, 0, NULL, 0, 0, NULL, f_flush);
 972                 tap_set_state(state);
 973         }
 974
 975         return rc;
 976 }
 977
 978 static int vdebug_jtag_scan(struct scan_command *cmd, uint8_t f_flush)
 979 {
 980         int rc = ERROR_OK;
 981
 982         uint8_t cur = tap_get_state();
 983         uint8_t state = cmd->ir_scan ? TAP_IRSHIFT : TAP_DRSHIFT;
 984         uint8_t tms_pre = tap_get_tms_path(cur, state);
 985         uint8_t num_pre = tap_get_tms_path_len(cur, state);
 986         uint8_t tms_post = tap_get_tms_path(state, cmd->end_state);
 987         uint8_t num_post = tap_get_tms_path_len(state, cmd->end_state);
 988         int num_bits = jtag_scan_size(cmd);
 989         LOG_DEBUG("scan len:%d fields:%d ir/!dr:%d state cur:%x end:%x",
 990                           num_bits, cmd->num_fields, cmd->ir_scan, cur, cmd->end_state);
 991         for (int i = 0; i < cmd->num_fields; i++) {
 992                 uint8_t cur_num_pre = i == 0 ? num_pre : 0;
 993                 uint8_t cur_tms_pre = i == 0 ? tms_pre : 0;
 994                 uint8_t cur_num_post = i == cmd->num_fields - 1 ? num_post : 0;
 995                 uint8_t cur_tms_post = i == cmd->num_fields - 1 ? tms_post : 0;
 996                 uint8_t cur_flush = i == cmd->num_fields - 1 ? f_flush : 0;
 997                 rc = vdebug_jtag_shift_tap(vdc.hsocket, pbuf, cur_num_pre, cur_tms_pre,
 998                                                                    cmd->fields[i].num_bits, cmd->fields[i].out_value, cur_num_post, cur_tms_post,
 999                                                          cmd->fields[i].in_value, cur_flush);
1000                 if (rc)
1001                         break;
1002         }
1003
1004         if (cur != cmd->end_state)
1005                 tap_set_state(cmd->end_state);
1006
1007         return rc;
1008 }
1009
1010 static int vdebug_jtag_runtest(int cycles, tap_state_t state, uint8_t f_flush)
1011 {
1012         uint8_t cur = tap_get_state();
1013         uint8_t tms_pre = tap_get_tms_path(cur, state);
1014         uint8_t num_pre = tap_get_tms_path_len(cur, state);
1015         LOG_DEBUG("idle len:%d state cur:%x end:%x", cycles, cur, state);
1016         int rc = vdebug_jtag_shift_tap(vdc.hsocket, pbuf, num_pre, tms_pre, cycles, NULL, 0, 0, NULL, f_flush);
1017         if (cur != state)
1018                 tap_set_state(state);
1019
1020         return rc;
1021 }
1022
1023 static int vdebug_jtag_stableclocks(int num, uint8_t f_flush)
1024 {
1025         LOG_INFO("stab len:%d state cur:%x", num, tap_get_state());
1026
1027         return vdebug_jtag_shift_tap(vdc.hsocket, pbuf, 0, 0, num, NULL, 0, 0, NULL, f_flush);
1028 }
1029
1030 static int vdebug_sleep(int us)
1031 {
1032         LOG_INFO("sleep %d us", us);
1033
1034         return vdebug_wait(vdc.hsocket, pbuf, us / 1000);
1035 }
1036
1037 static int vdebug_jtag_speed(int speed)
1038 {
1039         unsigned int clkmax = VD_SCALE_PSTOMS / (vdc.bfm_period * 2); /* kHz */
1040         unsigned int divval = clkmax / speed;
1041         LOG_INFO("jclk speed:%d kHz set, BFM divider %u", speed, divval);
1042
1043         return vdebug_jtag_clock(vdc.hsocket, pbuf, divval);
1044 }
1045
1046 static int vdebug_jtag_khz(int khz, int *jtag_speed)
1047 {
1048         unsigned int clkmax = VD_SCALE_PSTOMS / (vdc.bfm_period * 2); /* kHz */
1049         unsigned int divval = khz ? clkmax / khz : 1;
1050         *jtag_speed = clkmax / divval;
1051         LOG_DEBUG("khz  speed:%d from khz:%d", *jtag_speed, khz);
1052
1053         return ERROR_OK;
1054 }
1055
1056 static int vdebug_jtag_div(int speed, int *khz)
1057 {
1058         *khz = speed;
1059         LOG_DEBUG("div  khz:%d from speed:%d", *khz, speed);
1060
1061         return ERROR_OK;
1062 }
1063
1064 static int vdebug_jtag_execute_queue(void)
1065 {
1066         int rc = ERROR_OK;
1067
1068         for (struct jtag_command *cmd = jtag_command_queue; rc == ERROR_OK && cmd; cmd = cmd->next) {
1069                 switch (cmd->type) {
1070                 case JTAG_RUNTEST:
1071                         rc = vdebug_jtag_runtest(cmd->cmd.runtest->num_cycles, cmd->cmd.runtest->end_state, !cmd->next);
1072                         break;
1073                 case JTAG_STABLECLOCKS:
1074                         rc = vdebug_jtag_stableclocks(cmd->cmd.stableclocks->num_cycles, !cmd->next);
1075                         break;
1076                 case JTAG_TLR_RESET:
1077                         rc = vdebug_jtag_tlr(cmd->cmd.statemove->end_state, !cmd->next);
1078                         break;
1079                 case JTAG_PATHMOVE:
1080                         rc = vdebug_jtag_path_move(cmd->cmd.pathmove, !cmd->next);
1081                         break;
1082                 case JTAG_TMS:
1083                         rc = vdebug_jtag_tms_seq(cmd->cmd.tms->bits, cmd->cmd.tms->num_bits, !cmd->next);
1084                         break;
1085                 case JTAG_SLEEP:
1086                         rc = vdebug_sleep(cmd->cmd.sleep->us);
1087                         break;
1088                 case JTAG_SCAN:
1089                         rc = vdebug_jtag_scan(cmd->cmd.scan, !cmd->next);
1090                         break;
1091                 default:
1092                         LOG_ERROR("Unknown JTAG command type 0x%x encountered", cmd->type);
1093                         rc = ERROR_FAIL;
1094                 }
1095         }
1096
1097         return rc;
1098 }
1099
1100 static int vdebug_dap_connect(struct adiv5_dap *dap)
1101 {
1102         return dap_dp_init(dap);
1103 }
1104
1105 static int vdebug_dap_send_sequence(struct adiv5_dap *dap, enum swd_special_seq seq)
1106 {
1107         return ERROR_OK;
1108 }
1109
1110 static int vdebug_dap_queue_dp_read(struct adiv5_dap *dap, unsigned int reg, uint32_t *data)
1111 {
1112         return vdebug_reg_read(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, data, VD_ASPACE_DP, 0);
1113 }
1114
1115 static int vdebug_dap_queue_dp_write(struct adiv5_dap *dap, unsigned int reg, uint32_t data)
1116 {
1117         return vdebug_reg_write(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, data, VD_ASPACE_DP, 0);
1118 }
1119
1120 static int vdebug_dap_queue_ap_read(struct adiv5_ap *ap, unsigned int reg, uint32_t *data)
1121 {
1122         if ((reg & DP_SELECT_APBANK) != ap->dap->select) {
1123                 vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, reg & DP_SELECT_APBANK, VD_ASPACE_DP, 0);
1124                 ap->dap->select = reg & DP_SELECT_APBANK;
1125         }
1126
1127         vdebug_reg_read(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, NULL, VD_ASPACE_AP, 0);
1128
1129         return vdebug_reg_read(vdc.hsocket, pbuf, DP_RDBUFF >> 2, data, VD_ASPACE_DP, 0);
1130 }
1131
1132 static int vdebug_dap_queue_ap_write(struct adiv5_ap *ap, unsigned int reg, uint32_t data)
1133 {
1134         if ((reg & DP_SELECT_APBANK) != ap->dap->select) {
1135                 vdebug_reg_write(vdc.hsocket, pbuf, DP_SELECT >> 2, reg & DP_SELECT_APBANK, VD_ASPACE_DP, 0);
1136                 ap->dap->select = reg & DP_SELECT_APBANK;
1137         }
1138
1139         return vdebug_reg_write(vdc.hsocket, pbuf, (reg & DP_SELECT_DPBANK) >> 2, data, VD_ASPACE_AP, 0);
1140 }
1141
1142 static int vdebug_dap_queue_ap_abort(struct adiv5_dap *dap, uint8_t *ack)
1143 {
1144         return vdebug_reg_write(vdc.hsocket, pbuf, 0, 0x1, VD_ASPACE_AB, 0);
1145 }
1146
1147 static int vdebug_dap_run(struct adiv5_dap *dap)
1148 {
1149         if (pbuf->waddr)
1150                 return vdebug_run_reg_queue(vdc.hsocket, pbuf, le_to_h_u16(pbuf->waddr));
1151
1152         return ERROR_OK;
1153 }
1154
1155 COMMAND_HANDLER(vdebug_set_server)
1156 {
1157         if ((CMD_ARGC != 1) || !strchr(CMD_ARGV[0], ':'))
1158                 return ERROR_COMMAND_SYNTAX_ERROR;
1159
1160         char *pchar = strchr(CMD_ARGV[0], ':');
1161         *pchar = '\0';
1162         strncpy(vdc.server_name, CMD_ARGV[0], sizeof(vdc.server_name) - 1);
1163         int port = atoi(++pchar);
1164         if (port < 0 || port > UINT16_MAX) {
1165                 LOG_ERROR("invalid port number %d specified", port);
1166                 return ERROR_COMMAND_SYNTAX_ERROR;
1167         }
1168         vdc.server_port = port;
1169         LOG_DEBUG("server: %s port %u", vdc.server_name, vdc.server_port);
1170
1171         return ERROR_OK;
1172 }
1173
1174 COMMAND_HANDLER(vdebug_set_bfm)
1175 {
1176         char prefix;
1177
1178         if ((CMD_ARGC != 2) || (sscanf(CMD_ARGV[1], "%u%c", &vdc.bfm_period, &prefix) != 2))
1179                 return ERROR_COMMAND_SYNTAX_ERROR;
1180
1181         strncpy(vdc.bfm_path, CMD_ARGV[0], sizeof(vdc.bfm_path) - 1);
1182         switch (prefix) {
1183         case 'u':
1184                 vdc.bfm_period *= 1000000;
1185                 break;
1186         case 'n':
1187                 vdc.bfm_period *= 1000;
1188                 break;
1189         case 'p':
1190         default:
1191                 break;
1192         }
1193         if (transport_is_dapdirect_swd())
1194                 vdc.bfm_type = VD_BFM_SWDP;
1195         else
1196                 vdc.bfm_type = VD_BFM_JTAG;
1197         LOG_DEBUG("bfm_path: %s clk_period %ups", vdc.bfm_path, vdc.bfm_period);
1198
1199         return ERROR_OK;
1200 }
1201
1202 COMMAND_HANDLER(vdebug_set_mem)
1203 {
1204         if (CMD_ARGC != 3)
1205                 return ERROR_COMMAND_SYNTAX_ERROR;
1206
1207         if (vdc.mem_ndx >= VD_MAX_MEMORIES) {
1208                 LOG_ERROR("mem_path declared more than %d allowed times", VD_MAX_MEMORIES);
1209                 return ERROR_FAIL;
1210         }
1211
1212         strncpy(vdc.mem_path[vdc.mem_ndx], CMD_ARGV[0], sizeof(vdc.mem_path[vdc.mem_ndx]) - 1);
1213         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], vdc.mem_base[vdc.mem_ndx]);
1214         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], vdc.mem_size[vdc.mem_ndx]);
1215         LOG_DEBUG("mem_path: set %s @ 0x%08x+0x%08x", vdc.mem_path[vdc.mem_ndx],
1216                 vdc.mem_base[vdc.mem_ndx], vdc.mem_size[vdc.mem_ndx]);
1217         vdc.mem_ndx++;
1218
1219         return ERROR_OK;
1220 }
1221
1222 COMMAND_HANDLER(vdebug_set_batching)
1223 {
1224         if (CMD_ARGC != 1)
1225                 return ERROR_COMMAND_SYNTAX_ERROR;
1226
1227         if (isdigit((unsigned char)CMD_ARGV[0][0]))
1228                 vdc.trans_batch = (CMD_ARGV[0][0] == '0' ? 0 : (CMD_ARGV[0][0] == '1' ? 1 : 2));
1229         else if (CMD_ARGV[0][0] == 'r')
1230                 vdc.trans_batch = VD_BATCH_WR;
1231         else if (CMD_ARGV[0][0] == 'w')
1232                 vdc.trans_batch = VD_BATCH_WO;
1233         else
1234                 vdc.trans_batch = VD_BATCH_NO;
1235         LOG_DEBUG("batching: set to %u", vdc.trans_batch);
1236
1237         return ERROR_OK;
1238 }
1239
1240 COMMAND_HANDLER(vdebug_set_polling)
1241 {
1242         if (CMD_ARGC != 2)
1243                 return ERROR_COMMAND_SYNTAX_ERROR;
1244
1245         vdc.poll_min = atoi(CMD_ARGV[0]);
1246         vdc.poll_max = atoi(CMD_ARGV[1]);
1247         LOG_DEBUG("polling: set min %u max %u", vdc.poll_min, vdc.poll_max);
1248
1249         return ERROR_OK;
1250 }
1251
1252 static const struct command_registration vdebug_command_handlers[] = {
1253         {
1254                 .name = "server",
1255                 .handler = &vdebug_set_server,
1256                 .mode = COMMAND_CONFIG,
1257                 .help = "set the vdebug server name or address",
1258                 .usage = "<host:port>",
1259         },
1260         {
1261                 .name = "bfm_path",
1262                 .handler = &vdebug_set_bfm,
1263                 .mode = COMMAND_CONFIG,
1264                 .help = "set the vdebug BFM hierarchical path",
1265                 .usage = "<path> <clk_period[p|n|u]s>",
1266         },
1267         {
1268                 .name = "mem_path",
1269                 .handler = &vdebug_set_mem,
1270                 .mode = COMMAND_ANY,
1271                 .help = "set the design memory for the code load",
1272                 .usage = "<path> <base_address> <size>",
1273         },
1274         {
1275                 .name = "batching",
1276                 .handler = &vdebug_set_batching,
1277                 .mode = COMMAND_CONFIG,
1278                 .help = "set the transaction batching no|wr|rd [0|1|2]",
1279                 .usage = "<level>",
1280         },
1281         {
1282                 .name = "polling",
1283                 .handler = &vdebug_set_polling,
1284                 .mode = COMMAND_CONFIG,
1285                 .help = "set the polling pause, executing hardware cycles between min and max",
1286                 .usage = "<min cycles> <max cycles>",
1287         },
1288         COMMAND_REGISTRATION_DONE
1289 };
1290
1291 static const struct command_registration vdebug_command[] = {
1292         {
1293                 .name = "vdebug",
1294                 .chain = vdebug_command_handlers,
1295                 .mode = COMMAND_ANY,
1296                 .help = "vdebug command group",
1297                 .usage = "",
1298         },
1299         COMMAND_REGISTRATION_DONE
1300 };
1301
1302 static struct jtag_interface vdebug_jtag_ops = {
1303         .supported = DEBUG_CAP_TMS_SEQ,
1304         .execute_queue = vdebug_jtag_execute_queue,
1305 };
1306
1307 static const struct dap_ops vdebug_dap_ops = {
1308         .connect = vdebug_dap_connect,
1309         .send_sequence = vdebug_dap_send_sequence,
1310         .queue_dp_read = vdebug_dap_queue_dp_read,
1311         .queue_dp_write = vdebug_dap_queue_dp_write,
1312         .queue_ap_read = vdebug_dap_queue_ap_read,
1313         .queue_ap_write = vdebug_dap_queue_ap_write,
1314         .queue_ap_abort = vdebug_dap_queue_ap_abort,
1315         .run = vdebug_dap_run,
1316         .sync = NULL, /* optional */
1317         .quit = NULL, /* optional */
1318 };
1319
1320 static const char *const vdebug_transports[] = { "jtag", "dapdirect_swd", NULL };
1321
1322 struct adapter_driver vdebug_adapter_driver = {
1323         .name = "vdebug",
1324         .transports = vdebug_transports,
1325         .speed = vdebug_jtag_speed,
1326         .khz = vdebug_jtag_khz,
1327         .speed_div = vdebug_jtag_div,
1328         .commands = vdebug_command,
1329         .init = vdebug_init,
1330         .quit = vdebug_quit,
1331         .reset = vdebug_reset,
1332         .jtag_ops = &vdebug_jtag_ops,
1333         .dap_swd_ops = &vdebug_dap_ops,
1334 };