X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FModules%2FUSB%2FUHCI%2Fuhci.c;h=dd0721037043c3c25bddb89fbc4d9a0980eee708;hb=bfda4d16beda7bf0725690c0a540166a8ca99fc4;hp=8e840ab5018fd0ca2b8136208d773140e7fd7798;hpb=3bcfc9ded1d44d1fbec95f73b5894e26f498b73d;p=tpg%2Facess2.git

diff --git a/KernelLand/Modules/USB/UHCI/uhci.c b/KernelLand/Modules/USB/UHCI/uhci.c
index 8e840ab5..dd072103 100644
--- a/KernelLand/Modules/USB/UHCI/uhci.c
+++ b/KernelLand/Modules/USB/UHCI/uhci.c
@@ -4,7 +4,7 @@
  *
  * Universal Host Controller Interface
  */
-#define DEBUG	1
+#define DEBUG	0
 #define VERSION	VER2(0,5)
 #include <acess.h>
 #include <vfs.h>
@@ -16,10 +16,12 @@
 #include <semaphore.h>
 
 // === CONSTANTS ===
-#define	MAX_CONTROLLERS	4
+#define	MAX_CONTROLLERS	8
 //#define NUM_TDs	1024
-#define NUM_TDs	64
+#define NUM_TDs	(PAGE_SIZE/sizeof(tUHCI_TD))
 #define MAX_PACKET_SIZE	0x400
+#define MAX_INTERRUPT_LOAD	1024	// Maximum bytes per frame for interrupts
+
 #define PID_IN	0x69
 #define PID_OUT	0xE1
 #define PID_SETUP	0x2D
@@ -53,7 +55,7 @@ static Uint16	_InWord(tUHCI_Controller *Host, int Reg);
 
 // === GLOBALS ===
 MODULE_DEFINE(0, VERSION, USB_UHCI, UHCI_Initialise, NULL, "USB_Core", NULL);
-tUHCI_TD	gaUHCI_TDPool[NUM_TDs];
+tUHCI_TD	*gaUHCI_TDPool;
 tUHCI_Controller	gUHCI_Controllers[MAX_CONTROLLERS];
 tUSBHostDef	gUHCI_HostDef = {
 	.InterruptIN   = UHCI_InterruptIN,
@@ -91,7 +93,13 @@ int UHCI_Initialise(char **Arguments)
 		LEAVE('i', MODULE_ERR_NOTNEEDED);
 		return MODULE_ERR_NOTNEEDED;
 	}
-	
+
+	{
+		tPAddr	tmp;	
+		gaUHCI_TDPool = (void *) MM_AllocDMA(1, 32, &tmp);
+		memset(gaUHCI_TDPool, 0, PAGE_SIZE);
+	}
+
 	// Enumerate PCI Bus, getting a maximum of `MAX_CONTROLLERS` devices
 	while( (id = PCI_GetDeviceByClass(0x0C0300, 0xFFFFFF, id)) >= 0 && i < MAX_CONTROLLERS )
 	{
@@ -159,11 +167,6 @@ int UHCI_int_InitHost(tUHCI_Controller *Host)
 {
 	ENTER("pHost", Host);
 
-	_OutWord( Host, USBCMD, 4 );	// GRESET
-	Time_Delay(10);
-	_OutWord( Host, USBCMD, 0 );	// GRESET
-	
-	// Allocate Frame List
 	// - 1 Page, 32-bit address
 	// - 1 page = 1024  4 byte entries
 	Host->FrameList = (void *) MM_AllocDMA(1, 32, &Host->PhysFrameList);
@@ -192,47 +195,33 @@ int UHCI_int_InitHost(tUHCI_Controller *Host)
 		Host->FrameList[i] = addr | 2;
 	}
 	for( int i = 0; i < 64; i ++ ) {
-		 int	ofs = dest_offsets[ i & (32-1) ];
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_256ms[ofs] );
+		 int	ofs = dest_offsets[ i & (32-1) ] * 2 + (i >= 32);
+		Uint32	addr = Host->PhysTDQHPage + ofs * sizeof(tUHCI_QH);
+		LOG("Slot %i to (%i,%i,%i,%i) ms slots",
+			ofs, 0 + i*4, 256 + i*4, 512 + i*4, 768 + i*4);
 		Host->FrameList[  0 + i*4] = addr | 2;
 		Host->FrameList[256 + i*4] = addr | 2;
 		Host->FrameList[512 + i*4] = addr | 2;
 		Host->FrameList[768 + i*4] = addr | 2;
 	}
