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                         // Oops, PCI BIOS emulation time
  83                 }
  84                 Uint8   irq = PCI_GetIRQ(id);
  85                 if( irq == 0 ) {
  86                         // TODO: The same
  87                 }
  88
  89                 Log_Log("ECHI", "Controller at PCI %i 0x%x IRQ 0x%x",
  90                         id, addr, irq);
  91
  92                 if( EHCI_InitController(addr, irq) )
  93                 {
  94                         // TODO: Detect other forms of failure than "out of slots"
  95                         break ;
  96                 }
  97
  98                 // TODO: Register with the USB stack
  99         }
 100         return 0;
 101 }
 102
 103 int EHCI_Cleanup(void)
 104 {
 105         return 0;
 106 }
 107
 108 // --- Driver Init ---
 109 int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 110 {
 111         tEHCI_Controller        *cont = NULL;
 112
 113         for( int i = 0; i < EHCI_MAX_CONTROLLERS; i ++ )
 114         {
 115                 if( gaEHCI_Controllers[i].PhysBase == 0 ) {
 116                         cont = &gaEHCI_Controllers[i];
 117                         cont->PhysBase = BaseAddress;
 118                         break;
 119                 }
 120         }
 121         if(!cont) {
 122                 Log_Notice("EHCI", "Too many controllers (EHCI_MAX_CONTROLLERS=%i)",
 123                         EHCI_MAX_CONTROLLERS);
 124                 return 1;
 125         }
 126
 127         // - Nuke a couple of fields so error handling code doesn't derp
 128         cont->CapRegs = NULL;
 129         cont->PeriodicQueue = NULL;
 130         cont->TDPool = NULL;
 131
 132         // -- Build up structure --
 133         cont->CapRegs = (void*)MM_MapHWPages(BaseAddress, 1);
 134         if( !cont->CapRegs ) {
 135                 Log_Warning("EHCI", "Can't map 1 page at %P into kernel space", BaseAddress);
 136                 goto _error;
 137         }
 138         // TODO: Error check
 139         if( (cont->CapRegs->CapLength & 3) ) {
 140                 Log_Warning("EHCI", "Controller at %P non-aligned op regs", BaseAddress);
 141                 goto _error;
 142         }
 143         cont->OpRegs = (void*)( (Uint32*)cont->CapRegs + cont->CapRegs->CapLength / 4 );
 144         // - Allocate periodic queue
 145         tPAddr  unused;
 146         cont->PeriodicQueue = (void*)MM_AllocDMA(1, 32, &unused);
 147         if( !cont->PeriodicQueue ) {
 148                 Log_Warning("ECHI", "Can't allocate 1 32-bit page for periodic queue");
 149                 goto _error;
 150         }
 151         for( int i = 0; i < 1024; i ++ )
 152                 cont->PeriodicQueue[i] = 1;
 153         // TODO: Error check
 154         //  > Populate queue
 155
 156         // - Allocate TD pool
 157         cont->TDPool = (void*)MM_AllocDMA(1, 32, &unused);
 158         if( !cont->TDPool ) {
 159                 Log_Warning("ECHI", "Can't allocate 1 32-bit page for qTD pool");
 160                 goto _error;
 161         }
 162         for( int i = 0; i < TD_POOL_SIZE; i ++ ) {
 163                 cont->TDPool[i].Token = 3 << 8;
 164         }
 165
 166         // Get port count
 167         cont->nPorts = cont->CapRegs->HCSParams & 0xF;
 168
 169         // -- Bind IRQ --
 170         IRQ_AddHandler(InterruptNum, EHCI_InterruptHandler, cont);
 171         cont->InterruptThread = Proc_SpawnWorker(EHCI_int_InterruptThread, cont);
 172         if( !cont->InterruptThread ) {
 173                 Log_Warning("EHCI", "Can't spawn interrupt worker thread");
 174                 goto _error;
 175         }
 176         LOG("cont->InterruptThread = %p", cont->InterruptThread);
 177
 178         // -- Initialisation procedure (from ehci-r10) --
 179         // - Reset controller
 180         cont->OpRegs->USBCmd = USBCMD_HCReset;
 181         // - Set CTRLDSSEGMENT (TODO: 64-bit support)
 182         // - Set USBINTR
 183         cont->OpRegs->USBIntr = USBINTR_IOC|USBINTR_PortChange|USBINTR_FrameRollover;
 184         // - Set PERIODICLIST BASE
 185         cont->OpRegs->PeridocListBase = MM_GetPhysAddr( cont->PeriodicQueue );
 186         // - Enable controller
 187         cont->OpRegs->USBCmd = (0x40 << 16) | USBCMD_PeriodicEnable | USBCMD_Run;
 188         // - Route all ports
 189         cont->OpRegs->ConfigFlag = 1;
 190
 191         cont->DeadTD = EHCI_int_AllocateTD(cont, 0, NULL, 0, NULL, NULL);
 192         cont->DeadTD->Link = 1;
 193         cont->DeadTD->Link2 = 1;
 194         cont->DeadTD->Token = 0;
 195
 196         // -- Register with USB Core --
 197         cont->RootHub = USB_RegisterHost(&gEHCI_HostDef, cont, cont->nPorts);
 198
 199         return 0;
 200 _error:
 201         cont->PhysBase = 0;
 202         if( cont->CapRegs )
 203                 MM_Deallocate( (tVAddr)cont->CapRegs );
 204         if( cont->PeriodicQueue )
 205                 MM_Deallocate( (tVAddr)cont->PeriodicQueue );
 206         if( cont->TDPool )
 207                 MM_Deallocate( (tVAddr)cont->TDPool );
 208         return 2;
 209 }
 210
 211 void EHCI_InterruptHandler(int IRQ, void *Ptr)
 212 {
 213         tEHCI_Controller *Cont = Ptr;
 214         Uint32  sts = Cont->OpRegs->USBSts;
 215
 216         // Clear interrupts
 217         Cont->OpRegs->USBSts = sts;
 218
 219         if( sts & 0xFFFF0FC0 ) {
 220                 LOG("Oops, reserved bits set (%08x), funny hardware?", sts);
 221                 sts &= ~0xFFFF0FFC0;
 222         }
 223
 224         // Unmask read-only bits
 225         sts &= ~(0xF000);
 226
 227         if( sts & USBINTR_IOC ) {
 228                 // IOC
 229                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IOC);
 230                 sts &= ~USBINTR_IOC;
 231         }
 232
 233         if( sts & USBINTR_PortChange ) {
 234                 // Port change, determine what port and poke helper thread
 235                 LOG("Port status change");
 236                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_PORTSC);
 237                 sts &= ~USBINTR_PortChange;
 238         }
 239
 240         if( sts & USBINTR_FrameRollover ) {
 241                 // Frame rollover, used to aid timing (trigger per-second operations)
 242                 LOG("Frame rollover");
 243                 sts &= ~USBINTR_FrameRollover;
 244         }
 245
 246         if( sts ) {
 247                 // Unhandled interupt bits
 248                 // TODO: Warn
 249                 LOG("WARN - Bitmask %x unhandled", sts);
 250         }
 251
 252
 253 }
 254
 255 // --------------------------------------------------------------------
 256 // USB API
 257 // --------------------------------------------------------------------
 258 void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 259         tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 260 {
 261         tEHCI_Controller        *Cont = Ptr;
 262          int    pow2period, period_pow;
 263
 264         if( Endpoint >= 256*16 )
 265                 return NULL;
 266         if( Period <= 0 )
 267                 return NULL;
 268         if( Period > 256 )
 269                 Period = 256;
 270
 271         // Round the period to the closest power of two
 272         pow2period = 1;
 273         period_pow = 0;
 274         // - Find the first power above the period
 275         while( pow2period < Period )
 276         {
 277                 pow2period *= 2;
 278                 period_pow ++;
 279         }
 280         // - Check which is closest
 281         if( Period - pow2period / 2 > pow2period - Period )
 282                 Period = pow2period;
 283         else {
 284                 Period = pow2period/2;
 285                 period_pow --;
 286         }
 287
 288         // Allocate a QH
 289         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, Endpoint, Length);
 290         qh->Impl.IntPeriodPow = period_pow;
 291
 292         Mutex_Acquire(&Cont->PeriodicListLock);
 293
 294         // Choose an interrupt slot to use
 295         int minslot = 0, minslot_load = INT_MAX;
 296         for( int slot = 0; slot < Period; slot ++ )
 297         {
 298                  int    load = 0;
 299                 for( int i = 0; i < PERIODIC_SIZE; i += Period )
 300                         load += Cont->InterruptLoad[i+slot];
 301                 if( load == 0 ) break;
 302                 if( load < minslot_load ) {
 303                         minslot = slot;
 304                         minslot_load = load;
 305                 }
 306         }
 307         // Increase loading on the selected slot
 308         for( int i = minslot; i < PERIODIC_SIZE; i += Period )
 309                 Cont->InterruptLoad[i] += Length;
 310         qh->Impl.IntOfs = minslot;
 311
 312         // Allocate TD for the data
 313         tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length, Cb, CbData);
 314         EHCI_int_AppendTD(Cont, qh, td);
 315
 316         // Insert into the periodic list
 317         for( int i = 0; i < PERIODIC_SIZE; i += Period )
 318         {
 319                 // Walk list until
 320                 // - the end is reached
 321                 // - this QH is found
 322                 // - A QH with a lower period is encountered
 323                 tEHCI_QH        *pqh = NULL;
 324                 tEHCI_QH        *nqh;
 325                 for( nqh = Cont->PeriodicQueueV[i]; nqh; pqh = nqh, nqh = nqh->Impl.Next )
 326                 {
 327                         if( nqh == qh )
 328                                 break;
 329                         if( nqh->Impl.IntPeriodPow < period_pow )
 330                                 break;
 331                 }
 332
 333                 // Somehow, we've already been added to this queue.
 334                 if( nqh && nqh == qh )
 335                         continue ;
 336
 337                 if( qh->Impl.Next && qh->Impl.Next != nqh ) {
 338                         Log_Warning("EHCI", "Suspected bookkeeping error on %p - int list %i+%i overlap",
 339                                 Cont, period_pow, minslot);
 340                         break;
 341                 }
 342
 343                 if( nqh ) {
 344                         qh->Impl.Next = nqh;
 345                         qh->HLink = MM_GetPhysAddr(nqh) | 2;
 346                 }
 347                 else {
 348                         qh->Impl.Next = NULL;
 349                         qh->HLink = 2|1;        // QH, Terminate
 350                 }
 351
 352                 if( pqh ) {
 353                         pqh->Impl.Next = qh;
 354                         pqh->HLink = MM_GetPhysAddr(qh) | 2;
 355                 }
 356                 else {
 357                         Cont->PeriodicQueueV[i] = qh;
 358                         Cont->PeriodicQueue[i] = MM_GetPhysAddr(qh) | 2;
 359                 }
 360         }
 361         Mutex_Release(&Cont->PeriodicListLock);
 362
 363         return qh;
 364 }
 365
 366 void *EHCI_InitIsoch(void *Ptr, int Endpoint, size_t MaxPacketSize)
 367 {
 368         return (void*)(tVAddr)(Endpoint + 1);
 369 }
 370 void *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize)
 371 {
 372         tEHCI_Controller *Cont = Ptr;
 373
 374         // Allocate a QH
 375         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, Endpoint, MaxPacketSize);
 376         qh->CurrentTD = MM_GetPhysAddr(Cont->DeadTD);
 377
 378         // Append to async list
 379         if( Cont->LastAsyncHead ) {
 380                 Cont->LastAsyncHead->HLink = MM_GetPhysAddr(qh)|2;
 381                 Cont->LastAsyncHead->Impl.Next = qh;
 382                 LOG("- Placed after %p", Cont->LastAsyncHead);
 383         }
 384         else {
 385                 Cont->OpRegs->AsyncListAddr = MM_GetPhysAddr(qh)|2;
 386         }
 387         qh->HLink = Cont->OpRegs->AsyncListAddr;
 388         Cont->OpRegs->USBCmd |= USBCMD_AsyncEnable;
 389         Cont->LastAsyncHead = qh;
 390
 391         LOG("Created %p for %p Ep 0x%x - %i bytes MPS", qh, Ptr, Endpoint, MaxPacketSize);
 392
 393         return qh;
 394 }
 395 void *EHCI_InitBulk(void *Ptr, int Endpoint, size_t MaxPacketSize)
 396 {
 397         return EHCI_InitControl(Ptr, Endpoint, MaxPacketSize);
 398 }
 399 void EHCI_RemEndpoint(void *Ptr, void *Handle)
 400 {
 401         if( Handle == NULL )
 402                 return ;
 403         else if( (tVAddr)Handle <= 256*16 )
 404                 return ;        // Isoch
 405         else {
 406                 tEHCI_QH        *qh = Ptr;
 407
 408                 // Remove QH from list
 409                 // - If it's a polling endpoint, need to remove from all periodic lists
 410                 if( qh->Impl.IntPeriodPow != 0xFF) {
 411                         // Poll
 412                 }
 413                 else {
 414                         // GP
 415                 }
 416
 417                 // Deallocate QH
 418                 EHCI_int_DeallocateQH(Ptr, Handle);
 419         }
 420 }
 421
 422 void *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
 423         int isOutbound,
 424         const void *SetupData, size_t SetupLength,
 425         const void *OutData, size_t OutLength,
 426         void *InData, size_t InLength
 427         )
 428 {
 429         tEHCI_Controller *Cont = Ptr;
 430         tEHCI_qTD       *td_setup, *td_data, *td_status;
 431
 432         // Sanity checks
 433         if( (tVAddr)Dest <= 256*16 )
 434                 return NULL;
 435
 436         // Check size of SETUP and status
 437
 438         // Allocate TDs
 439         td_setup = EHCI_int_AllocateTD(Cont, PID_SETUP, (void*)SetupData, SetupLength, NULL, NULL);
 440         if( isOutbound )
 441         {
 442                 td_data = OutData ? EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength, NULL, NULL) : NULL;
 443                 td_status = EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength, Cb, CbData);
 444         }
 445         else
 446         {
 447                 td_data = InData ? EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength, NULL, NULL) : NULL;
 448                 td_status = EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength, Cb, CbData);
 449                 td_status->Token |= (1 << 15);  // IOC
 450         }
 451
 452         // Append TDs
 453         if( td_data ) {
 454                 td_setup->Link = MM_GetPhysAddr(td_data);
 455                 td_data->Link = MM_GetPhysAddr(td_status) | 1;
 456                 td_data->Token |= (1 << 8);     // Active
 457         }
 458         else {
 459                 td_setup->Link = MM_GetPhysAddr(td_status) | 1;
 460         }
 461         td_setup->Token |= (1 << 8);    // Active
 462         td_status->Token |= (1 << 8);
 463         EHCI_int_AppendTD(Cont, Dest, td_setup);
 464
 465         return td_status;
 466 }
 467
 468 void *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length)
 469 {
 470         tEHCI_Controller        *Cont = Ptr;
 471
 472         // Sanity check the pointer
 473         // - Can't be NULL or an isoch
 474         if( (tVAddr)Dest <= 256*16 )
 475                 return NULL;
 476
 477         // Allocate single TD
 478         tEHCI_qTD       *td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length, Cb, CbData);
 479         EHCI_int_AppendTD(Cont, Dest, td);
 480
 481         return td;
 482 }
 483
 484 void EHCI_FreeOp(void *Ptr, void *Handle)
 485 {
 486         tEHCI_Controller        *Cont = Ptr;
 487
 488         EHCI_int_DeallocateTD(Cont, Handle);
 489 }
 490
 491 Uint32 EHCI_int_RootHub_FeatToMask(int Feat)
 492 {
 493         switch(Feat)
 494         {
 495         case PORT_RESET:        return PORTSC_PortReset;
 496         case PORT_ENABLE:       return PORTSC_PortEnabled;
 497         default:
 498                 Log_Warning("EHCI", "Unknown root hub port feature %i", Feat);
 499                 return 0;
 500         }
 501 }
 502
 503 void EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat)
 504 {
 505         tEHCI_Controller        *Cont = Ptr;
 506         if(Port >= Cont->nPorts)        return;
 507
 508         Cont->OpRegs->PortSC[Port] |= EHCI_int_RootHub_FeatToMask(Feat);
 509 }
 510
 511 void EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat)
 512 {
 513         tEHCI_Controller        *Cont = Ptr;
 514         if(Port >= Cont->nPorts)        return;
 515
 516         Cont->OpRegs->PortSC[Port] &= ~EHCI_int_RootHub_FeatToMask(Feat);
 517 }
 518
 519 int EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag)
 520 {
 521         tEHCI_Controller        *Cont = Ptr;
 522         if(Port >= Cont->nPorts)        return 0;
 523
 524         return !!(Cont->OpRegs->PortSC[Port] & EHCI_int_RootHub_FeatToMask(Flag));
 525 }
 526
 527 // --------------------------------------------------------------------
 528 // Internals
 529 // --------------------------------------------------------------------
 530 tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
 531 {
 532         if( Addr == 0 ) return NULL;
 533         LOG("%p + (%x - %x)", Cont->TDPool, Addr, MM_GetPhysAddr(Cont->TDPool));
 534         return Cont->TDPool + (Addr - MM_GetPhysAddr(Cont->TDPool))/sizeof(tEHCI_qTD);
 535 }
 536
 537 tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length, tUSBHostCb Cb, void *CbData)
 538 {
 539 //      Semaphore_Wait(&Cont->TDSemaphore, 1);
 540         Mutex_Acquire(&Cont->TDPoolMutex);
 541         for( int i = 0; i < TD_POOL_SIZE; i ++ )
 542         {
 543                 if( ((Cont->TDPool[i].Token >> 8) & 3) != 3 )
 544                         continue ;
 545                 Cont->TDPool[i].Token = (PID << 8) | (Length << 16);
 546                 // NOTE: Assumes that `Length` is <= PAGE_SIZE
 547                 Cont->TDPool[i].Pages[0] = MM_GetPhysAddr(Data);
 548                 if( (Cont->TDPool[i].Pages[0] & (PAGE_SIZE-1)) + Length - 1 > PAGE_SIZE )
 549                         Cont->TDPool[i].Pages[1] = MM_GetPhysAddr((char*)Data + Length - 1) & ~(PAGE_SIZE-1);
 550                 Mutex_Release(&Cont->TDPoolMutex);
 551                 LOG("Allocated %p for PID %i on %p", &Cont->TDPool[i], PID, Cont);
 552                 return &Cont->TDPool[i];
 553         }
 554
 555         Mutex_Release(&Cont->TDPoolMutex);
 556         return NULL;
 557 }
 558
 559 void EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD)
 560 {
 561         UNIMPLEMENTED();
 562 }
 563
 564 void EHCI_int_AppendTD(tEHCI_Controller *Cont, tEHCI_QH *QH, tEHCI_qTD *TD)
 565 {
 566         tEHCI_qTD       *ptd = NULL;
 567         Uint32  link = QH->CurrentTD;
 568
 569         // TODO: Need locking and validation here
 570         while( link && !(link & 1) )
 571         {
 572                 ptd = EHCI_int_GetTDFromPhys(Cont, link);
 573                 link = ptd->Link;
 574         }
 575         // TODO: Figure out how to follow this properly
 576         if( !ptd ) {
 577                 QH->CurrentTD = MM_GetPhysAddr(TD);
 578                 LOG("Appended %p to beginning of %p", TD, QH);
 579         }
 580         else {
 581                 ptd->Link = MM_GetPhysAddr(TD);
 582                 LOG("Appended %p to end of %p", TD, QH);
 583         }
 584 }
 585
 586 tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize)
 587 {
 588         tEHCI_QH        *ret;
 589         Mutex_Acquire(&Cont->QHPoolMutex);
 590         for( int i = 0; i < QH_POOL_SIZE; i ++ )
 591         {
 592                 if( !MM_GetPhysAddr( Cont->QHPools[i/QH_POOL_NPERPAGE] ) ) {
 593                         tPAddr  tmp;
 594                         Cont->QHPools[i/QH_POOL_NPERPAGE] = (void*)MM_AllocDMA(1, 32, &tmp);
 595                         memset(Cont->QHPools[i/QH_POOL_NPERPAGE], 0, PAGE_SIZE);
 596                 }
 597
 598                 ret = &Cont->QHPools[i/QH_POOL_NPERPAGE][i%QH_POOL_NPERPAGE];
 599                 if( ret->HLink == 0 ) {
 600                         ret->HLink = 1;
 601                         ret->Overlay.Link = 1;
 602                         ret->Endpoint = (Endpoint >> 4) | 0x80 | ((Endpoint & 0xF) << 8)
 603                                 | (MaxPacketSize << 16);
 604                         // TODO: Endpoint speed (13:12) 0:Full, 1:Low, 2:High
 605                         // TODO: Control Endpoint Flag (27) 0:*, 1:Full/Low Control
 606                         Mutex_Release(&Cont->QHPoolMutex);
 607                         return ret;
 608                 }
 609         }
 610         Mutex_Release(&Cont->QHPoolMutex);
 611         return NULL;
 612 }
 613
 614 void EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH)
 615 {
 616         // TODO: Ensure it's unused (somehow)
 617         QH->HLink = 0;
 618 }
 619
 620 void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
 621 {
 622         // Connect Event
 623         if( Cont->OpRegs->PortSC[Port] & PORTSC_CurrentConnectStatus )
 624         {
 625                 // Is the device low-speed?
 626                 if( (Cont->OpRegs->PortSC[Port] & PORTSC_LineStatus_MASK) == PORTSC_LineStatus_Kstate )
 627                 {
 628                         LOG("Low speed device on %p Port %i, giving to companion", Cont, Port);
 629                         Cont->OpRegs->PortSC[Port] |= PORTSC_PortOwner;
 630                 }
 631                 // not a low-speed device, EHCI reset
 632                 else
 633                 {
 634                         LOG("Device connected on %p #%i", Cont, Port);
 635                         // Reset procedure.
 636                         USB_PortCtl_BeginReset(Cont->RootHub, Port);
 637                 }
 638         }
 639         // Disconnect Event
 640         else
 641         {
 642                 if( Cont->OpRegs->PortSC[Port] & PORTSC_PortOwner ) {
 643                         LOG("Companion port %i disconnected, taking it back", Port);
 644                         Cont->OpRegs->PortSC[Port] &= ~PORTSC_PortOwner;
 645                 }
 646                 else {
 647                         LOG("Port %i disconnected", Port);
 648                         USB_DeviceDisconnected(Cont->RootHub, Port);
 649                 }
 650         }
 651 }
 652
 653 void EHCI_int_InterruptThread(void *ControllerPtr)
 654 {
 655         tEHCI_Controller        *Cont = ControllerPtr;
 656         while(Cont->OpRegs)
 657         {
 658                 Uint32  events;
 659
 660                 LOG("sleeping for events");
 661                 events = Threads_WaitEvents(EHCI_THREADEVENT_IOC|EHCI_THREADEVENT_PORTSC);
 662                 if( !events ) {
 663                         // TODO: Should this cause a termination?
 664                 }
 665                 LOG("events = 0x%x", events);
 666
 667                 if( events & EHCI_THREADEVENT_IOC )
 668                 {
 669                         // IOC, Do whatever it is you do
 670                 }
 671
 672                 // Port status change interrupt
 673                 if( events & EHCI_THREADEVENT_PORTSC )
 674                 {
 675                         // Check for port status changes
 676                         for(int i = 0; i < Cont->nPorts; i ++ )
 677                         {
 678                                 Uint32  sts = Cont->OpRegs->PortSC[i];
 679                                 LOG("Port %i: sts = %x", i, sts);
 680                                 Cont->OpRegs->PortSC[i] = sts;
 681                                 if( sts & PORTSC_ConnectStatusChange )
 682                                         EHCI_int_HandlePortConnectChange(Cont, i);
 683
 684                                 if( sts & PORTSC_PortEnableChange )
 685                                 {
 686                                         // Handle enable/disable
 687                                 }
 688
 689                                 if( sts & PORTSC_OvercurrentChange )
 690                                 {
 691                                         // Handle over-current change
 692                                 }
 693                         }
 694                 }
 695
 696                 LOG("Going back to sleep");
 697         }
 698 }