X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FModules%2FUSB%2FEHCI%2Fehci.c;h=d202d067a5327e275c4c808bedad7e6fb6e42898;hb=5ee801f4fb22bba3298f10273027e67f53692e4c;hp=9d356540fa73a6d3fb24ce891f82c2f339f82289;hpb=a138e626bb3055b4736a7932cb6172553fc1f407;p=tpg%2Facess2.git

diff --git a/KernelLand/Modules/USB/EHCI/ehci.c b/KernelLand/Modules/USB/EHCI/ehci.c
index 9d356540..d202d067 100644
--- a/KernelLand/Modules/USB/EHCI/ehci.c
+++ b/KernelLand/Modules/USB/EHCI/ehci.c
@@ -5,7 +5,7 @@
  * ehci.c
  * - Driver Core
  */
-#define DEBUG	1
+#define DEBUG	0
 #define VERSION	VER2(0,1)
 #include <acess.h>
 #include <modules.h>
@@ -20,14 +20,15 @@
 #define EHCI_MAX_CONTROLLERS	4
 #define EHCI_THREADEVENT_IOC	THREAD_EVENT_USER1
 #define EHCI_THREADEVENT_PORTSC	THREAD_EVENT_USER2
+#define EHCI_THREADEVENT_IAAD	THREAD_EVENT_USER3
 
 // === PROTOTYPES ===
  int	EHCI_Initialise(char **Arguments);
  int	EHCI_Cleanup(void);
  int	EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum);
-void 	EHCI_InterruptHandler(int IRQ, void *Ptr);
 // -- API ---
-void	*EHCI_InitInterrupt(void *Ptr, int Endpoint, int bInput, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
+void	*EHCI_InitInterrupt(void *Ptr, int Endpoint, int bInput, int Period,
+	tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
 void	*EHCI_InitIsoch  (void *Ptr, int Endpoint, size_t MaxPacketSize);
 void	*EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize);
 void	*EHCI_InitBulk   (void *Ptr, int Endpoint, size_t MaxPacketSize);
@@ -45,11 +46,14 @@ void	EHCI_RootHub_SetPortFeature(void *Ptr, int Port, int Feat);
 void	EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat);
  int	EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag);
 // --- Internals ---
-tEHCI_qTD	*EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length, tUSBHostCb Cb, void *CbData);
+tEHCI_Endpoint	*EHCI_int_AllocateEndpt(tEHCI_Controller *Ctrlr, int Endpoint, size_t MPS, int PeriodPow2);
+tEHCI_qTD	*EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length);
 void	EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD);
-void	EHCI_int_AppendTD(tEHCI_Controller *Cont, tEHCI_QH *QH, tEHCI_qTD *TD);
-tEHCI_QH	*EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize);
+tEHCI_QH	*EHCI_int_AllocateQH(tEHCI_Controller *Cont, tEHCI_Endpoint *Endpoint, tUSBHostCb Cb, void *CbPtr);
 void	EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH);
+void 	EHCI_InterruptHandler(int IRQ, void *Ptr);
+// --- Interrupts ---
+void	EHCI_int_AppendQHToAsync(tEHCI_Controller *Cont, tEHCI_QH *QH);
 void	EHCI_int_InterruptThread(void *ControllerPtr);
 
 // === GLOBALS ===
