From: John Hodge <tpg@mutabah.net>
Date: Tue, 29 Nov 2011 06:51:11 +0000 (+0800)
Subject: Modules/USB - Callback support and Driver selection
X-Git-Tag: rel0.14~57
X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;h=89e41793b9d495d98633f90e6595722b8e5979d3;p=tpg%2Facess2.git

Modules/USB - Callback support and Driver selection
---

diff --git a/Modules/USB/Core/hub.c b/Modules/USB/Core/hub.c
index 08295b1f..b737f078 100644
--- a/Modules/USB/Core/hub.c
+++ b/Modules/USB/Core/hub.c
@@ -5,6 +5,7 @@
  * hub.c
  * - Basic hub driver
  */
+#define DEBUG	1
 #include <usb_hub.h>
 
 #define MAX_PORTS	32	// Not actually a max, but used for DeviceRemovable
@@ -39,7 +40,7 @@ struct sHubInfo
 // === PROTOTYPES ===
 void	Hub_Connected(tUSBInterface *Dev);
 void	Hub_Disconnected(tUSBInterface *Dev);
-void	Hub_PortStatusChange(tUSBInterface *Dev, int Length, void *Data);
+void	Hub_PortStatusChange(tUSBInterface *Dev, int Endpoint, int Length, void *Data);
 void	Hub_int_HandleChange(tUSBInterface *Dev, int Port);
 
 // === GLOBALS ===
@@ -68,8 +69,8 @@ void Hub_Connected(tUSBInterface *Dev)
 	struct sHubDescriptor	hub_desc;
 	struct sHubInfo	*info;	
 
-	// Read hub descriptor
-	USB_ReadDescriptor(Dev, 0x29, 0, sizeof(hub_desc), &hub_desc);
+	// Read hub descriptor (Class descriptor 0x29)
+	USB_ReadDescriptor(Dev, 0x129, 0, sizeof(hub_desc), &hub_desc);
 
 	LOG("%i Ports", hub_desc.NbrPorts);
 	LOG("Takes %i ms for power to stabilise", hub_desc.PwrOn2PwrGood*2);
@@ -100,7 +101,7 @@ void Hub_Disconnected(tUSBInterface *Dev)
 	free(info);
 }
 
-void Hub_PortStatusChange(tUSBInterface *Dev, int Length, void *Data)
+void Hub_PortStatusChange(tUSBInterface *Dev, int Endpoint, int Length, void *Data)
 {
 	Uint8	*status = Data;
 	struct sHubInfo	*info = USB_GetDeviceDataPtr(Dev);
diff --git a/Modules/USB/Core/include/usb_core.h b/Modules/USB/Core/include/usb_core.h
index 369de5ea..43af16f2 100644
--- a/Modules/USB/Core/include/usb_core.h
+++ b/Modules/USB/Core/include/usb_core.h
@@ -13,6 +13,8 @@
 typedef struct sUSBInterface	tUSBInterface;
 typedef struct sUSBDriver	tUSBDriver;
 
+typedef void	(*tUSB_DataCallback)(tUSBInterface *Dev, int EndPt, int Length, void *Data);
+
 /**
  */
 struct sUSBDriver
@@ -27,8 +29,8 @@ struct sUSBDriver
 			// 23:16 - Interface Class
 			// 15:8  - Interface Sub Class
 			// 7:0   - Interface Protocol
-			Uint32	ClassMask;
 			Uint32	ClassCode;
+			Uint32	ClassMask;
 		} Class;
 		struct {
 			Uint16	VendorID;
@@ -45,7 +47,7 @@ struct sUSBDriver
 		// NOTE: Top bit indicates the direction (1=Input)
 		Uint8	Attributes;
 		// Data availiable Callback
-		void	(*DataAvail)(tUSBInterface *Dev, int Length, void *Data);
+		tUSB_DataCallback	DataAvail;
 	} Endpoints[];
 };
 
