payloads/libpayload/drivers/usb/uhci.c

   1 /*
   2  * This file is part of the libpayload project.
   3  *
   4  * Copyright (C) 2008-2010 coresystems GmbH
   5  *
   6  * Redistribution and use in source and binary forms, with or without
   7  * modification, are permitted provided that the following conditions
   8  * are met:
   9  * 1. Redistributions of source code must retain the above copyright
  10  *    notice, this list of conditions and the following disclaimer.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  * 3. The name of the author may not be used to endorse or promote products
  15  *    derived from this software without specific prior written permission.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  27  * SUCH DAMAGE.
  28  */
  29
  30 //#define USB_DEBUG
  31
  32 #include <usb/usb.h>
  33 #include "uhci.h"
  34 #include <arch/virtual.h>
  35
  36 static void uhci_start (hci_t *controller);
  37 static void uhci_stop (hci_t *controller);
  38 static void uhci_reset (hci_t *controller);
  39 static void uhci_shutdown (hci_t *controller);
  40 static int uhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize);
  41 static int uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq,
  42                          int dalen, u8 *data);
  43 static void* uhci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount, int reqtiming);
  44 static void uhci_destroy_intr_queue (endpoint_t *ep, void *queue);
  45 static u8* uhci_poll_intr_queue (void *queue);
  46
  47 #if 0
  48 /* dump uhci */
  49 static void
  50 uhci_dump (hci_t *controller)
  51 {
  52         printf ("dump:\nUSBCMD: %x\n", uhci_reg_read16 (controller, USBCMD));
  53         printf ("USBSTS: %x\n", uhci_reg_read16 (controller, USBSTS));
  54         printf ("USBINTR: %x\n", uhci_reg_read16 (controller, USBINTR));
  55         printf ("FRNUM: %x\n", uhci_reg_read16 (controller, FRNUM));
  56         printf ("FLBASEADD: %x\n", uhci_reg_read32 (controller, FLBASEADD));
  57         printf ("SOFMOD: %x\n", uhci_reg_read8 (controller, SOFMOD));
  58         printf ("PORTSC1: %x\n", uhci_reg_read16 (controller, PORTSC1));
  59         printf ("PORTSC2: %x\n", uhci_reg_read16 (controller, PORTSC2));
  60 }
  61 #endif
  62
  63 static void
  64 td_dump (td_t *td)
  65 {
  66         char td_value[3];
  67         char *td_type;
  68         switch (td->pid) {
  69                 case SETUP:
  70                         td_type="SETUP";
  71                         break;
  72                 case IN:
  73                         td_type="IN";
  74                         break;
  75                 case OUT:
  76                         td_type="OUT";
  77                         break;
  78                 default:
  79                         sprintf(td_value, "%x", td->pid);
  80                         td_type=td_value;
  81         }
  82         printf ("%s packet (at %lx) to %x.%x failed\n", td_type,
  83                 virt_to_phys (td), td->dev_addr, td->endp);
  84         printf ("td (counter at %x) returns: ", td->counter);
  85         printf (" bitstuff err: %x, ", td->status_bitstuff_err);
  86         printf (" CRC err: %x, ", td->status_crc_err);
  87         printf (" NAK rcvd: %x, ", td->status_nakrcvd);
  88         printf (" Babble: %x, ", td->status_babble);
  89         printf (" Data Buffer err: %x, ", td->status_databuf_err);
  90         printf (" Stalled: %x, ", td->status_stalled);
  91         printf (" Active: %x\n", td->status_active);
  92         if (td->status_babble)
  93                 printf (" Babble because of %s\n",
  94                         td->status_bitstuff_err ? "host" : "device");
  95         if (td->status_active)
  96                 printf (" still active - timeout?\n");
  97 }
  98
  99 static void
 100 uhci_reset (hci_t *controller)
 101 {
 102         /* reset */
 103         uhci_reg_write16 (controller, USBCMD, 4);
 104         mdelay (50);
 105         uhci_reg_write16 (controller, USBCMD, 0);
 106         mdelay (10);
 107         uhci_reg_write16 (controller, USBCMD, 2);
 108         while ((uhci_reg_read16 (controller, USBCMD) & 2) != 0)
 109                 mdelay (1);
 110
 111         uhci_reg_write32 (controller, FLBASEADD,
 112                           (u32) virt_to_phys (UHCI_INST (controller)->
 113                                               framelistptr));
 114         //printf ("framelist at %p\n",UHCI_INST(controller)->framelistptr);
 115
 116         /* disable irqs */
 117         uhci_reg_write16 (controller, USBINTR, 0);
 118
 119         /* reset framelist index */
 120         uhci_reg_write16 (controller, FRNUM, 0);
 121
 122         uhci_reg_mask16 (controller, USBCMD, ~0, 0xc0); // max packets, configure flag
 123
 124         uhci_start (controller);
 125 }
 126
 127 hci_t *
 128 uhci_init (pcidev_t addr)
 129 {
 130         int i;
 131         u16 reg16;
 132
 133         hci_t *controller = new_controller ();
 134
 135         if (!controller)
 136                 usb_fatal("Could not create USB controller instance.\n");
 137
 138         controller->instance = malloc (sizeof (uhci_t));
 139         if(!controller->instance)
 140                 usb_fatal("Not enough memory creating USB controller instance.\n");
 141
 142         controller->start = uhci_start;
 143         controller->stop = uhci_stop;
 144         controller->reset = uhci_reset;
 145         controller->shutdown = uhci_shutdown;
 146         controller->bulk = uhci_bulk;
 147         controller->control = uhci_control;
 148         controller->create_intr_queue = uhci_create_intr_queue;
 149         controller->destroy_intr_queue = uhci_destroy_intr_queue;
 150         controller->poll_intr_queue = uhci_poll_intr_queue;
 151         for (i = 0; i < 128; i++) {
 152                 controller->devices[i] = 0;
 153         }
 154         init_device_entry (controller, 0);
 155         UHCI_INST (controller)->roothub = controller->devices[0];
 156
 157         controller->bus_address = addr;
 158         controller->reg_base = pci_read_config32 (controller->bus_address, 0x20) & ~1;  /* ~1 clears the register type indicator that is set to 1 for IO space */
 159
 160         /* kill legacy support handler */
 161         uhci_stop (controller);
 162         mdelay (1);
 163         uhci_reg_write16 (controller, USBSTS, 0x3f);
 164         reg16 = pci_read_config16(controller->bus_address, 0xc0);
 165         reg16 &= 0xdf80;
 166         pci_write_config16 (controller->bus_address, 0xc0, reg16);
 167
 168         UHCI_INST (controller)->framelistptr = memalign (0x1000, 1024 * sizeof (flistp_t *));   /* 4kb aligned to 4kb */
 169         if (! UHCI_INST (controller)->framelistptr)
 170                 usb_fatal("Not enough memory for USB frame list pointer.\n");
 171
 172         memset (UHCI_INST (controller)->framelistptr, 0,
 173                 1024 * sizeof (flistp_t));
 174
 175         /* According to the *BSD UHCI code, this one is needed on some
 176            PIIX chips, because otherwise they misbehave. It must be
 177            added to the last chain.
 178
 179            FIXME: this leaks, if the driver should ever be reinited
 180                   for some reason. Not a problem now.
 181            */
 182         td_t *antiberserk = memalign(16, sizeof(td_t));
 183         if (!antiberserk)
 184                 usb_fatal("Not enough memory for chipset workaround.\n");
 185         memset(antiberserk, 0, sizeof(td_t));
 186
 187         UHCI_INST (controller)->qh_prei = memalign (16, sizeof (qh_t));
 188         UHCI_INST (controller)->qh_intr = memalign (16, sizeof (qh_t));
 189         UHCI_INST (controller)->qh_data = memalign (16, sizeof (qh_t));
 190         UHCI_INST (controller)->qh_last = memalign (16, sizeof (qh_t));
 191
 192         if (! UHCI_INST (controller)->qh_prei ||
 193             ! UHCI_INST (controller)->qh_intr ||
 194             ! UHCI_INST (controller)->qh_data ||
 195             ! UHCI_INST (controller)->qh_last)
 196                 usb_fatal ("Not enough memory for USB controller queues.\n");
 197
 198         UHCI_INST (controller)->qh_prei->headlinkptr.ptr =
 199                 virt_to_phys (UHCI_INST (controller)->qh_intr);
 200         UHCI_INST (controller)->qh_prei->headlinkptr.queue_head = 1;
 201         UHCI_INST (controller)->qh_prei->elementlinkptr.ptr = 0;
 202         UHCI_INST (controller)->qh_prei->elementlinkptr.terminate = 1;
 203
 204         UHCI_INST (controller)->qh_intr->headlinkptr.ptr =
 205                 virt_to_phys (UHCI_INST (controller)->qh_data);
 206         UHCI_INST (controller)->qh_intr->headlinkptr.queue_head = 1;
 207         UHCI_INST (controller)->qh_intr->elementlinkptr.ptr = 0;
 208         UHCI_INST (controller)->qh_intr->elementlinkptr.terminate = 1;
 209
 210         UHCI_INST (controller)->qh_data->headlinkptr.ptr =
 211                 virt_to_phys (UHCI_INST (controller)->qh_last);
 212         UHCI_INST (controller)->qh_data->headlinkptr.queue_head = 1;
 213         UHCI_INST (controller)->qh_data->elementlinkptr.ptr = 0;
 214         UHCI_INST (controller)->qh_data->elementlinkptr.terminate = 1;
 215
 216         UHCI_INST (controller)->qh_last->headlinkptr.ptr = virt_to_phys (UHCI_INST (controller)->qh_data);
 217         UHCI_INST (controller)->qh_last->headlinkptr.terminate = 1;
 218         UHCI_INST (controller)->qh_last->elementlinkptr.ptr = virt_to_phys (antiberserk);
 219         UHCI_INST (controller)->qh_last->elementlinkptr.terminate = 1;
 220
 221         for (i = 0; i < 1024; i++) {
 222                 UHCI_INST (controller)->framelistptr[i].ptr =
 223                         virt_to_phys (UHCI_INST (controller)->qh_prei);
 224                 UHCI_INST (controller)->framelistptr[i].terminate = 0;
 225                 UHCI_INST (controller)->framelistptr[i].queue_head = 1;
 226         }
 227         controller->devices[0]->controller = controller;
 228         controller->devices[0]->init = uhci_rh_init;
 229         controller->devices[0]->init (controller->devices[0]);
 230         uhci_reset (controller);
 231         return controller;
 232 }
 233
 234 static void
 235 uhci_shutdown (hci_t *controller)
 236 {
 237         if (controller == 0)
 238                 return;
 239         detach_controller (controller);
 240         UHCI_INST (controller)->roothub->destroy (UHCI_INST (controller)->
 241                                                   roothub);
 242         uhci_reg_mask16 (controller, USBCMD, 0, 0);     // stop work
 243         free (UHCI_INST (controller)->framelistptr);
 244         free (UHCI_INST (controller)->qh_prei);
 245         free (UHCI_INST (controller)->qh_intr);
 246         free (UHCI_INST (controller)->qh_data);
 247         free (UHCI_INST (controller)->qh_last);
 248         free (UHCI_INST (controller));
 249         free (controller);
 250 }
 251
 252 static void
 253 uhci_start (hci_t *controller)
 254 {
 255         uhci_reg_mask16 (controller, USBCMD, ~0, 1);    // start work on schedule
 256 }
 257
 258 static void
 259 uhci_stop (hci_t *controller)
 260 {
 261         uhci_reg_mask16 (controller, USBCMD, ~1, 0);    // stop work on schedule
 262 }
 263
 264 #define GET_TD(x) ((void*)(((unsigned int)(x))&~0xf))
 265
 266 static td_t *
 267 wait_for_completed_qh (hci_t *controller, qh_t *qh)
 268 {
 269         int timeout = 1000000;  /* max 30 ms. */
 270         void *current = GET_TD (qh->elementlinkptr.ptr);
 271         while ((qh->elementlinkptr.terminate == 0) && (timeout-- > 0)) {
 272                 if (current != GET_TD (qh->elementlinkptr.ptr)) {
 273                         current = GET_TD (qh->elementlinkptr.ptr);
 274                         timeout = 1000000;
 275                 }
 276                 uhci_reg_mask16 (controller, USBSTS, ~0, 0);    // clear resettable registers
 277                 udelay (30);
 278         }
 279         return (GET_TD (qh->elementlinkptr.ptr) ==
 280                 0) ? 0 : GET_TD (phys_to_virt (qh->elementlinkptr.ptr));
 281 }
 282
 283 static int
 284 maxlen (int size)
 285 {
 286         return (size - 1) & 0x7ff;
 287 }
 288
 289 static int
 290 min (int a, int b)
 291 {
 292         if (a < b)
 293                 return a;
 294         else
 295                 return b;
 296 }
 297
 298 static int
 299 uhci_control (usbdev_t *dev, pid_t dir, int drlen, void *devreq, int dalen,
 300               unsigned char *data)
 301 {
 302         int endp = 0;           /* this is control: always 0 */
 303         int mlen = dev->endpoints[0].maxpacketsize;
 304         int count = (2 + (dalen + mlen - 1) / mlen);
 305         unsigned short req = ((unsigned short *) devreq)[0];
 306         int i;
 307         td_t *tds = memalign (16, sizeof (td_t) * count);
 308         memset (tds, 0, sizeof (td_t) * count);
 309         count--;                /* to compensate for 0-indexed array */
 310         for (i = 0; i < count; i++) {
 311                 tds[i].ptr = virt_to_phys (&tds[i + 1]);
 312                 tds[i].depth_first = 1;
 313                 tds[i].terminate = 0;
 314         }
 315         tds[count].ptr = 0;
 316         tds[count].depth_first = 1;
 317         tds[count].terminate = 1;
 318
 319         tds[0].pid = SETUP;
 320         tds[0].dev_addr = dev->address;
 321         tds[0].endp = endp;
 322         tds[0].maxlen = maxlen (drlen);
 323         tds[0].counter = 3;
 324         tds[0].data_toggle = 0;
 325         tds[0].lowspeed = dev->speed;
 326         tds[0].bufptr = virt_to_phys (devreq);
 327         tds[0].status_active = 1;
 328
 329         int toggle = 1;
 330         for (i = 1; i < count; i++) {
 331                 tds[i].pid = dir;
 332                 tds[i].dev_addr = dev->address;
 333                 tds[i].endp = endp;
 334                 tds[i].maxlen = maxlen (min (mlen, dalen));
 335                 tds[i].counter = 3;
 336                 tds[i].data_toggle = toggle;
 337                 tds[i].lowspeed = dev->speed;
 338                 tds[i].bufptr = virt_to_phys (data);
 339                 tds[i].status_active = 1;
 340                 toggle ^= 1;
 341                 dalen -= mlen;
 342                 data += mlen;
 343         }
 344
 345         tds[count].pid = (dir == OUT) ? IN : OUT;
 346         tds[count].dev_addr = dev->address;
 347         tds[count].endp = endp;
 348         tds[count].maxlen = maxlen (0);
 349         tds[count].counter = 0; /* as per linux 2.4.10 */
 350         tds[count].data_toggle = 1;
 351         tds[count].lowspeed = dev->speed;
 352         tds[count].bufptr = 0;
 353         tds[count].status_active = 1;
 354         UHCI_INST (dev->controller)->qh_data->elementlinkptr.ptr =
 355                 virt_to_phys (tds);
 356         UHCI_INST (dev->controller)->qh_data->elementlinkptr.queue_head = 0;
 357         UHCI_INST (dev->controller)->qh_data->elementlinkptr.terminate = 0;
 358         td_t *td = wait_for_completed_qh (dev->controller,
 359                                           UHCI_INST (dev->controller)->
 360                                           qh_data);
 361         int result;
 362         if (td == 0) {
 363                 result = 0;
 364         } else {
 365                 printf ("control packet, req %x\n", req);
 366                 td_dump (td);
 367                 result = 1;
 368         }
 369         free (tds);
 370         return result;
 371 }
 372
 373 static td_t *
 374 create_schedule (int numpackets)
 375 {
 376         if (numpackets == 0)
 377                 return 0;
 378         td_t *tds = memalign (16, sizeof (td_t) * numpackets);
 379         memset (tds, 0, sizeof (td_t) * numpackets);
 380         int i;
 381         for (i = 0; i < numpackets; i++) {
 382                 tds[i].ptr = virt_to_phys (&tds[i + 1]);
 383                 tds[i].terminate = 0;
 384                 tds[i].queue_head = 0;
 385                 tds[i].depth_first = 1;
 386         }
 387         tds[numpackets - 1].ptr = 0;
 388         tds[numpackets - 1].terminate = 1;
 389         tds[numpackets - 1].queue_head = 0;
 390         tds[numpackets - 1].depth_first = 0;
 391         return tds;
 392 }
 393
 394 static void
 395 fill_schedule (td_t *td, endpoint_t *ep, int length, unsigned char *data,
 396                int *toggle)
 397 {
 398         td->pid = ep->direction;
 399         td->dev_addr = ep->dev->address;
 400         td->endp = ep->endpoint & 0xf;
 401         td->maxlen = maxlen (length);
 402         if (ep->direction == SETUP)
 403                 td->counter = 3;
 404         else
 405                 td->counter = 0;
 406         td->data_toggle = *toggle & 1;
 407         td->lowspeed = ep->dev->speed;
 408         td->bufptr = virt_to_phys (data);
 409
 410         td->status_active = 1;
 411         *toggle ^= 1;
 412 }
 413
 414 static int
 415 run_schedule (usbdev_t *dev, td_t *td)
 416 {
 417         UHCI_INST (dev->controller)->qh_data->elementlinkptr.ptr =
 418                 virt_to_phys (td);
 419         UHCI_INST (dev->controller)->qh_data->elementlinkptr.queue_head = 0;
 420         UHCI_INST (dev->controller)->qh_data->elementlinkptr.terminate = 0;
 421         td = wait_for_completed_qh (dev->controller,
 422                                     UHCI_INST (dev->controller)->qh_data);
 423         if (td == 0) {
 424                 return 0;
 425         } else {
 426                 td_dump (td);
 427                 return 1;
 428         }
 429 }
 430
 431 /* finalize == 1: if data is of packet aligned size, add a zero length packet */
 432 static int
 433 uhci_bulk (endpoint_t *ep, int size, u8 *data, int finalize)
 434 {
 435         int maxpsize = ep->maxpacketsize;
 436         if (maxpsize == 0)
 437                 usb_fatal ("MaxPacketSize == 0!!!");
 438         int numpackets = (size + maxpsize - 1 + finalize) / maxpsize;
 439         if (numpackets == 0)
 440                 return 0;
 441         td_t *tds = create_schedule (numpackets);
 442         int i = 0, toggle = ep->toggle;
 443         while ((size > 0) || ((size == 0) && (finalize != 0))) {
 444                 fill_schedule (&tds[i], ep, min (size, maxpsize), data,
 445                                &toggle);
 446                 i++;
 447                 data += maxpsize;
 448                 size -= maxpsize;
 449         }
 450         if (run_schedule (ep->dev, tds) == 1) {
 451                 debug("Stalled. Trying to clean up.\n");
 452                 clear_stall (ep);
 453                 free (tds);
 454                 return 1;
 455         }
 456         ep->toggle = toggle;
 457         free (tds);
 458         return 0;
 459 }
 460
 461 typedef struct {
 462         qh_t *qh;
 463         td_t *tds;
 464         td_t *last_td;
 465         u8 *data;
 466         int lastread;
 467         int total;
 468         int reqsize;
 469 } intr_q;
 470
 471 /* create and hook-up an intr queue into device schedule */
 472 static void*
 473 uhci_create_intr_queue (endpoint_t *ep, int reqsize, int reqcount, int reqtiming)
 474 {
 475         u8 *data = malloc(reqsize*reqcount);
 476         td_t *tds = memalign(16, sizeof(td_t) * reqcount);
 477         qh_t *qh = memalign(16, sizeof(qh_t));
 478
 479         if (!data || !tds || !qh)
 480                 usb_fatal ("Not enough memory to create USB intr queue prerequisites.\n");
 481
 482         qh->elementlinkptr.ptr = virt_to_phys(tds);
 483         qh->elementlinkptr.queue_head = 0;
 484         qh->elementlinkptr.terminate = 0;
 485
 486         intr_q *q = malloc(sizeof(intr_q));
 487         if (!q)
 488                 usb_fatal ("Not enough memory to create USB intr queue.\n");
 489         q->qh = qh;
 490         q->tds = tds;
 491         q->data = data;
 492         q->lastread = 0;
 493         q->total = reqcount;
 494         q->reqsize = reqsize;
 495         q->last_td = &tds[reqcount - 1];
 496
 497         memset (tds, 0, sizeof (td_t) * reqcount);
 498         int i;
 499         for (i = 0; i < reqcount; i++) {
 500                 tds[i].ptr = virt_to_phys (&tds[i + 1]);
 501                 tds[i].terminate = 0;
 502                 tds[i].queue_head = 0;
 503                 tds[i].depth_first = 0;
 504
 505                 tds[i].pid = ep->direction;
 506                 tds[i].dev_addr = ep->dev->address;
 507                 tds[i].endp = ep->endpoint & 0xf;
 508                 tds[i].maxlen = maxlen (reqsize);
 509                 tds[i].counter = 0;
 510                 tds[i].data_toggle = ep->toggle & 1;
 511                 tds[i].lowspeed = ep->dev->speed;
 512                 tds[i].bufptr = virt_to_phys (data);
 513                 tds[i].status_active = 1;
 514                 ep->toggle ^= 1;
 515                 data += reqsize;
 516         }
 517         tds[reqcount - 1].ptr = 0;
 518         tds[reqcount - 1].terminate = 1;
 519         tds[reqcount - 1].queue_head = 0;
 520         tds[reqcount - 1].depth_first = 0;
 521         for (i = reqtiming; i < 1024; i += reqtiming) {
 522                 /* FIXME: wrap in another qh, one for each occurance of the qh in the framelist */
 523                 qh->headlinkptr.ptr = UHCI_INST (ep->dev->controller)->framelistptr[i].ptr;
 524                 qh->headlinkptr.terminate = 0;
 525                 UHCI_INST (ep->dev->controller)->framelistptr[i].ptr = virt_to_phys(qh);
 526                 UHCI_INST (ep->dev->controller)->framelistptr[i].terminate = 0;
 527                 UHCI_INST (ep->dev->controller)->framelistptr[i].queue_head = 1;
 528         }
 529         return q;
 530 }
 531
 532 /* remove queue from device schedule, dropping all data that came in */
 533 static void
 534 uhci_destroy_intr_queue (endpoint_t *ep, void *q_)
 535 {
 536         intr_q *q = (intr_q*)q_;
 537         u32 val = virt_to_phys (q->qh);
 538         u32 end = virt_to_phys (UHCI_INST (ep->dev->controller)->qh_intr);
 539         int i;
 540         for (i=0; i<1024; i++) {
 541                 u32 oldptr = 0;
 542                 u32 ptr = UHCI_INST (ep->dev->controller)->framelistptr[i].ptr;
 543                 while (ptr != end) {
 544                         if (((qh_t*)phys_to_virt(ptr))->elementlinkptr.ptr == val) {
 545                                 ((qh_t*)phys_to_virt(oldptr))->headlinkptr.ptr = ((qh_t*)phys_to_virt(ptr))->headlinkptr.ptr;
 546                                 free(phys_to_virt(ptr));
 547                                 break;
 548                         }
 549                         oldptr = ptr;
 550                         ptr = ((qh_t*)phys_to_virt(ptr))->headlinkptr.ptr;
 551                 }
 552         }
 553         free(q->data);
 554         free(q->tds);
 555         free(q->qh);
 556         free(q);
 557 }
 558
 559 /* read one intr-packet from queue, if available. extend the queue for new input.
 560    return NULL if nothing new available.
 561    Recommended use: while (data=poll_intr_queue(q)) process(data);
 562  */
 563 static u8*
 564 uhci_poll_intr_queue (void *q_)
 565 {
 566         intr_q *q = (intr_q*)q_;
 567         if (q->tds[q->lastread].status_active == 0) {
 568                 /* FIXME: handle errors */
 569                 int current = q->lastread;
 570                 int previous;
 571                 if (q->lastread == 0) {
 572                         previous = q->total - 1;
 573                 } else {
 574                         previous = q->lastread - 1;
 575                 }
 576                 q->tds[previous].status = 0;
 577                 q->tds[previous].ptr = 0;
 578                 q->tds[previous].terminate = 1;
 579                 if (q->last_td != &q->tds[previous]) {
 580                         q->last_td->ptr = virt_to_phys(&q->tds[previous]);
 581                         q->last_td->terminate = 0;
 582                         q->last_td = &q->tds[previous];
 583                 }
 584                 q->tds[previous].status_active = 1;
 585                 q->lastread = (q->lastread + 1) % q->total;
 586                 return &q->data[current*q->reqsize];
 587         }
 588         return NULL;
 589 }
 590
 591 void
 592 uhci_reg_write32 (hci_t *ctrl, usbreg reg, u32 value)
 593 {
 594         outl (value, ctrl->reg_base + reg);
 595 }
 596
 597 u32
 598 uhci_reg_read32 (hci_t *ctrl, usbreg reg)
 599 {
 600         return inl (ctrl->reg_base + reg);
 601 }
 602
 603 void
 604 uhci_reg_write16 (hci_t *ctrl, usbreg reg, u16 value)
 605 {
 606         outw (value, ctrl->reg_base + reg);
 607 }
 608
 609 u16
 610 uhci_reg_read16 (hci_t *ctrl, usbreg reg)
 611 {
 612         return inw (ctrl->reg_base + reg);
 613 }
 614
 615 void
 616 uhci_reg_write8 (hci_t *ctrl, usbreg reg, u8 value)
 617 {
 618         outb (value, ctrl->reg_base + reg);
 619 }
 620
 621 u8
 622 uhci_reg_read8 (hci_t *ctrl, usbreg reg)
 623 {
 624         return inb (ctrl->reg_base + reg);
 625 }
 626
 627 void
 628 uhci_reg_mask32 (hci_t *ctrl, usbreg reg, u32 andmask, u32 ormask)
 629 {
 630         uhci_reg_write32 (ctrl, reg,
 631                           (uhci_reg_read32 (ctrl, reg) & andmask) | ormask);
 632 }
 633
 634 void
 635 uhci_reg_mask16 (hci_t *ctrl, usbreg reg, u16 andmask, u16 ormask)
 636 {
 637         uhci_reg_write16 (ctrl, reg,
 638                           (uhci_reg_read16 (ctrl, reg) & andmask) | ormask);
 639 }
 640
 641 void
 642 uhci_reg_mask8 (hci_t *ctrl, usbreg reg, u8 andmask, u8 ormask)
 643 {
 644         uhci_reg_write8 (ctrl, reg,
 645                          (uhci_reg_read8 (ctrl, reg) & andmask) | ormask);
 646 }