@@ -77,22 +81,18 @@ int EHCI_Initialise(char **Arguments)
 {
 	for( int id = -1; (id = PCI_GetDeviceByClass(0x0C0320, 0xFFFFFF, id)) >= 0;  )
 	{
-		Uint32	addr = PCI_GetBAR(id, 0);
+		Uint32	addr = PCI_GetValidBAR(id, 0, PCI_BARTYPE_MEM);
 		if( addr == 0 ) {
-			// TODO: PCI BIOS emulation time
-		}
-		if( addr & 1 ) {
-			// TODO: Error
+			Log_Error("EHCI", "PCI%i BAR0 is not memory", id);
 			continue ;
 		}
-		addr &= ~0xF;
 		Uint8	irq = PCI_GetIRQ(id);
 		if( irq == 0 ) {
-			// TODO: Error?
+			Log_Error("EHCI", "PCI%i has no IRQ", id);
+			continue ;
 		}
 
-		Log_Log("ECHI", "Controller at PCI %i 0x%x IRQ 0x%x",
-			id, addr, irq);
+		Log_Log("ECHI", "Controller at PCI %i 0x%x IRQ 0x%x", id, addr, irq);
 
 		if( EHCI_InitController(addr, irq) )
 		{
@@ -141,6 +141,7 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 	ENTER("PBaseAddress iInterruptNum",
 		BaseAddress, InterruptNum);
 
+	// Allocate a controller structure
 	for( int i = 0; i < EHCI_MAX_CONTROLLERS; i ++ )
 	{
 		if( gaEHCI_Controllers[i].PhysBase == 0 ) {
@@ -168,7 +169,6 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 		goto _error;
 	}
 	LOG("cont->CapRegs = %p", cont->CapRegs);
-	// TODO: Error check
 	if( (cont->CapRegs->CapLength & 3) ) {
 		Log_Warning("EHCI", "Controller at %P non-aligned op regs (%x)",
 			BaseAddress, cont->CapRegs->CapLength);
@@ -189,10 +189,10 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 		Log_Warning("ECHI", "Can't allocate 1 32-bit page for periodic queue");
 		goto _error;
 	}
+	//  > Populate queue (with non-present entries)
 	for( int i = 0; i < 1024; i ++ )
 		cont->PeriodicQueue[i] = 1;
-	// TODO: Error check
-	//  > Populate queue
+	// TODO: For 64-bit, QH pool and periodic queue need to be in the same 32-bit segment
 
 	// - Allocate TD pool
 	cont->TDPool = (void*)MM_AllocDMA(1, 32, &unused);
@@ -201,7 +201,7 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 		goto _error;
 	}
 	for( int i = 0; i < TD_POOL_SIZE; i ++ ) {
-		cont->TDPool[i].Token = 3 << 8;
+		cont->TDPool[i].Token = 3 << 8;	// TODO: what is this value?
 	}
 
 	// Get port count
@@ -218,17 +218,19 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 	LOG("cont->InterruptThread = %p", cont->InterruptThread);
 
 	// Dummy TD
-	cont->DeadTD = EHCI_int_AllocateTD(cont, 0, NULL, 0, NULL, NULL);
+	cont->DeadTD = EHCI_int_AllocateTD(cont, 0, NULL, 0);
 	memset(cont->DeadTD, 0, sizeof(tEHCI_qTD));
 	cont->DeadTD->Link = 1;
 	cont->DeadTD->Link2 = 1;
 	cont->DeadTD->Token = QTD_TOKEN_STS_HALT;
 	
 	// Dummy QH
-	cont->DeadQH = EHCI_int_AllocateQH(cont, 0, 0);
+	// - HLink is set to itself, initing a circular list
+	cont->DeadQH = EHCI_int_AllocateQH(cont, NULL, NULL, NULL);
 	memset(cont->DeadQH, 0, sizeof(tEHCI_QH));
 	cont->DeadQH->HLink = MM_GetPhysAddr(cont->DeadQH)|2;
-	cont->DeadQH->Endpoint = (1<<15);	// H - Head of Reclamation List
+	cont->DeadQH->Impl.Next = cont->DeadQH;
+	cont->DeadQH->Endpoint = QH_ENDPT_H;	// H - Head of Reclamation List
 	cont->DeadQH->CurrentTD = MM_GetPhysAddr(cont->DeadTD);
 	cont->DeadQH->Overlay.Link = MM_GetPhysAddr(cont->DeadTD);
 
@@ -237,7 +239,7 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 	cont->OpRegs->USBCmd = USBCMD_HCReset;
 	// - Set CTRLDSSEGMENT (TODO: 64-bit support)
 	// - Set USBINTR
-	cont->OpRegs->USBIntr = USBINTR_IOC|USBINTR_PortChange|USBINTR_FrameRollover;
+	cont->OpRegs->USBIntr = USBINTR_IOC|USBINTR_PortChange|USBINTR_FrameRollover|USBINTR_IntrAsyncAdvance;
 	// - Set PERIODICLIST BASE
 	cont->OpRegs->PeridocListBase = MM_GetPhysAddr( cont->PeriodicQueue );
 	// - Set ASYNCLISTADDR
@@ -256,89 +258,21 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 _error:
 	cont->PhysBase = 0;
 	if( cont->CapRegs )
-		MM_Deallocate( (tVAddr)cont->CapRegs );
+		MM_Deallocate( cont->CapRegs );
 	if( cont->PeriodicQueue )
-		MM_Deallocate( (tVAddr)cont->PeriodicQueue );
+		MM_Deallocate( cont->PeriodicQueue );
 	if( cont->TDPool )
-		MM_Deallocate( (tVAddr)cont->TDPool );
+		MM_Deallocate( cont->TDPool );
 	LEAVE('i', 2);
 	return 2;
 }
 
-void EHCI_InterruptHandler(int IRQ, void *Ptr)
-{
-	tEHCI_Controller *Cont = Ptr;
-	Uint32	sts = Cont->OpRegs->USBSts;
-	Uint32	orig_sts = sts;
-
-	if( sts & 0xFFFF0FC0 ) {
-		LOG("Oops, reserved bits set (%08x), funny hardware?", sts);
-		sts &= ~0xFFFF0FFC0;
-	}
-
-	// Unmask read-only bits
-	sts &= ~(0xF000);
-
-	if( sts & USBINTR_IOC ) {
-		// IOC
-		LOG("%P IOC", Cont->PhysBase);
-		Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IOC);
-		sts &= ~USBINTR_IOC;
-	}
-
-	if( sts & USBINTR_AsyncAdvance ) {
-		LOG("%P IAAD", Cont->PhysBase);
-		#if 0
-		if( Cont->AsyncQHAddHead )
-		{
-		}
-		#endif
-		sts &= ~USBINTR_AsyncAdvance;
-	}
-
-	if( sts & USBINTR_PortChange ) {
-		// Port change, determine what port and poke helper thread
-		LOG("%P Port status change", Cont->PhysBase);
-		Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_PORTSC);
-		sts &= ~USBINTR_PortChange;
-	}
-	
-	if( sts & USBINTR_FrameRollover ) {
-		// Frame rollover, used to aid timing (trigger per-second operations)
-		//LOG("%p Frame rollover", Ptr);
-		sts &= ~USBINTR_FrameRollover;
-	}
-
-	if( sts ) {
-		// Unhandled interupt bits
-		// TODO: Warn
-		LOG("WARN - Bitmask %x unhandled", sts);
-	}
 
-
-	// Clear interrupts
-	Cont->OpRegs->USBSts = orig_sts;
-}
-
-// --------------------------------------------------------------------
-// USB API
-// --------------------------------------------------------------------
-void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
-	tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
+static inline int _GetClosestPower2(int Period)
 {
-	tEHCI_Controller	*Cont = Ptr;
-	 int	pow2period, period_pow;
-	
-	ASSERTCR(Endpoint, <, 256*16, NULL);
-	ASSERTCR(Period, >, 0, NULL);
-	if( Period > 256 )
-		Period = 256;
-
-	LOG("Endpoint=%x, bOutbound=%i, Period=%i, Length=%i", Endpoint, bOutbound, Period, Length);
-
 	// Round the period to the closest power of two
-	pow2period = 1;
-	period_pow = 0;
+	int pow2period = 1;
+	int period_pow = 0;
 	// - Find the first power above the period
 	while( pow2period < Period )
 	{
@@ -346,19 +280,19 @@ void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 		period_pow ++;
 	}
 	// - Check which is closest
-	if( Period - pow2period / 2 > pow2period - Period )
-		Period = pow2period;
+	if( Period - pow2period / 2 > pow2period - Period ) {
+		;
+	}
 	else {
-		Period = pow2period/2;
 		period_pow --;
 	}
-	LOG("period_pow = %i, Period = %ims", period_pow, Period);
-	
-	// Allocate a QH
-	tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, Endpoint, Length);
-	qh->Impl.IntPeriodPow = period_pow;
-	qh->EndpointExt |= 1;	// TODO: uFrame load balancing (8 entry bitfield)
+	LOG("period_pow = %i (%ims) from %ims", period_pow, 1 << period_pow, Period);
+	return period_pow;
+}
 