-	for( int i = 0; i < 32; i ++ ) {
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_128ms[i] );
-		Host->TDQHPage->InterruptQHs_256ms[i*2  ].Next = addr | 2;
-		Host->TDQHPage->InterruptQHs_256ms[i*2+1].Next = addr | 2;
-	}
-	for( int i = 0; i < 16; i ++ ) {
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_64ms[i] );
-		Host->TDQHPage->InterruptQHs_128ms[i*2  ].Next = addr | 2;
-		Host->TDQHPage->InterruptQHs_128ms[i*2+1].Next = addr | 2;
-	}
-	for( int i = 0; i < 8; i ++ ) {
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_32ms[i] );
-		Host->TDQHPage->InterruptQHs_64ms[i*2  ].Next = addr | 2;
-		Host->TDQHPage->InterruptQHs_64ms[i*2+1].Next = addr | 2;
-	}
-	for( int i = 0; i < 4; i ++ ) {
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_16ms[i] );
-		Host->TDQHPage->InterruptQHs_32ms[i*2  ].Next = addr | 2;
-		Host->TDQHPage->InterruptQHs_32ms[i*2+1].Next = addr | 2;
-	}
-	for( int i = 0; i < 2; i ++ ) {
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_8ms[i] );
-		Host->TDQHPage->InterruptQHs_16ms[i*2  ].Next = addr | 2;
-		Host->TDQHPage->InterruptQHs_16ms[i*2+1].Next = addr | 2;
-	}
-	for( int i = 0; i < 1; i ++ ) {
-		Uint32	addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->InterruptQHs_4ms[i] );
-		Host->TDQHPage->InterruptQHs_8ms[i*2  ].Next = addr | 2;
-		Host->TDQHPage->InterruptQHs_8ms[i*2+1].Next = addr | 2;
-	}
-	Host->TDQHPage->InterruptQHs_4ms[0].Next = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->ControlQH ) | 2;
-	// Set child pointers
-	for( int i = 0; i < 127; i ++ ) {
-		Host->TDQHPage->InterruptQHs_256ms[i].Child = 1;
+
+	// Build up interrupt binary tree	
+	{
+		tUHCI_QH	*dest = Host->TDQHPage->InterruptQHs;
+		Uint32	destphys = Host->PhysTDQHPage;
+		
+		// Set up next pointer to index to i/2 in the next step
+		for( int _count = 64; _count > 1; _count /= 2 )
+		{
+			for( int i = 0; i < _count; i ++ ) {
+				dest[i].Next = destphys + (_count + i/2) * sizeof(tUHCI_QH) + 2;
+				dest[i].Child = 1;
+			}
+			dest +=	_count; destphys += _count * sizeof(tUHCI_QH);
+		}
+		// Skip padding, and move to control QH
+		dest->Next = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->BulkQH ) | 2;
+		dest->Child = 1;
 	}
 
 	// Set up control and bulk queues
@@ -240,7 +229,13 @@ int UHCI_int_InitHost(tUHCI_Controller *Host)
 	Host->TDQHPage->ControlQH.Child = 1;
 	Host->TDQHPage->BulkQH.Next = 1;
 	Host->TDQHPage->BulkQH.Child = 1;
+
+	// Global reset	
+	_OutWord( Host, USBCMD, 4 );
+	Time_Delay(10);
+	_OutWord( Host, USBCMD, 0 );
 	
