From: John Hodge <tpg@ucc.asn.au>
Date: Mon, 17 Sep 2012 01:41:31 +0000 (+0800)
Subject: Modules/EHCI - Implimenting functions (doesn't fully work yet)
X-Git-Tag: rel0.15~706^2~5
X-Git-Url: https://git.ucc.asn.au/?p=tpg%2Facess2.git;a=commitdiff_plain;h=7e6a4348d6de7f7177df286b470d24f67d38df7c

Modules/EHCI - Implimenting functions (doesn't fully work yet)
---

diff --git a/KernelLand/Modules/USB/EHCI/ehci.c b/KernelLand/Modules/USB/EHCI/ehci.c
index 1f73cf6f..af990112 100644
--- a/KernelLand/Modules/USB/EHCI/ehci.c
+++ b/KernelLand/Modules/USB/EHCI/ehci.c
@@ -47,7 +47,7 @@ void	EHCI_RootHub_ClearPortFeature(void *Ptr, int Port, int Feat);
 // --- Internals ---
 tEHCI_qTD	*EHCI_int_AllocateTD(tEHCI_Controller *Cont, int PID, void *Data, size_t Length, tUSBHostCb Cb, void *CbData);
 void	EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD);
-void	EHCI_int_AppendTD(tEHCI_QH *QH, 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);
 void	EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH);
 void	EHCI_int_InterruptThread(void *ControllerPtr);
@@ -127,6 +127,7 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 	// - Nuke a couple of fields so error handling code doesn't derp
 	cont->CapRegs = NULL;
 	cont->PeriodicQueue = NULL;
+	cont->TDPool = NULL;
 
 	// -- Build up structure --
 	cont->CapRegs = (void*)MM_MapHWPages(BaseAddress, 1);
@@ -150,10 +151,19 @@ int EHCI_InitController(tPAddr BaseAddress, Uint8 InterruptNum)
 	// TODO: Error check
 	//  > Populate queue
 
+	// - Allocate TD pool
+	cont->TDPool = (void*)MM_AllocDMA(1, 32, &unused);
+	if( !cont->TDPool ) {
+		Log_Warning("ECHI", "Can't allocate 1 32-bit page for qTD pool");
+		goto _error;
+	}
+	for( int i = 0; i < TD_POOL_SIZE; i ++ ) {
+		cont->TDPool[i].Token = 3 << 8;
+	}
+
 	// Get port count
 	cont->nPorts = cont->CapRegs->HCSParams & 0xF;
 
-
 	// -- Bind IRQ --
 	IRQ_AddHandler(InterruptNum, EHCI_InterruptHandler, cont);
 	cont->InterruptThread = Proc_SpawnWorker(EHCI_int_InterruptThread, cont);
@@ -186,6 +196,8 @@ _error:
 		MM_Deallocate( (tVAddr)cont->CapRegs );
 	if( cont->PeriodicQueue )
 		MM_Deallocate( (tVAddr)cont->PeriodicQueue );
+	if( cont->TDPool )
+		MM_Deallocate( (tVAddr)cont->TDPool );
 	return 2;
 }
 
