+ // Active? Ok.
+ if( td->Control & (1 << 23) ) {
+ prev = td;
+ continue ;
+ }
+
+ LOG("Removed %p from QH %p", td, QH);
+
+ if( !prev )
+ QH->Child = td->Link;
+ else
+ prev->Link = td->Link;
+ cur_td = td->Link;
+ }
+
+ // 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( ;; )
+ {
+ LOG("zzzzz....");
+ // 0 = Take all
+ Semaphore_Wait(&gUHCI_InterruptSempahore, 0);
+ LOG("Huh?");
+
+ for( int i = 0; i < NUM_TDs; i ++ )