+	// Allocate Frame List
 	// Set frame length to 1 ms
 	_OutByte( Host, SOFMOD, 64 );
 	
@@ -271,9 +266,9 @@ tUHCI_TD *UHCI_int_AllocateTD(tUHCI_Controller *Cont)
 		if(gaUHCI_TDPool[i]._info.bActive == 0)
 		{
 			gaUHCI_TDPool[i].Link = 1;
-			gaUHCI_TDPool[i].Control = (1 << 23);
+			gaUHCI_TDPool[i].Control = TD_CTL_ACTIVE;
 			gaUHCI_TDPool[i]._info.bActive = 1;
-			gaUHCI_TDPool[i]._info.period_entry = 0;
+			gaUHCI_TDPool[i]._info.QueueIndex = 128;
 			Mutex_Release( &lock );
 			return &gaUHCI_TDPool[i];
 		}
@@ -289,7 +284,12 @@ void UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_QH *QH, tUHCI_TD *TD)
 	Mutex_Acquire(&lock);
 	
 	// Ensure that there is an interrupt for each used frame
-	TD->Control |= (1 << 24);
+	TD->Control |= TD_CTL_IOC;
+	TD->_info.QueueIndex = ((tVAddr)QH - (tVAddr)Cont->TDQHPage->InterruptQHs) / sizeof(tUHCI_QH);
+	LOG("TD(%p)->QueueIndex = %i", TD, TD->_info.QueueIndex);
+	// Update length
+	TD->Control &= ~0x7FF;
+	TD->Control |= (TD->Token >> 21) & 0x7FF;
 
 	// Stop controller
 	_OutWord( Cont, USBCMD, 0x0000 );
@@ -310,7 +310,7 @@ void UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_QH *QH, tUHCI_TD *TD)
 	
 	// DEBUG!
 	LOG("QH(%p)->Child = %x", QH, QH->Child);
-	LOG("TD(%p)->Control = %x", TD, TD->Control);
+	LOG("TD(%p)->Control = %x, ->Link = %x", TD, TD->Control, TD->Link);
 
 	Mutex_Release(&lock);
 }
@@ -343,7 +343,14 @@ tUHCI_TD *UHCI_int_CreateTD(
 		td, Length, Type, Addr);
 
 	td->Control = (Length - 1) & 0x7FF;
-	td->Control |= (1 << 23);
+	td->Control |= TD_CTL_ACTIVE;	// Active set
+	td->Control |= (3 << 27);	// 3 retries
+	// High speed device (must be explicitly enabled
+	if( Addr & 0x8000 )
+		;
+	else
+		td->Control |= 1 << 26;
+		
 	td->Token  = ((Length - 1) & 0x7FF) << 21;
 	td->Token |= (bTgl & 1) << 19;
 	td->Token |= (Addr & 0xF) << 15;
@@ -375,7 +382,7 @@ tUHCI_TD *UHCI_int_CreateTD(
 			tVAddr	ptr = MM_MapTemp(td->BufferPointer);
 			memcpy( (void*)ptr, Data, Length );
 			MM_FreeTemp(ptr);
-			td->Control |= (1 << 24);
+			td->Control |= TD_CTL_IOC;
 		}
 		td->_info.bFreePointer = 1;
 	}