+void EHCI_int_AddToPeriodic(tEHCI_Controller *Cont, tEHCI_QH *qh, int PeriodPow2, size_t Load)
+{
+	 int	Period = 1 << PeriodPow2;
 	Mutex_Acquire(&Cont->PeriodicListLock);
 
 	// Choose an interrupt slot to use	
@@ -376,14 +310,10 @@ void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 	}
 	// Increase loading on the selected slot
 	for( int i = minslot; i < PERIODIC_SIZE; i += Period )
-		Cont->InterruptLoad[i] += Length;
-	qh->Impl.IntOfs = minslot;
+		Cont->InterruptLoad[i] += Load;
+	qh->Impl.Endpt->InterruptOfs = minslot;
 	LOG("Using slot %i", minslot);
 
-	// Allocate TD for the data
-	tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length, Cb, CbData);
-	EHCI_int_AppendTD(Cont, qh, td);
-
 	// Insert into the periodic list
 	for( int i = 0; i < PERIODIC_SIZE; i += Period )
 	{
@@ -397,7 +327,7 @@ void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 		{
 			if( nqh == qh )
 				break;
-			if( nqh->Impl.IntPeriodPow < period_pow )
+			if( nqh->Impl.Endpt->PeriodPow2 < PeriodPow2 )
 				break;
 		}
 
@@ -407,7 +337,7 @@ void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 
 		if( qh->Impl.Next && qh->Impl.Next != nqh ) {
 			Log_Warning("EHCI", "Suspected bookkeeping error on %p - int list %i+%i overlap",
-				Cont, period_pow, minslot);
+				Cont, PeriodPow2, minslot);
 			break;
 		}
 
@@ -430,8 +360,43 @@ void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 		}
 	}
 	Mutex_Release(&Cont->PeriodicListLock);
+}
 
-	return qh;
+// --------------------------------------------------------------------
+// USB API
+// --------------------------------------------------------------------
+void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
+	tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
+{
+	tEHCI_Controller	*Cont = Ptr;
+	
+	ASSERTCR(Endpoint, <, 256*16, NULL);
+	ASSERTCR(Period, >, 0, NULL);
+	if( Period > 256 )
+		Period = 256;
+
+	LOG("Endpoint=%x, bOutbound=%i, Period=%i, Length=%i", Endpoint, bOutbound, Period, Length);
+
+	int period_pow = _GetClosestPower2(Period);
+
+	tEHCI_Endpoint	*endpt = EHCI_int_AllocateEndpt(Cont, Endpoint, Length, period_pow);
+	endpt->Next = Cont->FirstInterruptEndpt;
+	Cont->FirstInterruptEndpt = endpt;
+
+	// Allocate a QH
+	tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
+	qh->EndpointExt |= 1;	// TODO: uFrame load balancing (8 entry bitfield)
+
+	// Allocate TD for the data
+	tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length);
+	qh->CurrentTD = MM_GetPhysAddr(td);
+	qh->Impl.FirstTD = td;
+	qh->Impl.LastTD = td;
+
+	// - Insert into period queue
+	EHCI_int_AddToPeriodic(Cont, qh, period_pow, Length);
+
+	return endpt;
 }
 
 void *EHCI_InitIsoch(void *Ptr, int Endpoint, size_t MaxPacketSize)
