KernelLand/Modules/USB/UHCI/uhci.c

   1 /*
   2  * Acess 2 USB Stack
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * Universal Host Controller Interface
   6  */
   7 #define DEBUG   0
   8 #define VERSION VER2(0,5)
   9 #include <acess.h>
  10 #include <vfs.h>
  11 #include <drv_pci.h>
  12 #include <modules.h>
  13 #include <usb_host.h>
  14 #include "uhci.h"
  15 #include <timers.h>
  16 #include <semaphore.h>
  17
  18 // === CONSTANTS ===
  19 #define MAX_CONTROLLERS 4
  20 //#define NUM_TDs       1024
  21 #define NUM_TDs (PAGE_SIZE/sizeof(tUHCI_TD))
  22 #define MAX_PACKET_SIZE 0x400
  23 #define PID_IN  0x69
  24 #define PID_OUT 0xE1
  25 #define PID_SETUP       0x2D
  26
  27 // === PROTOTYPES ===
  28  int    UHCI_Initialise(char **Arguments);
  29 void    UHCI_Cleanup();
  30  int    UHCI_int_InitHost(tUHCI_Controller *Host);
  31 // -- List internals
  32 tUHCI_TD        *UHCI_int_AllocateTD(tUHCI_Controller *Cont);
  33 void    UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_QH *QH, tUHCI_TD *TD);
  34 tUHCI_TD        *UHCI_int_CreateTD(tUHCI_Controller *Cont, int Addr, Uint8 Type, int bTgl, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  35 // --- API
  36 void    *UHCI_InterruptIN(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  37 void    *UHCI_InterruptOUT(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  38 void    UHCI_StopInterrupt(void *Ptr, void *Handle);
  39 void    *UHCI_ControlSETUP(void *Ptr, int Dest, int Tgl, void *Data, size_t Length);
  40 void    *UHCI_ControlOUT(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData, void *Data, size_t Length);
  41 void    *UHCI_ControlIN(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData, void *Data, size_t Length);
  42 void    *UHCI_BulkOUT(void *Ptr, int Dest, int bToggle, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  43 void    *UHCI_BulkIN(void *Ptr, int Dest, int bToggle, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length);
  44
  45 void    UHCI_CheckPortUpdate(void *Ptr);
  46 void    UHCI_int_InterruptThread(void *Unused);
  47 void    UHCI_InterruptHandler(int IRQ, void *Ptr);
  48 //
  49 static void     _OutByte(tUHCI_Controller *Host, int Reg, Uint8 Value);
  50 static void     _OutWord(tUHCI_Controller *Host, int Reg, Uint16 Value);
  51 static void     _OutDWord(tUHCI_Controller *Host, int Reg, Uint32 Value);
  52 static Uint16   _InWord(tUHCI_Controller *Host, int Reg);
  53
  54 // === GLOBALS ===
  55 MODULE_DEFINE(0, VERSION, USB_UHCI, UHCI_Initialise, NULL, "USB_Core", NULL);
  56 tUHCI_TD        *gaUHCI_TDPool;
  57 tUHCI_Controller        gUHCI_Controllers[MAX_CONTROLLERS];
  58 tUSBHostDef     gUHCI_HostDef = {
  59         .InterruptIN   = UHCI_InterruptIN,
  60         .InterruptOUT  = UHCI_InterruptOUT,
  61         .StopInterrupt = UHCI_StopInterrupt,
  62
  63         .ControlSETUP = UHCI_ControlSETUP,
  64         .ControlIN    = UHCI_ControlIN,
  65         .ControlOUT   = UHCI_ControlOUT,
  66
  67         .BulkOUT = UHCI_BulkOUT,
  68         .BulkIN = UHCI_BulkIN,
  69
  70         .CheckPorts = UHCI_CheckPortUpdate
  71         };
  72 tSemaphore      gUHCI_InterruptSempahore;
  73
  74 // === CODE ===
  75 /**
  76  * \fn int UHCI_Initialise()
  77  * \brief Called to initialise the UHCI Driver
  78  */
  79 int UHCI_Initialise(char **Arguments)
  80 {
  81          int    i=0, id=-1;
  82          int    ret;
  83
  84         ENTER("");
  85
  86         // Initialise with no maximum value
  87         Semaphore_Init( &gUHCI_InterruptSempahore, 0, 0, "UHCI", "Interrupt Queue");
  88
  89         if( PCI_GetDeviceByClass(0x0C0300, 0xFFFFFF, -1) < 0 )
  90         {
  91                 LEAVE('i', MODULE_ERR_NOTNEEDED);
  92                 return MODULE_ERR_NOTNEEDED;
  93         }
  94
  95         {
  96                 tPAddr  tmp;
  97                 gaUHCI_TDPool = (void *) MM_AllocDMA(1, 32, &tmp);
  98         }
  99
 100         // Enumerate PCI Bus, getting a maximum of `MAX_CONTROLLERS` devices
 101         while( (id = PCI_GetDeviceByClass(0x0C0300, 0xFFFFFF, id)) >= 0 && i < MAX_CONTROLLERS )
 102         {
 103                 tUHCI_Controller        *cinfo = &gUHCI_Controllers[i];
 104                 Uint32  base_addr;
 105                 // NOTE: Check "protocol" from PCI?
 106
 107                 cinfo->PciId = id;
 108                 base_addr = PCI_GetBAR(id, 4);
 109
 110                 if( base_addr & 1 )
 111                 {
 112                         cinfo->IOBase = base_addr & ~1;
 113                         cinfo->MemIOMap = NULL;
 114                 }
 115                 else
 116                 {
 117                         cinfo->MemIOMap = (void*)MM_MapHWPages(base_addr, 1);
 118                 }
 119                 cinfo->IRQNum = PCI_GetIRQ(id);
 120
 121                 Log_Debug("UHCI", "Controller PCI #%i: IO Base = 0x%x, IRQ %i",
 122                         id, base_addr, cinfo->IRQNum);
 123
 124                 IRQ_AddHandler(cinfo->IRQNum, UHCI_InterruptHandler, cinfo);
 125
 126                 // Initialise Host
 127                 ret = UHCI_int_InitHost(cinfo);
 128                 // Detect an error
 129                 if(ret != 0) {
 130                         LEAVE('i', ret);
 131                         return ret;
 132                 }
 133
 134                 // Spin off interrupt handling thread
 135                 Proc_SpawnWorker( UHCI_int_InterruptThread, cinfo );
 136
 137
 138                 cinfo->RootHub = USB_RegisterHost(&gUHCI_HostDef, cinfo, 2);
 139                 LOG("cinfo->RootHub = %p", cinfo->RootHub);
 140
 141                 i ++;
 142         }
 143
 144         if(i == MAX_CONTROLLERS) {
 145                 Log_Warning("UHCI", "Over "EXPAND_STR(MAX_CONTROLLERS)" UHCI controllers detected, ignoring rest");
 146         }
 147         LEAVE('i', MODULE_ERR_OK);
 148         return MODULE_ERR_OK;
 149 }
 150
 151 /**
 152  * \fn void UHCI_Cleanup()
 153  * \brief Called just before module is unloaded
 154  */
 155 void UHCI_Cleanup()
 156 {
 157 }
 158
 159 /**
 160  * \brief Initialises a UHCI host controller
 161  * \param Host  Pointer - Host to initialise
 162  */
 163 int UHCI_int_InitHost(tUHCI_Controller *Host)
 164 {
 165         ENTER("pHost", Host);
 166
 167         // - 1 Page, 32-bit address
 168         // - 1 page = 1024  4 byte entries
 169         Host->FrameList = (void *) MM_AllocDMA(1, 32, &Host->PhysFrameList);
 170         if( !Host->FrameList ) {
 171                 Log_Warning("UHCI", "Unable to allocate frame list, aborting");
 172                 LEAVE('i', -1);
 173                 return -1;
 174         }
 175
 176         Host->TDQHPage = (void *) MM_AllocDMA(1, 32, &Host->PhysTDQHPage);
 177         if( !Host->TDQHPage ) {
 178                 // TODO: Clean up
 179                 Log_Warning("UHCI", "Unable to allocate QH page, aborting");
 180                 LEAVE('i', -1);
 181                 return -1;
 182         }
 183
 184         // Fill frame list
 185         // - The numbers 0...31, but bit reversed (16 (0b1000) = 1 (0b00001)
 186         const int       dest_offsets[] = {
 187                 0,16,8,24,4,20,12,28,2,18,10,26,6,22,14,30,
 188                 1,17,9,25,5,21,13,29,3,19,11,27,7,23,15,31
 189                 };
 190         for( int i = 0; i < 1024; i ++ ) {
 191                 Uint32  addr = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->ControlQH );
 192                 Host->FrameList[i] = addr | 2;
 193         }
 194         for( int i = 0; i < 64; i ++ ) {
 195                  int    ofs = dest_offsets[ i & (32-1) ];
 196                 Uint32  addr = Host->PhysTDQHPage + ofs * sizeof(tUHCI_QH);
 197                 Host->FrameList[  0 + i*4] = addr | 2;
 198                 Host->FrameList[256 + i*4] = addr | 2;
 199                 Host->FrameList[512 + i*4] = addr | 2;
 200                 Host->FrameList[768 + i*4] = addr | 2;
 201         }
 202
 203         // Build up interrupt binary tree
 204         {
 205                 tUHCI_QH        *dest = Host->TDQHPage->InterruptQHs;
 206                 Uint32  destphys = Host->PhysTDQHPage;
 207
 208                 // Set up next pointer to index to i/2 in the next step
 209                 for( int _count = 64; _count > 1; _count /= 2 )
 210                 {
 211                         for( int i = 0; i < _count; i ++ ) {
 212                                 dest[i].Next = destphys + (_count + i/2) * sizeof(tUHCI_QH) + 2;
 213                                 dest[i].Child = 1;
 214                         }
 215                         dest += _count; destphys += _count * sizeof(tUHCI_QH);
 216                 }
 217                 // Skip padding, and move to control QH
 218                 dest->Next = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->BulkQH ) | 2;
 219                 dest->Child = 1;
 220         }
 221
 222         // Set up control and bulk queues
 223         Host->TDQHPage->ControlQH.Next = MM_GetPhysAddr( (tVAddr)&Host->TDQHPage->BulkQH ) | 2;
 224         Host->TDQHPage->ControlQH.Child = 1;
 225         Host->TDQHPage->BulkQH.Next = 1;
 226         Host->TDQHPage->BulkQH.Child = 1;
 227
 228         // Global reset
 229         _OutWord( Host, USBCMD, 4 );
 230         Time_Delay(10);
 231         _OutWord( Host, USBCMD, 0 );
 232
 233         // Allocate Frame List
 234         // Set frame length to 1 ms
 235         _OutByte( Host, SOFMOD, 64 );
 236
 237         // Set Frame List
 238         _OutDWord( Host, FLBASEADD, Host->PhysFrameList );
 239         _OutWord( Host, FRNUM, 0 );
 240
 241         // Enable Interrupts
 242         _OutWord( Host, USBINTR, 0x000F );
 243         PCI_ConfigWrite( Host->PciId, 0xC0, 2, 0x2000 );
 244
 245         // Enable processing
 246         _OutWord( Host, USBCMD, 0x0001 );
 247
 248         LEAVE('i', 0);
 249         return 0;
 250 }
 251
 252 // --------------------------------------------------------------------
 253 // TDs and QH Allocation/Appending
 254 // --------------------------------------------------------------------
 255 tUHCI_TD *UHCI_int_AllocateTD(tUHCI_Controller *Cont)
 256 {
 257         static tMutex   lock;
 258         Mutex_Acquire( &lock );
 259         for( int i = 0; i < NUM_TDs; i ++ )
 260         {
 261                 if(gaUHCI_TDPool[i]._info.bActive == 0)
 262                 {
 263                         gaUHCI_TDPool[i].Link = 1;
 264                         gaUHCI_TDPool[i].Control = (1 << 23);
 265                         gaUHCI_TDPool[i]._info.bActive = 1;
 266                         gaUHCI_TDPool[i]._info.QueueIndex = 128;
 267                         Mutex_Release( &lock );
 268                         return &gaUHCI_TDPool[i];
 269                 }
 270         }
 271         Mutex_Release( &lock );
 272         return NULL;
 273 }
 274
 275 void UHCI_int_AppendTD(tUHCI_Controller *Cont, tUHCI_QH *QH, tUHCI_TD *TD)
 276 {
 277         static tMutex   lock;   // TODO: Should I use a shortlock (avoid being preempted)
 278
 279         Mutex_Acquire(&lock);
 280
 281         // Ensure that there is an interrupt for each used frame
 282         TD->Control |= (1 << 24);
 283         TD->_info.QueueIndex = ((tVAddr)QH - (tVAddr)Cont->TDQHPage->InterruptQHs) / sizeof(tUHCI_QH);
 284
 285         // Stop controller
 286         _OutWord( Cont, USBCMD, 0x0000 );
 287
 288         // Add
 289         TD->Link = 1;
 290         if( QH->Child & 1 ) {
 291                 QH->Child = MM_GetPhysAddr( (tVAddr)TD );
 292         }
 293         else {
 294                 // Depth first
 295                 QH->_LastItem->Link = MM_GetPhysAddr( (tVAddr)TD ) | 4;
 296         }
 297         QH->_LastItem = TD;
 298
 299         // Reenable controller
 300         _OutWord( Cont, USBCMD, 0x0001 );
 301
 302         // DEBUG!
 303         LOG("QH(%p)->Child = %x", QH, QH->Child);
 304         LOG("TD(%p)->Control = %x, ->Link = %x", TD, TD->Control, TD->Link);
 305
 306         Mutex_Release(&lock);
 307 }
 308
 309 /**
 310  * \brief Send a transaction to the USB bus
 311  * \param Cont  Controller pointer
 312  * \param Addr  Function Address * 16 + Endpoint
 313  * \param bTgl  Data toggle value
 314  */
 315 tUHCI_TD *UHCI_int_CreateTD(
 316         tUHCI_Controller *Cont, int Addr, Uint8 Type, int bTgl,
 317         tUSBHostCb Cb, void *CbData, void *Data, size_t Length)
 318 {
 319         tUHCI_TD        *td;
 320         tUHCI_ExtraTDInfo       *info = NULL;
 321
 322         if( Length > 0x400 ) {
 323                 Log_Error("UHCI", "Transaction length too large (%i > 0x400)", Length);
 324                 return NULL;    // Controller allows up to 0x500, but USB doesn't
 325         }
 326
 327         td = UHCI_int_AllocateTD(Cont);
 328         if( !td ) {
 329                 Log_Error("UHCI", "No avaliable TDs, transaction dropped");
 330                 return NULL;
 331         }
 332
 333         LOG("TD %p %i bytes, Type %x to 0x%x",
 334                 td, Length, Type, Addr);
 335
 336         td->Control = (Length - 1) & 0x7FF;
 337         td->Control |= (1 << 23);       // Active set
 338         td->Control |= (3 << 27);       // 3 retries
 339         td->Token  = ((Length - 1) & 0x7FF) << 21;
 340         td->Token |= (bTgl & 1) << 19;
 341         td->Token |= (Addr & 0xF) << 15;
 342         td->Token |= ((Addr/16) & 0xFF) << 8;
 343         td->Token |= Type;
 344
 345         if(
 346                 ((tVAddr)Data & (PAGE_SIZE-1)) + Length > PAGE_SIZE
 347         #if PHYS_BITS > 32
 348                 || MM_GetPhysAddr( (tVAddr)Data ) >> 32
 349         #endif
 350                 )
 351         {
 352                 td->BufferPointer = MM_AllocPhysRange(1, 32);
 353
 354                 LOG("Allocated page %x", td->BufferPointer);
 355
 356                 if( Type == 0x69 )      // IN token
 357                 {
 358                         LOG("Relocated IN");
 359                         info = calloc( sizeof(tUHCI_ExtraTDInfo), 1 );
 360                         info->Offset = ((tVAddr)Data & (PAGE_SIZE-1));
 361                         info->FirstPage = MM_GetPhysAddr( (tVAddr)Data );
 362                         info->SecondPage = MM_GetPhysAddr( (tVAddr)Data + Length - 1 );
 363                 }
 364                 else
 365                 {
 366                         LOG("Relocated OUT/SETUP");
 367                         tVAddr  ptr = MM_MapTemp(td->BufferPointer);
 368                         memcpy( (void*)ptr, Data, Length );
 369                         MM_FreeTemp(ptr);
 370                         td->Control |= (1 << 24);
 371                 }
 372                 td->_info.bFreePointer = 1;
 373         }
 374         else
 375         {
 376                 td->BufferPointer = MM_GetPhysAddr( (tVAddr)Data );
 377                 td->_info.bFreePointer = 0;
 378         }
 379
 380         // Interrupt on completion
 381         if( Cb )
 382         {
 383                 if( !info )
 384                         info = calloc( sizeof(tUHCI_ExtraTDInfo), 1 );
 385                 LOG("IOC Cb=%p CbData=%p", Cb, CbData);
 386                 info->Callback = Cb;
 387                 info->CallbackPtr = CbData;
 388         }
 389
 390         if( info ) {
 391                 LOG("info = %p", info);
 392                 td->Control |= (1 << 24);
 393                 td->_info.ExtraInfo = info;
 394         }
 395
 396         return td;
 397 }
 398
 399 void UHCI_int_SetInterruptPoll(tUHCI_Controller *Cont, tUHCI_TD *TD, int Period)
 400 {
 401         tUHCI_QH        *qh;
 402         const int       qh_offsets[] = { 0, 64, 96, 112, 120, 124, 126};
 403 //      const int       qh_sizes[]   = {64, 32, 16,   8,   4,   2,   1};
 404
 405         // Bounds limit
 406         if( Period < 0 )        return ;
 407         if( Period > 256 )      Period = 256;
 408
 409         // Get the log base2 of the period
 410          int    period_slot = 0;
 411         while( Period >>= 1 )   period_slot ++;
 412
 413         // Adjust for the 4ms minimum period
 414         if( period_slot < 2 )   period_slot = 0;
 415         else    period_slot -= 2;
 416
 417         TD->_info.QueueIndex = qh_offsets[period_slot]; // Actually re-filled in _AppendTD, but meh
 418         qh = Cont->TDQHPage->InterruptQHs + TD->_info.QueueIndex;
 419         // TODO: Find queue with lowest load
 420
 421         LOG("period_slot = %i, QueueIndex = %i",
 422                 period_slot, TD->_info.QueueIndex);
 423
 424         // Stop any errors causing the TD to stop (NAK will error)
 425         // - If the device goes away, the interrupt should be stopped anyway
 426         TD->Control &= ~(3 << 27);
 427
 428         UHCI_int_AppendTD(Cont, qh, TD);
 429 }
 430
 431 void *UHCI_InterruptIN(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 432 {
 433         tUHCI_TD        *td;
 434
 435         if( Period < 0 )        return NULL;
 436
 437         ENTER("pPtr xDest iPeriod pCb pCbData pBuf, iLength",
 438                 Ptr, Dest, Period, Cb, CbData, Buf, Length);
 439
 440         // TODO: Data toggle?
 441         td = UHCI_int_CreateTD(Ptr, Dest, PID_IN, 0, Cb, CbData, Buf, Length);
 442         if( !td )       return NULL;
 443
 444         UHCI_int_SetInterruptPoll(Ptr, td, Period);
 445
 446         LEAVE('p', td);
 447         return td;
 448 }
 449 // TODO: Does interrupt OUT make sense?
 450 void *UHCI_InterruptOUT(void *Ptr, int Dest, int Period, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 451 {
 452         tUHCI_TD        *td;
 453
 454         if( Period < 0 )        return NULL;
 455
 456         ENTER("pPtr xDest iPeriod pCb pCbData pBuf, iLength",
 457                 Ptr, Dest, Period, Cb, CbData, Buf, Length);
 458
 459         // TODO: Data toggle?
 460         td = UHCI_int_CreateTD(Ptr, Dest, PID_OUT, 0, Cb, CbData, Buf, Length);
 461         if( !td )       return NULL;
 462
 463         UHCI_int_SetInterruptPoll(Ptr, td, Period);
 464
 465         LEAVE('p', td);
 466         return td;
 467 }
 468
 469 void UHCI_StopInterrupt(void *Ptr, void *Handle)
 470 {
 471         // TODO: Stop interrupt transaction
 472         Log_Error("UHCI", "TODO: Implement UHCI_StopInterrupt");
 473 }
 474
 475 void *UHCI_ControlSETUP(void *Ptr, int Dest, int Tgl, void *Data, size_t Length)
 476 {
 477         tUHCI_Controller        *Cont = Ptr;
 478         tUHCI_QH        *qh = &Cont->TDQHPage->ControlQH;
 479         tUHCI_TD        *td;
 480
 481         ENTER("pPtr xDest iTgl pData iLength", Ptr, Dest, Tgl, Data, Length);
 482
 483         td = UHCI_int_CreateTD(Cont, Dest, PID_SETUP, Tgl, NULL, NULL, Data, Length);
 484         UHCI_int_AppendTD(Cont, qh, td);
 485
 486         LEAVE('p', td);
 487
 488         return td;
 489 }
 490 void *UHCI_ControlOUT(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData, void *Data, size_t Length)
 491 {
 492         tUHCI_Controller        *Cont = Ptr;
 493         tUHCI_QH        *qh = &Cont->TDQHPage->ControlQH;
 494         tUHCI_TD        *td;
 495
 496         ENTER("pPtr xDest iTgl pCb pCbData pData iLength", Ptr, Dest, Tgl, Cb, CbData, Data, Length);
 497
 498         td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, Tgl, Cb, CbData, Data, Length);
 499         UHCI_int_AppendTD(Cont, qh, td);
 500
 501         LEAVE('p', td);
 502         return td;
 503 }
 504 void *UHCI_ControlIN(void *Ptr, int Dest, int Tgl, tUSBHostCb Cb, void *CbData, void *Data, size_t Length)
 505 {
 506         tUHCI_Controller        *Cont = Ptr;
 507         tUHCI_QH        *qh = &Cont->TDQHPage->ControlQH;
 508         tUHCI_TD        *td;
 509
 510         ENTER("pPtr xDest iTgl pCb pCbData pData iLength", Ptr, Dest, Tgl, Cb, CbData, Data, Length);
 511
 512         td = UHCI_int_CreateTD(Cont, Dest, PID_IN, !!Tgl, Cb, CbData, Data, Length);
 513         UHCI_int_AppendTD(Cont, qh, td);
 514
 515         LEAVE('p', td);
 516         return td;
 517 }
 518
 519 void *UHCI_BulkOUT(void *Ptr, int Dest, int bToggle, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 520 {
 521         tUHCI_Controller        *Cont = Ptr;
 522         tUHCI_QH        *qh = &Cont->TDQHPage->BulkQH;
 523         tUHCI_TD        *td;
 524         char    *src = Buf;
 525
 526         ENTER("pPtr xDest ibToggle pCb pCbData pData iLength", Ptr, Dest, bToggle, Cb, CbData, Buf, Length);
 527
 528         while( Length > MAX_PACKET_SIZE )
 529         {
 530                 LOG("MaxPacket (rem = %i)", Length);
 531                 td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, bToggle, NULL, NULL, src, MAX_PACKET_SIZE);
 532                 UHCI_int_AppendTD(Cont, qh, td);
 533
 534                 bToggle = !bToggle;
 535                 Length -= MAX_PACKET_SIZE;
 536                 src += MAX_PACKET_SIZE;
 537         }
 538
 539         LOG("Final");
 540         td = UHCI_int_CreateTD(Cont, Dest, PID_OUT, bToggle, NULL, NULL, src, Length);
 541         UHCI_int_AppendTD(Cont, qh, td);
 542
 543         LEAVE('p', td);
 544         return td;
 545 }
 546 void *UHCI_BulkIN(void *Ptr, int Dest, int bToggle, tUSBHostCb Cb, void *CbData, void *Buf, size_t Length)
 547 {
 548         tUHCI_Controller        *Cont = Ptr;
 549         tUHCI_QH        *qh = &Cont->TDQHPage->BulkQH;
 550         tUHCI_TD        *td;
 551         char    *dst = Buf;
 552
 553         ENTER("pPtr xDest ibToggle pCb pCbData pData iLength", Ptr, Dest, bToggle, Cb, CbData, Buf, Length);
 554         while( Length > MAX_PACKET_SIZE )
 555         {
 556                 LOG("MaxPacket (rem = %i)", Length);
 557                 td = UHCI_int_CreateTD(Cont, Dest, PID_IN, bToggle, NULL, NULL, dst, MAX_PACKET_SIZE);
 558                 UHCI_int_AppendTD(Cont, qh, td);
 559
 560                 bToggle = !bToggle;
 561                 Length -= MAX_PACKET_SIZE;
 562                 dst += MAX_PACKET_SIZE;
 563         }
 564
 565         LOG("Final");
 566         td = UHCI_int_CreateTD(Cont, Dest, PID_IN, bToggle, NULL, NULL, dst, Length);
 567         UHCI_int_AppendTD(Cont, qh, td);
 568
 569         LEAVE('p', td);
 570         return td;
 571 }
 572
 573 // === INTERNAL FUNCTIONS ===
 574 void UHCI_CheckPortUpdate(void *Ptr)
 575 {
 576         tUHCI_Controller        *Host = Ptr;
 577         // Enable ports
 578         for( int i = 0; i < 2; i ++ )
 579         {
 580                  int    port = PORTSC1 + i*2;
 581                 Uint16  status;
 582
 583                 status = _InWord(Host, port);
 584                 // Check for port change
 585                 if( !(status & 0x0002) )        continue;
 586                 _OutWord(Host, port, 0x0002);
 587
 588                 // Check if the port is connected
 589                 if( !(status & 1) )
 590                 {
 591                         // Tell the USB code it's gone.
 592                         USB_DeviceDisconnected(Host->RootHub, i);
 593                         continue;
 594                 }
 595                 else
 596                 {
 597                         LOG("Port %i has something", i);
 598                         // Reset port (set bit 9)
 599                         LOG("Reset");
 600                         _OutWord(Host, port, 0x0200);
 601                         Time_Delay(50); // 50ms delay
 602                         _OutWord(Host, port, _InWord(Host, port) & ~0x0200);
 603                         // Enable port
 604                         LOG("Enable");
 605                         Time_Delay(50); // 50ms delay
 606                         _OutWord(Host, port, _InWord(Host, port) | 0x0004);
 607                         // Tell USB there's a new device
 608                         USB_DeviceConnected(Host->RootHub, i);
 609                 }
 610         }
 611 }
 612
 613 tUHCI_TD *UHCI_int_GetTDFromPhys(tUHCI_Controller *Controller, Uint32 PAddr)
 614 {
 615         if( PAddr >= Controller->PhysTDQHPage && PAddr < Controller->PhysTDQHPage + PAGE_SIZE )
 616         {
 617                 PAddr -= Controller->PhysTDQHPage;
 618                 PAddr -= (128+2)*sizeof(tUHCI_QH);
 619                 if( PAddr > PAGE_SIZE ) return NULL;    // Wrapping will bring above page size
 620                 PAddr /= sizeof(tUHCI_TD);
 621                 return &Controller->TDQHPage->LocalTDPool[PAddr];
 622         }
 623
 624
 625         tPAddr  global_pool = MM_GetPhysAddr( (tVAddr)gaUHCI_TDPool );
 626
 627         if( PAddr < global_pool || PAddr >= global_pool + PAGE_SIZE )   return NULL;
 628
 629         PAddr -= global_pool;
 630         PAddr /= sizeof(tUHCI_TD);
 631         return &gaUHCI_TDPool[PAddr];
 632 }
 633
 634 void UHCI_int_CleanQH(tUHCI_Controller *Cont, tUHCI_QH *QH)
 635 {
 636         tUHCI_TD        *td, *prev = NULL;
 637         Uint32  cur_td;
 638
 639         // Disable controller
 640         _OutWord( Cont, USBCMD, 0x0000 );
 641
 642         // Scan QH list
 643         cur_td = QH->Child;
 644         while( !(cur_td & 1) )
 645         {
 646                 td = UHCI_int_GetTDFromPhys(Cont, cur_td);
 647                 if(!td) {
 648                         Log_Warning("UHCI", "_int_CleanQH: QH %p contains TD %x, which was not from a pool",
 649                                 QH, cur_td);
 650                         break ;
 651                 }
 652
 653                 // Active? Ok.
 654                 if( td->Control & (1 << 23) ) {
 655                         prev = td;
 656                         continue ;
 657                 }
 658
 659                 LOG("Removed %p from QH %p", td, QH);
 660
 661                 if( !prev )
 662                         QH->Child = td->Link;
 663                 else
 664                         prev->Link = td->Link;
 665                 cur_td = td->Link;
 666         }
 667
 668         // re-enable controller
 669         _OutWord( Cont, USBCMD, 0x0001 );
 670 }
 671
 672 void UHCI_int_HandleTDComplete(tUHCI_Controller *Cont, tUHCI_TD *TD)
 673 {
 674          int    byte_count = (TD->Control & 0x7FF)+1;
 675         tUHCI_ExtraTDInfo       *info = TD->_info.ExtraInfo;
 676
 677         // Handle non page-aligned destination (or with a > 32-bit paddr)
 678         // TODO: Needs fixing for alignment issues
 679         if(info->FirstPage)
 680         {
 681                 char    *src, *dest;
 682                  int    src_ofs = TD->BufferPointer & (PAGE_SIZE-1);
 683                 src = (void *) MM_MapTemp(TD->BufferPointer);
 684                 dest = (void *) MM_MapTemp(info->FirstPage);
 685                 // Check for a single page transfer
 686                 if( byte_count + info->Offset <= PAGE_SIZE )
 687                 {
 688                         LOG("Single page copy %P to %P of %p",
 689                                 td->BufferPointer, info->FirstPage, td);
 690                         memcpy(dest + info->Offset, src + src_ofs, byte_count);
 691                 }
 692                 else
 693                 {
 694                         // Multi-page
 695                         LOG("Multi page copy %P to (%P,%P) of %p",
 696                                 td->BufferPointer, info->FirstPage, info->SecondPage, td);
 697                          int    part_len = PAGE_SIZE - info->Offset;
 698                         memcpy(dest + info->Offset, src + src_ofs, part_len);
 699                         MM_FreeTemp( (tVAddr)dest );
 700                         dest = (void *) MM_MapTemp(info->SecondPage);
 701                         memcpy(dest, src + src_ofs + part_len, byte_count - part_len);
 702                 }
 703                 MM_FreeTemp( (tVAddr)src );
 704                 MM_FreeTemp( (tVAddr)dest );
 705         }
 706
 707         // Callback
 708         if( info->Callback != NULL )
 709         {
 710                 LOG("Calling cb %p", info->Callback);
 711                 void    *ptr = (void *) MM_MapTemp( TD->BufferPointer );
 712                 info->Callback( info->CallbackPtr, ptr, byte_count );
 713                 MM_FreeTemp( (tVAddr)ptr );
 714         }
 715
 716         // Clean up info
 717         free( info );
 718         TD->_info.ExtraInfo = NULL;
 719 }
 720
 721 void UHCI_int_InterruptThread(void *Pointer)
 722 {
 723         tUHCI_Controller        *Cont = Pointer;
 724         Threads_SetName("UHCI Interrupt Handler");
 725         for( ;; )
 726         {
 727                 LOG("zzzzz....");
 728                 // 0 = Take all
 729                 Semaphore_Wait(&gUHCI_InterruptSempahore, 0);
 730                 LOG("Huh?");
 731
 732                 for( int i = 0; i < NUM_TDs; i ++ )
 733                 {
 734                         tUHCI_TD        *td;
 735
 736                         td = &gaUHCI_TDPool[i];
 737
 738                         // Skip completely inactive TDs
 739                         if( td->_info.bActive == 0 )    continue ;
 740                         // Skip ones that are still in use
 741                         if( td->Control & (1 << 23) )   continue ;
 742
 743                         // If no callback/alt buffer, mark as free and move on
 744                         if( td->_info.ExtraInfo )
 745                         {
 746                                 UHCI_int_HandleTDComplete(Cont, td);
 747                         }
 748
 749                         // Handle rescheduling of interrupt TDs
 750                         if( td->_info.QueueIndex <= 127 )
 751                         {
 752                                 LOG("Re-schedule interrupt %p (offset %i)", td, td->_info.QueueIndex);
 753                                 // TODO: Flip toggle?
 754                                 td->Control |= (1 << 23);
 755                                 // Add back into controller's interrupt list
 756                                 UHCI_int_AppendTD(
 757                                         Cont,
 758                                         Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex,
 759                                         td
 760                                         );
 761                                 continue ;
 762                         }
 763
 764                         // Clean up
 765                         if( td->_info.bFreePointer )
 766                                 MM_DerefPhys( td->BufferPointer );
 767
 768                         // Error check
 769                         if( td->Control & 0x00FF0000 ) {
 770                                 LOG("td->control(Status) = %s%s%s%s%s%s%s%s",
 771                                         td->Control & (1 << 23) ? "Active " : "",
 772                                         td->Control & (1 << 22) ? "Stalled " : "",
 773                                         td->Control & (1 << 21) ? "Data Buffer Error " : "",
 774                                         td->Control & (1 << 20) ? "Babble " : "",
 775                                         td->Control & (1 << 19) ? "NAK " : "",
 776                                         td->Control & (1 << 18) ? "CRC Error, " : "",
 777                                         td->Control & (1 << 17) ? "Bitstuff Error, " : "",
 778                                         td->Control & (1 << 16) ? "Reserved " : ""
 779                                         );
 780                                 // Clean up QH (removing all inactive entries)
 781                                 UHCI_int_CleanQH(Cont, Cont->TDQHPage->InterruptQHs + td->_info.QueueIndex);
 782                         }
 783
 784                         // Clean up
 785                         LOG("Cleaned %p (->Control = %x)", td, td->Control);
 786                         td->_info.bActive = 0;
 787                 }
 788         }
 789 }
 790
 791 void UHCI_InterruptHandler(int IRQ, void *Ptr)
 792 {
 793         tUHCI_Controller *Host = Ptr;
 794 //       int    frame = (_InWord(Host, FRNUM) - 1) & 0x3FF;
 795         Uint16  status = _InWord(Host, USBSTS);
 796
 797         // Interrupt-on-completion
 798         if( status & 1 )
 799         {
 800                 // TODO: Support isochronous transfers (will need updating the frame pointer)
 801                 Semaphore_Signal(&gUHCI_InterruptSempahore, 1);
 802         }
 803
 804         LOG("status = 0x%04x", status);
 805         _OutWord(Host, USBSTS, status);
 806 }
 807
 808 void _OutByte(tUHCI_Controller *Host, int Reg, Uint8 Value)
 809 {
 810         if( Host->MemIOMap )
 811                 ((Uint8*)Host->MemIOMap)[Reg] = Value;
 812         else
 813                 outb(Host->IOBase + Reg, Value);
 814 }
 815
 816 void _OutWord(tUHCI_Controller *Host, int Reg, Uint16 Value)
 817 {
 818         if( Host->MemIOMap )
 819                 Host->MemIOMap[Reg/2] = Value;
 820         else
 821                 outw(Host->IOBase + Reg, Value);
 822 }
 823
 824 void _OutDWord(tUHCI_Controller *Host, int Reg, Uint32 Value)
 825 {
 826         if( Host->MemIOMap )
 827                 ((Uint32*)Host->MemIOMap)[Reg/4] = Value;
 828         else
 829                 outd(Host->IOBase + Reg, Value);
 830 }
 831
 832 Uint16 _InWord(tUHCI_Controller *Host, int Reg)
 833 {
 834         if( Host->MemIOMap )
 835                 return Host->MemIOMap[Reg/2];
 836         else
 837                 return inw(Host->IOBase + Reg);
 838 }
 839