@@ -397,7 +404,7 @@ tUHCI_TD *UHCI_int_CreateTD(
 	
 	if( info ) {
 		LOG("info = %p", info);
-		td->Control |= (1 << 24);
+		td->Control |= TD_CTL_IOC;
 		td->_info.ExtraInfo = info;
 	}
 
@@ -407,12 +414,13 @@ tUHCI_TD *UHCI_int_CreateTD(
 void UHCI_int_SetInterruptPoll(tUHCI_Controller *Cont, tUHCI_TD *TD, int Period)
 {
 	tUHCI_QH	*qh;
-	const int	qh_offsets[] = { 0, 64, 96, 112, 120, 124, 126};
-//	const int	qh_sizes[]   = {64, 32, 16,   8,   4,   2,   1};
+	const int	qh_offsets[] = {126, 124, 120, 112, 96, 64,  0};
+	const int	qh_sizes[]   = {  1,   2,   4,   8, 16, 32, 64};
 	
 	// Bounds limit
 	if( Period < 0 )	return ;
 	if( Period > 256 )	Period = 256;
+	if( Period == 255 )	Period = 256;
 
 	// Get the log base2 of the period
 	 int	period_slot = 0;
@@ -422,12 +430,53 @@ void UHCI_int_SetInterruptPoll(tUHCI_Controller *Cont, tUHCI_TD *TD, int Period)
 	if( period_slot < 2 )	period_slot = 0;
 	else	period_slot -= 2;
 	
-	TD->_info.period_entry = qh_offsets[period_slot] + 1;
-	qh = Cont->TDQHPage->InterruptQHs_4ms + TD->_info.period_entry - 1;
+	// _AppendTD calculates this from qh, but we use it to determine qh
+	TD->_info.QueueIndex = qh_offsets[period_slot];
 	// TODO: Find queue with lowest load
+#if 1
+	 int	min_load = 0;
+	 int	min_load_slot = 0;
+	for( int i = 0; i < qh_sizes[period_slot]; i ++ )
+	{
+		int load, index;
+		index = qh_offsets[period_slot] + i;
+		load = 0;
+		while( index >= 0 && index < 127 )
+		{
+			qh = Cont->TDQHPage->InterruptQHs + index;
+			load += Cont->InterruptLoad[index];
+			index = ((qh->Next & ~3) - Cont->PhysTDQHPage)/sizeof(tUHCI_QH);
+		}
+
+		LOG("Slot %i (and below) load %i", qh_offsets[period_slot] + i, load);
+
+		// i = 0 will initialise the values, otherwise update if lower
+		if( i == 0 || load < min_load )
+		{
+			min_load = load;
+			min_load_slot = i;
+		}
+		// - Fast return if no load
+		if( load == 0 )	break;
+	}
+	min_load_slot += qh_offsets[period_slot];
+	TD->_info.QueueIndex = min_load_slot;
+	if( min_load + (TD->Control & 0x7FF) > MAX_INTERRUPT_LOAD )
+	{
+		Log_Warning("UHCI", "Interrupt load on %i ms is too high (slot %i load %i bytes)",
+			1 << (period_slot+2), min_load_slot, min_load
+			);
+	}
+	Cont->InterruptLoad[min_load_slot] += (TD->Control & 0x7FF);
+#endif
+	qh = Cont->TDQHPage->InterruptQHs + TD->_info.QueueIndex;
 
-	LOG("period_slot = %i, period_entry = %i (+1 when encoded)",
-		period_slot, TD->_info.period_entry);
+	LOG("period_slot = %i, QueueIndex = %i",
+		period_slot, TD->_info.QueueIndex);
+
+	// Stop any errors causing the TD to stop (NAK will error)
+	// - If the device is unplugged, the removal code should remove the interrupt
+	TD->Control &= ~(3 << 27);
 
 	UHCI_int_AppendTD(Cont, qh, TD);
 }
@@ -438,7 +487,7 @@ void *UHCI_InterruptIN(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbD
 
 	if( Period < 0 )	return NULL;
 
-	ENTER("pPtr xDest iPeriod pCb pCbData pBuf, iLength",
+	ENTER("pPtr xDest iPeriod pCb pCbData pBuf iLength",
 		Ptr, Dest, Period, Cb, CbData, Buf, Length);
 
 	// TODO: Data toggle?
@@ -473,6 +522,7 @@ void *UHCI_InterruptOUT(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *Cb
 void UHCI_StopInterrupt(void *Ptr, void *Handle)
 {
 	// TODO: Stop interrupt transaction
+	Log_Error("UHCI", "TODO: Implement UHCI_StopInterrupt");
 }
 
 void *UHCI_ControlSETUP(void *Ptr, int Dest, int Tgl, void *Data, size_t Length)
@@ -512,7 +562,7 @@ void *UHCI_ControlIN(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData,
 
 	ENTER("pPtr xDest iTgl pCb pCbData pData iLength", Ptr, Dest, Tgl, Cb, CbData, Data, Length);
 	
-	td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, !!Tgl, Cb, CbData, Data, Length);
+	td = UHCI_int_CreateTD(Cont, Dest, PID_IN, !!Tgl, Cb, CbData, Data, Length);
 	UHCI_int_AppendTD(Cont, qh, td);
 
 	LEAVE('p', td);
@@ -613,12 +663,134 @@ void UHCI_CheckPortUpdate(void *Ptr)
 	}
 }
 
+tUHCI_TD *UHCI_int_GetTDFromPhys(tUHCI_Controller *Controller, Uint32 PAddr)
+{
+	if( PAddr >= Controller->PhysTDQHPage && PAddr < Controller->PhysTDQHPage + PAGE_SIZE )
+	{
+		PAddr -= Controller->PhysTDQHPage;
+		PAddr -= (128+2)*sizeof(tUHCI_QH);
+		if( PAddr > PAGE_SIZE )	return NULL;	// Wrapping will bring above page size
+		PAddr /= sizeof(tUHCI_TD);
+		return &Controller->TDQHPage->LocalTDPool[PAddr];
+	}
+
+	
+	tPAddr	global_pool = MM_GetPhysAddr( (tVAddr)gaUHCI_TDPool );
+	
+	if( PAddr < global_pool || PAddr >= global_pool + PAGE_SIZE )	return NULL;
+	
+	PAddr -= global_pool;
+	PAddr /= sizeof(tUHCI_TD);
+	return &gaUHCI_TDPool[PAddr];
+}
+
+void UHCI_int_CleanQH(tUHCI_Controller *Cont, tUHCI_QH *QH)
+{
+	tUHCI_TD	*td, *prev = NULL;
+	Uint32	cur_td;
+	 int	nCleaned = 0;
+
+	// Disable controller
+	_OutWord( Cont, USBCMD, 0x0000 );
+	
+	// Scan QH list
+	cur_td = QH->Child;
+	LOG("cur_td = 0x%08x", cur_td);
+	while( !(cur_td & 1) )
+	{
+		td = UHCI_int_GetTDFromPhys(Cont, cur_td);
+		if(!td) {
+			Log_Warning("UHCI", "_int_CleanQH: QH %p contains TD %x, which was not from a pool",
+				QH, cur_td);
+			break ;
+		}
+		
+		// Active? Ok.
+		if( td->Control & TD_CTL_ACTIVE ) {
+			LOG("%p still active", td);
+			prev = td;
+			cur_td = td->Link;
+			continue ;
+		}
+
+		LOG("Removed %p from QH %p", td, QH);		
+
+		if( !prev )
+			QH->Child = td->Link;
+		else
+			prev->Link = td->Link;
+		cur_td = td->Link;
+		nCleaned ++;
+	}
+
+	if( nCleaned == 0 ) {
+		LOG("Nothing cleaned... what the?");
+	}
+
+	// re-enable controller
+	_OutWord( Cont, USBCMD, 0x0001 );	
+}
+
+void UHCI_int_HandleTDComplete(tUHCI_Controller *Cont, tUHCI_TD *TD)
+{
+	 int	byte_count = (TD->Control & 0x7FF)+1;
+	tUHCI_ExtraTDInfo	*info = TD->_info.ExtraInfo;
+
+	// Handle non page-aligned destination (or with a > 32-bit paddr)
+	// TODO: Needs fixing for alignment issues
+	if(info->FirstPage)
+	{
+		char	*src, *dest;
+		 int	src_ofs = TD->BufferPointer & (PAGE_SIZE-1);
+		src = (void *) MM_MapTemp(TD->BufferPointer);
+		dest = (void *) MM_MapTemp(info->FirstPage);
+		// Check for a single page transfer
+		if( byte_count + info->Offset <= PAGE_SIZE )
+		{
+			LOG("Single page copy %P to %P of %p",
+				TD->BufferPointer, info->FirstPage, TD);
+			memcpy(dest + info->Offset, src + src_ofs, byte_count);
+		}
+		else
+		{
+			// Multi-page
+			LOG("Multi page copy %P to (%P,%P) of %p",
+				TD->BufferPointer, info->FirstPage, info->SecondPage, TD);
+			 int	part_len = PAGE_SIZE - info->Offset;
+			memcpy(dest + info->Offset, src + src_ofs, part_len);
+			MM_FreeTemp( (tVAddr)dest );
+			dest = (void *) MM_MapTemp(info->SecondPage);
+			memcpy(dest, src + src_ofs + part_len, byte_count - part_len);
+		}
+		MM_FreeTemp( (tVAddr)src );
+		MM_FreeTemp( (tVAddr)dest );
+	}
+
+	// Callback
+	if( info->Callback != NULL )
+	{
+		LOG("Calling cb %p (%i bytes)", info->Callback, byte_count);
+		void	*ptr = (void *) MM_MapTemp( TD->BufferPointer );
+		info->Callback( info->CallbackPtr, ptr, byte_count );
+		MM_FreeTemp( (tVAddr)ptr );
+	}
+	
+	// Clean up info
+	if( TD->_info.QueueIndex > 127 )
+	{
+		free( info );
+		TD->_info.ExtraInfo = NULL;
+	}
+}
+
 void UHCI_int_InterruptThread(void *Pointer)
 {
 	tUHCI_Controller	*Cont = Pointer;
 	Threads_SetName("UHCI Interrupt Handler");
 	for( ;; )
 	{
+		 int	nSeen = 0;
+		
 		LOG("zzzzz....");
 		// 0 = Take all
 		Semaphore_Wait(&gUHCI_InterruptSempahore, 0);
@@ -626,87 +798,67 @@ void UHCI_int_InterruptThread(void *Pointer)
 	
 		for( int i = 0; i < NUM_TDs; i ++ )
 		{
-			 int	byte_count;
-			tUHCI_ExtraTDInfo	*info;
 			tUHCI_TD	*td;
 			
 			td = &gaUHCI_TDPool[i];
-			info = td->_info.ExtraInfo;
 
 			// Skip completely inactive TDs
 			if( td->_info.bActive == 0 )	continue ;
 			// Skip ones that are still in use
-			if( td->Control & (1 << 23) )	continue ;
+			if( td->Control & TD_CTL_ACTIVE )	continue ;
+
+			nSeen ++;
 
 			// If no callback/alt buffer, mark as free and move on
 			if( td->_info.ExtraInfo )
 			{
-				// Get size of transfer
-				byte_count = (td->Control & 0x7FF)+1;
-			
-				// Handle non page-aligned destination (or with a > 32-bit paddr)
-				if(info->FirstPage)
-				{
-					char	*src, *dest;
-					 int	src_ofs = td->BufferPointer & (PAGE_SIZE-1);
-					src = (void *) MM_MapTemp(td->BufferPointer);
-					dest = (void *) MM_MapTemp(info->FirstPage);
-					// Check for a single page transfer
-					if( byte_count + info->Offset <= PAGE_SIZE )
-					{
-						LOG("Single page copy %P to %P of %p",
-							td->BufferPointer, info->FirstPage, td);
-						memcpy(dest + info->Offset, src + src_ofs, byte_count);
-					}
-					else
-					{
-						// Multi-page
-						LOG("Multi page copy %P to (%P,%P) of %p",
-							td->BufferPointer, info->FirstPage, info->SecondPage, td);
-						 int	part_len = PAGE_SIZE - info->Offset;
-						memcpy(dest + info->Offset, src + src_ofs, part_len);
-						MM_FreeTemp( (tVAddr)dest );
-						dest = (void *) MM_MapTemp(info->SecondPage);
-						memcpy(dest, src + src_ofs + part_len, byte_count - part_len);
-					}
-					MM_FreeTemp( (tVAddr)src );
-					MM_FreeTemp( (tVAddr)dest );
-				}
-	
-				// Callback
-				if( info->Callback != NULL )
-				{
-					LOG("Calling cb %p", info->Callback);
-					void	*ptr = (void *) MM_MapTemp( td->BufferPointer );
-					info->Callback( info->CallbackPtr, ptr, byte_count );
-					MM_FreeTemp( (tVAddr)ptr );
-				}
-				
-				// Clean up info
-				free( info );
-				td->_info.ExtraInfo = NULL;
+				UHCI_int_HandleTDComplete(Cont, td);
 			}
 
-			if( td->_info.period_entry > 0 )
+			// Error check
+			if( td->Control & 0x00FF0000 ) {
+				LOG("td->control(Status) = %s%s%s%s%s%s%s%s",
+					td->Control & TD_CTL_ACTIVE     ? "Active, " : "",
+					td->Control & TD_CTL_STALLED    ? "Stalled, " : "",
+					td->Control & TD_CTL_DATABUFERR ? "Data Buffer Error, " : "",
+					td->Control & TD_CTL_BABBLE     ? "Babble, " : "",
+					td->Control & TD_CTL_NAK        ? "NAK, " : "",
+					td->Control & TD_CTL_CRCERR     ? "CRC Error, " : "",
+					td->Control & TD_CTL_BITSTUFF   ? "Bitstuff Error, " : "",
+					td->Control & TD_CTL_RESERVED   ? "Reserved " : ""
+					);
+				LOG("From queue %i", td->_info.QueueIndex);
+				// Clean up QH (removing all inactive entries)
+				UHCI_int_CleanQH(Cont, Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex);
+				td->Control = 0;
+			}
+	
+			// Handle rescheduling of interrupt TDs
+			if( td->_info.QueueIndex <= 127 )
 			{
-				LOG("Re-schedule interrupt %p (offset %i)", td, td->_info.period_entry-1);
+				LOG("Re-schedule interrupt %p (offset %i)", td, td->_info.QueueIndex);
 				// TODO: Flip toggle?
-				td->Control |= (1 << 23);
+				td->Control |= TD_CTL_ACTIVE;
 				// Add back into controller's interrupt list
 				UHCI_int_AppendTD(
 					Cont,
-					Cont->TDQHPage->InterruptQHs_256ms + td->_info.period_entry - 1,
+					Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex,
 					td
 					);
 				continue ;
 			}
 
+			// Clean up
 			if( td->_info.bFreePointer )
-				MM_DerefPhys( td->BufferPointer );			
-	
+				MM_DerefPhys( td->BufferPointer );
+
 			// Clean up
+			LOG("Cleaned %p (->Control = %x)", td, td->Control);
 			td->_info.bActive = 0;
-			LOG("Cleaned %p", td);
+		}
+
+		if( nSeen == 0 ) {
+			LOG("Why did you wake me?");
 		}
 	}
 }
@@ -717,6 +869,7 @@ void UHCI_InterruptHandler(int IRQ, void *Ptr)
 //	 int	frame = (_InWord(Host, FRNUM) - 1) & 0x3FF;
 	Uint16	status = _InWord(Host, USBSTS);
 	
+	LOG("%p: status = 0x%04x", Ptr, status);
 	// Interrupt-on-completion
 	if( status & 1 )
 	{
@@ -724,7 +877,6 @@ void UHCI_InterruptHandler(int IRQ, void *Ptr)
 		Semaphore_Signal(&gUHCI_InterruptSempahore, 1);
 	}
 
-	LOG("status = 0x%04x", status);
 	_OutWord(Host, USBSTS, status);
 }