payloads/libpayload/drivers/usb/uhci.c

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