@@ -442,31 +407,11 @@ void *EHCI_InitIsoch(void *Ptr, int Endpoint, size_t MaxPacketSize)
 }
 void *EHCI_InitControl(void *Ptr, int Endpoint, size_t MaxPacketSize)
 {
-	tEHCI_Controller *Cont = Ptr;
-	
-	ENTER("pPtr iEndpoint iMaxPacketSize",
-		Ptr, Endpoint, MaxPacketSize);
-	
-	// Allocate a QH
-	tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, Endpoint, MaxPacketSize);
-	qh->CurrentTD = MM_GetPhysAddr(Cont->DeadTD);
-
-	// Append to async list
-	// TODO: Lock async list
-	qh->Impl.Next = Cont->DeadQH->Impl.Next;
-	Cont->DeadQH->Impl.Next = qh;
-	qh->HLink = Cont->DeadQH->HLink;
-	Cont->DeadQH->HLink = MM_GetPhysAddr(qh)|2;
-
-	LOG("Created %p(%P) for %P Ep 0x%x - %i bytes MPS",
-		qh, MM_GetPhysAddr(qh), Cont->PhysBase, Endpoint, MaxPacketSize);
-
-	LEAVE('p', qh);
-	return qh;
+	return EHCI_int_AllocateEndpt(Ptr, Endpoint, MaxPacketSize, -1);
 }
 void *EHCI_InitBulk(void *Ptr, int Endpoint, size_t MaxPacketSize)
 {
-	return EHCI_InitControl(Ptr, Endpoint, MaxPacketSize);
+	return EHCI_int_AllocateEndpt(Ptr, Endpoint, MaxPacketSize, -2);
 }
 void EHCI_RemEndpoint(void *Ptr, void *Handle)
 {
@@ -475,19 +420,20 @@ void EHCI_RemEndpoint(void *Ptr, void *Handle)
 	else if( (tVAddr)Handle <= 256*16 )
 		return ;	// Isoch
 	else {
-		tEHCI_QH	*qh = Ptr;
+		tEHCI_Endpoint	*endpt = Ptr;
 
 		// Remove QH from list
 		// - If it's a polling endpoint, need to remove from all periodic lists
-		if( qh->Impl.IntPeriodPow != 0xFF) {
+		if( endpt->PeriodPow2 >= 0) {
 			// Poll
 		}
 		else {
-			// GP
+			// Control/Bulk
 		}
 		
-		// Deallocate QH
-		EHCI_int_DeallocateQH(Ptr, Handle);
+		// Deallocate endpoint
+		// TODO: Write EHCI_DeallocateEndpoint
+		free(endpt);
 	}
 }
 
@@ -499,74 +445,100 @@ void *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
 	)
 {
 	tEHCI_Controller *Cont = Ptr;
+	tEHCI_Endpoint	*endpt = Dest;
+	tEHCI_QH	*qh;
 	tEHCI_qTD	*td_setup, *td_data, *td_status;
 
 	// Sanity checks
 	if( (tVAddr)Dest <= 256*16 )
 		return NULL;
+	if( endpt->PeriodPow2 != -1 ) {
+		Log_Notice("EHCI", "Non-control endpoint passed to SendControl");
+		return NULL;
+	}
+	if( SetupLength > endpt->MaxPacketSize )
+		return NULL;
+	// TODO: Check size of status
+	
 
 	LOG("Dest=%p, isOutbound=%i, Lengths(Setup:%i,Out:%i,In:%i)",
 		Dest, isOutbound, SetupLength, OutLength, InLength);
 
-	// TODO: Check size of SETUP and status
-	
+	// Allocate a QH to work with
+	qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
+
 	// Allocate TDs
-	td_setup = EHCI_int_AllocateTD(Cont, PID_SETUP, (void*)SetupData, SetupLength, NULL, NULL);
+	td_setup = EHCI_int_AllocateTD(Cont, PID_SETUP, (void*)SetupData, SetupLength);
 	if( isOutbound )
 	{
-		td_data = OutData ? EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength, NULL, NULL) : NULL;
-		td_status = EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength, Cb, CbData);
+		td_data = OutData ? EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength) : NULL;
+		td_status = EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength);
 	}
 	else
 	{
-		td_data = InData ? EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength, NULL, NULL) : NULL;
-		td_status = EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength, Cb, CbData);
+		td_data = InData ? EHCI_int_AllocateTD(Cont, PID_IN, InData, InLength) : NULL;
+		td_status = EHCI_int_AllocateTD(Cont, PID_OUT, (void*)OutData, OutLength);
 	}
 	td_status->Token |= QTD_TOKEN_IOC;	// IOC
 
 	// Append TDs
 	if( td_data ) {
 		td_setup->Link = MM_GetPhysAddr(td_data);
-		td_setup->Next = td_data;
 		td_data->Link = MM_GetPhysAddr(td_status);
-		td_data->Next = td_status;
 		td_data->Token |= QTD_TOKEN_STS_ACTIVE;	// Active
 	}
 	else {
 		td_setup->Link = MM_GetPhysAddr(td_status);
-		td_setup->Next = td_status;
 	}
 	td_setup->Token |= QTD_TOKEN_STS_ACTIVE;	// Active
 	td_status->Token |= QTD_TOKEN_STS_ACTIVE;
 	td_status->Link = 1;
 	td_status->Link2 = 1;
