From 5f5e32323cf9da2b2f3cf2e910afe2a0b2e6ae12 Mon Sep 17 00:00:00 2001
From: John Hodge <tpg@mutabah.net>
Date: Wed, 29 Feb 2012 08:21:15 +0800
Subject: [PATCH] USB/UHCI - Cleanup

---
 KernelLand/Modules/USB/UHCI/uhci.c | 104 ++++++++++++++++-------------
 1 file changed, 57 insertions(+), 47 deletions(-)

diff --git a/KernelLand/Modules/USB/UHCI/uhci.c b/KernelLand/Modules/USB/UHCI/uhci.c
index fc007107..0af6f1a6 100644
--- a/KernelLand/Modules/USB/UHCI/uhci.c
+++ b/KernelLand/Modules/USB/UHCI/uhci.c
@@ -421,6 +421,10 @@ void UHCI_int_SetInterruptPoll(tUHCI_Controller *Cont, tUHCI_TD *TD, int Period)
 	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 goes away, the interrupt should be stopped anyway
+	TD->Control &= ~(3 << 27);
+
 	UHCI_int_AppendTD(Cont, qh, TD);
 }
 
@@ -465,6 +469,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)
@@ -664,6 +669,55 @@ void UHCI_int_CleanQH(tUHCI_Controller *Cont, tUHCI_QH *QH)
 	_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", 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;
+}
+
 void UHCI_int_InterruptThread(void *Pointer)
 {
 	tUHCI_Controller	*Cont = Pointer;
@@ -677,12 +731,9 @@ 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 ;
@@ -692,52 +743,10 @@ void UHCI_int_InterruptThread(void *Pointer)
 			// 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);
 			}
 
+			// Handle rescheduling of interrupt TDs
 			if( td->_info.QueueIndex <= 127 )
 			{
 				LOG("Re-schedule interrupt %p (offset %i)", td, td->_info.QueueIndex);
@@ -752,6 +761,7 @@ void UHCI_int_InterruptThread(void *Pointer)
 				continue ;
 			}
 
+			// Clean up
 			if( td->_info.bFreePointer )
 				MM_DerefPhys( td->BufferPointer );
 
-- 
2.20.1