@@ -58,6 +60,7 @@ extern void	USB_Request(tUSBInterface *Dev, int Endpoint, int Type, int Req, int
 // TODO: Async
 extern void	USB_SendData(tUSBInterface *Dev, int Endpoint, int Length, void *Data);
 extern void	USB_RecvData(tUSBInterface *Dev, int Endpoint, int Length, void *Data);
+extern void	USB_RecvDataA(tUSBInterface *Dev, int Endpoint, int Length, void *DataBuf, tUSB_DataCallback Callback);
 
 #endif
 
diff --git a/Modules/USB/Core/include/usb_host.h b/Modules/USB/Core/include/usb_host.h
index 4efe0f7a..c5f3e39d 100644
--- a/Modules/USB/Core/include/usb_host.h
+++ b/Modules/USB/Core/include/usb_host.h
@@ -13,9 +13,9 @@
 
 typedef struct sUSBHostDef	tUSBHostDef;
 
-typedef void	(*tUSBHostCb)(void *DataPtr);
+typedef void	(*tUSBHostCb)(void *DataPtr, void *Data, int Length);
 
-typedef void	*(*tUSBHostOp)(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb bIOC, void *Data, size_t Length);
+typedef void	*(*tUSBHostOp)(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb CB, void *CbData, void *Data, size_t Length);
 
 /**
  * \brief Defines a USB Host Controller type
diff --git a/Modules/USB/Core/usb.c b/Modules/USB/Core/usb.c
index ae80a113..b3195804 100644
--- a/Modules/USB/Core/usb.c
+++ b/Modules/USB/Core/usb.c
@@ -21,7 +21,7 @@ extern tUSBDriver gUSBHub_Driver;
 tUSBHub	*USB_RegisterHost(tUSBHostDef *HostDef, void *ControllerPtr, int nPorts);
 
 // === GLOBALS ===
-tUSBDriver	*gUSB_InterfaceDrivers = &gUSBHub_Driver;
+tUSBDriver	*gpUSB_InterfaceDrivers = &gUSBHub_Driver;
 
 // === CODE ===
 tUSBHub *USB_RegisterHost(tUSBHostDef *HostDef, void *ControllerPtr, int nPorts)
@@ -41,7 +41,7 @@ tUSBHub *USB_RegisterHost(tUSBHostDef *HostDef, void *ControllerPtr, int nPorts)
 	host->RootHubDev.Host = host;
 	host->RootHubDev.Address = 0;
 
-	host->RootHubIf.Next = NULL;
+//	host->RootHubIf.Next = NULL;
 	host->RootHubIf.Dev = &host->RootHubDev;
 	host->RootHubIf.Driver = NULL;
 	host->RootHubIf.Data = NULL;
@@ -58,11 +58,29 @@ tUSBHub *USB_RegisterHost(tUSBHostDef *HostDef, void *ControllerPtr, int nPorts)
 	return &host->RootHub;
 }
 
+// --- Drivers ---
 void USB_RegisterDriver(tUSBDriver *Driver)
 {
 	Log_Warning("USB", "TODO: Implement USB_RegisterDriver");
 }
 
+tUSBDriver *USB_int_FindDriverByClass(Uint32 ClassCode)
+{
+	ENTER("xClassCode", ClassCode);
+	for( tUSBDriver *ret = gpUSB_InterfaceDrivers; ret; ret = ret->Next )
+	{
+		LOG(" 0x%x & 0x%x == 0x%x?", ClassCode, ret->Match.Class.ClassMask, ret->Match.Class.ClassCode);
+		if( (ClassCode & ret->Match.Class.ClassMask) == ret->Match.Class.ClassCode )
+		{
+			LOG("Found '%s'", ret->Name);
+			LEAVE('p', ret);
+			return ret;
+		}
+	}
+	LEAVE('n');
+	return NULL;
+}
+
 // --- Hub Registration ---
 // NOTE: Doesn't do much nowdays
 tUSBHub *USB_RegisterHub(tUSBInterface *Device, int PortCount)
diff --git a/Modules/USB/Core/usb.h b/Modules/USB/Core/usb.h
index a61ce000..5a133630 100644
--- a/Modules/USB/Core/usb.h
+++ b/Modules/USB/Core/usb.h
@@ -30,15 +30,17 @@ struct sUSBHub
 
 struct sUSBEndpoint
 {
-	tUSBEndpoint	*Next;	// In the segmented list
+	tUSBEndpoint	*Next;	// (usb_poll.c) Clock list
 	tUSBInterface	*Interface;
-	 int	EndpointNum;
+	 int	EndpointIdx;	// Interface endpoint index
+	 int	EndpointNum;	// Device endpoint num
 	
 	 int	PollingPeriod;	// In 1ms intervals
-	 int	PollingAtoms;	// *INTERNAL* usb_poll.c
 	 int	MaxPacketSize;	// In bytes
-
 	Uint8	Type;	// Same as sUSBDriver.Endpoints.Type
+	
+	 int	PollingAtoms;	// (usb_poll.c) Period in clock list
+	void	*InputData;
 };
 
 /**
@@ -46,7 +48,7 @@ struct sUSBEndpoint
  */
 struct sUSBInterface
 {
-	tUSBInterface	*Next;
+//	tUSBInterface	*Next;
 	tUSBDevice	*Dev;
 
 	tUSBDriver	*Driver;
@@ -87,6 +89,6 @@ struct sUSBHost
 	tUSBHub	RootHub;
 };
 
-extern void	USB_NewDevice(tUSBHub *Hub);
+extern tUSBDriver	*USB_int_FindDriverByClass(Uint32 ClassCode);
 
 #endif
diff --git a/Modules/USB/Core/usb_devinit.c b/Modules/USB/Core/usb_devinit.c
index 8d901a53..d01d7d1c 100644
--- a/Modules/USB/Core/usb_devinit.c
+++ b/Modules/USB/Core/usb_devinit.c
@@ -24,7 +24,8 @@ void	USB_SetDeviceDataPtr(tUSBInterface *Dev, void *Ptr);
 // === CODE ===
 void USB_DeviceConnected(tUSBHub *Hub, int Port)
 {
-	tUSBDevice	*dev;
+	tUSBDevice	tmpdev;
+	tUSBDevice	*dev = &tmpdev;
 	if( Port >= Hub->nPorts )	return ;
 	if( Hub->Devices[Port] )	return ;
 
@@ -33,7 +34,6 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 	// Device should be in 'Default' state
 	
 	// Create structure
-	dev = malloc(sizeof(tUSBDevice));
 	dev->ParentHub = Hub;
 	dev->Host = Hub->Interface->Dev->Host;
 	dev->Address = 0;
@@ -92,6 +92,8 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 			free(tmp);
 		}
 	}