-	EHCI_int_AppendTD(Cont, Dest, td_setup);
+	
+	// Set QH's current pointer
+	qh->CurrentTD = MM_GetPhysAddr(td_setup);
+	qh->Impl.FirstTD = td_setup;
+	qh->Impl.LastTD = td_status;
+	EHCI_int_AppendQHToAsync(Cont, qh);
 
-	LOG("return td_status=%p", td_status);
-	return td_status;
+	return qh;
 }
 
 void *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir, void *Data, size_t Length)
 {
 	tEHCI_Controller	*Cont = Ptr;
+	tEHCI_Endpoint	*endpt = Dest;
 	
 	// Sanity check the pointer
 	// - Can't be NULL or an isoch
 	if( (tVAddr)Dest <= 256*16 )
 		return NULL;
+	if( endpt->PeriodPow2 != -2 ) {
+		return NULL;
+	}
+	LOG("Ptr=%p,Dest=%p(%x),Cb=%p(%p),DirIsOut=%i,Data=%p+0x%x)",
+		Ptr, Dest, endpt->EndpointID, Cb, CbData, Dir, Data, Length);
+
+	// Allocate a QH to work with
+	tEHCI_QH *qh = EHCI_int_AllocateQH(Cont, endpt, Cb, CbData);
 	
 	// Allocate single TD
-	tEHCI_qTD	*td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length, Cb, CbData);
-	EHCI_int_AppendTD(Cont, Dest, td);
+	tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length);
+	td->Token |= QTD_TOKEN_IOC | QTD_TOKEN_STS_ACTIVE;
+	qh->CurrentTD = MM_GetPhysAddr(td);
+	qh->Impl.FirstTD = td;
+	qh->Impl.LastTD = td;
+	
+	EHCI_int_AppendQHToAsync(Cont, qh);
 
-	return td;
+	return qh;
 }
 
 void EHCI_FreeOp(void *Ptr, void *Handle)
 {
 	tEHCI_Controller	*Cont = Ptr;
 
-	EHCI_int_DeallocateTD(Cont, Handle);
+	EHCI_int_DeallocateQH(Cont, Handle);
 }
 
 Uint32 EHCI_int_RootHub_FeatToMask(int Feat)
@@ -608,6 +580,27 @@ int EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag)
 // --------------------------------------------------------------------
 // Internals
 // --------------------------------------------------------------------
+tEHCI_Endpoint *EHCI_int_AllocateEndpt(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize, int PeriodPow2)
+{
+	ENTER("pCont iEndpoint iMaxPacketSize",
+		Cont, Endpoint, MaxPacketSize);
+	
+	tEHCI_Endpoint	*endpt = malloc( sizeof(*endpt) );
+	endpt->NextToggle = FALSE;
+	endpt->EndpointID = Endpoint;
+	endpt->MaxPacketSize = MaxPacketSize;	// TODO: Sanity Check
+	endpt->PeriodPow2 = PeriodPow2;
+	endpt->InterruptOfs = 0;
+	endpt->ActiveQHs = NULL;
+	// TODO: store endpoints on controller
+	
+	LOG("Created %p for %P Ep 0x%x - %i bytes MPS",
+		endpt, Cont->PhysBase, Endpoint, MaxPacketSize);
+
+	LEAVE('p', endpt);
+	return endpt;
+}
+
 tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
 {
 	if( Addr == 0 )	return NULL;
@@ -615,22 +608,26 @@ tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
 	return Cont->TDPool + (Addr - MM_GetPhysAddr(Cont->TDPool))/sizeof(tEHCI_qTD);
 }
 
-tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length, tUSBHostCb Cb, void *CbData)
+tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length)
 {
 //	Semaphore_Wait(&Cont->TDSemaphore, 1);
 	Mutex_Acquire(&Cont->TDPoolMutex);
 	for( int i = 0; i < TD_POOL_SIZE; i ++ )
 	{
+		// PID code == 3: Avaliable for use
 		if( ((Cont->TDPool[i].Token >> 8) & 3) != 3 )
 			continue ;
+		
 		Cont->TDPool[i].Token = (PID << 8) | (Length << 16);
 		Mutex_Release(&Cont->TDPoolMutex);
 		
 		tEHCI_qTD	*td = &Cont->TDPool[i];
-		td->Size = Length;
-		td->Callback = Cb;
-		td->CallbackData = CbData;
-		// NOTE: Assumes that `Length` is <= PAGE_SIZE
+		td->Impl.Ptr = Data;
+		td->Impl.Length = Length;
+		td->Link = 1;
+		td->Link2 = 1;
+		
+		// TODO: Support breaking across multiple pages
 		ASSERTC(Length, <, PAGE_SIZE);
 		// TODO: Handle bouncing >32-bit pages
 		#if PHYS_BITS > 32
@@ -639,8 +636,9 @@ tEHCI_qTD *EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size
 		td->Pages[0] = MM_GetPhysAddr(Data);
 		if( (td->Pages[0] & (PAGE_SIZE-1)) + Length - 1 > PAGE_SIZE )
 			td->Pages[1] = MM_GetPhysAddr((char*)Data + Length - 1) & ~(PAGE_SIZE-1);
-		LOG("Allocated %p(%P) for PID %i on %P",
-			td, MM_GetPhysAddr(td), PID, Cont->PhysBase);
+		LOG("Allocated %p(%P) PID%i on %P (Token=0x%x)",
+			td, MM_GetPhysAddr(td), PID, Cont->PhysBase,
+			td->Token);
 		return td;
 	}
 
@@ -653,52 +651,13 @@ void EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD)
 	UNIMPLEMENTED();
 }
 
