KernelLand/Modules/USB/EHCI/ehci.c

   1 /*
   2  * Acess2 EHCI Driver
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * ehci.c
   6  * - Driver Core
   7  */
   8 #define DEBUG   1
   9 #define VERSION VER2(0,1)
  10 #include <acess.h>
  11 #include <modules.h>
  12 #include <usb_host.h>
  13 #include "ehci.h"
  14 #include <drv_pci.h>
  15 #include <limits.h>
  16 #include <events.h>
  17 #include <timers.h>
  18
  19 // === CONSTANTS ===
  20 #define EHCI_MAX_CONTROLLERS    4
  21 #define EHCI_THREADEVENT_IOC    THREAD_EVENT_USER1
  22 #define EHCI_THREADEVENT_PORTSC THREAD_EVENT_USER2
  23
  24 // === PROTOTYPES ===
  25  int    EHCI_Initialise(char **Arguments);
  26  int    EHCI_Cleanup(void);
  27  int    EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum);
  28 void    EHCI_InterruptHandler(int IRQ, void *Ptr);
  29 // -- API ---
  30 void    *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bInput, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  31 void    *EHCI_InitIsoch  (void *Ptr, int Endpoint, size_t MaxPacketSize);
  32 void    *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize);
  33 void    *EHCI_InitBulk   (void *Ptr, int Endpoint, size_t MaxPacketSize);
  34 void    EHCI_RemEndpoint(void *Ptr, void *Handle);
  35 void    *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
  36         int isOutbound,
  37         const void *SetupData, size_t SetupLength,
  38         const void *OutData, size_t OutLength,
  39         void *InData, size_t InLength
  40         );
  41 void    *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length);
  42 void    EHCI_FreeOp(void *Ptr, void *Handle);
  43 Uint32  EHCI_int_RootHub_FeatToMask(int Feat);
  44 void    EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat);
  45 void    EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat);
  46  int    EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag);
  47 // --- Internals ---
  48 tEHCI_qTD       *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length, tUSBHostCb Cb, void *CbData);
  49 void    EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD);
  50 void    EHCI_int_AppendTD(tEHCI_Controller *Cont, tEHCI_QH *QH, tEHCI_qTD *TD);
  51 tEHCI_QH        *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize);
  52 void    EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH);
  53 void    EHCI_int_InterruptThread(void *ControllerPtr);
  54
  55 // === GLOBALS ===
  56 MODULE_DEFINE(0, VERSION, USB_EHCI, EHCI_Initialise, NULL, "USB_Core", NULL);
  57 tEHCI_Controller        gaEHCI_Controllers[EHCI_MAX_CONTROLLERS];
  58 tUSBHostDef     gEHCI_HostDef = {
  59         .InitInterrupt = EHCI_InitInterrupt,
  60         .InitIsoch     = EHCI_InitIsoch,
  61         .InitControl   = EHCI_InitControl,
  62         .InitBulk      = EHCI_InitBulk,
  63         .RemEndpoint   = EHCI_RemEndpoint,
  64         .SendIsoch   = NULL,
  65         .SendControl = EHCI_SendControl,
  66         .SendBulk    = EHCI_SendBulk,
  67         .FreeOp      = EHCI_FreeOp,
  68
  69         .CheckPorts = NULL,     // No need
  70         .SetPortFeature   = EHCI_RootHub_SetPortFeature,
  71         .ClearPortFeature = EHCI_RootHub_ClearPortFeature,
  72         .GetPortStatus    = EHCI_RootHub_GetPortStatus,
  73         };
  74
  75 // === CODE ===
  76 int EHCI_Initialise(char **Arguments)
  77 {
  78         for( int id = -1; (id = PCI_GetDeviceByClass(0x0C0320, 0xFFFFFF, id)) >= 0;  )
  79         {
  80                 Uint32  addr = PCI_GetBAR(id, 0);
  81                 if( addr == 0 ) {
  82                         // TODO: PCI BIOS emulation time
  83                 }
  84                 if( addr & 1 ) {
  85                         // TODO: Error
  86                         continue ;
  87                 }
  88                 addr &= ~0xF;
  89                 Uint8   irq = PCI_GetIRQ(id);
  90                 if( irq == 0 ) {
  91                         // TODO: Error?
  92                 }
  93
  94                 Log_Log("ECHI", "Controller at PCI %i 0x%x IRQ 0x%x",
  95                         id, addr, irq);
  96
  97                 if( EHCI_InitController(addr, irq) )
  98                 {
  99                         // TODO: Detect other forms of failure than "out of slots"
 100                         break ;
 101                 }
 102         }
 103
 104         for( int i = 0; Arguments && Arguments[i]; i ++ )
 105         {
 106                 char *pos = Arguments[i], *next;
 107                 LOG("pos = '%s'", pos);
 108                 tPAddr base = strtoull(pos, &next, 16);
 109                 if( base == 0 )
 110                         continue;
 111                 pos = next;
 112                 LOG("pos = '%s'", pos);
 113                 if( *pos++ != '-' )
 114                         continue;
 115                 LOG("pos = '%s'", pos);
 116                 int irq = strtol(pos, &next, 16);
 117                 if( irq == 0 )
 118                         continue ;
 119                 if( *next != 0 )
 120                         continue;
 121                 LOG("base=%x, irq=%i", base, irq);
 122                 if( EHCI_InitController(base, irq) )
 123                 {
 124                         continue ;
 125                 }
 126         }
 127
 128         return 0;
 129 }
 130
 131 int EHCI_Cleanup(void)
 132 {
 133         return 0;
 134 }
 135
 136 // --- Driver Init ---
 137 int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 138 {
 139         tEHCI_Controller        *cont = NULL;
 140
 141         ENTER("PBaseAddress iInterruptNum",
 142                 BaseAddress, InterruptNum);
 143
 144         for( int i = 0; i < EHCI_MAX_CONTROLLERS; i ++ )
 145         {
 146                 if( gaEHCI_Controllers[i].PhysBase == 0 ) {
 147                         cont = &gaEHCI_Controllers[i];
 148                         cont->PhysBase = BaseAddress;
 149                         break;
 150                 }
 151         }
 152         if(!cont) {
 153                 Log_Notice("EHCI", "Too many controllers (EHCI_MAX_CONTROLLERS=%i)",
 154                         EHCI_MAX_CONTROLLERS);
 155                 LEAVE('i', 1);
 156                 return 1;
 157         }
 158
 159         // - Nuke a couple of fields so error handling code doesn't derp
 160         cont->CapRegs = NULL;
 161         cont->PeriodicQueue = NULL;
 162         cont->TDPool = NULL;
 163
 164         // -- Build up structure --
 165         cont->CapRegs = (void*)( MM_MapHWPages(BaseAddress, 1) + (BaseAddress % PAGE_SIZE) );
 166         if( !cont->CapRegs ) {
 167                 Log_Warning("EHCI", "Can't map 1 page at %P into kernel space", BaseAddress);
 168                 goto _error;
 169         }
 170         LOG("cont->CapRegs = %p", cont->CapRegs);
 171         // TODO: Error check
 172         if( (cont->CapRegs->CapLength & 3) ) {
 173                 Log_Warning("EHCI", "Controller at %P non-aligned op regs (%x)",
 174                         BaseAddress, cont->CapRegs->CapLength);
 175                 goto _error;
 176         }
 177         if( BaseAddress % PAGE_SIZE + cont->CapRegs->CapLength + sizeof(tEHCI_CapRegs) > PAGE_SIZE ) {
 178                 Log_Warning("EHCI", "%P: Cap regs over page boundary (+0x%x bytes)",
 179                          BaseAddress % PAGE_SIZE + cont->CapRegs->CapLength + sizeof(tEHCI_CapRegs)
 180                          );
 181                 goto _error;
 182         }
 183         cont->OpRegs = (void*)( (Uint32*)cont->CapRegs + cont->CapRegs->CapLength / 4 );
 184         LOG("cont->OpRegs = %p", cont->OpRegs);
 185         // - Allocate periodic queue
 186         tPAddr  unused;
 187         cont->PeriodicQueue = (void*)MM_AllocDMA(1, 32, &unused);
 188         if( !cont->PeriodicQueue ) {
 189                 Log_Warning("ECHI", "Can't allocate 1 32-bit page for periodic queue");
 190                 goto _error;
 191         }
 192         for( int i = 0; i < 1024; i ++ )
 193                 cont->PeriodicQueue[i] = 1;
 194         // TODO: Error check
 195         //  > Populate queue
 196
 197         // - Allocate TD pool
 198         cont->TDPool = (void*)MM_AllocDMA(1, 32, &unused);
 199         if( !cont->TDPool ) {
 200                 Log_Warning("ECHI", "Can't allocate 1 32-bit page for qTD pool");
 201                 goto _error;
 202         }
 203         for( int i = 0; i < TD_POOL_SIZE; i ++ ) {
 204                 cont->TDPool[i].Token = 3 << 8;
 205         }
 206
 207         // Get port count
 208         cont->nPorts = cont->CapRegs->HCSParams & 0xF;
 209         LOG("cont->nPorts = %i", cont->nPorts);
 210
 211         // -- Bind IRQ --
 212         IRQ_AddHandler(InterruptNum, EHCI_InterruptHandler, cont);
 213         cont->InterruptThread = Proc_SpawnWorker(EHCI_int_InterruptThread, cont);
 214         if( !cont->InterruptThread ) {
 215                 Log_Warning("EHCI", "Can't spawn interrupt worker thread");
 216                 goto _error;
 217         }
 218         LOG("cont->InterruptThread = %p", cont->InterruptThread);
 219
 220         // Dummy TD
 221         cont->DeadTD = EHCI_int_AllocateTD(cont, 0, NULL, 0, NULL, NULL);
 222         memset(cont->DeadTD, 0, sizeof(tEHCI_qTD));
 223         cont->DeadTD->Link = 1;
 224         cont->DeadTD->Link2 = 1;
 225         cont->DeadTD->Token = QTD_TOKEN_STS_HALT;
 226
 227         // Dummy QH
 228         cont->DeadQH = EHCI_int_AllocateQH(cont, 0, 0);
 229         memset(cont->DeadQH, 0, sizeof(tEHCI_QH));
 230         cont->DeadQH->HLink = MM_GetPhysAddr(cont->DeadQH)|2;
 231         cont->DeadQH->Endpoint = (1<<15);       // H - Head of Reclamation List
 232         cont->DeadQH->CurrentTD = MM_GetPhysAddr(cont->DeadTD);
 233         cont->DeadQH->Overlay.Link = MM_GetPhysAddr(cont->DeadTD);
 234
 235         // -- Initialisation procedure (from ehci-r10) --
 236         // - Reset controller
 237         cont->OpRegs->USBCmd = USBCMD_HCReset;
 238         // - Set CTRLDSSEGMENT (TODO: 64-bit support)
 239         // - Set USBINTR
 240         cont->OpRegs->USBIntr = USBINTR_IOC|USBINTR_PortChange|USBINTR_FrameRollover;
 241         // - Set PERIODICLIST BASE
 242         cont->OpRegs->PeridocListBase = MM_GetPhysAddr( cont->PeriodicQueue );
 243         // - Set ASYNCLISTADDR
 244         cont->OpRegs->AsyncListAddr = MM_GetPhysAddr(cont->DeadQH);
 245         // - Enable controller
 246         cont->OpRegs->USBCmd = (0x40 << 16) | USBCMD_PeriodicEnable | USBCMD_AsyncEnable | USBCMD_Run;
 247         // - Route all ports
 248         cont->OpRegs->ConfigFlag = 1;
 249
 250         // -- Register with USB Core --
 251         cont->RootHub = USB_RegisterHost(&gEHCI_HostDef, cont, cont->nPorts);
 252         LOG("cont->RootHub = %p", cont->RootHub);
 253
 254         LEAVE('i', 0);
 255         return 0;
 256 _error:
 257         cont->PhysBase = 0;
 258         if( cont->CapRegs )
 259                 MM_Deallocate( (tVAddr)cont->CapRegs );
 260         if( cont->PeriodicQueue )
 261                 MM_Deallocate( (tVAddr)cont->PeriodicQueue );
 262         if( cont->TDPool )
 263                 MM_Deallocate( (tVAddr)cont->TDPool );
 264         LEAVE('i', 2);
 265         return 2;
 266 }
 267
 268 void EHCI_InterruptHandler(int IRQ, void *Ptr)
 269 {
 270         tEHCI_Controller *Cont = Ptr;
 271         Uint32  sts = Cont->OpRegs->USBSts;
 272         Uint32  orig_sts = sts;
 273
 274         if( sts & 0xFFFF0FC0 ) {
 275                 LOG("Oops, reserved bits set (%08x), funny hardware?", sts);
 276                 sts &= ~0xFFFF0FFC0;
 277         }
 278
 279         // Unmask read-only bits
 280         sts &= ~(0xF000);
 281
 282         if( sts & USBINTR_IOC ) {
 283                 // IOC
 284                 LOG("%P IOC", Cont->PhysBase);
 285                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IOC);
 286                 sts &= ~USBINTR_IOC;
 287         }
 288
 289         if( sts & USBINTR_AsyncAdvance ) {
 290                 LOG("%P IAAD", Cont->PhysBase);
 291                 #if 0
 292                 if( Cont->AsyncQHAddHead )
 293                 {
 294                 }
 295                 #endif
 296                 sts &= ~USBINTR_AsyncAdvance;
 297         }
 298
 299         if( sts & USBINTR_PortChange ) {
 300                 // Port change, determine what port and poke helper thread
 301                 LOG("%P Port status change", Cont->PhysBase);
 302                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_PORTSC);
 303                 sts &= ~USBINTR_PortChange;
 304         }
 305
 306         if( sts & USBINTR_FrameRollover ) {
 307                 // Frame rollover, used to aid timing (trigger per-second operations)
 308                 //LOG("%p Frame rollover", Ptr);
 309                 sts &= ~USBINTR_FrameRollover;
 310         }
 311
 312         if( sts ) {
 313                 // Unhandled interupt bits
 314                 // TODO: Warn
 315                 LOG("WARN - Bitmask %x unhandled", sts);
 316         }
 317
 318
 319         // Clear interrupts
 320         Cont->OpRegs->USBSts = orig_sts;
 321 }
 322
 323 // --------------------------------------------------------------------
 324 // USB API
 325 // --------------------------------------------------------------------
 326 void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 327         tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 328 {
 329         tEHCI_Controller        *Cont = Ptr;
 330          int    pow2period, period_pow;
 331
 332         ASSERTCR(Endpoint, <, 256*16, NULL);
 333         ASSERTCR(Period, >, 0, NULL);
 334         if( Period > 256 )
 335                 Period = 256;
 336
 337         LOG("Endpoint=%x, bOutbound=%i, Period=%i, Length=%i", Endpoint, bOutbound, Period, Length);
 338
 339         // Round the period to the closest power of two
 340         pow2period = 1;
 341         period_pow = 0;
 342         // - Find the first power above the period
 343         while( pow2period < Period )
 344         {
 345                 pow2period *= 2;
 346                 period_pow ++;
 347         }
 348         // - Check which is closest
 349         if( Period - pow2period / 2 > pow2period - Period )
 350                 Period = pow2period;
 351         else {
 352                 Period = pow2period/2;
 353                 period_pow --;
 354         }
 355         LOG("period_pow = %i, Period = %ims", period_pow, Period);
 356
 357         // Allocate a QH
 358         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, Endpoint, Length);
 359         qh->Impl.IntPeriodPow = period_pow;
 360         qh->EndpointExt |= 1;   // TODO: uFrame load balancing (8 entry bitfield)
 361
 362         Mutex_Acquire(&Cont->PeriodicListLock);
 363
 364         // Choose an interrupt slot to use
 365         int minslot = 0, minslot_load = INT_MAX;
 366         for( int slot = 0; slot < Period; slot ++ )
 367         {
 368                  int    load = 0;
 369                 for( int i = 0; i < PERIODIC_SIZE; i += Period )
 370                         load += Cont->InterruptLoad[i+slot];
 371                 if( load == 0 ) break;
 372                 if( load < minslot_load ) {
 373                         minslot = slot;
 374                         minslot_load = load;
 375                 }
 376         }
 377         // Increase loading on the selected slot
 378         for( int i = minslot; i < PERIODIC_SIZE; i += Period )
 379                 Cont->InterruptLoad[i] += Length;
 380         qh->Impl.IntOfs = minslot;
 381         LOG("Using slot %i", minslot);
 382
 383         // Allocate TD for the data
 384         tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length, Cb, CbData);
 385         EHCI_int_AppendTD(Cont, qh, td);
 386
 387         // Insert into the periodic list
 388         for( int i = 0; i < PERIODIC_SIZE; i += Period )
 389         {
 390                 // Walk list until
 391                 // - the end is reached
 392                 // - this QH is found
 393                 // - A QH with a lower period is encountered
 394                 tEHCI_QH        *pqh = NULL;
 395                 tEHCI_QH        *nqh;
 396                 for( nqh = Cont->PeriodicQueueV[i]; nqh; pqh = nqh, nqh = nqh->Impl.Next )
 397                 {
 398                         if( nqh == qh )
 399                                 break;
 400                         if( nqh->Impl.IntPeriodPow < period_pow )
 401                                 break;
 402                 }
 403
 404                 // Somehow, we've already been added to this queue.
 405                 if( nqh && nqh == qh )
 406                         continue ;
 407
 408                 if( qh->Impl.Next && qh->Impl.Next != nqh ) {
 409                         Log_Warning("EHCI", "Suspected bookkeeping error on %p - int list %i+%i overlap",
 410                                 Cont, period_pow, minslot);
 411                         break;
 412                 }
 413
 414                 if( nqh ) {
 415                         qh->Impl.Next = nqh;
 416                         qh->HLink = MM_GetPhysAddr(nqh) | 2;
 417                 }
 418                 else {
 419                         qh->Impl.Next = NULL;
 420                         qh->HLink = 2|1;        // QH, Terminate
 421                 }
 422
 423                 if( pqh ) {
 424                         pqh->Impl.Next = qh;
 425                         pqh->HLink = MM_GetPhysAddr(qh) | 2;
 426                 }
 427                 else {
 428                         Cont->PeriodicQueueV[i] = qh;
 429                         Cont->PeriodicQueue[i] = MM_GetPhysAddr(qh) | 2;
 430                 }
 431         }
 432         Mutex_Release(&Cont->PeriodicListLock);
 433
 434         return qh;
 435 }
 436
 437 void *EHCI_InitIsoch(void *Ptr, int Endpoint, size_t MaxPacketSize)
 438 {
 439         ENTER("pPtr iEndpoint iMaxPacketSize",
 440                 Ptr, Endpoint, MaxPacketSize);
 441         LEAVE_RET('p', (void*)(tVAddr)(Endpoint + 1));
 442 }
 443 void *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize)
 444 {
 445         tEHCI_Controller *Cont = Ptr;
 446
 447         ENTER("pPtr iEndpoint iMaxPacketSize",
 448                 Ptr, Endpoint, MaxPacketSize);
 449
 450         // Allocate a QH
 451         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, Endpoint, MaxPacketSize);
 452         qh->CurrentTD = MM_GetPhysAddr(Cont->DeadTD);
 453
 454         // Append to async list
 455         // TODO: Lock async list
 456         qh->Impl.Next = Cont->DeadQH->Impl.Next;
 457         Cont->DeadQH->Impl.Next = qh;
 458         qh->HLink = Cont->DeadQH->HLink;
 459         Cont->DeadQH->HLink = MM_GetPhysAddr(qh)|2;
 460
 461         LOG("Created %p(%P) for %P Ep 0x%x - %i bytes MPS",
 462                 qh, MM_GetPhysAddr(qh), Cont->PhysBase, Endpoint, MaxPacketSize);
 463
 464         LEAVE('p', qh);
 465         return qh;
 466 }
 467 void *EHCI_InitBulk(void *Ptr, int Endpoint, size_t MaxPacketSize)
 468 {
 469         return EHCI_InitControl(Ptr, Endpoint, MaxPacketSize);
 470 }
 471 void EHCI_RemEndpoint(void *Ptr, void *Handle)
 472 {
 473         if( Handle == NULL )
 474                 return ;
 475         else if( (tVAddr)Handle <= 256*16 )
 476                 return ;        // Isoch
 477         else {
 478                 tEHCI_QH        *qh = Ptr;
 479
 480                 // Remove QH from list
 481                 // - If it's a polling endpoint, need to remove from all periodic lists
 482                 if( qh->Impl.IntPeriodPow != 0xFF) {
 483                         // Poll
 484                 }
 485                 else {
 486                         // GP
 487                 }
 488
 489                 // Deallocate QH
 490                 EHCI_int_DeallocateQH(Ptr, Handle);
 491         }
 492 }
 493
 494 void *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
 495         int isOutbound,
 496         const void *SetupData, size_t SetupLength,
 497         const void *OutData, size_t OutLength,
 498         void *InData, size_t InLength
 499         )
 500 {
 501         tEHCI_Controller *Cont = Ptr;
 502         tEHCI_qTD       *td_setup, *td_data, *td_status;
 503
 504         // Sanity checks
 505         if( (tVAddr)Dest <= 256*16 )
 506                 return NULL;
 507
 508         LOG("Dest=%p, isOutbound=%i, Lengths(Setup:%i,Out:%i,In:%i)",
 509                 Dest, isOutbound, SetupLength, OutLength, InLength);
 510
 511         // TODO: Check size of SETUP and status
 512
 513         // Allocate TDs
 514         td_setup = EHCI_int_AllocateTD(Cont, PID_SETUP, (void*)SetupData, SetupLength, NULL, NULL);
 515         if( isOutbound )
 516         {
 517                 td_data = OutData ? EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength, NULL, NULL) : NULL;
 518                 td_status = EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength, Cb, CbData);
 519         }
 520         else
 521         {
 522                 td_data = InData ? EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength, NULL, NULL) : NULL;
 523                 td_status = EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength, Cb, CbData);
 524         }
 525         td_status->Token |= QTD_TOKEN_IOC;      // IOC
 526
 527         // Append TDs
 528         if( td_data ) {
 529                 td_setup->Link = MM_GetPhysAddr(td_data);
 530                 td_setup->Next = td_data;
 531                 td_data->Link = MM_GetPhysAddr(td_status);
 532                 td_data->Next = td_status;
 533                 td_data->Token |= QTD_TOKEN_STS_ACTIVE; // Active
 534         }
 535         else {
 536                 td_setup->Link = MM_GetPhysAddr(td_status);
 537                 td_setup->Next = td_status;
 538         }
 539         td_setup->Token |= QTD_TOKEN_STS_ACTIVE;        // Active
 540         td_status->Token |= QTD_TOKEN_STS_ACTIVE;
 541         td_status->Link = 1;
 542         td_status->Link2 = 1;
 543         EHCI_int_AppendTD(Cont, Dest, td_setup);
 544
 545         LOG("return td_status=%p", td_status);
 546         return td_status;
 547 }
 548
 549 void *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length)
 550 {
 551         tEHCI_Controller        *Cont = Ptr;
 552
 553         // Sanity check the pointer
 554         // - Can't be NULL or an isoch
 555         if( (tVAddr)Dest <= 256*16 )
 556                 return NULL;
 557
 558         // Allocate single TD
 559         tEHCI_qTD       *td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length, Cb, CbData);
 560         EHCI_int_AppendTD(Cont, Dest, td);
 561
 562         return td;
 563 }
 564
 565 void EHCI_FreeOp(void *Ptr, void *Handle)
 566 {
 567         tEHCI_Controller        *Cont = Ptr;
 568
 569         EHCI_int_DeallocateTD(Cont, Handle);
 570 }
 571
 572 Uint32 EHCI_int_RootHub_FeatToMask(int Feat)
 573 {
 574         switch(Feat)
 575         {
 576         case PORT_RESET:        return PORTSC_PortReset;
 577         case PORT_ENABLE:       return PORTSC_PortEnabled;
 578         default:
 579                 Log_Warning("EHCI", "Unknown root hub port feature %i", Feat);
 580                 return 0;
 581         }
 582 }
 583
 584 void EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat)
 585 {
 586         tEHCI_Controller        *Cont = Ptr;
 587         if(Port >= Cont->nPorts)        return;
 588
 589         Cont->OpRegs->PortSC[Port] |= EHCI_int_RootHub_FeatToMask(Feat);
 590 }
 591
 592 void EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat)
 593 {
 594         tEHCI_Controller        *Cont = Ptr;
 595         if(Port >= Cont->nPorts)        return;
 596
 597         Cont->OpRegs->PortSC[Port] &= ~EHCI_int_RootHub_FeatToMask(Feat);
 598 }
 599
 600 int EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag)
 601 {
 602         tEHCI_Controller        *Cont = Ptr;
 603         if(Port >= Cont->nPorts)        return 0;
 604
 605         return !!(Cont->OpRegs->PortSC[Port] & EHCI_int_RootHub_FeatToMask(Flag));
 606 }
 607
 608 // --------------------------------------------------------------------
 609 // Internals
 610 // --------------------------------------------------------------------
 611 tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
 612 {
 613         if( Addr == 0 ) return NULL;
 614         LOG("%p + (%x - %x)", Cont->TDPool, Addr, MM_GetPhysAddr(Cont->TDPool));
 615         return Cont->TDPool + (Addr - MM_GetPhysAddr(Cont->TDPool))/sizeof(tEHCI_qTD);
 616 }
 617
 618 tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length, tUSBHostCb Cb, void *CbData)
 619 {
 620 //      Semaphore_Wait(&Cont->TDSemaphore, 1);
 621         Mutex_Acquire(&Cont->TDPoolMutex);
 622         for( int i = 0; i < TD_POOL_SIZE; i ++ )
 623         {
 624                 if( ((Cont->TDPool[i].Token >> 8) & 3) != 3 )
 625                         continue ;
 626                 Cont->TDPool[i].Token = (PID << 8) | (Length << 16);
 627                 Mutex_Release(&Cont->TDPoolMutex);
 628
 629                 tEHCI_qTD       *td = &Cont->TDPool[i];
 630                 td->Size = Length;
 631                 td->Callback = Cb;
 632                 td->CallbackData = CbData;
 633                 // NOTE: Assumes that `Length` is <= PAGE_SIZE
 634                 ASSERTC(Length, <, PAGE_SIZE);
 635                 // TODO: Handle bouncing >32-bit pages
 636                 #if PHYS_BITS > 32
 637                 ASSERT( MM_GetPhysAddr(Data) >> 32 == 0 );
 638                 #endif
 639                 td->Pages[0] = MM_GetPhysAddr(Data);
 640                 if( (td->Pages[0] & (PAGE_SIZE-1)) + Length - 1 > PAGE_SIZE )
 641                         td->Pages[1] = MM_GetPhysAddr((char*)Data + Length - 1) & ~(PAGE_SIZE-1);
 642                 LOG("Allocated %p(%P) for PID %i on %P",
 643                         td, MM_GetPhysAddr(td), PID, Cont->PhysBase);
 644                 return td;
 645         }
 646
 647         Mutex_Release(&Cont->TDPoolMutex);
 648         return NULL;
 649 }
 650
 651 void EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD)
 652 {
 653         UNIMPLEMENTED();
 654 }
 655
 656 void EHCI_int_AppendTD(tEHCI_Controller *Cont, tEHCI_QH *QH, tEHCI_qTD *TD)
 657 {
 658         tEHCI_qTD       *ptd = NULL;
 659         Uint32  link = QH->CurrentTD;
 660         tPAddr  dead_td = MM_GetPhysAddr(Cont->DeadTD);
 661
 662         {
 663                 Mutex_Acquire(&Cont->ActiveTDsLock);
 664                 if( Cont->ActiveTDTail )
 665                         Cont->ActiveTDTail->Next = TD;
 666                 else
 667                         Cont->ActiveTDHead = TD;
 668                 tEHCI_qTD       *last;
 669                 for( last = TD; last->Next; last = last->Next )
 670                         ;
 671                 Cont->ActiveTDTail = last;
 672                 Mutex_Release(&Cont->ActiveTDsLock);
 673         }
 674
 675         // TODO: Need locking and validation here
 676         while( link && !(link & 1) && link != dead_td )
 677         {
 678                 ptd = EHCI_int_GetTDFromPhys(Cont, link);
 679                 link = ptd->Link;
 680         }
 681         // TODO: Figure out how to follow this properly
 682         if( !ptd ) {
 683                 QH->CurrentTD = MM_GetPhysAddr(TD)|2;
 684                 QH->Overlay.Link = QH->CurrentTD;
 685                 LOG("Appended %p to beginning of %p", TD, QH);
 686         }
 687         else {
 688                 ptd->Link = MM_GetPhysAddr(TD);
 689                 ptd->Link2 = MM_GetPhysAddr(TD);
 690                 LOG("Appended %p to end of %p", TD, QH);
 691         }
 692         QH->Endpoint |= QH_ENDPT_H;
 693         QH->Overlay.Token &= ~QTD_TOKEN_STS_HALT;
 694 }
 695
 696 tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize)
 697 {
 698         tEHCI_QH        *ret;
 699         Mutex_Acquire(&Cont->QHPoolMutex);
 700         for( int i = 0; i < QH_POOL_SIZE; i ++ )
 701         {
 702                 if( !MM_GetPhysAddr( Cont->QHPools[i/QH_POOL_NPERPAGE] ) ) {
 703                         tPAddr  tmp;
 704                         Cont->QHPools[i/QH_POOL_NPERPAGE] = (void*)MM_AllocDMA(1, 32, &tmp);
 705                         memset(Cont->QHPools[i/QH_POOL_NPERPAGE], 0, PAGE_SIZE);
 706                 }
 707
 708                 ret = &Cont->QHPools[i/QH_POOL_NPERPAGE][i%QH_POOL_NPERPAGE];
 709                 if( ret->HLink == 0 ) {
 710                         memset(ret, 0, sizeof(*ret));
 711                         ret->HLink = 1;
 712                         ret->Overlay.Link = MM_GetPhysAddr(Cont->DeadTD);
 713                         ret->Endpoint = (Endpoint >> 4) | 0x80 | ((Endpoint & 0xF) << 8)
 714                                 | (MaxPacketSize << 16);
 715                         ret->EndpointExt = (1<<30);
 716                         // TODO: Endpoint speed (13:12) 0:Full, 1:Low, 2:High
 717                         // TODO: Control Endpoint Flag (27) 0:*, 1:Full/Low Control
 718                         Mutex_Release(&Cont->QHPoolMutex);
 719                         return ret;
 720                 }
 721         }
 722         Mutex_Release(&Cont->QHPoolMutex);
 723         return NULL;
 724 }
 725
 726 void EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH)
 727 {
 728         // TODO: Ensure it's unused (somehow)
 729         QH->HLink = 0;
 730 }
 731
 732 void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
 733 {
 734         // Connect Event
 735         if( Cont->OpRegs->PortSC[Port] & PORTSC_CurrentConnectStatus )
 736         {
 737                 // Is the device low-speed?
 738                 if( (Cont->OpRegs->PortSC[Port] & PORTSC_LineStatus_MASK) == PORTSC_LineStatus_Kstate )
 739                 {
 740                         LOG("Low speed device on %P Port %i, giving to companion", Cont->PhysBase, Port);
 741                         Cont->OpRegs->PortSC[Port] |= PORTSC_PortOwner;
 742                 }
 743                 // not a low-speed device, EHCI reset
 744                 else
 745                 {
 746                         LOG("Device connected on %P Port %i", Cont->PhysBase, Port);
 747                         // Reset procedure.
 748                         USB_PortCtl_BeginReset(Cont->RootHub, Port);
 749                 }
 750         }
 751         // Disconnect Event
 752         else
 753         {
 754                 if( Cont->OpRegs->PortSC[Port] & PORTSC_PortOwner ) {
 755                         LOG("Device disconnected on %P Port %i (companion), taking it back",
 756                                 Cont->PhysBase, Port);
 757                         Cont->OpRegs->PortSC[Port] &= ~PORTSC_PortOwner;
 758                 }
 759                 else {
 760                         LOG("Device disconnected on %P Port %i", Cont->PhysBase, Port);
 761                         USB_DeviceDisconnected(Cont->RootHub, Port);
 762                 }
 763         }
 764 }
 765
 766 void EHCI_int_InterruptThread(void *ControllerPtr)
 767 {
 768         tEHCI_Controller        *Cont = ControllerPtr;
 769         while(Cont->OpRegs)
 770         {
 771                 Uint32  events;
 772
 773                 LOG("sleeping for events");
 774                 events = Threads_WaitEvents(EHCI_THREADEVENT_IOC|EHCI_THREADEVENT_PORTSC);
 775                 if( !events ) {
 776                         // TODO: Should this cause a termination?
 777                 }
 778                 LOG("events = 0x%x", events);
 779
 780                 if( events & EHCI_THREADEVENT_IOC )
 781                 {
 782                         // IOC, Do whatever it is you do
 783                         Log_Warning("EHCI", "%P IOC - TODO: Call registered callbacks and reclaim",
 784                                 Cont->PhysBase);
 785                         // Scan active TDs
 786                         Mutex_Acquire(&Cont->ActiveTDsLock);
 787                         tEHCI_qTD *prev = NULL;
 788                         for(tEHCI_qTD *td = Cont->ActiveTDHead; td; td = td->Next)
 789                         {
 790                                 LOG("td(%p)->Token = %x", td, td->Token);
 791                                 // If active, continue
 792                                 if( td->Token & QTD_TOKEN_STS_ACTIVE ) {
 793                                         prev = td;
 794                                         continue ;
 795                                 }
 796
 797                                 // Inactive
 798                                 LOG("%p Complete", td);
 799                                 // - call the callback
 800                                 if( td->Callback )
 801                                 {
 802                                         void *ptr = NULL;
 803                                         if( td->Pages[0] ) {
 804                                                 Log_Warning("EHCI", "TODO: Map %x,%x+%i for callback",
 805                                                         td->Pages[0], td->Pages[1], td->Size);
 806                                         }
 807                                         td->Callback(td->CallbackData, ptr, td->Size);
 808                                 }
 809
 810                                 // Remove and release
 811                                 *(prev ? &prev->Next : &Cont->ActiveTDHead) = td->Next;
 812                                 td->Token = 0;
 813                         }
 814                         Cont->ActiveTDTail = prev;
 815                         Mutex_Release(&Cont->ActiveTDsLock);
 816                 }
 817
 818                 // Port status change interrupt
 819                 if( events & EHCI_THREADEVENT_PORTSC )
 820                 {
 821                         // Check for port status changes
 822                         for(int i = 0; i < Cont->nPorts; i ++ )
 823                         {
 824                                 Uint32  sts = Cont->OpRegs->PortSC[i];
 825                                 LOG("Port %i: sts = %x", i, sts);
 826                                 Cont->OpRegs->PortSC[i] = sts;
 827                                 if( sts & PORTSC_ConnectStatusChange )
 828                                         EHCI_int_HandlePortConnectChange(Cont, i);
 829
 830                                 if( sts & PORTSC_PortEnableChange )
 831                                 {
 832                                         // Handle enable/disable
 833                                 }
 834
 835                                 if( sts & PORTSC_OvercurrentChange )
 836                                 {
 837                                         // Handle over-current change
 838                                 }
 839                         }
 840                 }
 841
 842                 LOG("Going back to sleep");
 843         }
 844 }