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   0
   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 #define EHCI_THREADEVENT_IAAD   THREAD_EVENT_USER3
  24
  25 // === PROTOTYPES ===
  26  int    EHCI_Initialise(char **Arguments);
  27  int    EHCI_Cleanup(void);
  28  int    EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum);
  29 // -- API ---
  30 void    *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bInput, int Period,
  31         tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  32 void    *EHCI_InitIsoch  (void *Ptr, int Endpoint, size_t MaxPacketSize);
  33 void    *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize);
  34 void    *EHCI_InitBulk   (void *Ptr, int Endpoint, size_t MaxPacketSize);
  35 void    EHCI_RemEndpoint(void *Ptr, void *Handle);
  36 void    *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
  37         int isOutbound,
  38         const void *SetupData, size_t SetupLength,
  39         const void *OutData, size_t OutLength,
  40         void *InData, size_t InLength
  41         );
  42 void    *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length);
  43 void    EHCI_FreeOp(void *Ptr, void *Handle);
  44 Uint32  EHCI_int_RootHub_FeatToMask(int Feat);
  45 void    EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat);
  46 void    EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat);
  47  int    EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag);
  48 // --- Internals ---
  49 tEHCI_Endpoint  *EHCI_int_AllocateEndpt(tEHCI_Controller *Ctrlr, int Endpoint, size_t MPS, int PeriodPow2);
  50 tEHCI_qTD       *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length);
  51 void    EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD);
  52 tEHCI_QH        *EHCI_int_AllocateQH(tEHCI_Controller *Cont, tEHCI_Endpoint *Endpoint, tUSBHostCb Cb, void *CbPtr);
  53 void    EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH);
  54 void    EHCI_InterruptHandler(int IRQ, void *Ptr);
  55 // --- Interrupts ---
  56 void    EHCI_int_AppendQHToAsync(tEHCI_Controller *Cont, tEHCI_QH *QH);
  57 void    EHCI_int_InterruptThread(void *ControllerPtr);
  58
  59 // === GLOBALS ===
  60 MODULE_DEFINE(0, VERSION, USB_EHCI, EHCI_Initialise, NULL, "USB_Core", NULL);
  61 tEHCI_Controller        gaEHCI_Controllers[EHCI_MAX_CONTROLLERS];
  62 tUSBHostDef     gEHCI_HostDef = {
  63         .InitInterrupt = EHCI_InitInterrupt,
  64         .InitIsoch     = EHCI_InitIsoch,
  65         .InitControl   = EHCI_InitControl,
  66         .InitBulk      = EHCI_InitBulk,
  67         .RemEndpoint   = EHCI_RemEndpoint,
  68         .SendIsoch   = NULL,
  69         .SendControl = EHCI_SendControl,
  70         .SendBulk    = EHCI_SendBulk,
  71         .FreeOp      = EHCI_FreeOp,
  72
  73         .CheckPorts = NULL,     // No need
  74         .SetPortFeature   = EHCI_RootHub_SetPortFeature,
  75         .ClearPortFeature = EHCI_RootHub_ClearPortFeature,
  76         .GetPortStatus    = EHCI_RootHub_GetPortStatus,
  77         };
  78
  79 // === CODE ===
  80 int EHCI_Initialise(char **Arguments)
  81 {
  82         for( int id = -1; (id = PCI_GetDeviceByClass(0x0C0320, 0xFFFFFF, id)) >= 0;  )
  83         {
  84                 Uint32  addr = PCI_GetValidBAR(id, 0, PCI_BARTYPE_MEM);
  85                 if( addr == 0 ) {
  86                         Log_Error("EHCI", "PCI%i BAR0 is not memory", id);
  87                         continue ;
  88                 }
  89                 Uint8   irq = PCI_GetIRQ(id);
  90                 if( irq == 0 ) {
  91                         Log_Error("EHCI", "PCI%i has no IRQ", id);
  92                         continue ;
  93                 }
  94
  95                 Log_Log("ECHI", "Controller at PCI %i 0x%x IRQ 0x%x", 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         // Allocate a controller structure
 145         for( int i = 0; i < EHCI_MAX_CONTROLLERS; i ++ )
 146         {
 147                 if( gaEHCI_Controllers[i].PhysBase == 0 ) {
 148                         cont = &gaEHCI_Controllers[i];
 149                         cont->PhysBase = BaseAddress;
 150                         break;
 151                 }
 152         }
 153         if(!cont) {
 154                 Log_Notice("EHCI", "Too many controllers (EHCI_MAX_CONTROLLERS=%i)",
 155                         EHCI_MAX_CONTROLLERS);
 156                 LEAVE('i', 1);
 157                 return 1;
 158         }
 159
 160         // - Nuke a couple of fields so error handling code doesn't derp
 161         cont->CapRegs = NULL;
 162         cont->PeriodicQueue = NULL;
 163         cont->TDPool = NULL;
 164
 165         // -- Build up structure --
 166         cont->CapRegs = (void*)( MM_MapHWPages(BaseAddress, 1) + (BaseAddress % PAGE_SIZE) );
 167         if( !cont->CapRegs ) {
 168                 Log_Warning("EHCI", "Can't map 1 page at %P into kernel space", BaseAddress);
 169                 goto _error;
 170         }
 171         LOG("cont->CapRegs = %p", cont->CapRegs);
 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         //  > Populate queue (with non-present entries)
 193         for( int i = 0; i < 1024; i ++ )
 194                 cont->PeriodicQueue[i] = 1;
 195         // TODO: For 64-bit, QH pool and periodic queue need to be in the same 32-bit segment
 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; // TODO: what is this value?
 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);
 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         // - HLink is set to itself, initing a circular list
 229         cont->DeadQH = EHCI_int_AllocateQH(cont, NULL, NULL, NULL);
 230         memset(cont->DeadQH, 0, sizeof(tEHCI_QH));
 231         cont->DeadQH->HLink = MM_GetPhysAddr(cont->DeadQH)|2;
 232         cont->DeadQH->Impl.Next = cont->DeadQH;
 233         cont->DeadQH->Endpoint = QH_ENDPT_H;    // H - Head of Reclamation List
 234         cont->DeadQH->CurrentTD = MM_GetPhysAddr(cont->DeadTD);
 235         cont->DeadQH->Overlay.Link = MM_GetPhysAddr(cont->DeadTD);
 236
 237         // -- Initialisation procedure (from ehci-r10) --
 238         // - Reset controller
 239         cont->OpRegs->USBCmd = USBCMD_HCReset;
 240         // - Set CTRLDSSEGMENT (TODO: 64-bit support)
 241         // - Set USBINTR
 242         cont->OpRegs->USBIntr = USBINTR_IOC|USBINTR_PortChange|USBINTR_FrameRollover|USBINTR_IntrAsyncAdvance;
 243         // - Set PERIODICLIST BASE
 244         cont->OpRegs->PeridocListBase = MM_GetPhysAddr( cont->PeriodicQueue );
 245         // - Set ASYNCLISTADDR
 246         cont->OpRegs->AsyncListAddr = MM_GetPhysAddr(cont->DeadQH);
 247         // - Enable controller
 248         cont->OpRegs->USBCmd = (0x40 << 16) | USBCMD_PeriodicEnable | USBCMD_AsyncEnable | USBCMD_Run;
 249         // - Route all ports
 250         cont->OpRegs->ConfigFlag = 1;
 251
 252         // -- Register with USB Core --
 253         cont->RootHub = USB_RegisterHost(&gEHCI_HostDef, cont, cont->nPorts);
 254         LOG("cont->RootHub = %p", cont->RootHub);
 255
 256         LEAVE('i', 0);
 257         return 0;
 258 _error:
 259         cont->PhysBase = 0;
 260         if( cont->CapRegs )
 261                 MM_Deallocate( cont->CapRegs );
 262         if( cont->PeriodicQueue )
 263                 MM_Deallocate( cont->PeriodicQueue );
 264         if( cont->TDPool )
 265                 MM_Deallocate( cont->TDPool );
 266         LEAVE('i', 2);
 267         return 2;
 268 }
 269
 270
 271 static inline int _GetClosestPower2(int Period)
 272 {
 273         // Round the period to the closest power of two
 274         int pow2period = 1;
 275         int period_pow = 0;
 276         // - Find the first power above the period
 277         while( pow2period < Period )
 278         {
 279                 pow2period *= 2;
 280                 period_pow ++;
 281         }
 282         // - Check which is closest
 283         if( Period - pow2period / 2 > pow2period - Period ) {
 284                 ;
 285         }
 286         else {
 287                 period_pow --;
 288         }
 289         LOG("period_pow = %i (%ims) from %ims", period_pow, 1 << period_pow, Period);
 290         return period_pow;
 291 }
 292
 293 void EHCI_int_AddToPeriodic(tEHCI_Controller *Cont, tEHCI_QH *qh, int PeriodPow2, size_t Load)
 294 {
 295          int    Period = 1 << PeriodPow2;
 296         Mutex_Acquire(&Cont->PeriodicListLock);
 297
 298         // Choose an interrupt slot to use
 299         int minslot = 0, minslot_load = INT_MAX;
 300         for( int slot = 0; slot < Period; slot ++ )
 301         {
 302                  int    load = 0;
 303                 for( int i = 0; i < PERIODIC_SIZE; i += Period )
 304                         load += Cont->InterruptLoad[i+slot];
 305                 if( load == 0 ) break;
 306                 if( load < minslot_load ) {
 307                         minslot = slot;
 308                         minslot_load = load;
 309                 }
 310         }
 311         // Increase loading on the selected slot
 312         for( int i = minslot; i < PERIODIC_SIZE; i += Period )
 313                 Cont->InterruptLoad[i] += Load;
 314         qh->Impl.Endpt->InterruptOfs = minslot;
 315         LOG("Using slot %i", minslot);
 316
 317         // Insert into the periodic list
 318         for( int i = 0; i < PERIODIC_SIZE; i += Period )
 319         {
 320                 // Walk list until
 321                 // - the end is reached
 322                 // - this QH is found
 323                 // - A QH with a lower period is encountered
 324                 tEHCI_QH        *pqh = NULL;
 325                 tEHCI_QH        *nqh;
 326                 for( nqh = Cont->PeriodicQueueV[i]; nqh; pqh = nqh, nqh = nqh->Impl.Next )
 327                 {
 328                         if( nqh == qh )
 329                                 break;
 330                         if( nqh->Impl.Endpt->PeriodPow2 < PeriodPow2 )
 331                                 break;
 332                 }
 333
 334                 // Somehow, we've already been added to this queue.
 335                 if( nqh && nqh == qh )
 336                         continue ;
 337
 338                 if( qh->Impl.Next && qh->Impl.Next != nqh ) {
 339                         Log_Warning("EHCI", "Suspected bookkeeping error on %p - int list %i+%i overlap",
 340                                 Cont, PeriodPow2, minslot);
 341                         break;
 342                 }
 343
 344                 if( nqh ) {
 345                         qh->Impl.Next = nqh;
 346                         qh->HLink = MM_GetPhysAddr(nqh) | 2;
 347                 }
 348                 else {
 349                         qh->Impl.Next = NULL;
 350                         qh->HLink = 2|1;        // QH, Terminate
 351                 }
 352
 353                 if( pqh ) {
 354                         pqh->Impl.Next = qh;
 355                         pqh->HLink = MM_GetPhysAddr(qh) | 2;
 356                 }
 357                 else {
 358                         Cont->PeriodicQueueV[i] = qh;
 359                         Cont->PeriodicQueue[i] = MM_GetPhysAddr(qh) | 2;
 360                 }
 361         }
 362         Mutex_Release(&Cont->PeriodicListLock);
 363 }
 364
 365 // --------------------------------------------------------------------
 366 // USB API
 367 // --------------------------------------------------------------------
 368 void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 369         tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 370 {
 371         tEHCI_Controller        *Cont = Ptr;
 372
 373         ASSERTCR(Endpoint, <, 256*16, NULL);
 374         ASSERTCR(Period, >, 0, NULL);
 375         if( Period > 256 )
 376                 Period = 256;
 377
 378         LOG("Endpoint=%x, bOutbound=%i, Period=%i, Length=%i", Endpoint, bOutbound, Period, Length);
 379
 380         int period_pow = _GetClosestPower2(Period);
 381
 382         tEHCI_Endpoint  *endpt = EHCI_int_AllocateEndpt(Cont, Endpoint, Length, period_pow);
 383         endpt->Next = Cont->FirstInterruptEndpt;
 384         Cont->FirstInterruptEndpt = endpt;
 385
 386         // Allocate a QH
 387         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
 388         qh->EndpointExt |= 1;   // TODO: uFrame load balancing (8 entry bitfield)
 389
 390         // Allocate TD for the data
 391         tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length);
 392         qh->CurrentTD = MM_GetPhysAddr(td);
 393         qh->Impl.FirstTD = td;
 394         qh->Impl.LastTD = td;
 395
 396         // - Insert into period queue
 397         EHCI_int_AddToPeriodic(Cont, qh, period_pow, Length);
 398
 399         return endpt;
 400 }
 401
 402 void *EHCI_InitIsoch(void *Ptr, int Endpoint, size_t MaxPacketSize)
 403 {
 404         ENTER("pPtr iEndpoint iMaxPacketSize",
 405                 Ptr, Endpoint, MaxPacketSize);
 406         LEAVE_RET('p', (void*)(tVAddr)(Endpoint + 1));
 407 }
 408 void *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize)
 409 {
 410         return EHCI_int_AllocateEndpt(Ptr, Endpoint, MaxPacketSize, -1);
 411 }
 412 void *EHCI_InitBulk(void *Ptr, int Endpoint, size_t MaxPacketSize)
 413 {
 414         return EHCI_int_AllocateEndpt(Ptr, Endpoint, MaxPacketSize, -2);
 415 }
 416 void EHCI_RemEndpoint(void *Ptr, void *Handle)
 417 {
 418         if( Handle == NULL )
 419                 return ;
 420         else if( (tVAddr)Handle <= 256*16 )
 421                 return ;        // Isoch
 422         else {
 423                 tEHCI_Endpoint  *endpt = Ptr;
 424
 425                 // Remove QH from list
 426                 // - If it's a polling endpoint, need to remove from all periodic lists
 427                 if( endpt->PeriodPow2 >= 0) {
 428                         // Poll
 429                 }
 430                 else {
 431                         // Control/Bulk
 432                 }
 433
 434                 // Deallocate endpoint
 435                 // TODO: Write EHCI_DeallocateEndpoint
 436                 free(endpt);
 437         }
 438 }
 439
 440 void *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
 441         int isOutbound,
 442         const void *SetupData, size_t SetupLength,
 443         const void *OutData, size_t OutLength,
 444         void *InData, size_t InLength
 445         )
 446 {
 447         tEHCI_Controller *Cont = Ptr;
 448         tEHCI_Endpoint  *endpt = Dest;
 449         tEHCI_QH        *qh;
 450         tEHCI_qTD       *td_setup, *td_data, *td_status;
 451
 452         // Sanity checks
 453         if( (tVAddr)Dest <= 256*16 )
 454                 return NULL;
 455         if( endpt->PeriodPow2 != -1 ) {
 456                 Log_Notice("EHCI", "Non-control endpoint passed to SendControl");
 457                 return NULL;
 458         }
 459         if( SetupLength > endpt->MaxPacketSize )
 460                 return NULL;
 461         // TODO: Check size of status
 462
 463
 464         LOG("Dest=%p, isOutbound=%i, Lengths(Setup:%i,Out:%i,In:%i)",
 465                 Dest, isOutbound, SetupLength, OutLength, InLength);
 466
 467         // Allocate a QH to work with
 468         qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
 469
 470         // Allocate TDs
 471         td_setup = EHCI_int_AllocateTD(Cont, PID_SETUP, (void*)SetupData, SetupLength);
 472         if( isOutbound )
 473         {
 474                 td_data = OutData ? EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength) : NULL;
 475                 td_status = EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength);
 476         }
 477         else
 478         {
 479                 td_data = InData ? EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength) : NULL;
 480                 td_status = EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength);
 481         }
 482         td_status->Token |= QTD_TOKEN_IOC;      // IOC
 483
 484         // Append TDs
 485         if( td_data ) {
 486                 td_setup->Link = MM_GetPhysAddr(td_data);
 487                 td_data->Link = MM_GetPhysAddr(td_status);
 488                 td_data->Token |= QTD_TOKEN_STS_ACTIVE; // Active
 489         }
 490         else {
 491                 td_setup->Link = MM_GetPhysAddr(td_status);
 492         }
 493         td_setup->Token |= QTD_TOKEN_STS_ACTIVE;        // Active
 494         td_status->Token |= QTD_TOKEN_STS_ACTIVE;
 495         td_status->Link = 1;
 496         td_status->Link2 = 1;
 497
 498         // Set QH's current pointer
 499         qh->CurrentTD = MM_GetPhysAddr(td_setup);
 500         qh->Impl.FirstTD = td_setup;
 501         qh->Impl.LastTD = td_status;
 502         EHCI_int_AppendQHToAsync(Cont, qh);
 503
 504         return qh;
 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         tEHCI_Endpoint  *endpt = Dest;
 511
 512         // Sanity check the pointer
 513         // - Can't be NULL or an isoch
 514         if( (tVAddr)Dest <= 256*16 )
 515                 return NULL;
 516         if( endpt->PeriodPow2 != -2 ) {
 517                 return NULL;
 518         }
 519         LOG("Ptr=%p,Dest=%p(%x),Cb=%p(%p),DirIsOut=%i,Data=%p+0x%x)",
 520                 Ptr, Dest, endpt->EndpointID, Cb, CbData, Dir, Data, Length);
 521
 522         // Allocate a QH to work with
 523         tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
 524
 525         // Allocate single TD
 526         tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length);
 527         td->Token |= QTD_TOKEN_IOC | QTD_TOKEN_STS_ACTIVE;
 528         qh->CurrentTD = MM_GetPhysAddr(td);
 529         qh->Impl.FirstTD = td;
 530         qh->Impl.LastTD = td;
 531
 532         EHCI_int_AppendQHToAsync(Cont, qh);
 533
 534         return qh;
 535 }
 536
 537 void EHCI_FreeOp(void *Ptr, void *Handle)
 538 {
 539         tEHCI_Controller        *Cont = Ptr;
 540
 541         EHCI_int_DeallocateQH(Cont, Handle);
 542 }
 543
 544 Uint32 EHCI_int_RootHub_FeatToMask(int Feat)
 545 {
 546         switch(Feat)
 547         {
 548         case PORT_RESET:        return PORTSC_PortReset;
 549         case PORT_ENABLE:       return PORTSC_PortEnabled;
 550         default:
 551                 Log_Warning("EHCI", "Unknown root hub port feature %i", Feat);
 552                 return 0;
 553         }
 554 }
 555
 556 void EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat)
 557 {
 558         tEHCI_Controller        *Cont = Ptr;
 559         if(Port >= Cont->nPorts)        return;
 560
 561         Cont->OpRegs->PortSC[Port] |= EHCI_int_RootHub_FeatToMask(Feat);
 562 }
 563
 564 void EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat)
 565 {
 566         tEHCI_Controller        *Cont = Ptr;
 567         if(Port >= Cont->nPorts)        return;
 568
 569         Cont->OpRegs->PortSC[Port] &= ~EHCI_int_RootHub_FeatToMask(Feat);
 570 }
 571
 572 int EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag)
 573 {
 574         tEHCI_Controller        *Cont = Ptr;
 575         if(Port >= Cont->nPorts)        return 0;
 576
 577         return !!(Cont->OpRegs->PortSC[Port] & EHCI_int_RootHub_FeatToMask(Flag));
 578 }
 579
 580 // --------------------------------------------------------------------
 581 // Internals
 582 // --------------------------------------------------------------------
 583 tEHCI_Endpoint *EHCI_int_AllocateEndpt(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize, int PeriodPow2)
 584 {
 585         ENTER("pCont iEndpoint iMaxPacketSize",
 586                 Cont, Endpoint, MaxPacketSize);
 587
 588         tEHCI_Endpoint  *endpt = malloc( sizeof(*endpt) );
 589         endpt->NextToggle = FALSE;
 590         endpt->EndpointID = Endpoint;
 591         endpt->MaxPacketSize = MaxPacketSize;   // TODO: Sanity Check
 592         endpt->PeriodPow2 = PeriodPow2;
 593         endpt->InterruptOfs = 0;
 594         endpt->ActiveQHs = NULL;
 595         // TODO: store endpoints on controller
 596
 597         LOG("Created %p for %P Ep 0x%x - %i bytes MPS",
 598                 endpt, Cont->PhysBase, Endpoint, MaxPacketSize);
 599
 600         LEAVE('p', endpt);
 601         return endpt;
 602 }
 603
 604 tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
 605 {
 606         if( Addr == 0 ) return NULL;
 607         LOG("%p + (%x - %x)", Cont->TDPool, Addr, MM_GetPhysAddr(Cont->TDPool));
 608         return Cont->TDPool + (Addr - MM_GetPhysAddr(Cont->TDPool))/sizeof(tEHCI_qTD);
 609 }
 610
 611 tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length)
 612 {
 613 //      Semaphore_Wait(&Cont->TDSemaphore, 1);
 614         Mutex_Acquire(&Cont->TDPoolMutex);
 615         for( int i = 0; i < TD_POOL_SIZE; i ++ )
 616         {
 617                 // PID code == 3: Avaliable for use
 618                 if( ((Cont->TDPool[i].Token >> 8) & 3) != 3 )
 619                         continue ;
 620
 621                 Cont->TDPool[i].Token = (PID << 8) | (Length << 16);
 622                 Mutex_Release(&Cont->TDPoolMutex);
 623
 624                 tEHCI_qTD       *td = &Cont->TDPool[i];
 625                 td->Impl.Ptr = Data;
 626                 td->Impl.Length = Length;
 627                 td->Link = 1;
 628                 td->Link2 = 1;
 629
 630                 // TODO: Support breaking across multiple pages
 631                 ASSERTC(Length, <, PAGE_SIZE);
 632                 // TODO: Handle bouncing >32-bit pages
 633                 #if PHYS_BITS > 32
 634                 ASSERT( MM_GetPhysAddr(Data) >> 32 == 0 );
 635                 #endif
 636                 td->Pages[0] = MM_GetPhysAddr(Data);
 637                 if( (td->Pages[0] & (PAGE_SIZE-1)) + Length - 1 > PAGE_SIZE )
 638                         td->Pages[1] = MM_GetPhysAddr((char*)Data + Length - 1) & ~(PAGE_SIZE-1);
 639                 LOG("Allocated %p(%P) PID%i on %P (Token=0x%x)",
 640                         td, MM_GetPhysAddr(td), PID, Cont->PhysBase,
 641                         td->Token);
 642                 return td;
 643         }
 644
 645         Mutex_Release(&Cont->TDPoolMutex);
 646         return NULL;
 647 }
 648
 649 void EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD)
 650 {
 651         UNIMPLEMENTED();
 652 }
 653
 654 tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, tEHCI_Endpoint *Endpoint, tUSBHostCb Cb, void *CbData)
 655 {
 656         tEHCI_QH        *ret;
 657         Mutex_Acquire(&Cont->QHPoolMutex);
 658         for( int i = 0; i < QH_POOL_SIZE; i ++ )
 659         {
 660                 // If page not yet allocated, allocate it
 661                 if( !MM_GetPhysAddr( Cont->QHPools[i/QH_POOL_NPERPAGE] ) ) {
 662                         tPAddr  tmp;
 663                         Cont->QHPools[i/QH_POOL_NPERPAGE] = (void*)MM_AllocDMA(1, 32, &tmp);
 664                         memset(Cont->QHPools[i/QH_POOL_NPERPAGE], 0, PAGE_SIZE);
 665                 }
 666
 667                 ret = &Cont->QHPools[i/QH_POOL_NPERPAGE][i%QH_POOL_NPERPAGE];
 668                 if( ret->HLink != 0 )
 669                         continue ;
 670
 671                 memset(ret, 0, sizeof(*ret));
 672                 ret->HLink = 1;
 673                 Mutex_Release(&Cont->QHPoolMutex);
 674
 675                 if( Endpoint )
 676                 {
 677                         ret->Endpoint = (Endpoint->EndpointID >> 4) | 0x80 | ((Endpoint->EndpointID & 0xF) << 8)
 678                                 | (Endpoint->MaxPacketSize << 16);
 679                 }
 680                 ret->EndpointExt = (1<<30);
 681                 ret->Impl.Next = NULL;
 682                 ret->Impl.Callback = Cb;
 683                 ret->Impl.CallbackData = CbData;
 684                 ret->Impl.Endpt = Endpoint;
 685                 // TODO: Endpoint speed (13:12) 0:Full, 1:Low, 2:High
 686                 // TODO: Control Endpoint Flag (27) 0:*, 1:Full/Low Control
 687                 return ret;
 688         }
 689         Mutex_Release(&Cont->QHPoolMutex);
 690         return NULL;
 691 }
 692
 693 void EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH)
 694 {
 695         UNIMPLEMENTED();
 696         // TODO: Ensure it's unused and clean up associated TDs
 697         QH->HLink = 0;
 698 }
 699
 700 void EHCI_int_AppendQHToAsync(tEHCI_Controller *Cont, tEHCI_QH *QH)
 701 {
 702         QH->Overlay.Token = QTD_TOKEN_STS_ACTIVE;
 703
 704         Mutex_Acquire(&Cont->lAsyncSchedule);
 705         // Insert into list
 706         QH->HLink = Cont->DeadQH->HLink;
 707         QH->Impl.Next = Cont->DeadQH->Impl.Next;
 708         Cont->DeadQH->HLink = MM_GetPhysAddr(QH)|2;
 709         Cont->DeadQH->Impl.Next = QH;
 710         // Start async schedule
 711         // - Set ASYNCENABLE
 712         // - Clear 'H' in dead QH
 713         Mutex_Release(&Cont->lAsyncSchedule);
 714         LOG("Appended %P to %p(%P)", MM_GetPhysAddr(QH), Cont, Cont->PhysBase);
 715 }
 716
 717 // --------------------------------------------------------------------
 718 // Interrupt actions
 719 // --------------------------------------------------------------------
 720 void EHCI_InterruptHandler(int IRQ, void *Ptr)
 721 {
 722         tEHCI_Controller *Cont = Ptr;
 723         Uint32  sts = Cont->OpRegs->USBSts;
 724         Uint32  orig_sts = sts;
 725         const Uint32    reserved_bits = 0xFFFF0FC0;
 726
 727         if( sts & reserved_bits ) {
 728                 LOG("Oops, reserved bits set (%08x), funny hardware?", sts);
 729                 sts &= ~reserved_bits;
 730         }
 731
 732         // Unmask read-only bits
 733         sts &= ~(0xF000);
 734
 735         if( sts & USBINTR_IOC ) {
 736                 // IOC
 737                 LOG("%P IOC", Cont->PhysBase);
 738                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IOC);
 739                 sts &= ~USBINTR_IOC;
 740         }
 741
 742         if( sts & USBINTR_IntrAsyncAdvance ) {
 743                 LOG("%P IAAD", Cont->PhysBase);
 744                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IAAD);
 745                 sts &= ~USBINTR_IntrAsyncAdvance;
 746         }
 747
 748         if( sts & USBINTR_PortChange ) {
 749                 // Port change, determine what port and poke helper thread
 750                 LOG("%P Port status change", Cont->PhysBase);
 751                 Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_PORTSC);
 752                 sts &= ~USBINTR_PortChange;
 753         }
 754
 755         if( sts & USBINTR_FrameRollover ) {
 756                 // Frame rollover, used to aid timing (trigger per-second operations)
 757                 //LOG("%p Frame rollover", Ptr);
 758                 sts &= ~USBINTR_FrameRollover;
 759         }
 760
 761         if( sts ) {
 762                 // Unhandled interupt bits
 763                 // TODO: Warn
 764                 LOG("WARN - Bitmask %x unhandled", sts);
 765         }
 766
 767
 768         // Clear interrupts
 769         Cont->OpRegs->USBSts = orig_sts;
 770 }
 771
 772 void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
 773 {
 774         // Connect Event
 775         if( Cont->OpRegs->PortSC[Port] & PORTSC_CurrentConnectStatus )
 776         {
 777                 // Is the device low-speed?
 778                 if( (Cont->OpRegs->PortSC[Port] & PORTSC_LineStatus_MASK) == PORTSC_LineStatus_Kstate )
 779                 {
 780                         LOG("Low speed device on %P Port %i, giving to companion", Cont->PhysBase, Port);
 781                         Cont->OpRegs->PortSC[Port] |= PORTSC_PortOwner;
 782                 }
 783                 // not a low-speed device, EHCI reset
 784                 else
 785                 {
 786                         LOG("Device connected on %P Port %i", Cont->PhysBase, Port);
 787                         // Reset procedure.
 788                         USB_PortCtl_BeginReset(Cont->RootHub, Port);
 789                 }
 790         }
 791         // Disconnect Event
 792         else
 793         {
 794                 if( Cont->OpRegs->PortSC[Port] & PORTSC_PortOwner ) {
 795                         LOG("Device disconnected on %P Port %i (companion), taking it back",
 796                                 Cont->PhysBase, Port);
 797                         Cont->OpRegs->PortSC[Port] &= ~PORTSC_PortOwner;
 798                 }
 799                 else {
 800                         LOG("Device disconnected on %P Port %i", Cont->PhysBase, Port);
 801                         USB_DeviceDisconnected(Cont->RootHub, Port);
 802                 }
 803         }
 804 }
 805
 806 void EHCI_int_CheckInterruptQHs(tEHCI_Controller *Cont)
 807 {
 808         for( tEHCI_Endpoint *endpt = Cont->FirstInterruptEndpt; endpt; endpt = endpt->Next )
 809         {
 810                 tEHCI_QH *qh = endpt->ActiveQHs;
 811                 // Check if the TD of the first QH is active
 812                 if( qh->Overlay.Token & QTD_TOKEN_STS_ACTIVE )
 813                         continue ;
 814                 // Inactive, fire interrupt and re-trigger
 815                 if( !qh->Impl.Callback ) {
 816                         // Umm... ?
 817                 }
 818                 else
 819                 {
 820                         tEHCI_qTD       *last = qh->Impl.LastTD;
 821                         size_t  remaining_len = (last->Token >> 16) & 0x7FFF;
 822                         if( remaining_len > last->Impl.Length )
 823                                 remaining_len = last->Impl.Length;
 824                         size_t  transferred_len = last->Impl.Length - remaining_len;
 825
 826                         LOG("Calling %p(%p) for Intr EndPt %x (%p+0x%x)",
 827                                 qh->Impl.Callback, qh->Impl.CallbackData,
 828                                 qh->Impl.Endpt->EndpointID, last->Impl.Ptr, transferred_len);
 829                         qh->Impl.Callback(qh->Impl.CallbackData, last->Impl.Ptr, transferred_len);
 830                 }
 831                 qh->Impl.FirstTD->Token |= QTD_TOKEN_STS_ACTIVE;
 832                 qh->Overlay.Token |= QTD_TOKEN_STS_ACTIVE;
 833         }
 834 }
 835
 836 void EHCI_int_RetireQHs(tEHCI_Controller *Cont)
 837 {
 838         tEHCI_QH        *prev = Cont->DeadQH;
 839         Mutex_Acquire(&Cont->lAsyncSchedule);
 840         for( tEHCI_QH *qh = prev->Impl.Next; qh != Cont->DeadQH; qh = prev->Impl.Next )
 841         {
 842                 ASSERT(qh);
 843                 ASSERT(qh != qh->Impl.Next);
 844                 if( qh->Overlay.Token & QTD_TOKEN_STS_ACTIVE ) {
 845                         prev = qh;
 846                         continue ;
 847                 }
 848
 849                 // Remove from async list
 850                 prev->HLink = qh->HLink;
 851                 prev->Impl.Next = qh->Impl.Next;
 852
 853                 // Add to reclaim list
 854                 qh->Impl.Next = Cont->ReclaimList;
 855                 Cont->ReclaimList = qh;
 856
 857                 // Ring doorbell!
 858                 Cont->OpRegs->USBCmd |= USBCMD_IAAD;
 859         }
 860         Mutex_Release(&Cont->lAsyncSchedule);
 861 }
 862
 863 /*
 864  * Fire callbacks on QHs and mark them as completed
 865  *
 866  * TODO: Possible bug with items being added to reclaim list before
 867  *       the last doorbell fires.
 868  */
 869 void EHCI_int_ReclaimQHs(tEHCI_Controller *Cont)
 870 {
 871         // Doorbell was rung, so reclaim QHs
 872         // - Actually just fires callbacks, now that we know that the QHs can be cleared
 873         tEHCI_QH        *qh;
 874         Mutex_Acquire(&Cont->lReclaimList);
 875         while( (qh = Cont->ReclaimList) )
 876         {
 877                 Cont->ReclaimList = qh->Impl.Next;
 878                 Mutex_Release(&Cont->lReclaimList);
 879
 880                 if( qh->Impl.Callback )
 881                 {
 882                         tEHCI_qTD       *last = qh->Impl.LastTD;
 883                         size_t  remaining_len = (last->Token >> 16) & 0x7FFF;
 884                         if( remaining_len > last->Impl.Length )
 885                                 remaining_len = last->Impl.Length;
 886                         size_t  transferred_len = last->Impl.Length - remaining_len;
 887
 888                         LOG("Calling %p(%p) for EndPt %x (%p+0x%x)",
 889                                 qh->Impl.Callback, qh->Impl.CallbackData,
 890                                 qh->Impl.Endpt->EndpointID, last->Impl.Ptr, transferred_len);
 891                         //if( last->Impl.Ptr )
 892                         //      Debug_HexDump("EHCI Callback Data", last->Impl.Ptr, transferred_len);
 893                         qh->Impl.Callback(qh->Impl.CallbackData, last->Impl.Ptr, transferred_len);
 894                 }
 895
 896                 Mutex_Acquire(&Cont->lReclaimList);
 897         }
 898         Mutex_Release(&Cont->lReclaimList);
 899 }
 900
 901 void EHCI_int_InterruptThread(void *ControllerPtr)
 902 {
 903         tEHCI_Controller        *Cont = ControllerPtr;
 904         Threads_SetName("EHCI Interrupt Worker");
 905         while(Cont->OpRegs)
 906         {
 907                 Uint32  events;
 908
 909                 LOG("sleeping for events");
 910                 events = Threads_WaitEvents(EHCI_THREADEVENT_IOC|EHCI_THREADEVENT_PORTSC|EHCI_THREADEVENT_IAAD);
 911                 if( !events ) {
 912                         // TODO: Should this cause a termination?
 913                 }
 914                 LOG("events = 0x%x", events);
 915
 916                 if( events & EHCI_THREADEVENT_IAAD )
 917                 {
 918                         EHCI_int_ReclaimQHs(Cont);
 919                 }
 920
 921                 if( events & EHCI_THREADEVENT_IOC )
 922                 {
 923                         // IOC, handle completed requests
 924                         // Search periodic lists for one that fired
 925                         EHCI_int_CheckInterruptQHs(Cont);
 926                         // Retire QHs
 927                         // - Remove them from the queue and ask the controller to bell when they're removable
 928                         EHCI_int_RetireQHs(Cont);
 929                 }
 930
 931                 // Port status change interrupt
 932                 if( events & EHCI_THREADEVENT_PORTSC )
 933                 {
 934                         // Check for port status changes
 935                         for(int i = 0; i < Cont->nPorts; i ++ )
 936                         {
 937                                 Uint32  sts = Cont->OpRegs->PortSC[i];
 938                                 //LOG("Port %i: sts = %x", i, sts);
 939                                 Cont->OpRegs->PortSC[i] = sts;
 940                                 if( sts & PORTSC_ConnectStatusChange )
 941                                 {
 942                                         LOG("Port %i: Connect Change", i);
 943                                         EHCI_int_HandlePortConnectChange(Cont, i);
 944                                 }
 945
 946                                 if( sts & PORTSC_PortEnableChange )
 947                                 {
 948                                         // Handle enable/disable
 949                                         LOG("Port %i: Enable Change", i);
 950                                 }
 951
 952                                 if( sts & PORTSC_OvercurrentChange )
 953                                 {
 954                                         // Handle over-current change
 955                                         LOG("Port %i: Over-Current Change", i);
 956                                 }
 957                         }
 958                 }
 959         }
 960 }