-void EHCI_int_AppendTD(tEHCI_Controller *Cont, tEHCI_QH *QH, tEHCI_qTD *TD)
-{
-	tEHCI_qTD	*ptd = NULL;
-	Uint32	link = QH->CurrentTD;
-	tPAddr	dead_td = MM_GetPhysAddr(Cont->DeadTD);
-	
-	{
-		Mutex_Acquire(&Cont->ActiveTDsLock);
-		if( Cont->ActiveTDTail )
-			Cont->ActiveTDTail->Next = TD;
-		else
-			Cont->ActiveTDHead = TD;
-		tEHCI_qTD	*last;
-		for( last = TD; last->Next; last = last->Next )
-			;
-		Cont->ActiveTDTail = last;
-		Mutex_Release(&Cont->ActiveTDsLock);
-	}
-
-	// TODO: Need locking and validation here
-	while( link && !(link & 1) && link != dead_td )
-	{
-		ptd = EHCI_int_GetTDFromPhys(Cont, link);
-		link = ptd->Link;
-	}
-	// TODO: Figure out how to follow this properly
-	if( !ptd ) {
-		QH->CurrentTD = MM_GetPhysAddr(TD)|2;
-		QH->Overlay.Link = QH->CurrentTD;
-		LOG("Appended %p to beginning of %p", TD, QH);
-	}
-	else {
-		ptd->Link = MM_GetPhysAddr(TD);
-		ptd->Link2 = MM_GetPhysAddr(TD);
-		LOG("Appended %p to end of %p", TD, QH);
-	}
-	QH->Endpoint |= QH_ENDPT_H;
-	QH->Overlay.Token &= ~QTD_TOKEN_STS_HALT;
-}
-
-tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize)
+tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, tEHCI_Endpoint *Endpoint, tUSBHostCb Cb, void *CbData)
 {
 	tEHCI_QH	*ret;
 	Mutex_Acquire(&Cont->QHPoolMutex);
 	for( int i = 0; i < QH_POOL_SIZE; i ++ )
 	{
+		// If page not yet allocated, allocate it
 		if( !MM_GetPhysAddr( Cont->QHPools[i/QH_POOL_NPERPAGE] ) ) {
 			tPAddr	tmp;
 			Cont->QHPools[i/QH_POOL_NPERPAGE] = (void*)MM_AllocDMA(1, 32, &tmp);
@@ -706,18 +665,26 @@ tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPa
 		}
 
 		ret = &Cont->QHPools[i/QH_POOL_NPERPAGE][i%QH_POOL_NPERPAGE];
-		if( ret->HLink == 0 ) {
-			memset(ret, 0, sizeof(*ret));
-			ret->HLink = 1;
-			ret->Overlay.Link = MM_GetPhysAddr(Cont->DeadTD);
-			ret->Endpoint = (Endpoint >> 4) | 0x80 | ((Endpoint & 0xF) << 8)
-				| (MaxPacketSize << 16);
-			ret->EndpointExt = (1<<30);
-			// TODO: Endpoint speed (13:12) 0:Full, 1:Low, 2:High
-			// TODO: Control Endpoint Flag (27) 0:*, 1:Full/Low Control
-			Mutex_Release(&Cont->QHPoolMutex);
-			return ret;
+		if( ret->HLink != 0 )
+			continue ;
+		
+		memset(ret, 0, sizeof(*ret));
+		ret->HLink = 1;
+		Mutex_Release(&Cont->QHPoolMutex);
+		
+		if( Endpoint )
+		{
+			ret->Endpoint = (Endpoint->EndpointID >> 4) | 0x80 | ((Endpoint->EndpointID & 0xF) << 8)
+				| (Endpoint->MaxPacketSize << 16);
 		}
+		ret->EndpointExt = (1<<30);
+		ret->Impl.Next = NULL;
+		ret->Impl.Callback = Cb;
+		ret->Impl.CallbackData = CbData;
+		ret->Impl.Endpt = Endpoint;
+		// TODO: Endpoint speed (13:12) 0:Full, 1:Low, 2:High
+		// TODO: Control Endpoint Flag (27) 0:*, 1:Full/Low Control
+		return ret;
 	}
 	Mutex_Release(&Cont->QHPoolMutex);
 	return NULL;
@@ -725,10 +692,83 @@ tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPa
 
 void EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH)
 {
-	// TODO: Ensure it's unused (somehow)
+	UNIMPLEMENTED();
+	// TODO: Ensure it's unused and clean up associated TDs
 	QH->HLink = 0;
 }
 
+void EHCI_int_AppendQHToAsync(tEHCI_Controller *Cont, tEHCI_QH *QH)
+{
+	QH->Overlay.Token = QTD_TOKEN_STS_ACTIVE;
+	
+	Mutex_Acquire(&Cont->lAsyncSchedule);
+	// Insert into list
+	QH->HLink = Cont->DeadQH->HLink;
+	QH->Impl.Next = Cont->DeadQH->Impl.Next;
+	Cont->DeadQH->HLink = MM_GetPhysAddr(QH)|2;
+	Cont->DeadQH->Impl.Next = QH;
+	// Start async schedule
+	// - Set ASYNCENABLE
+	// - Clear 'H' in dead QH
+	Mutex_Release(&Cont->lAsyncSchedule);
+	LOG("Appended %P to %p(%P)", MM_GetPhysAddr(QH), Cont, Cont->PhysBase);
+}
+
+// --------------------------------------------------------------------
+// Interrupt actions
+// --------------------------------------------------------------------
+void EHCI_InterruptHandler(int IRQ, void *Ptr)
+{
+	tEHCI_Controller *Cont = Ptr;
+	Uint32	sts = Cont->OpRegs->USBSts;
+	Uint32	orig_sts = sts;
+	const Uint32	reserved_bits = 0xFFFF0FC0;
+
+	if( sts & reserved_bits ) {
+		LOG("Oops, reserved bits set (%08x), funny hardware?", sts);
+		sts &= ~reserved_bits;
+	}
+
+	// Unmask read-only bits
+	sts &= ~(0xF000);
+
+	if( sts & USBINTR_IOC ) {
+		// IOC
+		LOG("%P IOC", Cont->PhysBase);
+		Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IOC);
+		sts &= ~USBINTR_IOC;
+	}
+
+	if( sts & USBINTR_IntrAsyncAdvance ) {
+		LOG("%P IAAD", Cont->PhysBase);
+		Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_IAAD);
+		sts &= ~USBINTR_IntrAsyncAdvance;
+	}
+
+	if( sts & USBINTR_PortChange ) {
+		// Port change, determine what port and poke helper thread
+		LOG("%P Port status change", Cont->PhysBase);
+		Threads_PostEvent(Cont->InterruptThread, EHCI_THREADEVENT_PORTSC);
+		sts &= ~USBINTR_PortChange;
+	}
+	
+	if( sts & USBINTR_FrameRollover ) {
+		// Frame rollover, used to aid timing (trigger per-second operations)
+		//LOG("%p Frame rollover", Ptr);
+		sts &= ~USBINTR_FrameRollover;
+	}
+
+	if( sts ) {
+		// Unhandled interupt bits
+		// TODO: Warn
+		LOG("WARN - Bitmask %x unhandled", sts);
+	}
+
+
+	// Clear interrupts
+	Cont->OpRegs->USBSts = orig_sts;
+}
+
 void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
 {
 	// Connect Event
@@ -763,56 +803,129 @@ void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
 	}
 }
 