@@ -292,7 +304,7 @@ void *EHCI_InitInterrupt(void *Ptr, int Endpoint, int bOutbound, int Period,
 
 	// Allocate TD for the data
 	tEHCI_qTD *td = EHCI_int_AllocateTD(Cont, (bOutbound ? PID_OUT : PID_IN), Buf, Length, Cb, CbData);
-	EHCI_int_AppendTD(qh, td);
+	EHCI_int_AppendTD(Cont, qh, td);
 
 	// Insert into the periodic list
 	for( int i = 0; i < PERIODIC_SIZE; i += Period )
@@ -422,13 +434,18 @@ void *EHCI_SendControl(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData,
 	{
 		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_status->Token |= (1 << 15);
 	}
 
 	// Append TDs
-	EHCI_int_AppendTD(Dest, td_setup);
-	if( td_data )
-		EHCI_int_AppendTD(Dest, td_data);
-	EHCI_int_AppendTD(Dest, td_status);
+	if( td_data ) {
+		td_setup->Link = MM_GetPhysAddr(td_data);
+		td_data->Link = MM_GetPhysAddr(td_status) | 1;
+	}
+	else {
+		td_setup->Link = MM_GetPhysAddr(td_status) | 1;
+	}
+	EHCI_int_AppendTD(Cont, Dest, td_setup);
 
 	return td_status;
 }
@@ -444,7 +461,7 @@ void *EHCI_SendBulk(void *Ptr, void *Dest, tUSBHostCb Cb, void *CbData, int Dir,
 	
 	// Allocate single TD
 	tEHCI_qTD	*td = EHCI_int_AllocateTD(Cont, (Dir ? PID_OUT : PID_IN), Data, Length, Cb, CbData);
-	EHCI_int_AppendTD(Dest, td);	
+	EHCI_int_AppendTD(Cont, Dest, td);	
 
 	return td;
 }
@@ -495,9 +512,32 @@ int EHCI_RootHub_GetPortStatus(void *Ptr, int Port, int Flag)
 // --------------------------------------------------------------------
 // Internals
 // --------------------------------------------------------------------
+tEHCI_qTD *EHCI_int_GetTDFromPhys(tEHCI_Controller *Cont, Uint32 Addr)
+{
+	if( Addr == 0 )	return NULL;
+	LOG("%p + (%x - %x)", Cont->TDPool, Addr, MM_GetPhysAddr(Cont->TDPool));
+	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)
 {
-	UNIMPLEMENTED();
+//	Semaphore_Wait(&Cont->TDSemaphore, 1);
+	Mutex_Acquire(&Cont->TDPoolMutex);
+	for( int i = 0; i < TD_POOL_SIZE; i ++ )
+	{
+		if( ((Cont->TDPool[i].Token >> 8) & 3) != 3 )
+			continue ;
+		Cont->TDPool[i].Token = (PID << 8) | (Length << 16);
+		// NOTE: Assumes that `Length` is <= PAGE_SIZE
+		Cont->TDPool[i].Pages[0] = MM_GetPhysAddr(Data);
+		if( (Cont->TDPool[i].Pages[0] & (PAGE_SIZE-1)) + Length - 1 > PAGE_SIZE )
+			Cont->TDPool[i].Pages[1] = MM_GetPhysAddr((char*)Data + Length - 1) & ~(PAGE_SIZE-1);
+		Mutex_Release(&Cont->TDPoolMutex);
+		LOG("Allocated %p for PID %i on %p", &Cont->TDPool[i], PID, Cont);
+		return &Cont->TDPool[i];
+	}
+
+	Mutex_Release(&Cont->TDPoolMutex);
 	return NULL;
 }
 
@@ -506,20 +546,58 @@ void EHCI_int_DeallocateTD(tEHCI_Controller *Cont, tEHCI_qTD *TD)
 	UNIMPLEMENTED();
 }
 