+
+	// TODO: Support alternate configurations
 	
 	// 3. Get configurations
 	for( int i = 0; i < 1; i ++ )
@@ -99,7 +101,7 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 		struct sDescriptor_Configuration	desc;
 		void	*full_buf;
 		char	*cur_ptr;
-		
+	
 		USB_int_ReadDescriptor(dev, 0, 2, i, sizeof(desc), &desc);
 		LOG("Configuration Descriptor %i = {", i);
 		LOG(" .Length = %i", desc.Length);
@@ -117,6 +119,14 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 			free(tmp);
 		}
 
+		// TODO: Split here and allow some method of selection
+
+		// Allocate device now that we have the configuration
+		dev = malloc(sizeof(tUSBDevice) + desc.NumInterfaces * sizeof(void*));
+		memcpy(dev, &tmpdev, sizeof(tUSBDevice));
+		dev->nInterfaces = desc.NumInterfaces;
+	
+		// Allocate a temp buffer for config info
 		cur_ptr = full_buf = malloc( LittleEndian16(desc.TotalLength) );
 		USB_int_ReadDescriptor(dev, 0, 2, i, desc.TotalLength, full_buf);
 
@@ -126,10 +136,11 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 		for( int j = 0; j < desc.NumInterfaces; j ++ )
 		{
 			struct sDescriptor_Interface *iface;
+			tUSBInterface	*dev_if;
 			iface = (void*)cur_ptr;
 			// TODO: Sanity check with remaining space
 			cur_ptr += sizeof(*iface);
-			
+
 			LOG("Interface %i/%i = {", i, j);
 			LOG(" .InterfaceNum = %i", iface->InterfaceNum);
 			LOG(" .NumEndpoints = %i", iface->NumEndpoints);
@@ -144,6 +155,14 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 			}
 			LOG("}");
 
