+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 = MM_MapTemp(TD->BufferPointer);
+ dest = 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( dest );
+ dest = MM_MapTemp(info->SecondPage);
+ memcpy(dest, src + src_ofs + part_len, byte_count - part_len);
+ }
+ MM_FreeTemp( src );
+ MM_FreeTemp( dest );
+ }
+
+ // Callback
+ if( info->Callback != NULL )
+ {
+ LOG("Calling cb %p (%i bytes)", info->Callback, byte_count);
+ void *ptr = MM_MapTemp( TD->BufferPointer );
+ info->Callback( info->CallbackPtr, ptr, byte_count );
+ MM_FreeTemp( 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);
+ LOG("Huh?");
+
+ for( int i = 0; i < NUM_TDs; i ++ )
+ {
+ tUHCI_TD *td;
+
+ td = &gaUHCI_TDPool[i];
+
+ // Skip completely inactive TDs
+ if( td->_info.bActive == 0 ) continue ;
+ // Skip ones that are still in use
+ if( td->Control & TD_CTL_ACTIVE ) continue ;
+
+ nSeen ++;
+
+ // If no callback/alt buffer, mark as free and move on
+ if( td->_info.ExtraInfo )
+ {
+ UHCI_int_HandleTDComplete(Cont, td);
+ }
+
+ // 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.QueueIndex);
+ // TODO: Flip toggle?
+ td->Control |= TD_CTL_ACTIVE;
+ // Add back into controller's interrupt list
+ UHCI_int_AppendTD(
+ Cont,
+ Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex,
+ td
+ );
+ continue ;
+ }
+
+ // Clean up
+ if( td->_info.bFreePointer )
+ MM_DerefPhys( td->BufferPointer );
+
+ // Clean up
+ LOG("Cleaned %p (->Control = %x)", td, td->Control);
+ td->_info.bActive = 0;
+ }
+
+ if( nSeen == 0 ) {
+ LOG("Why did you wake me?");
+ }
+ }
+}
+