drivers/usb/early/ehci-dbgp.c

   1 #include <linux/console.h>
   2 #include <linux/errno.h>
   3 #include <linux/pci_regs.h>
   4 #include <linux/pci_ids.h>
   5 #include <linux/usb/ch9.h>
   6 #include <linux/usb/ehci_def.h>
   7 #include <linux/delay.h>
   8 #include <asm/io.h>
   9 #include <asm/pci-direct.h>
  10 #include <asm/fixmap.h>
  11
  12 static struct ehci_caps __iomem *ehci_caps;
  13 static struct ehci_regs __iomem *ehci_regs;
  14 static struct ehci_dbg_port __iomem *ehci_debug;
  15 static unsigned int dbgp_endpoint_out;
  16
  17 struct ehci_dev {
  18         u32 bus;
  19         u32 slot;
  20         u32 func;
  21 };
  22
  23 static struct ehci_dev ehci_dev;
  24
  25 #define USB_DEBUG_DEVNUM 127
  26
  27 #define DBGP_DATA_TOGGLE        0x8800
  28
  29 static inline u32 dbgp_pid_update(u32 x, u32 tok)
  30 {
  31         return ((x ^ DBGP_DATA_TOGGLE) & 0xffff00) | (tok & 0xff);
  32 }
  33
  34 static inline u32 dbgp_len_update(u32 x, u32 len)
  35 {
  36         return (x & ~0x0f) | (len & 0x0f);
  37 }
  38
  39 /*
  40  * USB Packet IDs (PIDs)
  41  */
  42
  43 /* token */
  44 #define USB_PID_OUT             0xe1
  45 #define USB_PID_IN              0x69
  46 #define USB_PID_SOF             0xa5
  47 #define USB_PID_SETUP           0x2d
  48 /* handshake */
  49 #define USB_PID_ACK             0xd2
  50 #define USB_PID_NAK             0x5a
  51 #define USB_PID_STALL           0x1e
  52 #define USB_PID_NYET            0x96
  53 /* data */
  54 #define USB_PID_DATA0           0xc3
  55 #define USB_PID_DATA1           0x4b
  56 #define USB_PID_DATA2           0x87
  57 #define USB_PID_MDATA           0x0f
  58 /* Special */
  59 #define USB_PID_PREAMBLE        0x3c
  60 #define USB_PID_ERR             0x3c
  61 #define USB_PID_SPLIT           0x78
  62 #define USB_PID_PING            0xb4
  63 #define USB_PID_UNDEF_0         0xf0
  64
  65 #define USB_PID_DATA_TOGGLE     0x88
  66 #define DBGP_CLAIM (DBGP_OWNER | DBGP_ENABLED | DBGP_INUSE)
  67
  68 #define PCI_CAP_ID_EHCI_DEBUG   0xa
  69
  70 #define HUB_ROOT_RESET_TIME     50      /* times are in msec */
  71 #define HUB_SHORT_RESET_TIME    10
  72 #define HUB_LONG_RESET_TIME     200
  73 #define HUB_RESET_TIMEOUT       500
  74
  75 #define DBGP_MAX_PACKET         8
  76
  77 static int dbgp_wait_until_complete(void)
  78 {
  79         u32 ctrl;
  80         int loop = 0x100000;
  81
  82         do {
  83                 ctrl = readl(&ehci_debug->control);
  84                 /* Stop when the transaction is finished */
  85                 if (ctrl & DBGP_DONE)
  86                         break;
  87         } while (--loop > 0);
  88
  89         if (!loop)
  90                 return -1;
  91
  92         /*
  93          * Now that we have observed the completed transaction,
  94          * clear the done bit.
  95          */
  96         writel(ctrl | DBGP_DONE, &ehci_debug->control);
  97         return (ctrl & DBGP_ERROR) ? -DBGP_ERRCODE(ctrl) : DBGP_LEN(ctrl);
  98 }
  99
 100 static void __init dbgp_mdelay(int ms)
 101 {
 102         int i;
 103
 104         while (ms--) {
 105                 for (i = 0; i < 1000; i++)
 106                         outb(0x1, 0x80);
 107         }
 108 }
 109
 110 static void dbgp_breath(void)
 111 {
 112         /* Sleep to give the debug port a chance to breathe */
 113 }
 114
 115 static int dbgp_wait_until_done(unsigned ctrl)
 116 {
 117         u32 pids, lpid;
 118         int ret;
 119         int loop = 3;
 120
 121 retry:
 122         writel(ctrl | DBGP_GO, &ehci_debug->control);
 123         ret = dbgp_wait_until_complete();
 124         pids = readl(&ehci_debug->pids);
 125         lpid = DBGP_PID_GET(pids);
 126
 127         if (ret < 0)
 128                 return ret;
 129
 130         /*
 131          * If the port is getting full or it has dropped data
 132          * start pacing ourselves, not necessary but it's friendly.
 133          */
 134         if ((lpid == USB_PID_NAK) || (lpid == USB_PID_NYET))
 135                 dbgp_breath();
 136
 137         /* If I get a NACK reissue the transmission */
 138         if (lpid == USB_PID_NAK) {
 139                 if (--loop > 0)
 140                         goto retry;
 141         }
 142
 143         return ret;
 144 }
 145
 146 static void dbgp_set_data(const void *buf, int size)
 147 {
 148         const unsigned char *bytes = buf;
 149         u32 lo, hi;
 150         int i;
 151
 152         lo = hi = 0;
 153         for (i = 0; i < 4 && i < size; i++)
 154                 lo |= bytes[i] << (8*i);
 155         for (; i < 8 && i < size; i++)
 156                 hi |= bytes[i] << (8*(i - 4));
 157         writel(lo, &ehci_debug->data03);
 158         writel(hi, &ehci_debug->data47);
 159 }
 160
 161 static void __init dbgp_get_data(void *buf, int size)
 162 {
 163         unsigned char *bytes = buf;
 164         u32 lo, hi;
 165         int i;
 166
 167         lo = readl(&ehci_debug->data03);
 168         hi = readl(&ehci_debug->data47);
 169         for (i = 0; i < 4 && i < size; i++)
 170                 bytes[i] = (lo >> (8*i)) & 0xff;
 171         for (; i < 8 && i < size; i++)
 172                 bytes[i] = (hi >> (8*(i - 4))) & 0xff;
 173 }
 174
 175 static int dbgp_bulk_write(unsigned devnum, unsigned endpoint,
 176                          const char *bytes, int size)
 177 {
 178         u32 pids, addr, ctrl;
 179         int ret;
 180
 181         if (size > DBGP_MAX_PACKET)
 182                 return -1;
 183
 184         addr = DBGP_EPADDR(devnum, endpoint);
 185
 186         pids = readl(&ehci_debug->pids);
 187         pids = dbgp_pid_update(pids, USB_PID_OUT);
 188
 189         ctrl = readl(&ehci_debug->control);
 190         ctrl = dbgp_len_update(ctrl, size);
 191         ctrl |= DBGP_OUT;
 192         ctrl |= DBGP_GO;
 193
 194         dbgp_set_data(bytes, size);
 195         writel(addr, &ehci_debug->address);
 196         writel(pids, &ehci_debug->pids);
 197
 198         ret = dbgp_wait_until_done(ctrl);
 199         if (ret < 0)
 200                 return ret;
 201
 202         return ret;
 203 }
 204
 205 static int __init dbgp_bulk_read(unsigned devnum, unsigned endpoint, void *data,
 206                                  int size)
 207 {
 208         u32 pids, addr, ctrl;
 209         int ret;
 210
 211         if (size > DBGP_MAX_PACKET)
 212                 return -1;
 213
 214         addr = DBGP_EPADDR(devnum, endpoint);
 215
 216         pids = readl(&ehci_debug->pids);
 217         pids = dbgp_pid_update(pids, USB_PID_IN);
 218
 219         ctrl = readl(&ehci_debug->control);
 220         ctrl = dbgp_len_update(ctrl, size);
 221         ctrl &= ~DBGP_OUT;
 222         ctrl |= DBGP_GO;
 223
 224         writel(addr, &ehci_debug->address);
 225         writel(pids, &ehci_debug->pids);
 226         ret = dbgp_wait_until_done(ctrl);
 227         if (ret < 0)
 228                 return ret;
 229
 230         if (size > ret)
 231                 size = ret;
 232         dbgp_get_data(data, size);
 233         return ret;
 234 }
 235
 236 static int __init dbgp_control_msg(unsigned devnum, int requesttype,
 237         int request, int value, int index, void *data, int size)
 238 {
 239         u32 pids, addr, ctrl;
 240         struct usb_ctrlrequest req;
 241         int read;
 242         int ret;
 243
 244         read = (requesttype & USB_DIR_IN) != 0;
 245         if (size > (read ? DBGP_MAX_PACKET:0))
 246                 return -1;
 247
 248         /* Compute the control message */
 249         req.bRequestType = requesttype;
 250         req.bRequest = request;
 251         req.wValue = cpu_to_le16(value);
 252         req.wIndex = cpu_to_le16(index);
 253         req.wLength = cpu_to_le16(size);
 254
 255         pids = DBGP_PID_SET(USB_PID_DATA0, USB_PID_SETUP);
 256         addr = DBGP_EPADDR(devnum, 0);
 257
 258         ctrl = readl(&ehci_debug->control);
 259         ctrl = dbgp_len_update(ctrl, sizeof(req));
 260         ctrl |= DBGP_OUT;
 261         ctrl |= DBGP_GO;
 262
 263         /* Send the setup message */
 264         dbgp_set_data(&req, sizeof(req));
 265         writel(addr, &ehci_debug->address);
 266         writel(pids, &ehci_debug->pids);
 267         ret = dbgp_wait_until_done(ctrl);
 268         if (ret < 0)
 269                 return ret;
 270
 271         /* Read the result */
 272         return dbgp_bulk_read(devnum, 0, data, size);
 273 }
 274
 275
 276 /* Find a PCI capability */
 277 static u32 __init find_cap(u32 num, u32 slot, u32 func, int cap)
 278 {
 279         u8 pos;
 280         int bytes;
 281
 282         if (!(read_pci_config_16(num, slot, func, PCI_STATUS) &
 283                 PCI_STATUS_CAP_LIST))
 284                 return 0;
 285
 286         pos = read_pci_config_byte(num, slot, func, PCI_CAPABILITY_LIST);
 287         for (bytes = 0; bytes < 48 && pos >= 0x40; bytes++) {
 288                 u8 id;
 289
 290                 pos &= ~3;
 291                 id = read_pci_config_byte(num, slot, func, pos+PCI_CAP_LIST_ID);
 292                 if (id == 0xff)
 293                         break;
 294                 if (id == cap)
 295                         return pos;
 296
 297                 pos = read_pci_config_byte(num, slot, func,
 298                                                  pos+PCI_CAP_LIST_NEXT);
 299         }
 300         return 0;
 301 }
 302
 303 static u32 __init __find_dbgp(u32 bus, u32 slot, u32 func)
 304 {
 305         u32 class;
 306
 307         class = read_pci_config(bus, slot, func, PCI_CLASS_REVISION);
 308         if ((class >> 8) != PCI_CLASS_SERIAL_USB_EHCI)
 309                 return 0;
 310
 311         return find_cap(bus, slot, func, PCI_CAP_ID_EHCI_DEBUG);
 312 }
 313
 314 static u32 __init find_dbgp(int ehci_num, u32 *rbus, u32 *rslot, u32 *rfunc)
 315 {
 316         u32 bus, slot, func;
 317
 318         for (bus = 0; bus < 256; bus++) {
 319                 for (slot = 0; slot < 32; slot++) {
 320                         for (func = 0; func < 8; func++) {
 321                                 unsigned cap;
 322
 323                                 cap = __find_dbgp(bus, slot, func);
 324
 325                                 if (!cap)
 326                                         continue;
 327                                 if (ehci_num-- != 0)
 328                                         continue;
 329                                 *rbus = bus;
 330                                 *rslot = slot;
 331                                 *rfunc = func;
 332                                 return cap;
 333                         }
 334                 }
 335         }
 336         return 0;
 337 }
 338
 339 static int __init ehci_reset_port(int port)
 340 {
 341         u32 portsc;
 342         u32 delay_time, delay;
 343         int loop;
 344
 345         /* Reset the usb debug port */
 346         portsc = readl(&ehci_regs->port_status[port - 1]);
 347         portsc &= ~PORT_PE;
 348         portsc |= PORT_RESET;
 349         writel(portsc, &ehci_regs->port_status[port - 1]);
 350
 351         delay = HUB_ROOT_RESET_TIME;
 352         for (delay_time = 0; delay_time < HUB_RESET_TIMEOUT;
 353              delay_time += delay) {
 354                 dbgp_mdelay(delay);
 355
 356                 portsc = readl(&ehci_regs->port_status[port - 1]);
 357                 if (portsc & PORT_RESET) {
 358                         /* force reset to complete */
 359                         loop = 2;
 360                         writel(portsc & ~(PORT_RWC_BITS | PORT_RESET),
 361                                 &ehci_regs->port_status[port - 1]);
 362                         do {
 363                                 portsc = readl(&ehci_regs->port_status[port-1]);
 364                         } while ((portsc & PORT_RESET) && (--loop > 0));
 365                 }
 366
 367                 /* Device went away? */
 368                 if (!(portsc & PORT_CONNECT))
 369                         return -ENOTCONN;
 370
 371                 /* bomb out completely if something weird happend */
 372                 if ((portsc & PORT_CSC))
 373                         return -EINVAL;
 374
 375                 /* If we've finished resetting, then break out of the loop */
 376                 if (!(portsc & PORT_RESET) && (portsc & PORT_PE))
 377                         return 0;
 378         }
 379         return -EBUSY;
 380 }
 381
 382 static int __init ehci_wait_for_port(int port)
 383 {
 384         u32 status;
 385         int ret, reps;
 386
 387         for (reps = 0; reps < 3; reps++) {
 388                 dbgp_mdelay(100);
 389                 status = readl(&ehci_regs->status);
 390                 if (status & STS_PCD) {
 391                         ret = ehci_reset_port(port);
 392                         if (ret == 0)
 393                                 return 0;
 394                 }
 395         }
 396         return -ENOTCONN;
 397 }
 398
 399 #ifdef DBGP_DEBUG
 400 # define dbgp_printk early_printk
 401 #else
 402 static inline void dbgp_printk(const char *fmt, ...) { }
 403 #endif
 404
 405 typedef void (*set_debug_port_t)(int port);
 406
 407 static void __init default_set_debug_port(int port)
 408 {
 409 }
 410
 411 static set_debug_port_t __initdata set_debug_port = default_set_debug_port;
 412
 413 static void __init nvidia_set_debug_port(int port)
 414 {
 415         u32 dword;
 416         dword = read_pci_config(ehci_dev.bus, ehci_dev.slot, ehci_dev.func,
 417                                  0x74);
 418         dword &= ~(0x0f<<12);
 419         dword |= ((port & 0x0f)<<12);
 420         write_pci_config(ehci_dev.bus, ehci_dev.slot, ehci_dev.func, 0x74,
 421                                  dword);
 422         dbgp_printk("set debug port to %d\n", port);
 423 }
 424
 425 static void __init detect_set_debug_port(void)
 426 {
 427         u32 vendorid;
 428
 429         vendorid = read_pci_config(ehci_dev.bus, ehci_dev.slot, ehci_dev.func,
 430                  0x00);
 431
 432         if ((vendorid & 0xffff) == 0x10de) {
 433                 dbgp_printk("using nvidia set_debug_port\n");
 434                 set_debug_port = nvidia_set_debug_port;
 435         }
 436 }
 437
 438 /* The code in early_ehci_bios_handoff() is derived from the usb pci
 439  * quirk initialization, but altered so as to use the early PCI
 440  * routines. */
 441 #define EHCI_USBLEGSUP_BIOS     (1 << 16)       /* BIOS semaphore */
 442 #define EHCI_USBLEGCTLSTS       4               /* legacy control/status */
 443 static void __init early_ehci_bios_handoff(void)
 444 {
 445         u32 hcc_params = readl(&ehci_caps->hcc_params);
 446         int offset = (hcc_params >> 8) & 0xff;
 447         u32 cap;
 448         int msec;
 449
 450         if (!offset)
 451                 return;
 452
 453         cap = read_pci_config(ehci_dev.bus, ehci_dev.slot,
 454                               ehci_dev.func, offset);
 455         dbgp_printk("dbgp: ehci BIOS state %08x\n", cap);
 456
 457         if ((cap & 0xff) == 1 && (cap & EHCI_USBLEGSUP_BIOS)) {
 458                 dbgp_printk("dbgp: BIOS handoff\n");
 459                 write_pci_config_byte(ehci_dev.bus, ehci_dev.slot,
 460                                       ehci_dev.func, offset + 3, 1);
 461         }
 462
 463         /* if boot firmware now owns EHCI, spin till it hands it over. */
 464         msec = 1000;
 465         while ((cap & EHCI_USBLEGSUP_BIOS) && (msec > 0)) {
 466                 mdelay(10);
 467                 msec -= 10;
 468                 cap = read_pci_config(ehci_dev.bus, ehci_dev.slot,
 469                                       ehci_dev.func, offset);
 470         }
 471
 472         if (cap & EHCI_USBLEGSUP_BIOS) {
 473                 /* well, possibly buggy BIOS... try to shut it down,
 474                  * and hope nothing goes too wrong */
 475                 dbgp_printk("dbgp: BIOS handoff failed: %08x\n", cap);
 476                 write_pci_config_byte(ehci_dev.bus, ehci_dev.slot,
 477                                       ehci_dev.func, offset + 2, 0);
 478         }
 479
 480         /* just in case, always disable EHCI SMIs */
 481         write_pci_config_byte(ehci_dev.bus, ehci_dev.slot, ehci_dev.func,
 482                               offset + EHCI_USBLEGCTLSTS, 0);
 483 }
 484
 485 static int __init ehci_setup(void)
 486 {
 487         struct usb_debug_descriptor dbgp_desc;
 488         u32 cmd, ctrl, status, portsc, hcs_params;
 489         u32 debug_port, new_debug_port = 0, n_ports;
 490         u32  devnum;
 491         int ret, i;
 492         int loop;
 493         int port_map_tried;
 494         int playtimes = 3;
 495
 496         early_ehci_bios_handoff();
 497
 498 try_next_time:
 499         port_map_tried = 0;
 500
 501 try_next_port:
 502
 503         hcs_params = readl(&ehci_caps->hcs_params);
 504         debug_port = HCS_DEBUG_PORT(hcs_params);
 505         n_ports    = HCS_N_PORTS(hcs_params);
 506
 507         dbgp_printk("debug_port: %d\n", debug_port);
 508         dbgp_printk("n_ports:    %d\n", n_ports);
 509
 510         for (i = 1; i <= n_ports; i++) {
 511                 portsc = readl(&ehci_regs->port_status[i-1]);
 512                 dbgp_printk("portstatus%d: %08x\n", i, portsc);
 513         }
 514
 515         if (port_map_tried && (new_debug_port != debug_port)) {
 516                 if (--playtimes) {
 517                         set_debug_port(new_debug_port);
 518                         goto try_next_time;
 519                 }
 520                 return -1;
 521         }
 522
 523         loop = 100000;
 524         /* Reset the EHCI controller */
 525         cmd = readl(&ehci_regs->command);
 526         cmd |= CMD_RESET;
 527         writel(cmd, &ehci_regs->command);
 528         do {
 529                 cmd = readl(&ehci_regs->command);
 530         } while ((cmd & CMD_RESET) && (--loop > 0));
 531
 532         if (!loop) {
 533                 dbgp_printk("can not reset ehci\n");
 534                 return -1;
 535         }
 536         dbgp_printk("ehci reset done\n");
 537
 538         /* Claim ownership, but do not enable yet */
 539         ctrl = readl(&ehci_debug->control);
 540         ctrl |= DBGP_OWNER;
 541         ctrl &= ~(DBGP_ENABLED | DBGP_INUSE);
 542         writel(ctrl, &ehci_debug->control);
 543
 544         /* Start the ehci running */
 545         cmd = readl(&ehci_regs->command);
 546         cmd &= ~(CMD_LRESET | CMD_IAAD | CMD_PSE | CMD_ASE | CMD_RESET);
 547         cmd |= CMD_RUN;
 548         writel(cmd, &ehci_regs->command);
 549
 550         /* Ensure everything is routed to the EHCI */
 551         writel(FLAG_CF, &ehci_regs->configured_flag);
 552
 553         /* Wait until the controller is no longer halted */
 554         loop = 10;
 555         do {
 556                 status = readl(&ehci_regs->status);
 557         } while ((status & STS_HALT) && (--loop > 0));
 558
 559         if (!loop) {
 560                 dbgp_printk("ehci can be started\n");
 561                 return -1;
 562         }
 563         dbgp_printk("ehci started\n");
 564
 565         /* Wait for a device to show up in the debug port */
 566         ret = ehci_wait_for_port(debug_port);
 567         if (ret < 0) {
 568                 dbgp_printk("No device found in debug port\n");
 569                 goto next_debug_port;
 570         }
 571         dbgp_printk("ehci wait for port done\n");
 572
 573         /* Enable the debug port */
 574         ctrl = readl(&ehci_debug->control);
 575         ctrl |= DBGP_CLAIM;
 576         writel(ctrl, &ehci_debug->control);
 577         ctrl = readl(&ehci_debug->control);
 578         if ((ctrl & DBGP_CLAIM) != DBGP_CLAIM) {
 579                 dbgp_printk("No device in debug port\n");
 580                 writel(ctrl & ~DBGP_CLAIM, &ehci_debug->control);
 581                 goto err;
 582         }
 583         dbgp_printk("debug ported enabled\n");
 584
 585         /* Completely transfer the debug device to the debug controller */
 586         portsc = readl(&ehci_regs->port_status[debug_port - 1]);
 587         portsc &= ~PORT_PE;
 588         writel(portsc, &ehci_regs->port_status[debug_port - 1]);
 589
 590         dbgp_mdelay(100);
 591
 592         /* Find the debug device and make it device number 127 */
 593         for (devnum = 0; devnum <= 127; devnum++) {
 594                 ret = dbgp_control_msg(devnum,
 595                         USB_DIR_IN | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 596                         USB_REQ_GET_DESCRIPTOR, (USB_DT_DEBUG << 8), 0,
 597                         &dbgp_desc, sizeof(dbgp_desc));
 598                 if (ret > 0)
 599                         break;
 600         }
 601         if (devnum > 127) {
 602                 dbgp_printk("Could not find attached debug device\n");
 603                 goto err;
 604         }
 605         if (ret < 0) {
 606                 dbgp_printk("Attached device is not a debug device\n");
 607                 goto err;
 608         }
 609         dbgp_endpoint_out = dbgp_desc.bDebugOutEndpoint;
 610
 611         /* Move the device to 127 if it isn't already there */
 612         if (devnum != USB_DEBUG_DEVNUM) {
 613                 ret = dbgp_control_msg(devnum,
 614                         USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 615                         USB_REQ_SET_ADDRESS, USB_DEBUG_DEVNUM, 0, NULL, 0);
 616                 if (ret < 0) {
 617                         dbgp_printk("Could not move attached device to %d\n",
 618                                 USB_DEBUG_DEVNUM);
 619                         goto err;
 620                 }
 621                 devnum = USB_DEBUG_DEVNUM;
 622                 dbgp_printk("debug device renamed to 127\n");
 623         }
 624
 625         /* Enable the debug interface */
 626         ret = dbgp_control_msg(USB_DEBUG_DEVNUM,
 627                 USB_DIR_OUT | USB_TYPE_STANDARD | USB_RECIP_DEVICE,
 628                 USB_REQ_SET_FEATURE, USB_DEVICE_DEBUG_MODE, 0, NULL, 0);
 629         if (ret < 0) {
 630                 dbgp_printk(" Could not enable the debug device\n");
 631                 goto err;
 632         }
 633         dbgp_printk("debug interface enabled\n");
 634
 635         /* Perform a small write to get the even/odd data state in sync
 636          */
 637         ret = dbgp_bulk_write(USB_DEBUG_DEVNUM, dbgp_endpoint_out, " ", 1);
 638         if (ret < 0) {
 639                 dbgp_printk("dbgp_bulk_write failed: %d\n", ret);
 640                 goto err;
 641         }
 642         dbgp_printk("small write doned\n");
 643
 644         return 0;
 645 err:
 646         /* Things didn't work so remove my claim */
 647         ctrl = readl(&ehci_debug->control);
 648         ctrl &= ~(DBGP_CLAIM | DBGP_OUT);
 649         writel(ctrl, &ehci_debug->control);
 650         return -1;
 651
 652 next_debug_port:
 653         port_map_tried |= (1<<(debug_port - 1));
 654         new_debug_port = ((debug_port-1+1)%n_ports) + 1;
 655         if (port_map_tried != ((1<<n_ports) - 1)) {
 656                 set_debug_port(new_debug_port);
 657                 goto try_next_port;
 658         }
 659         if (--playtimes) {
 660                 set_debug_port(new_debug_port);
 661                 goto try_next_time;
 662         }
 663
 664         return -1;
 665 }
 666
 667 int __init early_dbgp_init(char *s)
 668 {
 669         u32 debug_port, bar, offset;
 670         u32 bus, slot, func, cap;
 671         void __iomem *ehci_bar;
 672         u32 dbgp_num;
 673         u32 bar_val;
 674         char *e;
 675         int ret;
 676         u8 byte;
 677
 678         if (!early_pci_allowed())
 679                 return -1;
 680
 681         dbgp_num = 0;
 682         if (*s)
 683                 dbgp_num = simple_strtoul(s, &e, 10);
 684         dbgp_printk("dbgp_num: %d\n", dbgp_num);
 685
 686         cap = find_dbgp(dbgp_num, &bus, &slot, &func);
 687         if (!cap)
 688                 return -1;
 689
 690         dbgp_printk("Found EHCI debug port on %02x:%02x.%1x\n", bus, slot,
 691                          func);
 692
 693         debug_port = read_pci_config(bus, slot, func, cap);
 694         bar = (debug_port >> 29) & 0x7;
 695         bar = (bar * 4) + 0xc;
 696         offset = (debug_port >> 16) & 0xfff;
 697         dbgp_printk("bar: %02x offset: %03x\n", bar, offset);
 698         if (bar != PCI_BASE_ADDRESS_0) {
 699                 dbgp_printk("only debug ports on bar 1 handled.\n");
 700
 701                 return -1;
 702         }
 703
 704         bar_val = read_pci_config(bus, slot, func, PCI_BASE_ADDRESS_0);
 705         dbgp_printk("bar_val: %02x offset: %03x\n", bar_val, offset);
 706         if (bar_val & ~PCI_BASE_ADDRESS_MEM_MASK) {
 707                 dbgp_printk("only simple 32bit mmio bars supported\n");
 708
 709                 return -1;
 710         }
 711
 712         /* double check if the mem space is enabled */
 713         byte = read_pci_config_byte(bus, slot, func, 0x04);
 714         if (!(byte & 0x2)) {
 715                 byte  |= 0x02;
 716                 write_pci_config_byte(bus, slot, func, 0x04, byte);
 717                 dbgp_printk("mmio for ehci enabled\n");
 718         }
 719
 720         /*
 721          * FIXME I don't have the bar size so just guess PAGE_SIZE is more
 722          * than enough.  1K is the biggest I have seen.
 723          */
 724         set_fixmap_nocache(FIX_DBGP_BASE, bar_val & PAGE_MASK);
 725         ehci_bar = (void __iomem *)__fix_to_virt(FIX_DBGP_BASE);
 726         ehci_bar += bar_val & ~PAGE_MASK;
 727         dbgp_printk("ehci_bar: %p\n", ehci_bar);
 728
 729         ehci_caps  = ehci_bar;
 730         ehci_regs  = ehci_bar + HC_LENGTH(readl(&ehci_caps->hc_capbase));
 731         ehci_debug = ehci_bar + offset;
 732         ehci_dev.bus = bus;
 733         ehci_dev.slot = slot;
 734         ehci_dev.func = func;
 735
 736         detect_set_debug_port();
 737
 738         ret = ehci_setup();
 739         if (ret < 0) {
 740                 dbgp_printk("ehci_setup failed\n");
 741                 ehci_debug = NULL;
 742
 743                 return -1;
 744         }
 745
 746         return 0;
 747 }
 748
 749 static void early_dbgp_write(struct console *con, const char *str, u32 n)
 750 {
 751         int chunk, ret;
 752         char buf[DBGP_MAX_PACKET];
 753         int use_cr = 0;
 754
 755         if (!ehci_debug)
 756                 return;
 757         while (n > 0) {
 758                 for (chunk = 0; chunk < DBGP_MAX_PACKET && n > 0;
 759                      str++, chunk++, n--) {
 760                         if (!use_cr && *str == '\n') {
 761                                 use_cr = 1;
 762                                 buf[chunk] = '\r';
 763                                 str--;
 764                                 n++;
 765                                 continue;
 766                         }
 767                         if (use_cr)
 768                                 use_cr = 0;
 769                         buf[chunk] = *str;
 770                 }
 771                 ret = dbgp_bulk_write(USB_DEBUG_DEVNUM,
 772                         dbgp_endpoint_out, buf, chunk);
 773         }
 774 }
 775
 776 struct console early_dbgp_console = {
 777         .name =         "earlydbg",
 778         .write =        early_dbgp_write,
 779         .flags =        CON_PRINTBUFFER,
 780         .index =        -1,
 781 };
 782