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