+			dev_if = malloc(sizeof(tUSBInterface) + iface->NumEndpoints*sizeof(dev_if->Endpoints[0]));
+			dev_if->Dev = dev;
+			dev_if->Driver = NULL;
+			dev_if->Data = NULL;
+			dev_if->nEndpoints = iface->NumEndpoints;
+			dev->Interfaces[j] = dev_if;
+
+			// Copy interface data
 			for( int k = 0; k < iface->NumEndpoints; k ++ )
 			{
 				struct sDescriptor_Endpoint *endpt;
@@ -157,6 +176,31 @@ void USB_DeviceConnected(tUSBHub *Hub, int Port)
 				LOG(" .MaxPacketSize = %i", LittleEndian16(endpt->MaxPacketSize));
 				LOG(" .PollingInterval = %i", endpt->PollingInterval);
 				LOG("}");
+				
+				dev_if->Endpoints[k].Next = NULL;
+				dev_if->Endpoints[k].Interface = dev_if;
+				dev_if->Endpoints[k].EndpointIdx = k;
+				dev_if->Endpoints[k].EndpointNum = endpt->Address & 0x7F;
+				dev_if->Endpoints[k].PollingPeriod = endpt->PollingInterval;
+				dev_if->Endpoints[k].MaxPacketSize = LittleEndian16(endpt->MaxPacketSize);
+				dev_if->Endpoints[k].Type = endpt->Attributes | (endpt->Address & 0x80);
+				dev_if->Endpoints[k].PollingAtoms = 0;
+				dev_if->Endpoints[k].InputData = NULL;
+			}
+			
+			// Initialise driver
+			dev_if->Driver = USB_int_FindDriverByClass(
+				 ((int)iface->InterfaceClass << 16)
+				|((int)iface->InterfaceSubClass << 8)
+				|((int)iface->InterfaceProtocol << 0)
+				);
+			if(!dev_if->Driver) {
+				Log_Notice("USB", "No driver for Class %02x:%02x:%02x",
+					iface->InterfaceClass, iface->InterfaceSubClass, iface->InterfaceProtocol
+					);
+			}
+			else {
+				dev_if->Driver->Connected( dev_if );
 			}
 		}
 		
diff --git a/Modules/USB/Core/usb_io.c b/Modules/USB/Core/usb_io.c
index ec88d753..993bf0cc 100644
--- a/Modules/USB/Core/usb_io.c
+++ b/Modules/USB/Core/usb_io.c
@@ -10,12 +10,24 @@
 #include <usb_core.h>
 #include "usb.h"
 #include "usb_lowlevel.h"
+#include <semaphore.h>
+
+typedef struct sAsyncOp	tAsyncOp;
+
+struct sAsyncOp
+{
+	tAsyncOp	*Next;
+	tUSBEndpoint	*Endpt;
+	 int	Length;
+	void	*Data;
+};
 
 // === PROTOTYPES ===
 void	USB_ReadDescriptor(tUSBInterface *Iface, int Type, int Index, int Length, void *Data);
 void	USB_Request(tUSBInterface *Iface, int Endpoint, int Type, int Req, int Value, int Index, int Len, void *Data);
 
 // === GLOBALS ===
+tSemaphore	glUSB_AsyncQueue;
 
 // === CODE ===
 void USB_ReadDescriptor(tUSBInterface *Iface, int Type, int Index, int Length, void *Data)
@@ -40,3 +52,28 @@ void USB_Request(tUSBInterface *Iface, int Endpoint, int Type, int Req, int Valu
 	USB_int_Request(Iface->Dev->Host, Iface->Dev->Address, endpt, Type, Req, Value, Index, Len, Data);
 }
 