+void EHCI_int_CheckInterruptQHs(tEHCI_Controller *Cont)
+{
+	for( tEHCI_Endpoint *endpt = Cont->FirstInterruptEndpt; endpt; endpt = endpt->Next )
+	{
+		tEHCI_QH *qh = endpt->ActiveQHs;
+		// Check if the TD of the first QH is active
+		if( qh->Overlay.Token & QTD_TOKEN_STS_ACTIVE )
+			continue ;
+		// Inactive, fire interrupt and re-trigger
+		if( !qh->Impl.Callback ) {
+			// Umm... ?
+		}
+		else
+		{
+			tEHCI_qTD	*last = qh->Impl.LastTD;
+			size_t	remaining_len = (last->Token >> 16) & 0x7FFF;
+			if( remaining_len > last->Impl.Length )
+				remaining_len = last->Impl.Length;
+			size_t	transferred_len = last->Impl.Length - remaining_len;
+			
+			LOG("Calling %p(%p) for Intr EndPt %x (%p+0x%x)",
+				qh->Impl.Callback, qh->Impl.CallbackData,
+				qh->Impl.Endpt->EndpointID, last->Impl.Ptr, transferred_len);
+			qh->Impl.Callback(qh->Impl.CallbackData, last->Impl.Ptr, transferred_len);
+		}
+		qh->Impl.FirstTD->Token |= QTD_TOKEN_STS_ACTIVE;
+		qh->Overlay.Token |= QTD_TOKEN_STS_ACTIVE;
+	}
+}
+
+void EHCI_int_RetireQHs(tEHCI_Controller *Cont)
+{
+	tEHCI_QH	*prev = Cont->DeadQH;
+	Mutex_Acquire(&Cont->lAsyncSchedule);
+	for( tEHCI_QH *qh = prev->Impl.Next; qh != Cont->DeadQH; qh = prev->Impl.Next )
+	{
+		ASSERT(qh);
+		ASSERT(qh != qh->Impl.Next);
+		if( qh->Overlay.Token & QTD_TOKEN_STS_ACTIVE ) {
+			prev = qh;
+			continue ;
+		}
+		
+		// Remove from async list
+		prev->HLink = qh->HLink;
+		prev->Impl.Next = qh->Impl.Next;
+		
+		// Add to reclaim list
+		qh->Impl.Next = Cont->ReclaimList;
+		Cont->ReclaimList = qh;
+		
+		// Ring doorbell!
+		Cont->OpRegs->USBCmd |= USBCMD_IAAD;
+	}
+	Mutex_Release(&Cont->lAsyncSchedule);
+}
+
+/*
+ * Fire callbacks on QHs and mark them as completed
+ * 
+ * TODO: Possible bug with items being added to reclaim list before
+ *       the last doorbell fires.
+ */
+void EHCI_int_ReclaimQHs(tEHCI_Controller *Cont)
+{
+	// Doorbell was rung, so reclaim QHs
+	// - Actually just fires callbacks, now that we know that the QHs can be cleared
+	tEHCI_QH	*qh;
+	Mutex_Acquire(&Cont->lReclaimList);
+	while( (qh = Cont->ReclaimList) )
+	{
+		Cont->ReclaimList = qh->Impl.Next;
+		Mutex_Release(&Cont->lReclaimList);
+
+		if( qh->Impl.Callback )
+		{
+			tEHCI_qTD	*last = qh->Impl.LastTD;
+			size_t	remaining_len = (last->Token >> 16) & 0x7FFF;
+			if( remaining_len > last->Impl.Length )
+				remaining_len = last->Impl.Length;
+			size_t	transferred_len = last->Impl.Length - remaining_len;
+			
+			LOG("Calling %p(%p) for EndPt %x (%p+0x%x)",
+				qh->Impl.Callback, qh->Impl.CallbackData,
+				qh->Impl.Endpt->EndpointID, last->Impl.Ptr, transferred_len);
+			//if( last->Impl.Ptr )
+			//	Debug_HexDump("EHCI Callback Data", last->Impl.Ptr, transferred_len);
+			qh->Impl.Callback(qh->Impl.CallbackData, last->Impl.Ptr, transferred_len);
+		}
+		
+		Mutex_Acquire(&Cont->lReclaimList);
+	}
+	Mutex_Release(&Cont->lReclaimList);
+}
+
 void EHCI_int_InterruptThread(void *ControllerPtr)
 {
 	tEHCI_Controller	*Cont = ControllerPtr;
+	Threads_SetName("EHCI Interrupt Worker");
 	while(Cont->OpRegs)
 	{
 		Uint32	events;
 		
 		LOG("sleeping for events");
-		events = Threads_WaitEvents(EHCI_THREADEVENT_IOC|EHCI_THREADEVENT_PORTSC);
+		events = Threads_WaitEvents(EHCI_THREADEVENT_IOC|EHCI_THREADEVENT_PORTSC|EHCI_THREADEVENT_IAAD);
 		if( !events ) {
 			// TODO: Should this cause a termination?
 		}
 		LOG("events = 0x%x", events);
 
+		if( events & EHCI_THREADEVENT_IAAD )
+		{
+			EHCI_int_ReclaimQHs(Cont);
+		}
+
 		if( events & EHCI_THREADEVENT_IOC )
 		{
-			// IOC, Do whatever it is you do
-			Log_Warning("EHCI", "%P IOC - TODO: Call registered callbacks and reclaim",
-				Cont->PhysBase);
-			// Scan active TDs
-			Mutex_Acquire(&Cont->ActiveTDsLock);
-			tEHCI_qTD *prev = NULL;
-			for(tEHCI_qTD *td = Cont->ActiveTDHead; td; td = td->Next)
-			{
-				LOG("td(%p)->Token = %x", td, td->Token);
-				// If active, continue
-				if( td->Token & QTD_TOKEN_STS_ACTIVE ) {
-					prev = td;
-					continue ;
-				}
-				
-				// Inactive
-				LOG("%p Complete", td);
-				// - call the callback
-				if( td->Callback )
-				{
-					void *ptr = NULL;
-					if( td->Pages[0] ) {
-						Log_Warning("EHCI", "TODO: Map %x,%x+%i for callback",
-							td->Pages[0], td->Pages[1], td->Size);
-					}
-					td->Callback(td->CallbackData, ptr, td->Size);
-				}
-			
-				// Remove and release
-				*(prev ? &prev->Next : &Cont->ActiveTDHead) = td->Next;
-				td->Token = 0;
-			}
-			Cont->ActiveTDTail = prev;
-			Mutex_Release(&Cont->ActiveTDsLock);
+			// IOC, handle completed requests
+			// Search periodic lists for one that fired
+			EHCI_int_CheckInterruptQHs(Cont);
+			// Retire QHs
+			// - Remove them from the queue and ask the controller to bell when they're removable
+			EHCI_int_RetireQHs(Cont);
 		}
 
 		// Port status change interrupt
@@ -822,23 +935,26 @@ void EHCI_int_InterruptThread(void *ControllerPtr)
 			for(int i = 0; i < Cont->nPorts; i ++ )
 			{
 				Uint32	sts = Cont->OpRegs->PortSC[i];
-				LOG("Port %i: sts = %x", i, sts);
+				//LOG("Port %i: sts = %x", i, sts);
 				Cont->OpRegs->PortSC[i] = sts;
 				if( sts & PORTSC_ConnectStatusChange )
+				{
+					LOG("Port %i: Connect Change", i);
 					EHCI_int_HandlePortConnectChange(Cont, i);
+				}
 
 				if( sts & PORTSC_PortEnableChange )
 				{
 					// Handle enable/disable
+					LOG("Port %i: Enable Change", i);
 				}
 
 				if( sts & PORTSC_OvercurrentChange )
 				{
 					// Handle over-current change
+					LOG("Port %i: Over-Current Change", i);
 				}
 			}
 		}
-
-		LOG("Going back to sleep");
 	}
 }