src/drivers/usb/ehci_debug.c

   1 /* SPDX-License-Identifier: GPL-2.0-only */
   2
   3 #include <stdint.h>
   4 #include <console/console.h>
   5 #include <console/usb.h>
   6 #include <arch/io.h>
   7 #include <device/mmio.h>
   8 #include <arch/symbols.h>
   9 #include <string.h>
  10 #include <cbmem.h>
  11
  12 #include "ehci_debug.h"
  13 #include "usb_ch9.h"
  14 #include "ehci.h"
  15
  16 struct ehci_debug_info {
  17         u64 ehci_base;
  18         u64 ehci_debug;
  19
  20         struct dbgp_pipe ep_pipe[DBGP_MAX_ENDPOINTS];
  21 } __packed;
  22
  23 /* With CONFIG(DEBUG_CONSOLE_INIT), you can debug the connection of
  24  * usbdebug dongle. EHCI port register bits and USB packets are dumped
  25  * on console, assuming some other console already works.
  26  */
  27 #define dprintk(LEVEL, args...)                                         \
  28         do {                                                            \
  29                 if (CONFIG(DEBUG_CONSOLE_INIT) && !dbgp_enabled())      \
  30                         printk(LEVEL, ##args);                          \
  31         } while (0)
  32
  33 #define DBGP_LEN_UPDATE(x, len) (((x) & ~0x0f) | ((len) & 0x0f))
  34
  35 #define DBGP_CLAIM (DBGP_OWNER | DBGP_ENABLED | DBGP_INUSE)
  36
  37 #define HUB_ROOT_RESET_TIME     50      /* times are in msec */
  38 #define HUB_SHORT_RESET_TIME    10
  39 #define HUB_LONG_RESET_TIME     200
  40 #define HUB_RESET_TIMEOUT       500
  41
  42 #define DBGP_MICROFRAME_TIMEOUT_LOOPS   1000
  43 #define DBGP_MICROFRAME_RETRIES         10
  44 #define DBGP_MAX_PACKET         8
  45
  46 static int dbgp_enabled(void);
  47 static void dbgp_print_data(struct ehci_dbg_port *ehci_debug);
  48
  49 static struct ehci_debug_info glob_dbg_info;
  50 static struct ehci_debug_info *glob_dbg_info_p;
  51
  52 static inline struct ehci_debug_info *dbgp_ehci_info(void)
  53 {
  54         if (glob_dbg_info_p == NULL) {
  55                 struct ehci_debug_info *info;
  56                 if (ENV_BOOTBLOCK || ENV_SEPARATE_VERSTAGE || ENV_SEPARATE_ROMSTAGE) {
  57                         /* The message likely does not show if we hit this. */
  58                         if (sizeof(*info) > _car_ehci_dbg_info_size)
  59                                 die("BUG: Increase ehci_dbg_info reserve in CAR");
  60                         info = (void *)_car_ehci_dbg_info;
  61                 } else {
  62                         info = &glob_dbg_info;
  63                 }
  64                 glob_dbg_info_p = info;
  65         }
  66         return glob_dbg_info_p;
  67 }
  68
  69 static int dbgp_wait_until_complete(struct ehci_dbg_port *ehci_debug)
  70 {
  71         u32 ctrl;
  72         int loop = 0;
  73
  74         do {
  75                 ctrl = read32(&ehci_debug->control);
  76                 /* Stop when the transaction is finished */
  77                 if (ctrl & DBGP_DONE)
  78                         break;
  79         } while (++loop < DBGP_MICROFRAME_TIMEOUT_LOOPS);
  80
  81         if (! (ctrl & DBGP_DONE)) {
  82                 dprintk(BIOS_ERR, "%s: retry timeout.\n", __func__);
  83                 return -DBGP_ERR_SIGNAL;
  84         }
  85
  86         /* Now that we have observed the completed transaction,
  87          * clear the done bit.
  88          */
  89         write32(&ehci_debug->control, ctrl | DBGP_DONE);
  90         return (ctrl & DBGP_ERROR) ? -DBGP_ERRCODE(ctrl) : DBGP_LEN(ctrl);
  91 }
  92
  93 static void dbgp_breath(void)
  94 {
  95         /* Sleep to give the debug port a chance to breathe */
  96 }
  97
  98 static int dbgp_wait_until_done(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe,
  99         unsigned int ctrl, const int timeout)
 100 {
 101         u32 rd_ctrl, rd_pids;
 102         u32 ctrl_prev = 0, pids_prev = 0;
 103         u8 lpid;
 104         int ret, host_retries;
 105         int loop;
 106
 107         loop = 0;
 108 device_retry:
 109         host_retries = 0;
 110         if (loop++ >= timeout)
 111                 return -DBGP_ERR_BAD;
 112
 113 host_retry:
 114         if (host_retries++ >= DBGP_MICROFRAME_RETRIES)
 115                 return -DBGP_ERR_BAD;
 116         if (loop == 1 || host_retries > 1)
 117                 dprintk(BIOS_SPEW, "dbgp:  start (@ %3d,%d) ctrl=%08x\n",
 118                         loop, host_retries, ctrl | DBGP_GO);
 119         write32(&ehci_debug->control, ctrl | DBGP_GO);
 120         ret = dbgp_wait_until_complete(ehci_debug);
 121         rd_ctrl = read32(&ehci_debug->control);
 122         rd_pids = read32(&ehci_debug->pids);
 123
 124         if (rd_ctrl != ctrl_prev || rd_pids != pids_prev || (ret<0)) {
 125                 ctrl_prev = rd_ctrl;
 126                 pids_prev = rd_pids;
 127                 dprintk(BIOS_SPEW, "dbgp: status (@ %3d,%d) ctrl=%08x pids=%08x ret=%d\n",
 128                         loop, host_retries, rd_ctrl, rd_pids, ret);
 129         }
 130
 131         /* Controller hardware failure. */
 132         if (ret == -DBGP_ERR_SIGNAL) {
 133                 return ret;
 134
 135         /* Bus failure (corrupted microframe). */
 136         } else if (ret == -DBGP_ERR_BAD) {
 137                 goto host_retry;
 138         }
 139
 140         lpid = DBGP_PID_GET(rd_pids);
 141
 142         /* If I get an ACK or in-sync DATA PID, we are done. */
 143         if ((lpid == USB_PID_ACK) || (lpid == pipe->pid)) {
 144                 pipe->pid ^= USB_PID_DATA_TOGGLE;
 145         }
 146
 147         /* If the port is getting full or it has dropped data
 148          * start pacing ourselves, not necessary but it's friendly.
 149          */
 150         else if (lpid == USB_PID_NYET) {
 151                 dbgp_breath();
 152                 goto device_retry;
 153         }
 154
 155         /* If I get a NACK or out-of-sync DATA PID, reissue the transmission. */
 156         else if ((lpid == USB_PID_NAK) || (lpid == (pipe->pid ^ USB_PID_DATA_TOGGLE))) {
 157                 goto device_retry;
 158         }
 159
 160         /* Abort on STALL handshake for endpoint 0.*/
 161         else if ((lpid == USB_PID_STALL) && (pipe->endpoint == 0x0)) {
 162                 ret = -DBGP_ERR_BAD;
 163         }
 164
 165         dbgp_print_data(ehci_debug);
 166
 167         return ret;
 168 }
 169
 170 static void dbgp_set_data(struct ehci_dbg_port *ehci_debug, const void *buf, int size)
 171 {
 172         const unsigned char *bytes = buf;
 173         u32 lo, hi;
 174         int i;
 175
 176         lo = hi = 0;
 177         for (i = 0; i < 4 && i < size; i++)
 178                 lo |= bytes[i] << (8*i);
 179         for (; i < 8 && i < size; i++)
 180                 hi |= bytes[i] << (8*(i - 4));
 181         write32(&ehci_debug->data03, lo);
 182         write32(&ehci_debug->data47, hi);
 183 }
 184
 185 static void dbgp_get_data(struct ehci_dbg_port *ehci_debug, void *buf, int size)
 186 {
 187         unsigned char *bytes = buf;
 188         u32 lo, hi;
 189         int i;
 190
 191         lo = read32(&ehci_debug->data03);
 192         hi = read32(&ehci_debug->data47);
 193         for (i = 0; i < 4 && i < size; i++)
 194                 bytes[i] = (lo >> (8*i)) & 0xff;
 195         for (; i < 8 && i < size; i++)
 196                 bytes[i] = (hi >> (8*(i - 4))) & 0xff;
 197 }
 198
 199 static void dbgp_print_data(struct ehci_dbg_port *ehci_debug)
 200 {
 201         int len;
 202         u32 ctrl, lo, hi;
 203
 204         if (!CONFIG(DEBUG_CONSOLE_INIT) || dbgp_enabled())
 205                 return;
 206
 207         ctrl = read32(&ehci_debug->control);
 208         lo = read32(&ehci_debug->data03);
 209         hi = read32(&ehci_debug->data47);
 210
 211         len = DBGP_LEN(ctrl);
 212         if (len) {
 213                 int i;
 214                 dprintk(BIOS_SPEW, "dbgp:    buf:");
 215                 for (i = 0; i < 4 && i < len; i++)
 216                         dprintk(BIOS_SPEW, " %02x", (lo >> (8*i)) & 0xff);
 217                 for (; i < 8 && i < len; i++)
 218                         dprintk(BIOS_SPEW, " %02x", (hi >> (8*(i - 4))) & 0xff);
 219                 dprintk(BIOS_SPEW, "\n");
 220         }
 221 }
 222
 223 static int dbgp_bulk_write(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe,
 224         const char *bytes, int size)
 225 {
 226         u32 pids, addr, ctrl;
 227         int ret;
 228
 229         if (size > DBGP_MAX_PACKET)
 230                 return -1;
 231
 232         addr = DBGP_EPADDR(pipe->devnum, pipe->endpoint);
 233         pids = DBGP_PID_SET(pipe->pid, USB_PID_OUT);
 234
 235         ctrl = read32(&ehci_debug->control);
 236         ctrl = DBGP_LEN_UPDATE(ctrl, size);
 237         ctrl |= DBGP_OUT;
 238
 239         dbgp_set_data(ehci_debug, bytes, size);
 240         write32(&ehci_debug->address, addr);
 241         write32(&ehci_debug->pids, pids);
 242
 243         ret = dbgp_wait_until_done(ehci_debug, pipe, ctrl, pipe->timeout);
 244
 245         return ret;
 246 }
 247
 248 int dbgp_bulk_write_x(struct dbgp_pipe *pipe, const char *bytes, int size)
 249 {
 250         struct ehci_debug_info *dbg_info = dbgp_ehci_info();
 251         struct ehci_dbg_port *port;
 252         port = (void *)(uintptr_t)dbg_info->ehci_debug;
 253         return dbgp_bulk_write(port, pipe, bytes, size);
 254 }
 255
 256 static int dbgp_bulk_read(struct ehci_dbg_port *ehci_debug, struct dbgp_pipe *pipe,
 257         void *data, int size)
 258 {
 259         u32 pids, addr, ctrl;
 260         int ret;
 261
 262         if (size > DBGP_MAX_PACKET)
 263                 return -1;
 264
 265         addr = DBGP_EPADDR(pipe->devnum, pipe->endpoint);
 266         pids = DBGP_PID_SET(pipe->pid, USB_PID_IN);
 267
 268         ctrl = read32(&ehci_debug->control);
 269         ctrl = DBGP_LEN_UPDATE(ctrl, size);
 270         ctrl &= ~DBGP_OUT;
 271
 272         write32(&ehci_debug->address, addr);
 273         write32(&ehci_debug->pids, pids);
 274         ret = dbgp_wait_until_done(ehci_debug, pipe, ctrl, pipe->timeout);
 275         if (ret < 0)
 276                 return ret;
 277
 278         if (size > ret)
 279                 size = ret;
 280         dbgp_get_data(ehci_debug, data, size);
 281         return ret;
 282 }
 283
 284 int dbgp_bulk_read_x(struct dbgp_pipe *pipe, void *data, int size)
 285 {
 286         struct ehci_debug_info *dbg_info = dbgp_ehci_info();
 287         struct ehci_dbg_port *port;
 288         port = (void *)(uintptr_t)dbg_info->ehci_debug;
 289         return dbgp_bulk_read(port, pipe, data, size);
 290 }
 291
 292 void dbgp_mdelay(int ms)
 293 {
 294         int i;
 295
 296         while (ms--) {
 297                 for (i = 0; i < 1000; i++)
 298                         inb(0x80);
 299         }
 300 }
 301
 302 int dbgp_control_msg(struct ehci_dbg_port *ehci_debug, unsigned int devnum, int requesttype,
 303                 int request, int value, int index, void *data, int size)
 304 {
 305         struct ehci_debug_info *info = dbgp_ehci_info();
 306         struct dbgp_pipe *pipe = &info->ep_pipe[DBGP_SETUP_EP0];
 307         u32 pids, addr, ctrl;
 308         struct usb_ctrlrequest req;
 309         int read;
 310         int ret, ret2;
 311
 312         read = (requesttype & USB_DIR_IN) != 0;
 313         if (size > DBGP_MAX_PACKET)
 314                 return -1;
 315
 316         /* Compute the control message */
 317         req.bRequestType = requesttype;
 318         req.bRequest = request;
 319         req.wValue = cpu_to_le16(value);
 320         req.wIndex = cpu_to_le16(index);
 321         req.wLength = cpu_to_le16(size);
 322
 323         pipe->devnum = devnum;
 324         pipe->endpoint = 0;
 325         pipe->pid = USB_PID_DATA0;
 326         pipe->timeout = 1000;
 327         addr = DBGP_EPADDR(pipe->devnum, pipe->endpoint);
 328         pids = DBGP_PID_SET(pipe->pid, USB_PID_SETUP);
 329
 330         ctrl = read32(&ehci_debug->control);
 331         ctrl = DBGP_LEN_UPDATE(ctrl, sizeof(req));
 332         ctrl |= DBGP_OUT;
 333
 334         /* Setup stage */
 335         dbgp_set_data(ehci_debug, &req, sizeof(req));
 336         write32(&ehci_debug->address, addr);
 337         write32(&ehci_debug->pids, pids);
 338         ret = dbgp_wait_until_done(ehci_debug, pipe, ctrl, 1);
 339         if (ret < 0)
 340                 return ret;
 341
 342         /* Data stage (optional) */
 343         if (read && size)
 344                 ret = dbgp_bulk_read(ehci_debug, pipe, data, size);
 345         else if (!read && size)
 346                 ret = dbgp_bulk_write(ehci_debug, pipe, data, size);
 347
 348         /* Status stage in opposite direction */
 349         pipe->pid = USB_PID_DATA1;
 350         ctrl = read32(&ehci_debug->control);
 351         ctrl = DBGP_LEN_UPDATE(ctrl, 0);
 352         if (read) {
 353                 pids = DBGP_PID_SET(pipe->pid, USB_PID_OUT);
 354                 ctrl |= DBGP_OUT;
 355         } else {
 356                 pids = DBGP_PID_SET(pipe->pid, USB_PID_IN);
 357                 ctrl &= ~DBGP_OUT;
 358         }
 359
 360         write32(&ehci_debug->pids, pids);
 361         ret2 = dbgp_wait_until_done(ehci_debug, pipe, ctrl, pipe->timeout);
 362         if (ret2 < 0)
 363                 return ret2;
 364
 365         return ret;
 366 }
 367
 368 static int ehci_reset_port(struct ehci_regs *ehci_regs, int port)
 369 {
 370         u32 portsc;
 371         int loop;
 372
 373         /* Reset the USB debug port */
 374         portsc = read32(&ehci_regs->port_status[port - 1]);
 375         portsc &= ~PORT_PE;
 376         portsc |= PORT_RESET;
 377         write32(&ehci_regs->port_status[port - 1], portsc);
 378
 379         dbgp_mdelay(HUB_ROOT_RESET_TIME);
 380
 381         portsc = read32(&ehci_regs->port_status[port - 1]);
 382         write32(&ehci_regs->port_status[port - 1],
 383                         portsc & ~(PORT_RWC_BITS | PORT_RESET));
 384
 385         loop = 100;
 386         do {
 387                 dbgp_mdelay(1);
 388                 portsc = read32(&ehci_regs->port_status[port - 1]);
 389         } while ((portsc & PORT_RESET) && (--loop > 0));
 390
 391         /* Device went away? */
 392         if (!(portsc & PORT_CONNECT))
 393                 return -1; //-ENOTCONN;
 394
 395         /* bomb out completely if something weird happened */
 396         if ((portsc & PORT_CSC))
 397                 return -2; //-EINVAL;
 398
 399         /* If we've finished resetting, then break out of the loop */
 400         if (!(portsc & PORT_RESET) && (portsc & PORT_PE))
 401                 return 0;
 402
 403         return -3; //-EBUSY;
 404 }
 405
 406 static int ehci_wait_for_port(struct ehci_regs *ehci_regs, int port)
 407 {
 408         u32 status;
 409         int ret, reps;
 410
 411         for (reps = 0; reps < 3; reps++) {
 412                 dbgp_mdelay(100);
 413                 status = read32(&ehci_regs->status);
 414                 if (status & STS_PCD) {
 415                         ret = ehci_reset_port(ehci_regs, port);
 416                         if (ret == 0)
 417                                 return 0;
 418                 }
 419         }
 420         return -1; //-ENOTCONN;
 421 }
 422
 423 static int usbdebug_init_(uintptr_t ehci_bar, unsigned int offset, struct ehci_debug_info *info)
 424 {
 425         struct ehci_caps *ehci_caps;
 426         struct ehci_regs *ehci_regs;
 427
 428         u32 cmd, ctrl, status, portsc, hcs_params;
 429         u32 debug_port, new_debug_port = 0, n_ports;
 430         int ret, i;
 431         int loop;
 432         int port_map_tried;
 433         int playtimes = 3;
 434
 435         /* Keep all endpoints disabled before any printk() call. */
 436         memset(info, 0, sizeof(*info));
 437         info->ehci_base = ehci_bar;
 438         info->ehci_debug = ehci_bar + offset;
 439         info->ep_pipe[0].status |= DBGP_EP_NOT_PRESENT;
 440
 441         dprintk(BIOS_INFO, "ehci_bar: 0x%lx debug_offset 0x%x\n", ehci_bar, offset);
 442
 443         ehci_caps  = (struct ehci_caps *)ehci_bar;
 444         ehci_regs  = (struct ehci_regs *)(ehci_bar +
 445                         HC_LENGTH(read32(&ehci_caps->hc_capbase)));
 446
 447         struct ehci_dbg_port *ehci_debug = (void *)(uintptr_t)info->ehci_debug;
 448
 449         if (CONFIG_USBDEBUG_DEFAULT_PORT > 0)
 450                 ehci_debug_select_port(CONFIG_USBDEBUG_DEFAULT_PORT);
 451         else
 452                 ehci_debug_select_port(1);
 453
 454 try_next_time:
 455         port_map_tried = 0;
 456
 457 try_next_port:
 458         hcs_params = read32(&ehci_caps->hcs_params);
 459         debug_port = HCS_DEBUG_PORT(hcs_params);
 460         n_ports    = HCS_N_PORTS(hcs_params);
 461
 462         dprintk(BIOS_INFO, "debug_port: %d\n", debug_port);
 463         dprintk(BIOS_INFO, "n_ports:    %d\n", n_ports);
 464
 465         for (i = 1; i <= n_ports; i++) {
 466                 portsc = read32(&ehci_regs->port_status[i-1]);
 467                 dprintk(BIOS_INFO, "PORTSC #%d: %08x\n", i, portsc);
 468         }
 469
 470         if (port_map_tried && (new_debug_port != debug_port)) {
 471                 if (--playtimes) {
 472                         ehci_debug_select_port(debug_port);
 473                         goto try_next_time;
 474                 }
 475                 return -1;
 476         }
 477
 478         /* Wait until the controller is halted */
 479         status = read32(&ehci_regs->status);
 480         if (!(status & STS_HALT)) {
 481                 cmd = read32(&ehci_regs->command);
 482                 cmd &= ~CMD_RUN;
 483                 write32(&ehci_regs->command, cmd);
 484                 loop = 100;
 485                 do {
 486                         dbgp_mdelay(10);
 487                         status = read32(&ehci_regs->status);
 488                 } while (!(status & STS_HALT) && (--loop > 0));
 489                 if (status & STS_HALT)
 490                         dprintk(BIOS_INFO, "EHCI controller halted successfully.\n");
 491                 else
 492                         dprintk(BIOS_INFO, "EHCI controller is not halted. Reset may fail.\n");
 493         }
 494
 495         loop = 100;
 496         /* Reset the EHCI controller */
 497         cmd = read32(&ehci_regs->command);
 498         cmd |= CMD_RESET;
 499         write32(&ehci_regs->command, cmd);
 500         do {
 501                 dbgp_mdelay(10);
 502                 cmd = read32(&ehci_regs->command);
 503         } while ((cmd & CMD_RESET) && (--loop > 0));
 504
 505         if (!loop) {
 506                 dprintk(BIOS_INFO, "Could not reset EHCI controller.\n");
 507                 // on some systems it works without succeeding here.
 508                 // return -2;
 509         } else {
 510                 dprintk(BIOS_INFO, "EHCI controller reset successfully.\n");
 511         }
 512
 513         /* Claim ownership, but do not enable yet */
 514         ctrl = read32(&ehci_debug->control);
 515         ctrl |= DBGP_OWNER;
 516         ctrl &= ~(DBGP_ENABLED | DBGP_INUSE);
 517         write32(&ehci_debug->control, ctrl);
 518
 519         /* Start EHCI controller */
 520         cmd = read32(&ehci_regs->command);
 521         cmd &= ~(CMD_LRESET | CMD_IAAD | CMD_PSE | CMD_ASE | CMD_RESET);
 522         cmd |= CMD_RUN;
 523         write32(&ehci_regs->command, cmd);
 524
 525         /* Ensure everything is routed to the EHCI */
 526         write32(&ehci_regs->configured_flag, FLAG_CF);
 527
 528         /* Wait until the controller is no longer halted */
 529         loop = 10;
 530         do {
 531                 dbgp_mdelay(10);
 532                 status = read32(&ehci_regs->status);
 533         } while ((status & STS_HALT) && (--loop > 0));
 534
 535         if (!loop) {
 536                 dprintk(BIOS_INFO, "EHCI could not be started.\n");
 537                 return -3;
 538         }
 539         dprintk(BIOS_INFO, "EHCI started.\n");
 540
 541         /* Wait for a device to show up in the debug port */
 542         ret = ehci_wait_for_port(ehci_regs, debug_port);
 543         if (ret < 0) {
 544                 dprintk(BIOS_INFO, "No device found in debug port %d\n", debug_port);
 545                 goto next_debug_port;
 546         }
 547         dprintk(BIOS_INFO, "EHCI done waiting for port.\n");
 548
 549         /* Enable the debug port */
 550         ctrl = read32(&ehci_debug->control);
 551         ctrl |= DBGP_CLAIM;
 552         write32(&ehci_debug->control, ctrl);
 553         ctrl = read32(&ehci_debug->control);
 554         if ((ctrl & DBGP_CLAIM) != DBGP_CLAIM) {
 555                 dprintk(BIOS_INFO, "No device in EHCI debug port.\n");
 556                 write32(&ehci_debug->control, ctrl & ~DBGP_CLAIM);
 557                 ret = -4;
 558                 goto err;
 559         }
 560         dprintk(BIOS_INFO, "EHCI debug port enabled.\n");
 561
 562         dbgp_mdelay(100);
 563
 564         struct ehci_dbg_port *port = (void *)(uintptr_t)info->ehci_debug;
 565         ret = dbgp_probe_gadget(port, &info->ep_pipe[0]);
 566         if (ret < 0) {
 567                 dprintk(BIOS_INFO, "Could not probe gadget on debug port.\n");
 568                 ret = -6;
 569                 goto err;
 570         }
 571
 572         info->ep_pipe[0].status &= ~DBGP_EP_NOT_PRESENT;
 573
 574         return 0;
 575 err:
 576         /* Things didn't work so remove my claim */
 577         ctrl = read32(&ehci_debug->control);
 578         ctrl &= ~(DBGP_CLAIM | DBGP_OUT);
 579         write32(&ehci_debug->control, ctrl);
 580         //return ret;
 581
 582 next_debug_port:
 583         if (CONFIG_USBDEBUG_DEFAULT_PORT == 0) {
 584                 port_map_tried |= (1 << (debug_port - 1));
 585                 new_debug_port = ((debug_port-1 + 1) % n_ports) + 1;
 586                 if (port_map_tried != ((1 << n_ports) - 1)) {
 587                         ehci_debug_select_port(new_debug_port);
 588                         goto try_next_port;
 589                 }
 590                 if (--playtimes) {
 591                         ehci_debug_select_port(new_debug_port);
 592                         goto try_next_time;
 593                 }
 594         } else {
 595                 if (--playtimes)
 596                         goto try_next_time;
 597         }
 598
 599         return ret;
 600 }
 601
 602 static int dbgp_enabled(void)
 603 {
 604         struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
 605         return (globals->status & DBGP_EP_ENABLED);
 606 }
 607
 608 static int dbgp_not_present(void)
 609 {
 610         struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
 611         return (globals->status & DBGP_EP_NOT_PRESENT);
 612 }
 613
 614 int dbgp_try_get(struct dbgp_pipe *pipe)
 615 {
 616         struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
 617         if (!dbgp_ep_is_active(pipe) || (globals->status & DBGP_EP_BUSY))
 618                 return 0;
 619         globals->status |= DBGP_EP_BUSY;
 620         pipe->status |= DBGP_EP_BUSY;
 621         return 1;
 622 }
 623
 624 void dbgp_put(struct dbgp_pipe *pipe)
 625 {
 626         struct dbgp_pipe *globals = &dbgp_ehci_info()->ep_pipe[DBGP_SETUP_EP0];
 627         globals->status &= ~DBGP_EP_BUSY;
 628         pipe->status &= ~DBGP_EP_BUSY;
 629 }
 630
 631 #if ENV_RAMSTAGE
 632 void usbdebug_re_enable(uintptr_t ehci_base)
 633 {
 634         struct ehci_debug_info *dbg_info = dbgp_ehci_info();
 635         u64 diff;
 636         int i;
 637
 638         diff = dbg_info->ehci_base - ehci_base;
 639         dbg_info->ehci_debug -= diff;
 640         dbg_info->ehci_base = ehci_base;
 641
 642         for (i=0; i<DBGP_MAX_ENDPOINTS; i++)
 643                 if (dbg_info->ep_pipe[i].status & DBGP_EP_VALID)
 644                         dbg_info->ep_pipe[i].status |= DBGP_EP_ENABLED;
 645 }
 646
 647 void usbdebug_disable(void)
 648 {
 649         struct ehci_debug_info *dbg_info = dbgp_ehci_info();
 650         int i;
 651         for (i=0; i<DBGP_MAX_ENDPOINTS; i++)
 652                 dbg_info->ep_pipe[i].status &= ~DBGP_EP_ENABLED;
 653 }
 654
 655 #endif
 656
 657 int usbdebug_hw_init(bool force)
 658 {
 659         struct ehci_debug_info *dbg_info = dbgp_ehci_info();
 660         u32 ehci_base, dbg_offset;
 661
 662         if (dbgp_enabled() && !force)
 663                 return 0;
 664
 665         if (dbgp_not_present() && !force)
 666                 return -1;
 667
 668         /* Do not attempt slow gadget init in postcar. */
 669         if (ENV_POSTCAR)
 670                 return -1;
 671
 672         /* Do full init if state claims we are still not enabled. */
 673         if (ehci_debug_hw_enable(&ehci_base, &dbg_offset))
 674                 return -1;
 675         return usbdebug_init_(ehci_base, dbg_offset, dbg_info);
 676 }
 677
 678 static void migrate_ehci_debug(int is_recovery)
 679 {
 680         struct ehci_debug_info *dbg_info_cbmem;
 681         int rv;
 682
 683         if (ENV_CREATES_CBMEM) {
 684                 /* Move state from CAR to CBMEM. */
 685                 struct ehci_debug_info *dbg_info = dbgp_ehci_info();
 686                 dbg_info_cbmem = cbmem_add(CBMEM_ID_EHCI_DEBUG,
 687                                         sizeof(*dbg_info));
 688                 if (dbg_info_cbmem == NULL)
 689                         return;
 690                 memcpy(dbg_info_cbmem, dbg_info, sizeof(*dbg_info));
 691                 glob_dbg_info_p = dbg_info_cbmem;
 692                 return;
 693         }
 694
 695         if (CONFIG(USBDEBUG_IN_PRE_RAM)) {
 696                 /* Use state in CBMEM. */
 697                 dbg_info_cbmem = cbmem_find(CBMEM_ID_EHCI_DEBUG);
 698                 if (dbg_info_cbmem)
 699                         glob_dbg_info_p = dbg_info_cbmem;
 700         }
 701
 702         rv = usbdebug_hw_init(false);
 703         if (rv < 0)
 704                 printk(BIOS_DEBUG, "usbdebug: Failed hardware init\n");
 705         else
 706                 printk(BIOS_DEBUG, "usbdebug: " ENV_STRING " starting...\n");
 707 }
 708
 709 CBMEM_READY_HOOK(migrate_ehci_debug);
 710
 711 int dbgp_ep_is_active(struct dbgp_pipe *pipe)
 712 {
 713         return (pipe->status & DBGP_EP_STATMASK) == (DBGP_EP_VALID | DBGP_EP_ENABLED);
 714 }
 715
 716 struct dbgp_pipe *dbgp_console_output(void)
 717 {
 718         return &dbgp_ehci_info()->ep_pipe[DBGP_CONSOLE_EPOUT];
 719 }
 720
 721 struct dbgp_pipe *dbgp_console_input(void)
 722 {
 723         return &dbgp_ehci_info()->ep_pipe[DBGP_CONSOLE_EPIN];
 724 }
 725
 726 void usbdebug_init(void)
 727 {
 728         /* USB console init is done early in romstage, yet delayed to
 729          * CBMEM_READY_HOOKs for postcar and ramstage as we recover state
 730          * from CBMEM.
 731          */
 732         if (CONFIG(USBDEBUG_IN_PRE_RAM)
 733             && (ENV_SEPARATE_ROMSTAGE || ENV_BOOTBLOCK))
 734                 usbdebug_hw_init(false);
 735
 736         /* USB console init is done early in ramstage if it was
 737          * not done in romstage, this does not require CBMEM.
 738          */
 739         if (!CONFIG(USBDEBUG_IN_PRE_RAM) && ENV_RAMSTAGE)
 740                 usbdebug_hw_init(false);
 741 }