-void EHCI_int_AppendTD(tEHCI_QH *QH, tEHCI_qTD *TD)
+void EHCI_int_AppendTD(tEHCI_Controller *Cont, tEHCI_QH *QH, tEHCI_qTD *TD)
 {
-	UNIMPLEMENTED();
+	tEHCI_qTD	*ptd = NULL;
+	Uint32	link = QH->CurrentTD;
+
+	// TODO: Need locking and validation here
+	while( link && !(link & 1) )
+	{
+		ptd = EHCI_int_GetTDFromPhys(Cont, link);
+		link = ptd->Link;
+	}
+	// TODO: Figure out how to follow this properly
+	if( !ptd ) {
+		QH->CurrentTD = MM_GetPhysAddr(TD);
+	}
+	else
+		ptd->Link = MM_GetPhysAddr(TD);
 }
 
 tEHCI_QH *EHCI_int_AllocateQH(tEHCI_Controller *Cont, int Endpoint, size_t MaxPacketSize)
 {
-	UNIMPLEMENTED();
+	tEHCI_QH	*ret;
+	Mutex_Acquire(&Cont->QHPoolMutex);
+	for( int i = 0; i < QH_POOL_SIZE; i ++ )
+	{
+		if( !MM_GetPhysAddr( Cont->QHPools[i/QH_POOL_NPERPAGE] ) ) {
+			tPAddr	tmp;
+			Cont->QHPools[i/QH_POOL_NPERPAGE] = (void*)MM_AllocDMA(1, 32, &tmp);
+			memset(Cont->QHPools[i/QH_POOL_NPERPAGE], 0, PAGE_SIZE);
+		}
+
+		ret = &Cont->QHPools[i/QH_POOL_NPERPAGE][i%QH_POOL_NPERPAGE];
+		if( ret->HLink == 0 ) {
+			ret->HLink = 1;
+			ret->CurrentTD = 0;
+			ret->Overlay.Link = 1;
+			ret->Endpoint = (Endpoint >> 4) | ((Endpoint & 0xF) << 8)
+				| (MaxPacketSize << 16);
+			// 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;
+		}
+	}
+	Mutex_Release(&Cont->QHPoolMutex);
 	return NULL;
 }
 
 void EHCI_int_DeallocateQH(tEHCI_Controller *Cont, tEHCI_QH *QH)
 {
-	UNIMPLEMENTED();
+	// TODO: Ensure it's unused (somehow)
+	QH->HLink = 0;
 }
 
 void EHCI_int_HandlePortConnectChange(tEHCI_Controller *Cont, int Port)
diff --git a/KernelLand/Modules/USB/EHCI/ehci.h b/KernelLand/Modules/USB/EHCI/ehci.h
index 6c6d8a81..9cc4bb25 100644
--- a/KernelLand/Modules/USB/EHCI/ehci.h
+++ b/KernelLand/Modules/USB/EHCI/ehci.h
@@ -250,19 +250,30 @@ struct sEHCI_QH
 	Uint32	Endpoint;
 	Uint32	EndpointExt;
 	Uint32	CurrentTD;
-	tEHCI_qTD	Overlay;
+	struct {
+		Uint32	Link;
+		Uint32	Link2;
+		Uint32	Token;
+		Uint32	Pages[5];
+	}	Overlay;
 	struct {
 		Uint8	IntOfs;
 		Uint8	IntPeriodPow;
 		tEHCI_QH	*Next;
 	} Impl;
 } __attribute__((aligned(32)));
-// sizeof = 48 (64)
+// sizeof = 48 (round up to 64)
 
 #define PID_OUT 	0
 #define PID_IN  	1
 #define PID_SETUP	2
 
+#define TD_POOL_SIZE	(PAGE_SIZE/sizeof(tEHCI_qTD))
+// - 256 addresses * 16 endpoints
+#define QH_POOL_SIZE	(256*16)
+#define QH_POOL_PAGES	(QH_POOL_SIZE*sizeof(tEHCI_QH)/PAGE_SIZE)
+#define QH_POOL_NPERPAGE	(PAGE_SIZE/sizeof(tEHCI_QH))
+
 struct sEHCI_Controller
 {
 	tUSBHub	*RootHub;
@@ -280,8 +291,10 @@ struct sEHCI_Controller
 	Uint32  	*PeriodicQueue;
 	tEHCI_QH	*PeriodicQueueV[PERIODIC_SIZE];
 	
-	tEHCI_QH	*QHPools[(256*16)*sizeof(tEHCI_QH)/PAGE_SIZE];	// [PAGE_SIZE/64]
-	tEHCI_qTD	*TDPool[PAGE_SIZE/sizeof(tEHCI_qTD)];
+	tMutex	QHPoolMutex;
+	tEHCI_QH	*QHPools[QH_POOL_PAGES];	// [PAGE_SIZE/64]
+	tMutex	TDPoolMutex;
+	tEHCI_qTD	*TDPool;	// [TD_POOL_SIZE]
 };
 
 #endif