+
+void USB_SendData(tUSBInterface *Dev, int Endpoint, int Length, void *Data)
+{
+	Log_Warning("USB", "TODO: Implement USB_SendData");
+}
+
+void USB_RecvData(tUSBInterface *Dev, int Endpoint, int Length, void *Data)
+{
+	Log_Warning("USB", "TODO: Implement USB_RecvData");
+}
+
+void USB_RecvDataA(tUSBInterface *Dev, int Endpoint, int Length, void *DataBuf, tUSB_DataCallback Callback)
+{
+	Log_Warning("USB", "TODO: Implement USB_RecvDataA");
+}
+
+void USB_AsyncThread(void *Unused)
+{
+	for(;;)
+	{
+		Semaphore_Wait(&glUSB_AsyncQueue, 1);
+		
+	}
+}
+
diff --git a/Modules/USB/Core/usb_lowlevel.c b/Modules/USB/Core/usb_lowlevel.c
index 4c0fb730..99423a73 100644
--- a/Modules/USB/Core/usb_lowlevel.c
+++ b/Modules/USB/Core/usb_lowlevel.c
@@ -30,7 +30,7 @@ void *USB_int_Request(tUSBHost *Host, int Addr, int EndPt, int Type, int Req, in
 	req.Index = LittleEndian16( Indx );
 	req.Length = LittleEndian16( Len );
 	
-	hdl = Host->HostDef->SendSETUP(Host->Ptr, Addr, EndPt, 0, NULL, &req, sizeof(req));
+	hdl = Host->HostDef->SendSETUP(Host->Ptr, Addr, EndPt, 0, NULL, NULL, &req, sizeof(req));
 
 	// TODO: Data toggle?
 	// TODO: Multi-packet transfers
@@ -38,9 +38,9 @@ void *USB_int_Request(tUSBHost *Host, int Addr, int EndPt, int Type, int Req, in
 	{
 		void	*hdl2;
 		
-		hdl = Host->HostDef->SendIN(Host->Ptr, Addr, EndPt, 0, NULL, Data, Len);
+		hdl = Host->HostDef->SendIN(Host->Ptr, Addr, EndPt, 0, NULL, NULL, Data, Len);
 
-		hdl2 = Host->HostDef->SendOUT(Host->Ptr, Addr, EndPt, 0, NULL, NULL, 0);
+		hdl2 = Host->HostDef->SendOUT(Host->Ptr, Addr, EndPt, 0, NULL, NULL, NULL, 0);
 		while( Host->HostDef->IsOpComplete(Host->Ptr, hdl2) == 0 )
 			Time_Delay(1);
 	}
@@ -49,12 +49,12 @@ void *USB_int_Request(tUSBHost *Host, int Addr, int EndPt, int Type, int Req, in
 		void	*hdl2;
 		
 		if( Len > 0 )
-			hdl = Host->HostDef->SendOUT(Host->Ptr, Addr, EndPt, 0, NULL, Data, Len);
+			hdl = Host->HostDef->SendOUT(Host->Ptr, Addr, EndPt, 0, NULL, NULL, Data, Len);
 		else
 			hdl = NULL;
 		
 		// Status phase (DataToggle=1)
-		hdl2 = Host->HostDef->SendIN(Host->Ptr, Addr, EndPt, 1, NULL, NULL, 0);
+		hdl2 = Host->HostDef->SendIN(Host->Ptr, Addr, EndPt, 1, NULL, NULL, NULL, 0);
 		while( Host->HostDef->IsOpComplete(Host->Ptr, hdl2) == 0 )
 			Time_Delay(1);
 	}
@@ -75,6 +75,13 @@ int USB_int_ReadDescriptor(tUSBDevice *Dev, int Endpoint, int Type, int Index, i
 	void	*final;
 
 	req.ReqType = 0x80;
+	switch( Type & 0xF00 )
+	{
+	case 0x000:	req.ReqType |= (0 << 5);	break;	// Standard
+	case 0x100:	req.ReqType |= (1 << 5);	break;	// Class
+	case 0x200:	req.ReqType |= (2 << 5);	break;	// Vendor
+	}
+
 	req.Request = 6;	// GET_DESCRIPTOR
 	req.Value = LittleEndian16( ((Type & 0xFF) << 8) | (Index & 0xFF) );
 	req.Index = LittleEndian16( 0 );	// TODO: Language ID
@@ -82,7 +89,7 @@ int USB_int_ReadDescriptor(tUSBDevice *Dev, int Endpoint, int Type, int Index, i
 	
 	Dev->Host->HostDef->SendSETUP(
 		Dev->Host->Ptr, Dev->Address, Endpoint,
-		0, NULL,
+		0, NULL, NULL,
 		&req, sizeof(req)
 		);
 	
@@ -91,7 +98,7 @@ int USB_int_ReadDescriptor(tUSBDevice *Dev, int Endpoint, int Type, int Index, i
 	{
 		Dev->Host->HostDef->SendIN(
 			Dev->Host->Ptr, Dev->Address, Endpoint,
-			bToggle, NULL,
+			bToggle, NULL, NULL,
 			Dest, ciMaxPacketSize
 			);
 		bToggle = !bToggle;
@@ -100,7 +107,7 @@ int USB_int_ReadDescriptor(tUSBDevice *Dev, int Endpoint, int Type, int Index, i
 
 	final = Dev->Host->HostDef->SendIN(
 		Dev->Host->Ptr, Dev->Address, Endpoint,
-		bToggle, INVLPTR,
+		bToggle, INVLPTR, NULL,
 		Dest, Length
 		);
 
diff --git a/Modules/USB/Core/usb_poll.c b/Modules/USB/Core/usb_poll.c
index a4e8d448..92e02922 100644
--- a/Modules/USB/Core/usb_poll.c
+++ b/Modules/USB/Core/usb_poll.c
@@ -33,6 +33,8 @@ void USB_StartPollingEndpoint(tUSBInterface *Iface, int Endpoint)
 
 	// TODO: Check that this endpoint isn't already on the queue
 
+	endpt->InputData = malloc(endpt->MaxPacketSize);
+
 	// Determine polling period in atoms
 	endpt->PollingAtoms = (endpt->PollingPeriod + POLL_ATOM-1) / POLL_ATOM;
 	if(endpt->PollingAtoms > POLL_SLOTS)	endpt->PollingAtoms = POLL_SLOTS;
@@ -78,6 +80,12 @@ int USB_PollThread(void *unused)
 			// TODO: Check the endpoint
 			// TODO: Async checking?
 			// - Send the read request on all of them then wait for the first to complete
+			USB_RecvDataA(
+				ep->Interface, ep->EndpointIdx,
+				ep->MaxPacketSize, ep->InputData,
+				ep->Interface->Driver->Endpoints[ep->EndpointIdx].DataAvail
+				);
+				
 			// Call callback
 
 			// Reschedule
diff --git a/Modules/USB/UHCI/uhci.c b/Modules/USB/UHCI/uhci.c
index 18fd3e81..8df6c287 100644
--- a/Modules/USB/UHCI/uhci.c
+++ b/Modules/USB/UHCI/uhci.c
@@ -22,10 +22,10 @@
 void	UHCI_Cleanup();
 tUHCI_TD	*UHCI_int_AllocateTD(tUHCI_Controller *Cont);
 void	UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_TD *TD);
-void	*UHCI_int_SendTransaction(tUHCI_Controller *Cont, int Addr, Uint8 Type, int bTgl, tUSBHostCb Cb, void *Data, size_t Length);
-void	*UHCI_DataIN(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *Data, size_t Length);
-void	*UHCI_DataOUT(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *Data, size_t Length);
-void	*UHCI_SendSetup(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *Data, size_t Length);
+void	*UHCI_int_SendTransaction(tUHCI_Controller *Cont, int Addr, Uint8 Type, int bTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
+void	*UHCI_DataIN(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
+void	*UHCI_DataOUT(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *CbData,  void *Buf, size_t Length);
+void	*UHCI_SendSetup(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  int	UHCI_IsTransferComplete(void *Ptr, void *Handle);
  int	UHCI_Int_InitHost(tUHCI_Controller *Host);
 void	UHCI_CheckPortUpdate(void *Ptr);
@@ -132,10 +132,17 @@ tUHCI_TD *UHCI_int_AllocateTD(tUHCI_Controller *Cont)
 	return NULL;
 }
 
+tUHCI_TD *UHCI_int_GetTDFromPhys(tPAddr PAddr)
+{
+	// TODO: Fix this to work with a non-contiguous pool
+	static tPAddr	td_pool_base;
+	if(!td_pool_base)	td_pool_base = MM_GetPhysAddr( (tVAddr)gaUHCI_TDPool );
+	return gaUHCI_TDPool + (PAddr - td_pool_base) / sizeof(gaUHCI_TDPool[0]);
+}
+
 void UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_TD *TD)
 {
 	 int	next_frame = (inw(Cont->IOBase + FRNUM) + 2) & (1024-1);
-	tPAddr	td_pool_base = MM_GetPhysAddr( (tVAddr)gaUHCI_TDPool );
 	tUHCI_TD	*prev_td;
 	Uint32	link;
 
@@ -153,8 +160,7 @@ void UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_TD *TD)
 	// Find the end of the list
 	link = Cont->FrameList[next_frame];
 	do {
-		// TODO: Fix this to work with a non-contiguous pool
-		prev_td = gaUHCI_TDPool + (link - td_pool_base) / sizeof(gaUHCI_TDPool[0]);
+		prev_td = UHCI_int_GetTDFromPhys(link);
 		link = prev_td->Link;
 	} while( !(link & 1) );
 	
@@ -170,7 +176,9 @@ void UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_TD *TD)
  * \param Addr	Function Address * 16 + Endpoint
  * \param bTgl	Data toggle value
  */
-void *UHCI_int_SendTransaction(tUHCI_Controller *Cont, int Addr, Uint8 Type, int bTgl, tUSBHostCb Cb, void *Data, size_t Length)
+void *UHCI_int_SendTransaction(
+	tUHCI_Controller *Cont, int Addr, Uint8 Type, int bTgl,
+	tUSBHostCb Cb, void *CbData, void *Data, size_t Length)
 {
 	tUHCI_TD	*td;
 
@@ -179,7 +187,7 @@ void *UHCI_int_SendTransaction(tUHCI_Controller *Cont, int Addr, Uint8 Type, int
 	td = UHCI_int_AllocateTD(Cont);
 
 	if( !td ) {
-		// 
+		// TODO: Wait for one to free?
 		Log_Error("UHCI", "No avaliable TDs, transaction dropped");
 		return NULL;
 	}
@@ -200,48 +208,53 @@ void *UHCI_int_SendTransaction(tUHCI_Controller *Cont, int Addr, Uint8 Type, int
 			);
 		// TODO: Need to enable IOC to copy the data back
 //		td->BufferPointer = 
+		td->_info.bCopyData = 1;
 		return NULL;
 	}
 	else {
 		td->BufferPointer = MM_GetPhysAddr( (tVAddr)Data );
+		td->_info.bCopyData = 0;
 	}
 
 	// Interrupt on completion
 	if( Cb ) {
 		td->Control |= (1 << 24);
 		td->_info.Callback = Cb;	// NOTE: if ERRPTR then the TD is kept allocated until checked
-		if( Cb != INVLPTR )
-		{
-			Log_Warning("UHCI", "TODO: Support IOC... somehow");
-		}
+		td->_info.CallbackPtr = CbData;
 	}
 	
-	td->_info.DestPtr = Data;
+	td->_info.DataPtr = Data;
 
 	UHCI_int_AppendTD(Cont, td);
 
 	return td;
 }
 
-void *UHCI_DataIN(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *Data, size_t Length)
+void *UHCI_DataIN(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 {
-	return UHCI_int_SendTransaction(Ptr, Fcn*16+Endpt, 0x69, DataTgl, Cb, Data, Length);
+	return UHCI_int_SendTransaction(Ptr, Fcn*16+Endpt, 0x69, DataTgl, Cb, CbData, Buf, Length);
 }
 
-void *UHCI_DataOUT(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *Data, size_t Length)
+void *UHCI_DataOUT(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 {
-	return UHCI_int_SendTransaction(Ptr, Fcn*16+Endpt, 0xE1, DataTgl, Cb, Data, Length);
+	return UHCI_int_SendTransaction(Ptr, Fcn*16+Endpt, 0xE1, DataTgl, Cb, CbData, Buf, Length);
 }
 
-void *UHCI_SendSetup(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *Data, size_t Length)
+void *UHCI_SendSetup(void *Ptr, int Fcn, int Endpt, int DataTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 {
-	return UHCI_int_SendTransaction(Ptr, Fcn*16+Endpt, 0x2D, DataTgl, Cb, Data, Length);
+	return UHCI_int_SendTransaction(Ptr, Fcn*16+Endpt, 0x2D, DataTgl, Cb, CbData, Buf, Length);
 }
 
 int UHCI_IsTransferComplete(void *Ptr, void *Handle)
 {
 	tUHCI_TD	*td = Handle;
-	return !(td->Control & (1 << 23));
+	 int	ret;
+	ret = !(td->Control & (1 << 23));
+	if(ret) {
+		td->_info.Callback = NULL;
+		td->Link = 1;
+	}
+	return ret;
 }
 
 // === INTERNAL FUNCTIONS ===
@@ -333,9 +346,39 @@ void UHCI_InterruptHandler(int IRQ, void *Ptr)
 	Uint16	status = inw(Host->IOBase + USBSTS);
 	Log_Debug("UHCI", "UHIC Interrupt, status = 0x%x", status);
 	
+	// Interrupt-on-completion
 	if( status & 1 )
 	{
-		// Interrupt-on-completion
+		tPAddr	link;
+		 int	frame = inw(Host->IOBase + FRNUM) - 1;
+		if(frame < 0)	frame += 1024;
+		
+		link = Host->FrameList[frame];
+		Host->FrameList[frame] = 1;
+		while( !(link & 1) )
+		{
+			tUHCI_TD *td = UHCI_int_GetTDFromPhys(link);
+			 int	byte_count = (td->Control&0x7FF)+1;
+			// Handle non-page aligned destination
+			// TODO: This will break if the destination is not in global memory
+			if(td->_info.bCopyData)
+			{
+				void *ptr = (void*)MM_MapTemp(td->BufferPointer);
+				memcpy(td->_info.DataPtr, ptr, byte_count);
+				MM_FreeTemp((tVAddr)ptr);
+			}
+			// Callback
+			if(td->_info.Callback && td->_info.Callback != INVLPTR)
+			{
+				td->_info.Callback(td->_info.CallbackPtr, td->_info.DataPtr, byte_count);
+				td->_info.Callback = NULL;
+			}
+			link = td->Link;
+			if( td->_info.Callback != INVLPTR )
+				td->Link = 1;
+		}
+		
+//		Host->LastCleanedFrame = frame;
 	}
 
 	outw(Host->IOBase + USBSTS, status);
diff --git a/Modules/USB/UHCI/uhci.h b/Modules/USB/UHCI/uhci.h
index 6056c987..e67dac3b 100644
--- a/Modules/USB/UHCI/uhci.h
+++ b/Modules/USB/UHCI/uhci.h
@@ -104,20 +104,14 @@ struct sUHCI_TD
 	 */
 	Uint32	BufferPointer;
 
-	union
+	struct
 	{
-		/**
-		 * \brief Avaliable for use by software
-		 */
-		Uint32	Avaliable[4];
-		
-		struct
-		{
-			void	*Callback;
-			void	*DestPtr;
-		} _info;
-	};
-};
+		tUSBHostCb	Callback;
+		void	*CallbackPtr;
+		void	*DataPtr;
+		 int	bCopyData;
+	} _info;
+} __attribute__((aligned(16)));
 
 struct sUHCI_QH
 {